289 files changed, 146855 insertions, 0 deletions
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c
new file mode 100644
index 0000000000..4d1b5848a5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c
@@ -0,0 +1,2522 @@
+/** @file
+  The file for AHCI mode of ATA host controller.
+
+  Copyright (c) 2010 - 2014, 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 "AtaAtapiPassThru.h"
+
+/**
+  Read AHCI Operation register.
+
+  @param  PciIo        The PCI IO protocol instance.
+  @param  Offset       The operation register offset.
+
+  @return The register content read.
+
+**/
+UINT32
+EFIAPI
+AhciReadReg (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+
+  ASSERT (PciIo != NULL);
+
+  Data = 0;
+
+  PciIo->Mem.Read (
+               PciIo,
+               EfiPciIoWidthUint32,
+               EFI_AHCI_BAR_INDEX,
+               (UINT64) Offset,
+               1,
+               &Data
+               );
+
+  return Data;
+}
+
+/**
+  Write AHCI Operation register.
+
+  @param  PciIo        The PCI IO protocol instance.
+  @param  Offset       The operation register offset.
+  @param  Data         The data used to write down.
+
+**/
+VOID
+EFIAPI
+AhciWriteReg (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  ASSERT (PciIo != NULL);
+
+  PciIo->Mem.Write (
+               PciIo,
+               EfiPciIoWidthUint32,
+               EFI_AHCI_BAR_INDEX,
+               (UINT64) Offset,
+               1,
+               &Data
+               );
+
+  return ;
+}
+
+/**
+  Do AND operation with the value of AHCI Operation register.
+
+  @param  PciIo        The PCI IO protocol instance.
+  @param  Offset       The operation register offset.
+  @param  AndData      The data used to do AND operation.
+
+**/
+VOID
+EFIAPI
+AhciAndReg (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT32               Offset,
+  IN UINT32               AndData
+  )
+{
+  UINT32 Data;
+
+  ASSERT (PciIo != NULL);
+
+  Data  = AhciReadReg (PciIo, Offset);
+
+  Data &= AndData;
+
+  AhciWriteReg (PciIo, Offset, Data);
+}
+
+/**
+  Do OR operation with the value of AHCI Operation register.
+
+  @param  PciIo        The PCI IO protocol instance.
+  @param  Offset       The operation register offset.
+  @param  OrData       The data used to do OR operation.
+
+**/
+VOID
+EFIAPI
+AhciOrReg (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT32               Offset,
+  IN UINT32               OrData
+  )
+{
+  UINT32 Data;
+
+  ASSERT (PciIo != NULL);
+
+  Data  = AhciReadReg (PciIo, Offset);
+
+  Data |= OrData;
+
+  AhciWriteReg (PciIo, Offset, Data);
+}
+
+/**
+  Wait for the value of the specified MMIO register set to the test value.
+
+  @param  PciIo             The PCI IO protocol instance.
+  @param  Offset            The MMIO address to test.
+  @param  MaskValue         The mask value of memory.
+  @param  TestValue         The test value of memory.
+  @param  Timeout           The time out value for wait memory set, uses 100ns as a unit.
+
+  @retval EFI_TIMEOUT       The MMIO setting is time out.
+  @retval EFI_SUCCESS       The MMIO is correct set.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciWaitMmioSet (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINTN                     Offset,
+  IN  UINT32                    MaskValue,
+  IN  UINT32                    TestValue,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32     Value;
+  UINT64     Delay;
+  BOOLEAN    InfiniteWait;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32 (Timeout, 1000) + 1;
+
+  do {
+    //
+    // Access PCI MMIO space to see if the value is the tested one.
+    //
+    Value = AhciReadReg (PciIo, (UINT32) Offset) & MaskValue;
+
+    if (Value == TestValue) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Wait for the value of the specified system memory set to the test value.
+
+  @param  Address           The system memory address to test.
+  @param  MaskValue         The mask value of memory.
+  @param  TestValue         The test value of memory.
+  @param  Timeout           The time out value for wait memory set, uses 100ns as a unit.
+
+  @retval EFI_TIMEOUT       The system memory setting is time out.
+  @retval EFI_SUCCESS       The system memory is correct set.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciWaitMemSet (
+  IN  EFI_PHYSICAL_ADDRESS      Address,
+  IN  UINT32                    MaskValue,
+  IN  UINT32                    TestValue,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32     Value;
+  UINT64     Delay;
+  BOOLEAN    InfiniteWait;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay =  DivU64x32 (Timeout, 1000) + 1;
+
+  do {
+    //
+    // Access sytem memory to see if the value is the tested one.
+    //
+    // The system memory pointed by Address will be updated by the
+    // SATA Host Controller, "volatile" is introduced to prevent
+    // compiler from optimizing the access to the memory address
+    // to only read once.
+    //
+    Value  = *(volatile UINT32 *) (UINTN) Address;
+    Value &= MaskValue;
+
+    if (Value == TestValue) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Check the memory status to the test value.
+
+  @param[in]       Address           The memory address to test.
+  @param[in]       MaskValue         The mask value of memory.
+  @param[in]       TestValue         The test value of memory.
+  @param[in, out]  Task              Optional. Pointer to the ATA_NONBLOCK_TASK used by
+                                     non-blocking mode. If NULL, then just try once.
+
+  @retval EFI_NOTREADY      The memory is not set.
+  @retval EFI_TIMEOUT       The memory setting retry times out.
+  @retval EFI_SUCCESS       The memory is correct set.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciCheckMemSet (
+  IN     UINTN                     Address,
+  IN     UINT32                    MaskValue,
+  IN     UINT32                    TestValue,
+  IN OUT ATA_NONBLOCK_TASK         *Task
+  )
+{
+  UINT32     Value;
+
+  if (Task != NULL) {
+    Task->RetryTimes--;
+  }
+
+  Value  = *(volatile UINT32 *) Address;
+  Value &= MaskValue;
+
+  if (Value == TestValue) {
+    return EFI_SUCCESS;
+  }
+
+  if ((Task != NULL) && !Task->InfiniteWait && (Task->RetryTimes == 0)) {
+    return EFI_TIMEOUT;
+  } else {
+    return EFI_NOT_READY;
+  }
+}
+
+/**
+  Check if the device is still on port. It also checks if the AHCI controller
+  supports the address and data count will be transferred.
+
+  @param  PciIo            The PCI IO protocol instance.
+  @param  Port             The number of port.
+
+  @retval EFI_SUCCESS      The device is attached to port and the transfer data is
+                           supported by AHCI controller.
+  @retval EFI_UNSUPPORTED  The transfer address and count is not supported by AHCI
+                           controller.
+  @retval EFI_NOT_READY    The physical communication between AHCI controller and device
+                           is not ready.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciCheckDeviceStatus (
+  IN  EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN  UINT8                  Port
+  )
+{
+  UINT32      Data;
+  UINT32      Offset;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SSTS;
+
+  Data   = AhciReadReg (PciIo, Offset) & EFI_AHCI_PORT_SSTS_DET_MASK;
+
+  if (Data == EFI_AHCI_PORT_SSTS_DET_PCE) {
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_READY;
+}
+
+/**
+
+  Clear the port interrupt and error status. It will also clear
+  HBA interrupt status.
+
+  @param      PciIo          The PCI IO protocol instance.
+  @param      Port           The number of port.
+
+**/
+VOID
+EFIAPI
+AhciClearPortStatus (
+  IN  EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN  UINT8                  Port
+  )
+{
+  UINT32 Offset;
+
+  //
+  // Clear any error status
+  //
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SERR;
+  AhciWriteReg (PciIo, Offset, AhciReadReg (PciIo, Offset));
+
+  //
+  // Clear any port interrupt status
+  //
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_IS;
+  AhciWriteReg (PciIo, Offset, AhciReadReg (PciIo, Offset));
+
+  //
+  // Clear any HBA interrupt status
+  //
+  AhciWriteReg (PciIo, EFI_AHCI_IS_OFFSET, AhciReadReg (PciIo, EFI_AHCI_IS_OFFSET));
+}
+
+/**
+  This function is used to dump the Status Registers and if there is ERR bit set
+  in the Status Register, the Error Register's value is also be dumped.
+
+  @param  PciIo            The PCI IO protocol instance.
+  @param  Port             The number of port.
+  @param  AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+**/
+VOID
+EFIAPI
+AhciDumpPortStatus (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     UINT8                      Port,
+  IN OUT EFI_ATA_STATUS_BLOCK       *AtaStatusBlock
+  )
+{
+  UINT32               Offset;
+  UINT32               Data;
+
+  ASSERT (PciIo != NULL);
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+  Data   = AhciReadReg (PciIo, Offset);
+
+  if (AtaStatusBlock != NULL) {
+    ZeroMem (AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+    AtaStatusBlock->AtaStatus  = (UINT8)Data;
+    if ((AtaStatusBlock->AtaStatus & BIT0) != 0) {
+      AtaStatusBlock->AtaError = (UINT8)(Data >> 8);
+    }
+  }
+}
+
+
+/**
+  Enable the FIS running for giving port.
+
+  @param      PciIo          The PCI IO protocol instance.
+  @param      Port           The number of port.
+  @param      Timeout        The timeout value of enabling FIS, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The FIS enable setting fails.
+  @retval EFI_TIMEOUT        The FIS enable setting is time out.
+  @retval EFI_SUCCESS        The FIS enable successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciEnableFisReceive (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32 Offset;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_FRE);
+
+  return AhciWaitMmioSet (
+           PciIo,
+           Offset,
+           EFI_AHCI_PORT_CMD_FR,
+           EFI_AHCI_PORT_CMD_FR,
+           Timeout
+           );
+}
+
+/**
+  Disable the FIS running for giving port.
+
+  @param      PciIo          The PCI IO protocol instance.
+  @param      Port           The number of port.
+  @param      Timeout        The timeout value of disabling FIS, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The FIS disable setting fails.
+  @retval EFI_TIMEOUT        The FIS disable setting is time out.
+  @retval EFI_UNSUPPORTED    The port is in running state.
+  @retval EFI_SUCCESS        The FIS disable successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciDisableFisReceive (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32 Offset;
+  UINT32 Data;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  Data   = AhciReadReg (PciIo, Offset);
+
+  //
+  // Before disabling Fis receive, the DMA engine of the port should NOT be in running status.
+  //
+  if ((Data & (EFI_AHCI_PORT_CMD_ST | EFI_AHCI_PORT_CMD_CR)) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check if the Fis receive DMA engine for the port is running.
+  //
+  if ((Data & EFI_AHCI_PORT_CMD_FR) != EFI_AHCI_PORT_CMD_FR) {
+    return EFI_SUCCESS;
+  }
+
+  AhciAndReg (PciIo, Offset, (UINT32)~(EFI_AHCI_PORT_CMD_FRE));
+
+  return AhciWaitMmioSet (
+           PciIo,
+           Offset,
+           EFI_AHCI_PORT_CMD_FR,
+           0,
+           Timeout
+           );
+}
+
+
+
+/**
+  Build the command list, command table and prepare the fis receiver.
+
+  @param    PciIo                 The PCI IO protocol instance.
+  @param    AhciRegisters         The pointer to the EFI_AHCI_REGISTERS.
+  @param    Port                  The number of port.
+  @param    PortMultiplier        The timeout value of stop.
+  @param    CommandFis            The control fis will be used for the transfer.
+  @param    CommandList           The command list will be used for the transfer.
+  @param    AtapiCommand          The atapi command will be used for the transfer.
+  @param    AtapiCommandLength    The length of the atapi command.
+  @param    CommandSlotNumber     The command slot will be used for the transfer.
+  @param    DataPhysicalAddr      The pointer to the data buffer pci bus master address.
+  @param    DataLength            The data count to be transferred.
+
+**/
+VOID
+EFIAPI
+AhciBuildCommand (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     EFI_AHCI_REGISTERS         *AhciRegisters,
+  IN     UINT8                      Port,
+  IN     UINT8                      PortMultiplier,
+  IN     EFI_AHCI_COMMAND_FIS       *CommandFis,
+  IN     EFI_AHCI_COMMAND_LIST      *CommandList,
+  IN     EFI_AHCI_ATAPI_COMMAND     *AtapiCommand OPTIONAL,
+  IN     UINT8                      AtapiCommandLength,
+  IN     UINT8                      CommandSlotNumber,
+  IN OUT VOID                       *DataPhysicalAddr,
+  IN     UINT32                     DataLength
+  )
+{
+  UINT64     BaseAddr;
+  UINT32     PrdtNumber;
+  UINT32     PrdtIndex;
+  UINTN      RemainedData;
+  UINTN      MemAddr;
+  DATA_64    Data64;
+  UINT32     Offset;
+
+  //
+  // Filling the PRDT
+  //
+  PrdtNumber = (UINT32)DivU64x32 (((UINT64)DataLength + EFI_AHCI_MAX_DATA_PER_PRDT - 1), EFI_AHCI_MAX_DATA_PER_PRDT);
+
+  //
+  // According to AHCI 1.3 spec, a PRDT entry can point to a maximum 4MB data block.
+  // It also limits that the maximum amount of the PRDT entry in the command table
+  // is 65535.
+  //
+  ASSERT (PrdtNumber <= 65535);
+
+  Data64.Uint64 = (UINTN) (AhciRegisters->AhciRFis) + sizeof (EFI_AHCI_RECEIVED_FIS) * Port;
+
+  BaseAddr = Data64.Uint64;
+
+  ZeroMem ((VOID *)((UINTN) BaseAddr), sizeof (EFI_AHCI_RECEIVED_FIS));
+
+  ZeroMem (AhciRegisters->AhciCommandTable, sizeof (EFI_AHCI_COMMAND_TABLE));
+
+  CommandFis->AhciCFisPmNum = PortMultiplier;
+
+  CopyMem (&AhciRegisters->AhciCommandTable->CommandFis, CommandFis, sizeof (EFI_AHCI_COMMAND_FIS));
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  if (AtapiCommand != NULL) {
+    CopyMem (
+      &AhciRegisters->AhciCommandTable->AtapiCmd,
+      AtapiCommand,
+      AtapiCommandLength
+      );
+
+    CommandList->AhciCmdA = 1;
+    CommandList->AhciCmdP = 1;
+
+    AhciOrReg (PciIo, Offset, (EFI_AHCI_PORT_CMD_DLAE | EFI_AHCI_PORT_CMD_ATAPI));
+  } else {
+    AhciAndReg (PciIo, Offset, (UINT32)~(EFI_AHCI_PORT_CMD_DLAE | EFI_AHCI_PORT_CMD_ATAPI));
+  }
+
+  RemainedData = (UINTN) DataLength;
+  MemAddr      = (UINTN) DataPhysicalAddr;
+  CommandList->AhciCmdPrdtl = PrdtNumber;
+
+  for (PrdtIndex = 0; PrdtIndex < PrdtNumber; PrdtIndex++) {
+    if (RemainedData < EFI_AHCI_MAX_DATA_PER_PRDT) {
+      AhciRegisters->AhciCommandTable->PrdtTable[PrdtIndex].AhciPrdtDbc = (UINT32)RemainedData - 1;
+    } else {
+      AhciRegisters->AhciCommandTable->PrdtTable[PrdtIndex].AhciPrdtDbc = EFI_AHCI_MAX_DATA_PER_PRDT - 1;
+    }
+
+    Data64.Uint64 = (UINT64)MemAddr;
+    AhciRegisters->AhciCommandTable->PrdtTable[PrdtIndex].AhciPrdtDba  = Data64.Uint32.Lower32;
+    AhciRegisters->AhciCommandTable->PrdtTable[PrdtIndex].AhciPrdtDbau = Data64.Uint32.Upper32;
+    RemainedData -= EFI_AHCI_MAX_DATA_PER_PRDT;
+    MemAddr      += EFI_AHCI_MAX_DATA_PER_PRDT;
+  }
+
+  //
+  // Set the last PRDT to Interrupt On Complete
+  //
+  if (PrdtNumber > 0) {
+    AhciRegisters->AhciCommandTable->PrdtTable[PrdtNumber - 1].AhciPrdtIoc = 1;
+  }
+
+  CopyMem (
+    (VOID *) ((UINTN) AhciRegisters->AhciCmdList + (UINTN) CommandSlotNumber * sizeof (EFI_AHCI_COMMAND_LIST)),
+    CommandList,
+    sizeof (EFI_AHCI_COMMAND_LIST)
+    );
+
+  Data64.Uint64 = (UINT64)(UINTN) AhciRegisters->AhciCommandTablePciAddr;
+  AhciRegisters->AhciCmdList[CommandSlotNumber].AhciCmdCtba  = Data64.Uint32.Lower32;
+  AhciRegisters->AhciCmdList[CommandSlotNumber].AhciCmdCtbau = Data64.Uint32.Upper32;
+  AhciRegisters->AhciCmdList[CommandSlotNumber].AhciCmdPmp   = PortMultiplier;
+
+}
+
+/**
+  Buid a command FIS.
+
+  @param  CmdFis            A pointer to the EFI_AHCI_COMMAND_FIS data structure.
+  @param  AtaCommandBlock   A pointer to the AhciBuildCommandFis data structure.
+
+**/
+VOID
+EFIAPI
+AhciBuildCommandFis (
+  IN OUT EFI_AHCI_COMMAND_FIS    *CmdFis,
+  IN     EFI_ATA_COMMAND_BLOCK   *AtaCommandBlock
+  )
+{
+  ZeroMem (CmdFis, sizeof (EFI_AHCI_COMMAND_FIS));
+
+  CmdFis->AhciCFisType = EFI_AHCI_FIS_REGISTER_H2D;
+  //
+  // Indicator it's a command
+  //
+  CmdFis->AhciCFisCmdInd      = 0x1;
+  CmdFis->AhciCFisCmd         = AtaCommandBlock->AtaCommand;
+
+  CmdFis->AhciCFisFeature     = AtaCommandBlock->AtaFeatures;
+  CmdFis->AhciCFisFeatureExp  = AtaCommandBlock->AtaFeaturesExp;
+
+  CmdFis->AhciCFisSecNum      = AtaCommandBlock->AtaSectorNumber;
+  CmdFis->AhciCFisSecNumExp   = AtaCommandBlock->AtaSectorNumberExp;
+
+  CmdFis->AhciCFisClyLow      = AtaCommandBlock->AtaCylinderLow;
+  CmdFis->AhciCFisClyLowExp   = AtaCommandBlock->AtaCylinderLowExp;
+
+  CmdFis->AhciCFisClyHigh     = AtaCommandBlock->AtaCylinderHigh;
+  CmdFis->AhciCFisClyHighExp  = AtaCommandBlock->AtaCylinderHighExp;
+
+  CmdFis->AhciCFisSecCount    = AtaCommandBlock->AtaSectorCount;
+  CmdFis->AhciCFisSecCountExp = AtaCommandBlock->AtaSectorCountExp;
+
+  CmdFis->AhciCFisDevHead     = (UINT8) (AtaCommandBlock->AtaDeviceHead | 0xE0);
+}
+
+/**
+  Start a PIO data transfer on specific port.
+
+  @param[in]       PciIo               The PCI IO protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       Read                The transfer direction.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in, out]  MemoryAddr          The pointer to the data buffer.
+  @param[in]       DataCount           The data count to be transferred.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The PIO data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The PIO data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPioTransfer (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     EFI_AHCI_REGISTERS         *AhciRegisters,
+  IN     UINT8                      Port,
+  IN     UINT8                      PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND     *AtapiCommand OPTIONAL,
+  IN     UINT8                      AtapiCommandLength,
+  IN     BOOLEAN                    Read,
+  IN     EFI_ATA_COMMAND_BLOCK      *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK       *AtaStatusBlock,
+  IN OUT VOID                       *MemoryAddr,
+  IN     UINT32                     DataCount,
+  IN     UINT64                     Timeout,
+  IN     ATA_NONBLOCK_TASK          *Task
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         FisBaseAddr;
+  UINTN                         Offset;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  VOID                          *Map;
+  UINTN                         MapLength;
+  EFI_PCI_IO_PROTOCOL_OPERATION Flag;
+  UINT64                        Delay;
+  EFI_AHCI_COMMAND_FIS          CFis;
+  EFI_AHCI_COMMAND_LIST         CmdList;
+  UINT32                        PortTfd;
+  UINT32                        PrdCount;
+  BOOLEAN                       InfiniteWait;
+  BOOLEAN                       PioFisReceived;
+  BOOLEAN                       D2hFisReceived;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  if (Read) {
+    Flag = EfiPciIoOperationBusMasterWrite;
+  } else {
+    Flag = EfiPciIoOperationBusMasterRead;
+  }
+
+  //
+  // construct command list and command table with pci bus address
+  //
+  MapLength = DataCount;
+  Status = PciIo->Map (
+                    PciIo,
+                    Flag,
+                    MemoryAddr,
+                    &MapLength,
+                    &PhyAddr,
+                    &Map
+                    );
+
+  if (EFI_ERROR (Status) || (DataCount != MapLength)) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  //
+  // Package read needed
+  //
+  AhciBuildCommandFis (&CFis, AtaCommandBlock);
+
+  ZeroMem (&CmdList, sizeof (EFI_AHCI_COMMAND_LIST));
+
+  CmdList.AhciCmdCfl = EFI_AHCI_FIS_REGISTER_H2D_LENGTH / 4;
+  CmdList.AhciCmdW   = Read ? 0 : 1;
+
+  AhciBuildCommand (
+    PciIo,
+    AhciRegisters,
+    Port,
+    PortMultiplier,
+    &CFis,
+    &CmdList,
+    AtapiCommand,
+    AtapiCommandLength,
+    0,
+    (VOID *)(UINTN)PhyAddr,
+    DataCount
+    );
+
+  Status = AhciStartCommand (
+             PciIo,
+             Port,
+             0,
+             Timeout
+             );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Check the status and wait the driver sending data
+  //
+  FisBaseAddr = (UINTN)AhciRegisters->AhciRFis + Port * sizeof (EFI_AHCI_RECEIVED_FIS);
+
+  if (Read && (AtapiCommand == 0)) {
+    //
+    // Wait device sends the PIO setup fis before data transfer
+    //
+    Status = EFI_TIMEOUT;
+    Delay  = DivU64x32 (Timeout, 1000) + 1;
+    do {
+      PioFisReceived = FALSE;
+      D2hFisReceived = FALSE;
+      Offset = FisBaseAddr + EFI_AHCI_PIO_FIS_OFFSET;
+      Status = AhciCheckMemSet (Offset, EFI_AHCI_FIS_TYPE_MASK, EFI_AHCI_FIS_PIO_SETUP, NULL);
+      if (!EFI_ERROR (Status)) {
+        PioFisReceived = TRUE;
+      }
+      //
+      // According to SATA 2.6 spec section 11.7, D2h FIS means an error encountered.
+      // But Qemu and Marvel 9230 sata controller may just receive a D2h FIS from device
+      // after the transaction is finished successfully.
+      // To get better device compatibilities, we further check if the PxTFD's ERR bit is set.
+      // By this way, we can know if there is a real error happened.
+      //
+      Offset = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
+      Status = AhciCheckMemSet (Offset, EFI_AHCI_FIS_TYPE_MASK, EFI_AHCI_FIS_REGISTER_D2H, NULL);
+      if (!EFI_ERROR (Status)) {
+        D2hFisReceived = TRUE;
+      }
+
+      if (PioFisReceived || D2hFisReceived) {
+        Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+        PortTfd = AhciReadReg (PciIo, (UINT32) Offset);
+        //
+        // PxTFD will be updated if there is a D2H or SetupFIS received. 
+        //
+        if ((PortTfd & EFI_AHCI_PORT_TFD_ERR) != 0) {
+          Status = EFI_DEVICE_ERROR;
+          break;
+        }
+
+        PrdCount = *(volatile UINT32 *) (&(AhciRegisters->AhciCmdList[0].AhciCmdPrdbc));
+        if (PrdCount == DataCount) {
+          Status = EFI_SUCCESS;
+          break;
+        }
+      }
+
+      //
+      // Stall for 100 microseconds.
+      //
+      MicroSecondDelay(100);
+
+      Delay--;
+      if (Delay == 0) {
+        Status = EFI_TIMEOUT;
+      }
+    } while (InfiniteWait || (Delay > 0));
+  } else {
+    //
+    // Wait for D2H Fis is received
+    //
+    Offset = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
+    Status = AhciWaitMemSet (
+               Offset,
+               EFI_AHCI_FIS_TYPE_MASK,
+               EFI_AHCI_FIS_REGISTER_D2H,
+               Timeout
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+    PortTfd = AhciReadReg (PciIo, (UINT32) Offset);
+    if ((PortTfd & EFI_AHCI_PORT_TFD_ERR) != 0) {
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+Exit:
+  AhciStopCommand (
+    PciIo,
+    Port,
+    Timeout
+    );
+
+  AhciDisableFisReceive (
+    PciIo,
+    Port,
+    Timeout
+    );
+
+  PciIo->Unmap (
+    PciIo,
+    Map
+    );
+
+  AhciDumpPortStatus (PciIo, Port, AtaStatusBlock);
+
+  return Status;
+}
+
+/**
+  Start a DMA data transfer on specific port
+
+  @param[in]       Instance            The ATA_ATAPI_PASS_THRU_INSTANCE protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       Read                The transfer direction.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in, out]  MemoryAddr          The pointer to the data buffer.
+  @param[in]       DataCount           The data count to be transferred.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The DMA data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The DMA data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciDmaTransfer (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE *Instance,
+  IN     EFI_AHCI_REGISTERS         *AhciRegisters,
+  IN     UINT8                      Port,
+  IN     UINT8                      PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND     *AtapiCommand OPTIONAL,
+  IN     UINT8                      AtapiCommandLength,
+  IN     BOOLEAN                    Read,
+  IN     EFI_ATA_COMMAND_BLOCK      *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK       *AtaStatusBlock,
+  IN OUT VOID                       *MemoryAddr,
+  IN     UINT32                     DataCount,
+  IN     UINT64                     Timeout,
+  IN     ATA_NONBLOCK_TASK          *Task
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         Offset;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  VOID                          *Map;
+  UINTN                         MapLength;
+  EFI_PCI_IO_PROTOCOL_OPERATION Flag;
+  EFI_AHCI_COMMAND_FIS          CFis;
+  EFI_AHCI_COMMAND_LIST         CmdList;
+  UINTN                         FisBaseAddr;
+  UINT32                        PortTfd;
+
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  EFI_TPL                       OldTpl;
+
+  Map   = NULL;
+  PciIo = Instance->PciIo;
+
+  if (PciIo == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Before starting the Blocking BlockIO operation, push to finish all non-blocking
+  // BlockIO tasks.
+  // Delay 100us to simulate the blocking time out checking.
+  //
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  while ((Task == NULL) && (!IsListEmpty (&Instance->NonBlockingTaskList))) {
+    AsyncNonBlockingTransferRoutine (NULL, Instance);
+    //
+    // Stall for 100us.
+    //
+    MicroSecondDelay (100);
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  if ((Task == NULL) || ((Task != NULL) && (!Task->IsStart))) {
+    //
+    // Mark the Task to indicate that it has been started.
+    //
+    if (Task != NULL) {
+      Task->IsStart      = TRUE;
+    }
+    if (Read) {
+      Flag = EfiPciIoOperationBusMasterWrite;
+    } else {
+      Flag = EfiPciIoOperationBusMasterRead;
+    }
+
+    //
+    // Construct command list and command table with pci bus address.
+    //
+    MapLength = DataCount;
+    Status = PciIo->Map (
+                      PciIo,
+                      Flag,
+                      MemoryAddr,
+                      &MapLength,
+                      &PhyAddr,
+                      &Map
+                      );
+
+    if (EFI_ERROR (Status) || (DataCount != MapLength)) {
+      return EFI_BAD_BUFFER_SIZE;
+    }
+
+    if (Task != NULL) {
+      Task->Map = Map;
+    }
+    //
+    // Package read needed
+    //
+    AhciBuildCommandFis (&CFis, AtaCommandBlock);
+
+    ZeroMem (&CmdList, sizeof (EFI_AHCI_COMMAND_LIST));
+
+    CmdList.AhciCmdCfl = EFI_AHCI_FIS_REGISTER_H2D_LENGTH / 4;
+    CmdList.AhciCmdW   = Read ? 0 : 1;
+
+    AhciBuildCommand (
+      PciIo,
+      AhciRegisters,
+      Port,
+      PortMultiplier,
+      &CFis,
+      &CmdList,
+      AtapiCommand,
+      AtapiCommandLength,
+      0,
+      (VOID *)(UINTN)PhyAddr,
+      DataCount
+      );
+
+    Status = AhciStartCommand (
+               PciIo,
+               Port,
+               0,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+  }
+
+  //
+  // Wait for command compelte
+  //
+  FisBaseAddr = (UINTN)AhciRegisters->AhciRFis + Port * sizeof (EFI_AHCI_RECEIVED_FIS);
+  Offset      = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
+  if (Task != NULL) {
+    //
+    // For Non-blocking
+    //
+    Status = AhciCheckMemSet (
+               Offset,
+               EFI_AHCI_FIS_TYPE_MASK,
+               EFI_AHCI_FIS_REGISTER_D2H,
+               Task
+               );
+  } else {
+    Status = AhciWaitMemSet (
+               Offset,
+               EFI_AHCI_FIS_TYPE_MASK,
+               EFI_AHCI_FIS_REGISTER_D2H,
+               Timeout
+               );
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+  PortTfd = AhciReadReg (PciIo, (UINT32) Offset);
+  if ((PortTfd & EFI_AHCI_PORT_TFD_ERR) != 0) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  //
+  // For Blocking mode, the command should be stopped, the Fis should be disabled
+  // and the PciIo should be unmapped.
+  // For non-blocking mode, only when a error is happened (if the return status is
+  // EFI_NOT_READY that means the command doesn't finished, try again.), first do the
+  // context cleanup, then set the packet's Asb status.
+  //
+  if (Task == NULL ||
+      ((Task != NULL) && (Status != EFI_NOT_READY))
+     ) {
+    AhciStopCommand (
+      PciIo,
+      Port,
+      Timeout
+      );
+
+    AhciDisableFisReceive (
+      PciIo,
+      Port,
+      Timeout
+      );
+
+    PciIo->Unmap (
+             PciIo,
+             (Task != NULL) ? Task->Map : Map
+             );
+
+    if (Task != NULL) {
+      Task->Packet->Asb->AtaStatus = 0x01;
+    }
+  }
+
+  AhciDumpPortStatus (PciIo, Port, AtaStatusBlock);
+  return Status;
+}
+
+/**
+  Start a non data transfer on specific port.
+
+  @param[in]       PciIo               The PCI IO protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The non data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The non data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciNonDataTransfer (
+  IN     EFI_PCI_IO_PROTOCOL           *PciIo,
+  IN     EFI_AHCI_REGISTERS            *AhciRegisters,
+  IN     UINT8                         Port,
+  IN     UINT8                         PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND        *AtapiCommand OPTIONAL,
+  IN     UINT8                         AtapiCommandLength,
+  IN     EFI_ATA_COMMAND_BLOCK         *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock,
+  IN     UINT64                        Timeout,
+  IN     ATA_NONBLOCK_TASK             *Task
+  )
+{
+  EFI_STATUS                   Status;
+  UINTN                        FisBaseAddr;
+  UINTN                        Offset;
+  UINT32                       PortTfd;
+  EFI_AHCI_COMMAND_FIS         CFis;
+  EFI_AHCI_COMMAND_LIST        CmdList;
+
+  //
+  // Package read needed
+  //
+  AhciBuildCommandFis (&CFis, AtaCommandBlock);
+
+  ZeroMem (&CmdList, sizeof (EFI_AHCI_COMMAND_LIST));
+
+  CmdList.AhciCmdCfl = EFI_AHCI_FIS_REGISTER_H2D_LENGTH / 4;
+
+  AhciBuildCommand (
+    PciIo,
+    AhciRegisters,
+    Port,
+    PortMultiplier,
+    &CFis,
+    &CmdList,
+    AtapiCommand,
+    AtapiCommandLength,
+    0,
+    NULL,
+    0
+    );
+
+  Status = AhciStartCommand (
+             PciIo,
+             Port,
+             0,
+             Timeout
+             );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Wait device sends the Response Fis
+  //
+  FisBaseAddr = (UINTN)AhciRegisters->AhciRFis + Port * sizeof (EFI_AHCI_RECEIVED_FIS);
+  Offset      = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
+  Status      = AhciWaitMemSet (
+                  Offset,
+                  EFI_AHCI_FIS_TYPE_MASK,
+                  EFI_AHCI_FIS_REGISTER_D2H,
+                  Timeout
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+  PortTfd = AhciReadReg (PciIo, (UINT32) Offset);
+  if ((PortTfd & EFI_AHCI_PORT_TFD_ERR) != 0) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  AhciStopCommand (
+    PciIo,
+    Port,
+    Timeout
+    );
+
+  AhciDisableFisReceive (
+    PciIo,
+    Port,
+    Timeout
+    );
+
+  AhciDumpPortStatus (PciIo, Port, AtaStatusBlock);
+
+  return Status;
+}
+
+/**
+  Stop command running for giving port
+
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  Timeout            The timeout value of stop, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The command stop unsuccessfully.
+  @retval EFI_TIMEOUT        The operation is time out.
+  @retval EFI_SUCCESS        The command stop successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciStopCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32 Offset;
+  UINT32 Data;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  Data   = AhciReadReg (PciIo, Offset);
+
+  if ((Data & (EFI_AHCI_PORT_CMD_ST |  EFI_AHCI_PORT_CMD_CR)) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if ((Data & EFI_AHCI_PORT_CMD_ST) != 0) {
+    AhciAndReg (PciIo, Offset, (UINT32)~(EFI_AHCI_PORT_CMD_ST));
+  }
+
+  return AhciWaitMmioSet (
+           PciIo,
+           Offset,
+           EFI_AHCI_PORT_CMD_CR,
+           0,
+           Timeout
+           );
+}
+
+/**
+  Start command for give slot on specific port.
+
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  CommandSlot        The number of Command Slot.
+  @param  Timeout            The timeout value of start, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The command start unsuccessfully.
+  @retval EFI_TIMEOUT        The operation is time out.
+  @retval EFI_SUCCESS        The command start successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciStartCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT8                     CommandSlot,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32     CmdSlotBit;
+  EFI_STATUS Status;
+  UINT32     PortStatus;
+  UINT32     StartCmd;
+  UINT32     PortTfd;
+  UINT32     Offset;
+  UINT32     Capability;
+
+  //
+  // Collect AHCI controller information
+  //
+  Capability = AhciReadReg(PciIo, EFI_AHCI_CAPABILITY_OFFSET);
+
+  CmdSlotBit = (UINT32) (1 << CommandSlot);
+
+  AhciClearPortStatus (
+    PciIo,
+    Port
+    );
+
+  Status = AhciEnableFisReceive (
+             PciIo,
+             Port,
+             Timeout
+             );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  PortStatus = AhciReadReg (PciIo, Offset);
+
+  StartCmd = 0;
+  if ((PortStatus & EFI_AHCI_PORT_CMD_ALPE) != 0) {
+    StartCmd = AhciReadReg (PciIo, Offset);
+    StartCmd &= ~EFI_AHCI_PORT_CMD_ICC_MASK;
+    StartCmd |= EFI_AHCI_PORT_CMD_ACTIVE;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+  PortTfd = AhciReadReg (PciIo, Offset);
+
+  if ((PortTfd & (EFI_AHCI_PORT_TFD_BSY | EFI_AHCI_PORT_TFD_DRQ)) != 0) {
+    if ((Capability & BIT24) != 0) {
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+      AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_CLO);
+
+      AhciWaitMmioSet (
+        PciIo,
+        Offset,
+        EFI_AHCI_PORT_CMD_CLO,
+        0,
+        Timeout
+        );
+    }
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_ST | StartCmd);
+
+  //
+  // Setting the command
+  //
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CI;
+  AhciAndReg (PciIo, Offset, 0);
+  AhciOrReg (PciIo, Offset, CmdSlotBit);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Do AHCI port reset.
+
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  Timeout            The timeout value of reset, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The port reset unsuccessfully
+  @retval EFI_TIMEOUT        The reset operation is time out.
+  @retval EFI_SUCCESS        The port reset successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPortReset (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  )
+{
+  EFI_STATUS      Status;
+  UINT32          Offset;
+
+  AhciClearPortStatus (PciIo, Port);
+
+  AhciStopCommand (PciIo, Port, Timeout);
+
+  AhciDisableFisReceive (PciIo, Port, Timeout);
+
+  AhciEnableFisReceive (PciIo, Port, Timeout);
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SCTL;
+
+  AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_SCTL_DET_INIT);
+
+  //
+  // wait 5 millisecond before de-assert DET
+  //
+  MicroSecondDelay (5000);
+
+  AhciAndReg (PciIo, Offset, (UINT32)EFI_AHCI_PORT_SCTL_MASK);
+
+  //
+  // wait 5 millisecond before de-assert DET
+  //
+  MicroSecondDelay (5000);
+
+  //
+  // Wait for communication to be re-established
+  //
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SSTS;
+  Status = AhciWaitMmioSet (
+             PciIo,
+             Offset,
+             EFI_AHCI_PORT_SSTS_DET_MASK,
+             EFI_AHCI_PORT_SSTS_DET_PCE,
+             Timeout
+             );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SERR;
+  AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_ERR_CLEAR);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Do AHCI HBA reset.
+
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Timeout            The timeout value of reset, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   AHCI controller is failed to complete hardware reset.
+  @retval EFI_TIMEOUT        The reset operation is time out.
+  @retval EFI_SUCCESS        AHCI controller is reset successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciReset (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT64                 Delay;
+  UINT32                 Value;
+  UINT32                 Capability;
+
+  //
+  // Collect AHCI controller information
+  //
+  Capability = AhciReadReg (PciIo, EFI_AHCI_CAPABILITY_OFFSET);
+  
+  //
+  // Enable AE before accessing any AHCI registers if Supports AHCI Mode Only is not set
+  //
+  if ((Capability & EFI_AHCI_CAP_SAM) == 0) {
+    AhciOrReg (PciIo, EFI_AHCI_GHC_OFFSET, EFI_AHCI_GHC_ENABLE);
+  }
+
+  AhciOrReg (PciIo, EFI_AHCI_GHC_OFFSET, EFI_AHCI_GHC_RESET);
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+
+  do {
+    Value = AhciReadReg(PciIo, EFI_AHCI_GHC_OFFSET);
+
+    if ((Value & EFI_AHCI_GHC_RESET) == 0) {
+      break;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay(100);
+
+    Delay--;
+  } while (Delay > 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send SMART Return Status command to check if the execution of SMART cmd is successful or not.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  AtaStatusBlock      A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS     Successfully get the return status of S.M.A.R.T command execution.
+  @retval Others          Fail to get return status data.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciAtaSmartReturnStatusCheck (
+  IN EFI_PCI_IO_PROTOCOL         *PciIo,
+  IN EFI_AHCI_REGISTERS          *AhciRegisters,
+  IN UINT8                       Port,
+  IN UINT8                       PortMultiplier,
+  IN OUT EFI_ATA_STATUS_BLOCK    *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+  UINT8                   LBAMid;
+  UINT8                   LBAHigh;
+  UINTN                   FisBaseAddr;
+  UINT32                  Value;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+  AtaCommandBlock.AtaFeatures     = ATA_SMART_RETURN_STATUS;
+  AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+  AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+
+  //
+  // Send S.M.A.R.T Read Return Status command to device
+  //
+  Status = AhciNonDataTransfer (
+             PciIo,
+             AhciRegisters,
+             (UINT8)Port,
+             (UINT8)PortMultiplier,
+             NULL,
+             0,
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_DISABLED)
+      );
+    return EFI_DEVICE_ERROR;
+  }
+
+  REPORT_STATUS_CODE (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_ENABLE)
+    );
+
+  FisBaseAddr = (UINTN)AhciRegisters->AhciRFis + Port * sizeof (EFI_AHCI_RECEIVED_FIS);
+
+  Value = *(UINT32 *) (FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET);
+
+  if ((Value & EFI_AHCI_FIS_TYPE_MASK) == EFI_AHCI_FIS_REGISTER_D2H) {
+    LBAMid  = ((UINT8 *)(UINTN)(FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET))[5];
+    LBAHigh = ((UINT8 *)(UINTN)(FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET))[6];
+
+    if ((LBAMid == 0x4f) && (LBAHigh == 0xc2)) {
+      //
+      // The threshold exceeded condition is not detected by the device
+      //
+      DEBUG ((EFI_D_INFO, "The S.M.A.R.T threshold exceeded condition is not detected\n"));
+      REPORT_STATUS_CODE (
+            EFI_PROGRESS_CODE,
+            (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_UNDERTHRESHOLD)
+            );
+    } else if ((LBAMid == 0xf4) && (LBAHigh == 0x2c)) {
+      //
+      // The threshold exceeded condition is detected by the device
+      //
+      DEBUG ((EFI_D_INFO, "The S.M.A.R.T threshold exceeded condition is detected\n"));
+      REPORT_STATUS_CODE (
+           EFI_PROGRESS_CODE,
+           (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_OVERTHRESHOLD)
+           );
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Enable SMART command of the disk if supported.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  IdentifyData        A pointer to data buffer which is used to contain IDENTIFY data.
+  @param  AtaStatusBlock      A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+**/
+VOID
+EFIAPI
+AhciAtaSmartSupport (
+  IN EFI_PCI_IO_PROTOCOL           *PciIo,
+  IN EFI_AHCI_REGISTERS            *AhciRegisters,
+  IN UINT8                         Port,
+  IN UINT8                         PortMultiplier,
+  IN EFI_IDENTIFY_DATA             *IdentifyData,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock
+  )
+{
+  EFI_STATUS               Status;
+  EFI_ATA_COMMAND_BLOCK    AtaCommandBlock;
+
+  //
+  // Detect if the device supports S.M.A.R.T.
+  //
+  if ((IdentifyData->AtaData.command_set_supported_82 & 0x0001) != 0x0001) {
+    //
+    // S.M.A.R.T is not supported by the device
+    //
+    DEBUG ((EFI_D_INFO, "S.M.A.R.T feature is not supported at port [%d] PortMultiplier [%d]!\n",
+            Port, PortMultiplier));
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_NOTSUPPORTED)
+      );
+  } else {
+    //
+    // Check if the feature is enabled. If not, then enable S.M.A.R.T.
+    //
+    if ((IdentifyData->AtaData.command_set_feature_enb_85 & 0x0001) != 0x0001) {
+
+      REPORT_STATUS_CODE (
+        EFI_PROGRESS_CODE,
+        (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_DISABLE)
+        );
+
+      ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+      AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+      AtaCommandBlock.AtaFeatures     = ATA_SMART_ENABLE_OPERATION;
+      AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+      AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+
+      //
+      // Send S.M.A.R.T Enable command to device
+      //
+      Status = AhciNonDataTransfer (
+                 PciIo,
+                 AhciRegisters,
+                 (UINT8)Port,
+                 (UINT8)PortMultiplier,
+                 NULL,
+                 0,
+                 &AtaCommandBlock,
+                 AtaStatusBlock,
+                 ATA_ATAPI_TIMEOUT,
+                 NULL
+                 );
+
+
+      if (!EFI_ERROR (Status)) {
+        //
+        // Send S.M.A.R.T AutoSave command to device
+        //
+        ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+        AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+        AtaCommandBlock.AtaFeatures     = 0xD2;
+        AtaCommandBlock.AtaSectorCount  = 0xF1;
+        AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+        AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+
+        Status = AhciNonDataTransfer (
+                   PciIo,
+                   AhciRegisters,
+                   (UINT8)Port,
+                   (UINT8)PortMultiplier,
+                   NULL,
+                   0,
+                   &AtaCommandBlock,
+                   AtaStatusBlock,
+                   ATA_ATAPI_TIMEOUT,
+                   NULL
+                   );
+
+        if (!EFI_ERROR (Status)) {
+          Status = AhciAtaSmartReturnStatusCheck (
+                     PciIo,
+                     AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplier,
+                     AtaStatusBlock
+                     );
+        }
+      }
+    }
+    DEBUG ((EFI_D_INFO, "Enabled S.M.A.R.T feature at port [%d] PortMultiplier [%d]!\n",
+            Port, PortMultiplier));
+  }
+
+  return ;
+}
+
+/**
+  Send Buffer cmd to specific device.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  Buffer              The data buffer to store IDENTIFY PACKET data.
+
+  @retval EFI_DEVICE_ERROR    The cmd abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for executing.
+  @retval EFI_SUCCESS         The cmd executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciIdentify (
+  IN EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN EFI_AHCI_REGISTERS       *AhciRegisters,
+  IN UINT8                    Port,
+  IN UINT8                    PortMultiplier,
+  IN OUT EFI_IDENTIFY_DATA    *Buffer
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_ATA_COMMAND_BLOCK        AtaCommandBlock;
+  EFI_ATA_STATUS_BLOCK         AtaStatusBlock;
+
+  if (PciIo == NULL || AhciRegisters == NULL || Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+  ZeroMem (&AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+  AtaCommandBlock.AtaCommand     = ATA_CMD_IDENTIFY_DRIVE;
+  AtaCommandBlock.AtaSectorCount = 1;
+
+  Status = AhciPioTransfer (
+             PciIo,
+             AhciRegisters,
+             Port,
+             PortMultiplier,
+             NULL,
+             0,
+             TRUE,
+             &AtaCommandBlock,
+             &AtaStatusBlock,
+             Buffer,
+             sizeof (EFI_IDENTIFY_DATA),
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  Send Buffer cmd to specific device.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  Buffer              The data buffer to store IDENTIFY PACKET data.
+
+  @retval EFI_DEVICE_ERROR    The cmd abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for executing.
+  @retval EFI_SUCCESS         The cmd executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciIdentifyPacket (
+  IN EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN EFI_AHCI_REGISTERS       *AhciRegisters,
+  IN UINT8                    Port,
+  IN UINT8                    PortMultiplier,
+  IN OUT EFI_IDENTIFY_DATA    *Buffer
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_ATA_COMMAND_BLOCK        AtaCommandBlock;
+  EFI_ATA_STATUS_BLOCK         AtaStatusBlock;
+
+  if (PciIo == NULL || AhciRegisters == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+  ZeroMem (&AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+  AtaCommandBlock.AtaCommand     = ATA_CMD_IDENTIFY_DEVICE;
+  AtaCommandBlock.AtaSectorCount = 1;
+
+  Status = AhciPioTransfer (
+             PciIo,
+             AhciRegisters,
+             Port,
+             PortMultiplier,
+             NULL,
+             0,
+             TRUE,
+             &AtaCommandBlock,
+             &AtaStatusBlock,
+             Buffer,
+             sizeof (EFI_IDENTIFY_DATA),
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  Send SET FEATURE cmd on specific device.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  Feature             The data to send Feature register.
+  @param  FeatureSpecificData The specific data for SET FEATURE cmd.
+
+  @retval EFI_DEVICE_ERROR    The cmd abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for executing.
+  @retval EFI_SUCCESS         The cmd executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciDeviceSetFeature (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN EFI_AHCI_REGISTERS     *AhciRegisters,
+  IN UINT8                  Port,
+  IN UINT8                  PortMultiplier,
+  IN UINT16                 Feature,
+  IN UINT32                 FeatureSpecificData
+  )
+{
+  EFI_STATUS               Status;
+  EFI_ATA_COMMAND_BLOCK    AtaCommandBlock;
+  EFI_ATA_STATUS_BLOCK     AtaStatusBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+  ZeroMem (&AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+  AtaCommandBlock.AtaCommand      = ATA_CMD_SET_FEATURES;
+  AtaCommandBlock.AtaFeatures     = (UINT8) Feature;
+  AtaCommandBlock.AtaFeaturesExp  = (UINT8) (Feature >> 8);
+  AtaCommandBlock.AtaSectorCount  = (UINT8) FeatureSpecificData;
+  AtaCommandBlock.AtaSectorNumber = (UINT8) (FeatureSpecificData >> 8);
+  AtaCommandBlock.AtaCylinderLow  = (UINT8) (FeatureSpecificData >> 16);
+  AtaCommandBlock.AtaCylinderHigh = (UINT8) (FeatureSpecificData >> 24);
+
+  Status = AhciNonDataTransfer (
+             PciIo,
+             AhciRegisters,
+             (UINT8)Port,
+             (UINT8)PortMultiplier,
+             NULL,
+             0,
+             &AtaCommandBlock,
+             &AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  This function is used to send out ATAPI commands conforms to the Packet Command
+  with PIO Protocol.
+
+  @param PciIo              The PCI IO protocol instance.
+  @param AhciRegisters      The pointer to the EFI_AHCI_REGISTERS.
+  @param Port               The number of port.
+  @param PortMultiplier     The number of port multiplier.
+  @param Packet             A pointer to EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET structure.
+
+  @retval EFI_SUCCESS       send out the ATAPI packet command successfully
+                            and device sends data successfully.
+  @retval EFI_DEVICE_ERROR  the device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPacketCommandExecute (
+  IN  EFI_PCI_IO_PROTOCOL                           *PciIo,
+  IN  EFI_AHCI_REGISTERS                            *AhciRegisters,
+  IN  UINT8                                         Port,
+  IN  UINT8                                         PortMultiplier,
+  IN  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet
+  )
+{
+  EFI_STATUS                   Status;
+  VOID                         *Buffer;
+  UINT32                       Length;
+  EFI_ATA_COMMAND_BLOCK        AtaCommandBlock;
+  EFI_ATA_STATUS_BLOCK         AtaStatusBlock;
+  BOOLEAN                      Read;
+
+  if (Packet == NULL || Packet->Cdb == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+  ZeroMem (&AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+  AtaCommandBlock.AtaCommand      = ATA_CMD_PACKET;
+  //
+  // No OVL; No DMA
+  //
+  AtaCommandBlock.AtaFeatures     = 0x00;
+  //
+  // set the transfersize to ATAPI_MAX_BYTE_COUNT to let the device
+  // determine how many data should be transferred.
+  //
+  AtaCommandBlock.AtaCylinderLow  = (UINT8) (ATAPI_MAX_BYTE_COUNT & 0x00ff);
+  AtaCommandBlock.AtaCylinderHigh = (UINT8) (ATAPI_MAX_BYTE_COUNT >> 8);
+
+  if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+    Buffer = Packet->InDataBuffer;
+    Length = Packet->InTransferLength;
+    Read = TRUE;
+  } else {
+    Buffer = Packet->OutDataBuffer;
+    Length = Packet->OutTransferLength;
+    Read = FALSE;
+  }
+
+  if (Length == 0) {
+    Status = AhciNonDataTransfer (
+               PciIo,
+               AhciRegisters,
+               Port,
+               PortMultiplier,
+               Packet->Cdb,
+               Packet->CdbLength,
+               &AtaCommandBlock,
+               &AtaStatusBlock,
+               Packet->Timeout,
+               NULL
+               );
+  } else {
+    Status = AhciPioTransfer (
+               PciIo,
+               AhciRegisters,
+               Port,
+               PortMultiplier,
+               Packet->Cdb,
+               Packet->CdbLength,
+               Read,
+               &AtaCommandBlock,
+               &AtaStatusBlock,
+               Buffer,
+               Length,
+               Packet->Timeout,
+               NULL
+               );
+  }
+  return Status;
+}
+
+/**
+  Allocate transfer-related data struct which is used at AHCI mode.
+
+  @param  PciIo                 The PCI IO protocol instance.
+  @param  AhciRegisters         The pointer to the EFI_AHCI_REGISTERS.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciCreateTransferDescriptor (
+  IN     EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN OUT EFI_AHCI_REGISTERS     *AhciRegisters
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 Bytes;
+  VOID                  *Buffer;
+
+  UINT32                Capability;
+  UINT32                PortImplementBitMap;
+  UINT8                 MaxPortNumber;
+  UINT8                 MaxCommandSlotNumber;
+  BOOLEAN               Support64Bit;
+  UINT64                MaxReceiveFisSize;
+  UINT64                MaxCommandListSize;
+  UINT64                MaxCommandTableSize;
+  EFI_PHYSICAL_ADDRESS  AhciRFisPciAddr;
+  EFI_PHYSICAL_ADDRESS  AhciCmdListPciAddr;
+  EFI_PHYSICAL_ADDRESS  AhciCommandTablePciAddr;
+
+  Buffer = NULL;
+  //
+  // Collect AHCI controller information
+  //
+  Capability           = AhciReadReg(PciIo, EFI_AHCI_CAPABILITY_OFFSET);
+  //
+  // Get the number of command slots per port supported by this HBA.
+  //
+  MaxCommandSlotNumber = (UINT8) (((Capability & 0x1F00) >> 8) + 1);
+  Support64Bit         = (BOOLEAN) (((Capability & BIT31) != 0) ? TRUE : FALSE);
+  
+  PortImplementBitMap  = AhciReadReg(PciIo, EFI_AHCI_PI_OFFSET);
+  //
+  // Get the highest bit of implemented ports which decides how many bytes are allocated for recived FIS.
+  //
+  MaxPortNumber        = (UINT8)(UINTN)(HighBitSet32(PortImplementBitMap) + 1);
+  if (MaxPortNumber == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  MaxReceiveFisSize    = MaxPortNumber * sizeof (EFI_AHCI_RECEIVED_FIS);
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    EFI_SIZE_TO_PAGES ((UINTN) MaxReceiveFisSize),
+                    &Buffer,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ZeroMem (Buffer, (UINTN)MaxReceiveFisSize);
+
+  AhciRegisters->AhciRFis          = Buffer;
+  AhciRegisters->MaxReceiveFisSize = MaxReceiveFisSize;
+  Bytes  = (UINTN)MaxReceiveFisSize;
+
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    Buffer,
+                    &Bytes,
+                    &AhciRFisPciAddr,
+                    &AhciRegisters->MapRFis
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != MaxReceiveFisSize)) {
+    //
+    // Map error or unable to map the whole RFis buffer into a contiguous region.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error6;
+  }
+
+  if ((!Support64Bit) && (AhciRFisPciAddr > 0x100000000ULL)) {
+    //
+    // The AHCI HBA doesn't support 64bit addressing, so should not get a >4G pci bus master address.
+    //
+    Status = EFI_DEVICE_ERROR;
+    goto Error5;
+  }
+  AhciRegisters->AhciRFisPciAddr = (EFI_AHCI_RECEIVED_FIS *)(UINTN)AhciRFisPciAddr;
+
+  //
+  // Allocate memory for command list
+  // Note that the implemenation is a single task model which only use a command list for all ports.
+  //
+  Buffer = NULL;
+  MaxCommandListSize = MaxCommandSlotNumber * sizeof (EFI_AHCI_COMMAND_LIST);
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    EFI_SIZE_TO_PAGES ((UINTN) MaxCommandListSize),
+                    &Buffer,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Free mapped resource.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error5;
+  }
+
+  ZeroMem (Buffer, (UINTN)MaxCommandListSize);
+
+  AhciRegisters->AhciCmdList        = Buffer;
+  AhciRegisters->MaxCommandListSize = MaxCommandListSize;
+  Bytes  = (UINTN)MaxCommandListSize;
+
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    Buffer,
+                    &Bytes,
+                    &AhciCmdListPciAddr,
+                    &AhciRegisters->MapCmdList
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != MaxCommandListSize)) {
+    //
+    // Map error or unable to map the whole cmd list buffer into a contiguous region.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error4;
+  }
+
+  if ((!Support64Bit) && (AhciCmdListPciAddr > 0x100000000ULL)) {
+    //
+    // The AHCI HBA doesn't support 64bit addressing, so should not get a >4G pci bus master address.
+    //
+    Status = EFI_DEVICE_ERROR;
+    goto Error3;
+  }
+  AhciRegisters->AhciCmdListPciAddr = (EFI_AHCI_COMMAND_LIST *)(UINTN)AhciCmdListPciAddr;
+
+  //
+  // Allocate memory for command table
+  // According to AHCI 1.3 spec, a PRD table can contain maximum 65535 entries.
+  //
+  Buffer = NULL;
+  MaxCommandTableSize = sizeof (EFI_AHCI_COMMAND_TABLE);
+
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    EFI_SIZE_TO_PAGES ((UINTN) MaxCommandTableSize),
+                    &Buffer,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Free mapped resource.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error3;
+  }
+
+  ZeroMem (Buffer, (UINTN)MaxCommandTableSize);
+
+  AhciRegisters->AhciCommandTable    = Buffer;
+  AhciRegisters->MaxCommandTableSize = MaxCommandTableSize;
+  Bytes  = (UINTN)MaxCommandTableSize;
+
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    Buffer,
+                    &Bytes,
+                    &AhciCommandTablePciAddr,
+                    &AhciRegisters->MapCommandTable
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != MaxCommandTableSize)) {
+    //
+    // Map error or unable to map the whole cmd list buffer into a contiguous region.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error2;
+  }
+
+  if ((!Support64Bit) && (AhciCommandTablePciAddr > 0x100000000ULL)) {
+    //
+    // The AHCI HBA doesn't support 64bit addressing, so should not get a >4G pci bus master address.
+    //
+    Status = EFI_DEVICE_ERROR;
+    goto Error1;
+  }
+  AhciRegisters->AhciCommandTablePciAddr = (EFI_AHCI_COMMAND_TABLE *)(UINTN)AhciCommandTablePciAddr;
+
+  return EFI_SUCCESS;
+  //
+  // Map error or unable to map the whole CmdList buffer into a contiguous region.
+  //
+Error1:
+  PciIo->Unmap (
+           PciIo,
+           AhciRegisters->MapCommandTable
+           );
+Error2:
+  PciIo->FreeBuffer (
+           PciIo,
+           EFI_SIZE_TO_PAGES ((UINTN) MaxCommandTableSize),
+           AhciRegisters->AhciCommandTable
+           );
+Error3:
+  PciIo->Unmap (
+           PciIo,
+           AhciRegisters->MapCmdList
+           );
+Error4:
+  PciIo->FreeBuffer (
+           PciIo,
+           EFI_SIZE_TO_PAGES ((UINTN) MaxCommandListSize),
+           AhciRegisters->AhciCmdList
+           );
+Error5:
+  PciIo->Unmap (
+           PciIo,
+           AhciRegisters->MapRFis
+           );
+Error6:
+  PciIo->FreeBuffer (
+           PciIo,
+           EFI_SIZE_TO_PAGES ((UINTN) MaxReceiveFisSize),
+           AhciRegisters->AhciRFis
+           );
+
+  return Status;
+}
+
+/**
+  Initialize ATA host controller at AHCI mode.
+
+  The function is designed to initialize ATA host controller.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciModeInitialization (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_PCI_IO_PROTOCOL              *PciIo;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL *IdeInit;
+  UINT32                           Capability;
+  UINT8                            MaxPortNumber;
+  UINT32                           PortImplementBitMap;
+
+  EFI_AHCI_REGISTERS               *AhciRegisters;
+
+  UINT8                            Port;
+  DATA_64                          Data64;
+  UINT32                           Offset;
+  UINT32                           Data;
+  EFI_IDENTIFY_DATA                Buffer;
+  EFI_ATA_DEVICE_TYPE              DeviceType;
+  EFI_ATA_COLLECTIVE_MODE          *SupportedModes;
+  EFI_ATA_TRANSFER_MODE            TransferMode;
+  UINT32                           PhyDetectDelay;
+
+  if (Instance == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PciIo   = Instance->PciIo;
+  IdeInit = Instance->IdeControllerInit;
+
+  Status = AhciReset (PciIo, EFI_AHCI_BUS_RESET_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Collect AHCI controller information
+  //
+  Capability = AhciReadReg (PciIo, EFI_AHCI_CAPABILITY_OFFSET);
+  
+  //
+  // Enable AE before accessing any AHCI registers if Supports AHCI Mode Only is not set
+  //
+  if ((Capability & EFI_AHCI_CAP_SAM) == 0) {
+    AhciOrReg (PciIo, EFI_AHCI_GHC_OFFSET, EFI_AHCI_GHC_ENABLE);
+  }
+  
+  //
+  // Get the number of command slots per port supported by this HBA.
+  //
+  MaxPortNumber        = (UINT8) ((Capability & 0x1F) + 1);
+
+  //
+  // Get the bit map of those ports exposed by this HBA.
+  // It indicates which ports that the HBA supports are available for software to use.
+  //
+  PortImplementBitMap  = AhciReadReg(PciIo, EFI_AHCI_PI_OFFSET);
+
+  AhciRegisters = &Instance->AhciRegisters;
+  Status = AhciCreateTransferDescriptor (PciIo, AhciRegisters);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  for (Port = 0; Port < EFI_AHCI_MAX_PORTS; Port ++) {
+    if ((PortImplementBitMap & (BIT0 << Port)) != 0) {
+      //
+      // According to AHCI spec, MaxPortNumber should be equal or greater than the number of implemented ports.
+      //
+      if ((MaxPortNumber--) == 0) {
+        //
+        // Should never be here.
+        //
+        ASSERT (FALSE);
+        return EFI_SUCCESS;
+      }
+
+      IdeInit->NotifyPhase (IdeInit, EfiIdeBeforeChannelEnumeration, Port);
+
+      //
+      // Initialize FIS Base Address Register and Command List Base Address Register for use.
+      //
+      Data64.Uint64 = (UINTN) (AhciRegisters->AhciRFisPciAddr) + sizeof (EFI_AHCI_RECEIVED_FIS) * Port;
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_FB;
+      AhciWriteReg (PciIo, Offset, Data64.Uint32.Lower32);
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_FBU;
+      AhciWriteReg (PciIo, Offset, Data64.Uint32.Upper32);
+
+      Data64.Uint64 = (UINTN) (AhciRegisters->AhciCmdListPciAddr);
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CLB;
+      AhciWriteReg (PciIo, Offset, Data64.Uint32.Lower32);
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CLBU;
+      AhciWriteReg (PciIo, Offset, Data64.Uint32.Upper32);
+
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+      Data = AhciReadReg (PciIo, Offset);
+      if ((Data & EFI_AHCI_PORT_CMD_CPD) != 0) {
+        AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_POD);
+      }
+
+      if ((Capability & EFI_AHCI_CAP_SSS) != 0) {
+        AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_SUD);
+      }
+
+      //
+      // Disable aggressive power management.
+      //
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SCTL;
+      AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_SCTL_IPM_INIT);
+      //
+      // Disable the reporting of the corresponding interrupt to system software.
+      //
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_IE;
+      AhciAndReg (PciIo, Offset, 0);
+
+      //
+      // Now inform the IDE Controller Init Module.
+      //
+      IdeInit->NotifyPhase (IdeInit, EfiIdeBusBeforeDevicePresenceDetection, Port);
+
+      //
+      // Enable FIS Receive DMA engine for the first D2H FIS.
+      //
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+      AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_FRE);
+      Status = AhciWaitMmioSet (
+                 PciIo,
+                 Offset,
+                 EFI_AHCI_PORT_CMD_FR,
+                 EFI_AHCI_PORT_CMD_FR,
+                 EFI_AHCI_PORT_CMD_FR_CLEAR_TIMEOUT
+                 );
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      //
+      // Wait no longer than 10 ms to wait the Phy to detect the presence of a device.
+      // It's the requirment from SATA1.0a spec section 5.2.
+      //
+      PhyDetectDelay = EFI_AHCI_BUS_PHY_DETECT_TIMEOUT;
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SSTS;
+      do {
+        Data = AhciReadReg (PciIo, Offset) & EFI_AHCI_PORT_SSTS_DET_MASK;
+        if ((Data == EFI_AHCI_PORT_SSTS_DET_PCE) || (Data == EFI_AHCI_PORT_SSTS_DET)) {
+          break;
+        }
+
+        MicroSecondDelay (1000);
+        PhyDetectDelay--;
+      } while (PhyDetectDelay > 0);
+
+      if (PhyDetectDelay == 0) {
+        //
+        // No device detected at this port.
+        // Clear PxCMD.SUD for those ports at which there are no device present.
+        //
+        Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+        AhciAndReg (PciIo, Offset, (UINT32) ~(EFI_AHCI_PORT_CMD_SUD));
+        continue;
+      }
+
+      //
+      // According to SATA1.0a spec section 5.2, we need to wait for PxTFD.BSY and PxTFD.DRQ
+      // and PxTFD.ERR to be zero. The maximum wait time is 16s which is defined at ATA spec.
+      //
+      PhyDetectDelay = 16 * 1000;
+      do {
+        Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SERR;
+        if (AhciReadReg(PciIo, Offset) != 0) {
+          AhciWriteReg (PciIo, Offset, AhciReadReg(PciIo, Offset));
+        }
+        Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+
+        Data = AhciReadReg (PciIo, Offset) & EFI_AHCI_PORT_TFD_MASK;
+        if (Data == 0) {
+          break;
+        }
+
+        MicroSecondDelay (1000);
+        PhyDetectDelay--;
+      } while (PhyDetectDelay > 0);
+
+      if (PhyDetectDelay == 0) {
+        continue;
+      }
+
+      //
+      // When the first D2H register FIS is received, the content of PxSIG register is updated.
+      //
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SIG;
+      Status = AhciWaitMmioSet (
+                 PciIo,
+                 Offset,
+                 0x0000FFFF,
+                 0x00000101,
+                 EFI_TIMER_PERIOD_SECONDS(16)
+                 );
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      Data = AhciReadReg (PciIo, Offset);
+      if ((Data & EFI_AHCI_ATAPI_SIG_MASK) == EFI_AHCI_ATAPI_DEVICE_SIG) {
+        Status = AhciIdentifyPacket (PciIo, AhciRegisters, Port, 0, &Buffer);
+
+        if (EFI_ERROR (Status)) {
+          continue;
+        }
+
+        DeviceType = EfiIdeCdrom;
+      } else if ((Data & EFI_AHCI_ATAPI_SIG_MASK) == EFI_AHCI_ATA_DEVICE_SIG) {
+        Status = AhciIdentify (PciIo, AhciRegisters, Port, 0, &Buffer);
+
+        if (EFI_ERROR (Status)) {
+          REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_PERIPHERAL_FIXED_MEDIA | EFI_P_EC_NOT_DETECTED));
+          continue;
+        }
+
+        DeviceType = EfiIdeHarddisk;
+      } else {
+        continue;
+      }
+      DEBUG ((EFI_D_INFO, "port [%d] port mulitplier [%d] has a [%a]\n",
+              Port, 0, DeviceType == EfiIdeCdrom ? "cdrom" : "harddisk"));
+
+      //
+      // If the device is a hard disk, then try to enable S.M.A.R.T feature
+      //
+      if ((DeviceType == EfiIdeHarddisk) && PcdGetBool (PcdAtaSmartEnable)) {
+        AhciAtaSmartSupport (
+          PciIo,
+          AhciRegisters,
+          Port,
+          0,
+          &Buffer,
+          NULL
+          );
+      }
+
+      //
+      // Submit identify data to IDE controller init driver
+      //
+      IdeInit->SubmitData (IdeInit, Port, 0, &Buffer);
+
+      //
+      // Now start to config ide device parameter and transfer mode.
+      //
+      Status = IdeInit->CalculateMode (
+                          IdeInit,
+                          Port,
+                          0,
+                          &SupportedModes
+                          );
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Calculate Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+
+      //
+      // Set best supported PIO mode on this IDE device
+      //
+      if (SupportedModes->PioMode.Mode <= EfiAtaPioMode2) {
+        TransferMode.ModeCategory = EFI_ATA_MODE_DEFAULT_PIO;
+      } else {
+        TransferMode.ModeCategory = EFI_ATA_MODE_FLOW_PIO;
+      }
+
+      TransferMode.ModeNumber = (UINT8) (SupportedModes->PioMode.Mode);
+
+      //
+      // Set supported DMA mode on this IDE device. Note that UDMA & MDMA cann't
+      // be set together. Only one DMA mode can be set to a device. If setting
+      // DMA mode operation fails, we can continue moving on because we only use
+      // PIO mode at boot time. DMA modes are used by certain kind of OS booting
+      //
+      if (SupportedModes->UdmaMode.Valid) {
+        TransferMode.ModeCategory = EFI_ATA_MODE_UDMA;
+        TransferMode.ModeNumber = (UINT8) (SupportedModes->UdmaMode.Mode);
+      } else if (SupportedModes->MultiWordDmaMode.Valid) {
+        TransferMode.ModeCategory = EFI_ATA_MODE_MDMA;
+        TransferMode.ModeNumber = (UINT8) SupportedModes->MultiWordDmaMode.Mode;
+      }
+
+      Status = AhciDeviceSetFeature (PciIo, AhciRegisters, Port, 0, 0x03, (UINT32)(*(UINT8 *)&TransferMode));
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Set transfer Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+
+      //
+      // Found a ATA or ATAPI device, add it into the device list.
+      //
+      CreateNewDeviceInfo (Instance, Port, 0, DeviceType, &Buffer);
+      if (DeviceType == EfiIdeHarddisk) {
+        REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_PERIPHERAL_FIXED_MEDIA | EFI_P_PC_ENABLE));
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h
new file mode 100644
index 0000000000..6401fb2e9f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h
@@ -0,0 +1,370 @@
+/** @file
+  Header file for AHCI mode of ATA host controller.
+  
+  Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials                          
+  are licensed and made available under the terms and conditions of the BSD License         
+  which accompanies this distribution.  The full text of the license may be found at        
+  http://opensource.org/licenses/bsd-license.php                                            
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             
+
+**/
+#ifndef __ATA_HC_AHCI_MODE_H__
+#define __ATA_HC_AHCI_MODE_H__
+
+#define EFI_AHCI_BAR_INDEX                     0x05
+
+#define EFI_AHCI_CAPABILITY_OFFSET             0x0000
+#define   EFI_AHCI_CAP_SAM                     BIT18
+#define   EFI_AHCI_CAP_SSS                     BIT27
+#define   EFI_AHCI_CAP_S64A                    BIT31
+#define EFI_AHCI_GHC_OFFSET                    0x0004
+#define   EFI_AHCI_GHC_RESET                   BIT0
+#define   EFI_AHCI_GHC_IE                      BIT1
+#define   EFI_AHCI_GHC_ENABLE                  BIT31
+#define EFI_AHCI_IS_OFFSET                     0x0008
+#define EFI_AHCI_PI_OFFSET                     0x000C
+
+#define EFI_AHCI_MAX_PORTS                     32
+
+typedef struct {
+  UINT32  Lower32;
+  UINT32  Upper32;
+} DATA_32;
+
+typedef union {
+  DATA_32   Uint32;
+  UINT64    Uint64;
+} DATA_64;
+
+//
+// Refer SATA1.0a spec section 5.2, the Phy detection time should be less than 10ms.
+//
+#define  EFI_AHCI_BUS_PHY_DETECT_TIMEOUT       10
+//
+// Refer SATA1.0a spec, the FIS enable time should be less than 500ms.
+//
+#define  EFI_AHCI_PORT_CMD_FR_CLEAR_TIMEOUT    EFI_TIMER_PERIOD_MILLISECONDS(500)
+//
+// Refer SATA1.0a spec, the bus reset time should be less than 1s.
+//
+#define  EFI_AHCI_BUS_RESET_TIMEOUT            EFI_TIMER_PERIOD_SECONDS(1)
+
+#define  EFI_AHCI_ATAPI_DEVICE_SIG             0xEB140000
+#define  EFI_AHCI_ATA_DEVICE_SIG               0x00000000
+#define  EFI_AHCI_PORT_MULTIPLIER_SIG          0x96690000
+#define  EFI_AHCI_ATAPI_SIG_MASK               0xFFFF0000
+
+//
+// Each PRDT entry can point to a memory block up to 4M byte
+//
+#define EFI_AHCI_MAX_DATA_PER_PRDT             0x400000
+
+#define EFI_AHCI_FIS_REGISTER_H2D              0x27      //Register FIS - Host to Device
+#define   EFI_AHCI_FIS_REGISTER_H2D_LENGTH     20 
+#define EFI_AHCI_FIS_REGISTER_D2H              0x34      //Register FIS - Device to Host
+#define   EFI_AHCI_FIS_REGISTER_D2H_LENGTH     20
+#define EFI_AHCI_FIS_DMA_ACTIVATE              0x39      //DMA Activate FIS - Device to Host
+#define   EFI_AHCI_FIS_DMA_ACTIVATE_LENGTH     4
+#define EFI_AHCI_FIS_DMA_SETUP                 0x41      //DMA Setup FIS - Bi-directional
+#define   EFI_AHCI_FIS_DMA_SETUP_LENGTH        28
+#define EFI_AHCI_FIS_DATA                      0x46      //Data FIS - Bi-directional
+#define EFI_AHCI_FIS_BIST                      0x58      //BIST Activate FIS - Bi-directional
+#define   EFI_AHCI_FIS_BIST_LENGTH             12
+#define EFI_AHCI_FIS_PIO_SETUP                 0x5F      //PIO Setup FIS - Device to Host
+#define   EFI_AHCI_FIS_PIO_SETUP_LENGTH        20
+#define EFI_AHCI_FIS_SET_DEVICE                0xA1      //Set Device Bits FIS - Device to Host
+#define   EFI_AHCI_FIS_SET_DEVICE_LENGTH       8
+
+#define EFI_AHCI_D2H_FIS_OFFSET                0x40
+#define EFI_AHCI_DMA_FIS_OFFSET                0x00
+#define EFI_AHCI_PIO_FIS_OFFSET                0x20
+#define EFI_AHCI_SDB_FIS_OFFSET                0x58
+#define EFI_AHCI_FIS_TYPE_MASK                 0xFF
+#define EFI_AHCI_U_FIS_OFFSET                  0x60
+
+//
+// Port register
+//
+#define EFI_AHCI_PORT_START                    0x0100
+#define EFI_AHCI_PORT_REG_WIDTH                0x0080
+#define EFI_AHCI_PORT_CLB                      0x0000
+#define EFI_AHCI_PORT_CLBU                     0x0004
+#define EFI_AHCI_PORT_FB                       0x0008
+#define EFI_AHCI_PORT_FBU                      0x000C
+#define EFI_AHCI_PORT_IS                       0x0010
+#define   EFI_AHCI_PORT_IS_DHRS                BIT0
+#define   EFI_AHCI_PORT_IS_PSS                 BIT1
+#define   EFI_AHCI_PORT_IS_SSS                 BIT2
+#define   EFI_AHCI_PORT_IS_SDBS                BIT3
+#define   EFI_AHCI_PORT_IS_UFS                 BIT4
+#define   EFI_AHCI_PORT_IS_DPS                 BIT5
+#define   EFI_AHCI_PORT_IS_PCS                 BIT6
+#define   EFI_AHCI_PORT_IS_DIS                 BIT7
+#define   EFI_AHCI_PORT_IS_PRCS                BIT22
+#define   EFI_AHCI_PORT_IS_IPMS                BIT23
+#define   EFI_AHCI_PORT_IS_OFS                 BIT24
+#define   EFI_AHCI_PORT_IS_INFS                BIT26
+#define   EFI_AHCI_PORT_IS_IFS                 BIT27
+#define   EFI_AHCI_PORT_IS_HBDS                BIT28
+#define   EFI_AHCI_PORT_IS_HBFS                BIT29
+#define   EFI_AHCI_PORT_IS_TFES                BIT30
+#define   EFI_AHCI_PORT_IS_CPDS                BIT31
+#define   EFI_AHCI_PORT_IS_CLEAR               0xFFFFFFFF
+#define   EFI_AHCI_PORT_IS_FIS_CLEAR           0x0000000F
+
+#define EFI_AHCI_PORT_IE                       0x0014
+#define EFI_AHCI_PORT_CMD                      0x0018
+#define   EFI_AHCI_PORT_CMD_ST_MASK            0xFFFFFFFE
+#define   EFI_AHCI_PORT_CMD_ST                 BIT0
+#define   EFI_AHCI_PORT_CMD_SUD                BIT1
+#define   EFI_AHCI_PORT_CMD_POD                BIT2
+#define   EFI_AHCI_PORT_CMD_CLO                BIT3
+#define   EFI_AHCI_PORT_CMD_CR                 BIT15
+#define   EFI_AHCI_PORT_CMD_FRE                BIT4
+#define   EFI_AHCI_PORT_CMD_FR                 BIT14
+#define   EFI_AHCI_PORT_CMD_MASK               ~(EFI_AHCI_PORT_CMD_ST | EFI_AHCI_PORT_CMD_FRE | EFI_AHCI_PORT_CMD_COL)
+#define   EFI_AHCI_PORT_CMD_PMA                BIT17
+#define   EFI_AHCI_PORT_CMD_HPCP               BIT18
+#define   EFI_AHCI_PORT_CMD_MPSP               BIT19
+#define   EFI_AHCI_PORT_CMD_CPD                BIT20
+#define   EFI_AHCI_PORT_CMD_ESP                BIT21
+#define   EFI_AHCI_PORT_CMD_ATAPI              BIT24
+#define   EFI_AHCI_PORT_CMD_DLAE               BIT25
+#define   EFI_AHCI_PORT_CMD_ALPE               BIT26
+#define   EFI_AHCI_PORT_CMD_ASP                BIT27
+#define   EFI_AHCI_PORT_CMD_ICC_MASK           (BIT28 | BIT29 | BIT30 | BIT31)
+#define   EFI_AHCI_PORT_CMD_ACTIVE             (1 << 28 )
+#define EFI_AHCI_PORT_TFD                      0x0020
+#define   EFI_AHCI_PORT_TFD_MASK               (BIT7 | BIT3 | BIT0)
+#define   EFI_AHCI_PORT_TFD_BSY                BIT7
+#define   EFI_AHCI_PORT_TFD_DRQ                BIT3
+#define   EFI_AHCI_PORT_TFD_ERR                BIT0
+#define   EFI_AHCI_PORT_TFD_ERR_MASK           0x00FF00
+#define EFI_AHCI_PORT_SIG                      0x0024
+#define EFI_AHCI_PORT_SSTS                     0x0028
+#define   EFI_AHCI_PORT_SSTS_DET_MASK          0x000F
+#define   EFI_AHCI_PORT_SSTS_DET               0x0001
+#define   EFI_AHCI_PORT_SSTS_DET_PCE           0x0003
+#define   EFI_AHCI_PORT_SSTS_SPD_MASK          0x00F0
+#define EFI_AHCI_PORT_SCTL                     0x002C
+#define   EFI_AHCI_PORT_SCTL_DET_MASK          0x000F
+#define   EFI_AHCI_PORT_SCTL_MASK              (~EFI_AHCI_PORT_SCTL_DET_MASK)
+#define   EFI_AHCI_PORT_SCTL_DET_INIT          0x0001
+#define   EFI_AHCI_PORT_SCTL_DET_PHYCOMM       0x0003
+#define   EFI_AHCI_PORT_SCTL_SPD_MASK          0x00F0
+#define   EFI_AHCI_PORT_SCTL_IPM_MASK          0x0F00
+#define   EFI_AHCI_PORT_SCTL_IPM_INIT          0x0300
+#define   EFI_AHCI_PORT_SCTL_IPM_PSD           0x0100
+#define   EFI_AHCI_PORT_SCTL_IPM_SSD           0x0200
+#define EFI_AHCI_PORT_SERR                     0x0030
+#define   EFI_AHCI_PORT_SERR_RDIE              BIT0
+#define   EFI_AHCI_PORT_SERR_RCE               BIT1
+#define   EFI_AHCI_PORT_SERR_TDIE              BIT8
+#define   EFI_AHCI_PORT_SERR_PCDIE             BIT9
+#define   EFI_AHCI_PORT_SERR_PE                BIT10
+#define   EFI_AHCI_PORT_SERR_IE                BIT11
+#define   EFI_AHCI_PORT_SERR_PRC               BIT16
+#define   EFI_AHCI_PORT_SERR_PIE               BIT17
+#define   EFI_AHCI_PORT_SERR_CW                BIT18
+#define   EFI_AHCI_PORT_SERR_BDE               BIT19
+#define   EFI_AHCI_PORT_SERR_DE                BIT20
+#define   EFI_AHCI_PORT_SERR_CRCE              BIT21
+#define   EFI_AHCI_PORT_SERR_HE                BIT22
+#define   EFI_AHCI_PORT_SERR_LSE               BIT23
+#define   EFI_AHCI_PORT_SERR_TSTE              BIT24
+#define   EFI_AHCI_PORT_SERR_UFT               BIT25
+#define   EFI_AHCI_PORT_SERR_EX                BIT26
+#define   EFI_AHCI_PORT_ERR_CLEAR              0xFFFFFFFF
+#define EFI_AHCI_PORT_SACT                     0x0034
+#define EFI_AHCI_PORT_CI                       0x0038
+#define EFI_AHCI_PORT_SNTF                     0x003C
+
+
+#pragma pack(1)
+//
+// Command List structure includes total 32 entries.
+// The entry data structure is listed at the following.
+//
+typedef struct {
+  UINT32   AhciCmdCfl:5;      //Command FIS Length
+  UINT32   AhciCmdA:1;        //ATAPI
+  UINT32   AhciCmdW:1;        //Write
+  UINT32   AhciCmdP:1;        //Prefetchable
+  UINT32   AhciCmdR:1;        //Reset
+  UINT32   AhciCmdB:1;        //BIST
+  UINT32   AhciCmdC:1;        //Clear Busy upon R_OK
+  UINT32   AhciCmdRsvd:1;
+  UINT32   AhciCmdPmp:4;      //Port Multiplier Port
+  UINT32   AhciCmdPrdtl:16;   //Physical Region Descriptor Table Length
+  UINT32   AhciCmdPrdbc;      //Physical Region Descriptor Byte Count
+  UINT32   AhciCmdCtba;       //Command Table Descriptor Base Address
+  UINT32   AhciCmdCtbau;      //Command Table Descriptor Base Address Upper 32-BITs
+  UINT32   AhciCmdRsvd1[4]; 
+} EFI_AHCI_COMMAND_LIST;
+
+//
+// This is a software constructed FIS.
+// For data transfer operations, this is the H2D Register FIS format as 
+// specified in the Serial ATA Revision 2.6 specification.
+//
+typedef struct {
+  UINT8    AhciCFisType;
+  UINT8    AhciCFisPmNum:4;
+  UINT8    AhciCFisRsvd:1;
+  UINT8    AhciCFisRsvd1:1;
+  UINT8    AhciCFisRsvd2:1;
+  UINT8    AhciCFisCmdInd:1;
+  UINT8    AhciCFisCmd;
+  UINT8    AhciCFisFeature;
+  UINT8    AhciCFisSecNum;
+  UINT8    AhciCFisClyLow;
+  UINT8    AhciCFisClyHigh;
+  UINT8    AhciCFisDevHead;
+  UINT8    AhciCFisSecNumExp;
+  UINT8    AhciCFisClyLowExp;
+  UINT8    AhciCFisClyHighExp;
+  UINT8    AhciCFisFeatureExp;
+  UINT8    AhciCFisSecCount;
+  UINT8    AhciCFisSecCountExp;
+  UINT8    AhciCFisRsvd3;
+  UINT8    AhciCFisControl;
+  UINT8    AhciCFisRsvd4[4];
+  UINT8    AhciCFisRsvd5[44];
+} EFI_AHCI_COMMAND_FIS;
+
+//
+// ACMD: ATAPI command (12 or 16 bytes)
+//
+typedef struct {
+  UINT8    AtapiCmd[0x10];
+} EFI_AHCI_ATAPI_COMMAND;
+
+//
+// Physical Region Descriptor Table includes up to 65535 entries
+// The entry data structure is listed at the following.
+// the actual entry number comes from the PRDTL field in the command
+// list entry for this command slot. 
+//
+typedef struct {
+  UINT32   AhciPrdtDba;       //Data Base Address
+  UINT32   AhciPrdtDbau;      //Data Base Address Upper 32-BITs
+  UINT32   AhciPrdtRsvd;
+  UINT32   AhciPrdtDbc:22;    //Data Byte Count
+  UINT32   AhciPrdtRsvd1:9;
+  UINT32   AhciPrdtIoc:1;     //Interrupt on Completion
+} EFI_AHCI_COMMAND_PRDT;
+
+//
+// Command table data strucute which is pointed to by the entry in the command list
+//
+typedef struct {
+  EFI_AHCI_COMMAND_FIS      CommandFis;       // A software constructed FIS.
+  EFI_AHCI_ATAPI_COMMAND    AtapiCmd;         // 12 or 16 bytes ATAPI cmd.
+  UINT8                     Reserved[0x30];
+  EFI_AHCI_COMMAND_PRDT     PrdtTable[65535];     // The scatter/gather list for data transfer
+} EFI_AHCI_COMMAND_TABLE;
+
+//
+// Received FIS structure
+//
+typedef struct {
+  UINT8    AhciDmaSetupFis[0x1C];         // Dma Setup Fis: offset 0x00
+  UINT8    AhciDmaSetupFisRsvd[0x04];
+  UINT8    AhciPioSetupFis[0x14];         // Pio Setup Fis: offset 0x20
+  UINT8    AhciPioSetupFisRsvd[0x0C];     
+  UINT8    AhciD2HRegisterFis[0x14];      // D2H Register Fis: offset 0x40
+  UINT8    AhciD2HRegisterFisRsvd[0x04];
+  UINT64   AhciSetDeviceBitsFis;          // Set Device Bits Fix: offset 0x58
+  UINT8    AhciUnknownFis[0x40];          // Unkonwn Fis: offset 0x60
+  UINT8    AhciUnknownFisRsvd[0x60];      
+} EFI_AHCI_RECEIVED_FIS; 
+
+#pragma pack()
+
+typedef struct {
+  EFI_AHCI_RECEIVED_FIS     *AhciRFis;
+  EFI_AHCI_COMMAND_LIST     *AhciCmdList;
+  EFI_AHCI_COMMAND_TABLE    *AhciCommandTable;
+  EFI_AHCI_RECEIVED_FIS     *AhciRFisPciAddr;
+  EFI_AHCI_COMMAND_LIST     *AhciCmdListPciAddr;
+  EFI_AHCI_COMMAND_TABLE    *AhciCommandTablePciAddr;
+  UINT64                    MaxCommandListSize;
+  UINT64                    MaxCommandTableSize;
+  UINT64                    MaxReceiveFisSize;
+  VOID                      *MapRFis;
+  VOID                      *MapCmdList;
+  VOID                      *MapCommandTable;
+} EFI_AHCI_REGISTERS;
+
+/**
+  This function is used to send out ATAPI commands conforms to the Packet Command 
+  with PIO Protocol.
+
+  @param PciIo              The PCI IO protocol instance.
+  @param AhciRegisters      The pointer to the EFI_AHCI_REGISTERS.
+  @param Port               The number of port.     
+  @param PortMultiplier     The number of port multiplier.
+  @param Packet             A pointer to EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET structure.
+
+  @retval EFI_SUCCESS       send out the ATAPI packet command successfully
+                            and device sends data successfully.
+  @retval EFI_DEVICE_ERROR  the device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPacketCommandExecute (
+  IN  EFI_PCI_IO_PROTOCOL                           *PciIo,
+  IN  EFI_AHCI_REGISTERS                            *AhciRegisters,
+  IN  UINT8                                         Port,
+  IN  UINT8                                         PortMultiplier,
+  IN  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet
+  );
+
+/**
+  Start command for give slot on specific port.
+    
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  CommandSlot        The number of CommandSlot.
+  @param  Timeout            The timeout value of start, uses 100ns as a unit.
+   
+  @retval EFI_DEVICE_ERROR   The command start unsuccessfully.
+  @retval EFI_TIMEOUT        The operation is time out.
+  @retval EFI_SUCCESS        The command start successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciStartCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT8                     CommandSlot,
+  IN  UINT64                    Timeout
+  );
+
+/**
+  Stop command running for giving port
+    
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  Timeout            The timeout value of stop, uses 100ns as a unit.
+   
+  @retval EFI_DEVICE_ERROR   The command stop unsuccessfully.
+  @retval EFI_TIMEOUT        The operation is time out.
+  @retval EFI_SUCCESS        The command stop successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciStopCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.c b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.c
new file mode 100644
index 0000000000..870900f106
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.c
@@ -0,0 +1,2604 @@
+/** @file
+  This file implements ATA_PASSTHRU_PROCTOCOL and EXT_SCSI_PASSTHRU_PROTOCOL interfaces
+  for managed ATA controllers.
+
+  Copyright (c) 2010 - 2015, 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 "AtaAtapiPassThru.h"
+
+//
+//  EFI_DRIVER_BINDING_PROTOCOL instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gAtaAtapiPassThruDriverBinding = {
+  AtaAtapiPassThruSupported,
+  AtaAtapiPassThruStart,
+  AtaAtapiPassThruStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+ATA_ATAPI_PASS_THRU_INSTANCE gAtaAtapiPassThruInstanceTemplate = {
+  ATA_ATAPI_PASS_THRU_SIGNATURE,
+  0,                  // Controller Handle
+  NULL,               // PciIo Protocol
+  NULL,               // IdeControllerInit Protocol
+  {                   // AtaPassThruMode
+    //
+    // According to UEFI2.3 spec Section 12.10, Drivers for non-RAID ATA controllers should set
+    // both EFI_ATA_PASS_THRU_ATTRIBUTES_PHYSICAL and EFI_ATA_PASS_THRU_ATTRIBUTES_LOGICAL
+    // bits.
+    // Note that the driver doesn't support AtaPassThru non blocking I/O.
+    //
+    EFI_ATA_PASS_THRU_ATTRIBUTES_PHYSICAL | EFI_ATA_PASS_THRU_ATTRIBUTES_LOGICAL | EFI_ATA_PASS_THRU_ATTRIBUTES_NONBLOCKIO,
+    //
+    // IoAlign
+    //
+    sizeof (UINTN)
+  },
+  {                   // AtaPassThru
+    NULL,
+    AtaPassThruPassThru,
+    AtaPassThruGetNextPort,
+    AtaPassThruGetNextDevice,
+    AtaPassThruBuildDevicePath,
+    AtaPassThruGetDevice,
+    AtaPassThruResetPort,
+    AtaPassThruResetDevice
+  },
+  {                   // ExtScsiPassThruMode
+    //
+    // AdapterId
+    //
+    0,
+    //
+    // According to UEFI2.3 spec Section 14.7, Drivers for non-RAID SCSI controllers should set
+    // both EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_PHYSICAL and EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL
+    // bits.
+    // Note that the driver doesn't support ExtScsiPassThru non blocking I/O.
+    //
+    EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_PHYSICAL | EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL,
+    //
+    // IoAlign
+    //
+    sizeof (UINTN)
+  },
+  {                   // ExtScsiPassThru
+    NULL,
+    ExtScsiPassThruPassThru,
+    ExtScsiPassThruGetNextTargetLun,
+    ExtScsiPassThruBuildDevicePath,
+    ExtScsiPassThruGetTargetLun,
+    ExtScsiPassThruResetChannel,
+    ExtScsiPassThruResetTargetLun,
+    ExtScsiPassThruGetNextTarget
+  },
+  EfiAtaUnknownMode,  // Work Mode
+  {                   // IdeRegisters
+    {0},
+    {0}
+  },
+  {                   // AhciRegisters
+    0
+  },
+  {                   // DeviceList
+    NULL,
+    NULL
+  },
+  0,                  // OriginalAttributes
+  0,                  // PreviousPort
+  0,                  // PreviousPortMultiplier
+  0,                  // PreviousTargetId
+  0,                  // PreviousLun
+  NULL,               // Timer event
+  {                   // NonBlocking TaskList
+    NULL,
+    NULL
+  }
+};
+
+ATAPI_DEVICE_PATH    mAtapiDevicePathTemplate = {
+  {
+    MESSAGING_DEVICE_PATH,
+    MSG_ATAPI_DP,
+    {
+      (UINT8) (sizeof (ATAPI_DEVICE_PATH)),
+      (UINT8) ((sizeof (ATAPI_DEVICE_PATH)) >> 8)
+    }
+  },
+  0,
+  0,
+  0
+};
+
+SATA_DEVICE_PATH    mSataDevicePathTemplate = {
+  {
+    MESSAGING_DEVICE_PATH,
+    MSG_SATA_DP,
+    {
+      (UINT8) (sizeof (SATA_DEVICE_PATH)),
+      (UINT8) ((sizeof (SATA_DEVICE_PATH)) >> 8)
+    }
+  },
+  0,
+  0,
+  0
+};
+
+UINT8 mScsiId[TARGET_MAX_BYTES] = {
+  0xFF, 0xFF, 0xFF, 0xFF,
+  0xFF, 0xFF, 0xFF, 0xFF,
+  0xFF, 0xFF, 0xFF, 0xFF,
+  0xFF, 0xFF, 0xFF, 0xFF
+};
+
+/**
+  Sends an ATA command to an ATA device that is attached to the ATA controller. This function
+  supports both blocking I/O and non-blocking I/O. The blocking I/O functionality is required,
+  and the non-blocking I/O functionality is optional.
+
+  @param[in]      Port               The port number of the ATA device to send the command.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device to send the command.
+                                     If there is no port multiplier, then specify 0.
+  @param[in, out] Packet             A pointer to the ATA command to send to the ATA device specified by Port
+                                     and PortMultiplierPort.
+  @param[in]      Instance           Pointer to the ATA_ATAPI_PASS_THRU_INSTANCE.
+  @param[in]      Task               Optional. Pointer to the ATA_NONBLOCK_TASK
+                                     used by non-blocking mode.
+
+  @retval EFI_SUCCESS                The ATA command was sent by the host. For
+                                     bi-directional commands, InTransferLength bytes
+                                     were transferred from InDataBuffer. For
+                                     write and bi-directional commands, OutTransferLength
+                                     bytes were transferred by OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The ATA command was not executed. The number
+                                     of bytes that could be transferred is returned
+                                     in InTransferLength. For write and bi-directional
+                                     commands, OutTransferLength bytes were transferred
+                                     by OutDataBuffer.
+  @retval EFI_NOT_READY              The ATA command could not be sent because
+                                     there are too many ATA commands already
+                                     queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting
+                                     to send the ATA command.
+  @retval EFI_INVALID_PARAMETER      Port, PortMultiplierPort, or the contents
+                                     of Acb are invalid. The ATA command was
+                                     not sent, so no additional status information
+                                     is available.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruPassThruExecute (
+  IN     UINT16                           Port,
+  IN     UINT16                           PortMultiplierPort,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *Packet,
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE     *Instance,
+  IN     ATA_NONBLOCK_TASK                *Task OPTIONAL
+  )
+{
+  EFI_ATA_PASS_THRU_CMD_PROTOCOL  Protocol;
+  EFI_ATA_HC_WORK_MODE            Mode;
+  EFI_STATUS                      Status;
+
+  Protocol = Packet->Protocol;
+
+  Mode = Instance->Mode;
+  switch (Mode) {
+    case EfiAtaIdeMode:
+      //
+      // Reassign IDE mode io port registers' base addresses
+      //
+      Status = GetIdeRegisterIoAddr (Instance->PciIo, Instance->IdeRegisters);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      switch (Protocol) {
+        case EFI_ATA_PASS_THRU_PROTOCOL_ATA_NON_DATA:
+          Status = AtaNonDataCommandIn (
+                     Instance->PciIo,
+                     &Instance->IdeRegisters[Port],
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN:
+          Status = AtaPioDataInOut (
+                     Instance->PciIo,
+                     &Instance->IdeRegisters[Port],
+                     Packet->InDataBuffer,
+                     Packet->InTransferLength,
+                     TRUE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_OUT:
+          Status = AtaPioDataInOut (
+                     Instance->PciIo,
+                     &Instance->IdeRegisters[Port],
+                     Packet->OutDataBuffer,
+                     Packet->OutTransferLength,
+                     FALSE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_IN:
+          Status = AtaUdmaInOut (
+                     Instance,
+                     &Instance->IdeRegisters[Port],
+                     TRUE,
+                     Packet->InDataBuffer,
+                     Packet->InTransferLength,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_OUT:
+          Status = AtaUdmaInOut (
+                     Instance,
+                     &Instance->IdeRegisters[Port],
+                     FALSE,
+                     Packet->OutDataBuffer,
+                     Packet->OutTransferLength,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        default :
+          return EFI_UNSUPPORTED;
+      }
+      break;
+    case EfiAtaAhciMode :
+      switch (Protocol) {
+        case EFI_ATA_PASS_THRU_PROTOCOL_ATA_NON_DATA:
+          Status = AhciNonDataTransfer (
+                     Instance->PciIo,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN:
+          Status = AhciPioTransfer (
+                     Instance->PciIo,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     TRUE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->InDataBuffer,
+                     Packet->InTransferLength,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_OUT:
+          Status = AhciPioTransfer (
+                     Instance->PciIo,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     FALSE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->OutDataBuffer,
+                     Packet->OutTransferLength,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_IN:
+          Status = AhciDmaTransfer (
+                     Instance,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     TRUE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->InDataBuffer,
+                     Packet->InTransferLength,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_OUT:
+          Status = AhciDmaTransfer (
+                     Instance,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     FALSE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->OutDataBuffer,
+                     Packet->OutTransferLength,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        default :
+          return EFI_UNSUPPORTED;
+      }
+      break;
+
+    default:
+      Status = EFI_DEVICE_ERROR;
+      break;
+  }
+
+  return Status;
+}
+
+/**
+  Call back function when the timer event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the
+                        Event.
+
+**/
+VOID
+EFIAPI
+AsyncNonBlockingTransferRoutine (
+  EFI_EVENT  Event,
+  VOID*      Context
+  )
+{
+  LIST_ENTRY                   *Entry;
+  LIST_ENTRY                   *EntryHeader;
+  ATA_NONBLOCK_TASK            *Task;
+  EFI_STATUS                   Status;
+  ATA_ATAPI_PASS_THRU_INSTANCE *Instance;
+
+  Instance   = (ATA_ATAPI_PASS_THRU_INSTANCE *) Context;
+  EntryHeader = &Instance->NonBlockingTaskList;
+  //
+  // Get the Taks from the Taks List and execute it, until there is
+  // no task in the list or the device is busy with task (EFI_NOT_READY).
+  //
+  while (TRUE) {
+    if (!IsListEmpty (EntryHeader)) {
+      Entry = GetFirstNode (EntryHeader);
+      Task  = ATA_NON_BLOCK_TASK_FROM_ENTRY (Entry);
+    } else {
+      return;
+    }
+
+    Status = AtaPassThruPassThruExecute (
+               Task->Port,
+               Task->PortMultiplier,
+               Task->Packet,
+               Instance,
+               Task
+               );
+
+    //
+    // If the data transfer meet a error, remove all tasks in the list since these tasks are
+    // associated with one task from Ata Bus and signal the event with error status.
+    //
+    if ((Status != EFI_NOT_READY) && (Status != EFI_SUCCESS)) {
+      DestroyAsynTaskList (Instance, TRUE);
+      break;
+    }
+
+    //
+    // For Non blocking mode, the Status of EFI_NOT_READY means the operation
+    // is not finished yet. Otherwise the operation is successful.
+    //
+    if (Status == EFI_NOT_READY) {
+      break;
+    } else {
+      RemoveEntryList (&Task->Link);
+      gBS->SignalEvent (Task->Event);
+      FreePool (Task);
+    }
+  }
+}
+
+/**
+  The Entry Point of module.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeAtaAtapiPassThru (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gAtaAtapiPassThruDriverBinding,
+             ImageHandle,
+             &gAtaAtapiPassThruComponentName,
+             &gAtaAtapiPassThruComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Because ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN EFI_HANDLE                        Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL          *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_DEVICE_PATH_PROTOCOL          *ParentDevicePath;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  PCI_TYPE00                        PciData;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeControllerInit;
+
+  //
+  // SATA Controller is a device driver, and should ingore the
+  // "RemainingDevicePath" according to UEFI spec
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID *) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+  //
+  // Close the protocol because we don't use it here
+  //
+  gBS->CloseProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  This->DriverBindingHandle,
+                  Controller
+                  );
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiIdeControllerInitProtocolGuid,
+                  (VOID **) &IdeControllerInit,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiIdeControllerInitProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Now test the EfiPciIoProtocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Now further check the PCI header: Base class (offset 0x0B) and
+  // Sub Class (offset 0x0A). This controller should be an ATA controller
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (PciData.Hdr.ClassCode),
+                        PciData.Hdr.ClassCode
+                        );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (IS_PCI_IDE (&PciData) || IS_PCI_SATADPA (&PciData)) {
+    return EFI_SUCCESS;
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeControllerInit;
+  ATA_ATAPI_PASS_THRU_INSTANCE      *Instance;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  UINT64                            Supports;
+  UINT64                            OriginalPciAttributes;
+
+  Status                = EFI_SUCCESS;
+  IdeControllerInit     = NULL;
+  Instance              = NULL;
+  OriginalPciAttributes = 0;
+
+  DEBUG ((EFI_D_INFO, "==AtaAtapiPassThru Start== Controller = %x\n", Controller));
+
+  Status  = gBS->OpenProtocol (
+                   Controller,
+                   &gEfiIdeControllerInitProtocolGuid,
+                   (VOID **) &IdeControllerInit,
+                   This->DriverBindingHandle,
+                   Controller,
+                   EFI_OPEN_PROTOCOL_BY_DRIVER
+                   );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Open Ide_Controller_Init Error, Status=%r", Status));
+    goto ErrorExit;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Get Pci_Io Protocol Error, Status=%r", Status));
+    goto ErrorExit;
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &OriginalPciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status = PciIo->Attributes (
+                      PciIo,
+                      EfiPciIoAttributeOperationEnable,
+                      Supports,
+                      NULL
+                      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Allocate a buffer to store the ATA_ATAPI_PASS_THRU_INSTANCE data structure
+  //
+  Instance = AllocateCopyPool (sizeof (ATA_ATAPI_PASS_THRU_INSTANCE), &gAtaAtapiPassThruInstanceTemplate);
+  if (Instance == NULL) {
+    goto ErrorExit;
+  }
+
+  Instance->ControllerHandle      = Controller;
+  Instance->IdeControllerInit     = IdeControllerInit;
+  Instance->PciIo                 = PciIo;
+  Instance->OriginalPciAttributes = OriginalPciAttributes;
+  Instance->AtaPassThru.Mode      = &Instance->AtaPassThruMode;
+  Instance->ExtScsiPassThru.Mode  = &Instance->ExtScsiPassThruMode;
+  InitializeListHead(&Instance->DeviceList);
+  InitializeListHead(&Instance->NonBlockingTaskList);
+
+  Instance->TimerEvent = NULL;
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  AsyncNonBlockingTransferRoutine,
+                  Instance,
+                  &Instance->TimerEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Set 1ms timer.
+  //
+  Status = gBS->SetTimer (Instance->TimerEvent, TimerPeriodic, 10000);
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Enumerate all inserted ATA devices.
+  //
+  Status = EnumerateAttachedDevice (Instance);
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiAtaPassThruProtocolGuid, &(Instance->AtaPassThru),
+                  &gEfiExtScsiPassThruProtocolGuid, &(Instance->ExtScsiPassThru),
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+
+ErrorExit:
+  if (IdeControllerInit != NULL) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiIdeControllerInitProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+
+  if ((Instance != NULL) && (Instance->TimerEvent != NULL)) {
+    gBS->CloseEvent (Instance->TimerEvent);
+  }
+
+  //
+  // Remove all inserted ATA devices.
+  //
+  DestroyDeviceInfoList(Instance);
+
+  if (Instance != NULL) {
+    FreePool (Instance);
+  }
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                        Status;
+  ATA_ATAPI_PASS_THRU_INSTANCE      *Instance;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_AHCI_REGISTERS                *AhciRegisters;
+  UINT64                            Supports;
+
+  DEBUG ((EFI_D_INFO, "==AtaAtapiPassThru Stop== Controller = %x\n", Controller));
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiAtaPassThruProtocolGuid,
+                  (VOID **) &AtaPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (AtaPassThru);
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Controller,
+                  &gEfiAtaPassThruProtocolGuid, &(Instance->AtaPassThru),
+                  &gEfiExtScsiPassThruProtocolGuid, &(Instance->ExtScsiPassThru),
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Close protocols opened by AtaAtapiPassThru controller driver
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiIdeControllerInitProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  //
+  // Close Non-Blocking timer and free Task list.
+  //
+  if (Instance->TimerEvent != NULL) {
+    gBS->CloseEvent (Instance->TimerEvent);
+    Instance->TimerEvent = NULL;
+  }
+  DestroyAsynTaskList (Instance, FALSE);
+  //
+  // Free allocated resource
+  //
+  DestroyDeviceInfoList (Instance);
+
+  //
+  // If the current working mode is AHCI mode, then pre-allocated resource
+  // for AHCI initialization should be released.
+  //
+  PciIo = Instance->PciIo;
+
+  if (Instance->Mode == EfiAtaAhciMode) {
+    AhciRegisters = &Instance->AhciRegisters;
+    PciIo->Unmap (
+             PciIo,
+             AhciRegisters->MapCommandTable
+             );
+    PciIo->FreeBuffer (
+             PciIo,
+             EFI_SIZE_TO_PAGES ((UINTN) AhciRegisters->MaxCommandTableSize),
+             AhciRegisters->AhciCommandTable
+             );
+    PciIo->Unmap (
+             PciIo,
+             AhciRegisters->MapCmdList
+             );
+    PciIo->FreeBuffer (
+             PciIo,
+             EFI_SIZE_TO_PAGES ((UINTN) AhciRegisters->MaxCommandListSize),
+             AhciRegisters->AhciCmdList
+             );
+    PciIo->Unmap (
+             PciIo,
+             AhciRegisters->MapRFis
+             );
+    PciIo->FreeBuffer (
+             PciIo,
+             EFI_SIZE_TO_PAGES ((UINTN) AhciRegisters->MaxReceiveFisSize),
+             AhciRegisters->AhciRFis
+             );
+  }
+
+  //
+  // Disable this ATA host controller.
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    PciIo->Attributes (
+             PciIo,
+             EfiPciIoAttributeOperationDisable,
+             Supports,
+             NULL
+             );
+  }
+
+  //
+  // Restore original PCI attributes
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSet,
+                    Instance->OriginalPciAttributes,
+                    NULL
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  FreePool (Instance);
+
+  return Status;
+}
+
+/**
+  Traverse the attached ATA devices list to find out the device to access.
+
+  @param[in]  Instance            A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  Port                The port number of the ATA device to send the command.
+  @param[in]  PortMultiplierPort  The port multiplier port number of the ATA device to send the command.
+                                  If there is no port multiplier, then specify 0.
+  @param[in]  DeviceType          The device type of the ATA device.
+
+  @retval     The pointer to the data structure of the device info to access.
+
+**/
+LIST_ENTRY *
+EFIAPI
+SearchDeviceInfoList (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN  UINT16                         Port,
+  IN  UINT16                         PortMultiplier,
+  IN  EFI_ATA_DEVICE_TYPE            DeviceType
+  )
+{
+  EFI_ATA_DEVICE_INFO  *DeviceInfo;
+  LIST_ENTRY           *Node;
+
+  Node = GetFirstNode (&Instance->DeviceList);
+  while (!IsNull (&Instance->DeviceList, Node)) {
+    DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+    if ((DeviceInfo->Type == DeviceType) &&
+        (Port == DeviceInfo->Port) &&
+        (PortMultiplier == DeviceInfo->PortMultiplier)) {
+      return Node;
+    }
+
+    Node = GetNextNode (&Instance->DeviceList, Node);
+  }
+
+  return NULL;
+}
+
+/**
+  Allocate device info data structure to contain device info.
+  And insert the data structure to the tail of device list for tracing.
+
+  @param[in]  Instance            A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  Port                The port number of the ATA device to send the command.
+  @param[in]  PortMultiplierPort  The port multiplier port number of the ATA device to send the command.
+                                  If there is no port multiplier, then specify 0.
+  @param[in]  DeviceType          The device type of the ATA device.
+  @param[in]  IdentifyData        The data buffer to store the output of the IDENTIFY cmd.
+
+  @retval EFI_SUCCESS             Successfully insert the ata device to the tail of device list.
+  @retval EFI_OUT_OF_RESOURCES    Can not allocate enough resource for use.
+
+**/
+EFI_STATUS
+EFIAPI
+CreateNewDeviceInfo (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE   *Instance,
+  IN  UINT16                         Port,
+  IN  UINT16                         PortMultiplier,
+  IN  EFI_ATA_DEVICE_TYPE            DeviceType,
+  IN  EFI_IDENTIFY_DATA              *IdentifyData
+  )
+{
+  EFI_ATA_DEVICE_INFO  *DeviceInfo;
+
+  DeviceInfo = AllocateZeroPool (sizeof (EFI_ATA_DEVICE_INFO));
+
+  if (DeviceInfo == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DeviceInfo->Signature      = ATA_ATAPI_DEVICE_SIGNATURE;
+  DeviceInfo->Port           = Port;
+  DeviceInfo->PortMultiplier = PortMultiplier;
+  DeviceInfo->Type           = DeviceType;
+
+  if (IdentifyData != NULL) {
+    DeviceInfo->IdentifyData = AllocateCopyPool (sizeof (EFI_IDENTIFY_DATA), IdentifyData);
+    if (DeviceInfo->IdentifyData == NULL) {
+      FreePool (DeviceInfo);
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  InsertTailList (&Instance->DeviceList, &DeviceInfo->Link);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Destroy all attached ATA devices info.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+VOID
+EFIAPI
+DestroyDeviceInfoList (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance
+  )
+{
+  EFI_ATA_DEVICE_INFO  *DeviceInfo;
+  LIST_ENTRY           *Node;
+
+  Node = GetFirstNode (&Instance->DeviceList);
+  while (!IsNull (&Instance->DeviceList, Node)) {
+    DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+    Node = GetNextNode (&Instance->DeviceList, Node);
+
+    RemoveEntryList (&DeviceInfo->Link);
+    if (DeviceInfo->IdentifyData != NULL) {
+      FreePool (DeviceInfo->IdentifyData);
+    }
+    FreePool (DeviceInfo);
+  }
+}
+
+/**
+  Destroy all pending non blocking tasks.
+
+  @param[in]  Instance    A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  IsSigEvent  Indicate whether signal the task event when remove the
+                          task.
+
+**/
+VOID
+EFIAPI
+DestroyAsynTaskList (
+  IN ATA_ATAPI_PASS_THRU_INSTANCE *Instance,
+  IN BOOLEAN                       IsSigEvent
+  )
+{
+  LIST_ENTRY           *Entry;
+  LIST_ENTRY           *DelEntry;
+  ATA_NONBLOCK_TASK    *Task;
+  EFI_TPL              OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  if (!IsListEmpty (&Instance->NonBlockingTaskList)) {
+    //
+    // Free the Subtask list.
+    //
+    for (Entry = (&Instance->NonBlockingTaskList)->ForwardLink;
+        Entry != (&Instance->NonBlockingTaskList);
+       ) {
+      DelEntry = Entry;
+      Entry    = Entry->ForwardLink;
+      Task     = ATA_NON_BLOCK_TASK_FROM_ENTRY (DelEntry);
+
+      RemoveEntryList (DelEntry);
+      if (IsSigEvent) {
+        Task->Packet->Asb->AtaStatus = 0x01;
+        gBS->SignalEvent (Task->Event);
+      }
+      FreePool (Task);
+    }
+  }
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Enumerate all attached ATA devices at IDE mode or AHCI mode separately.
+
+  The function is designed to enumerate all attached ATA devices.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+  @retval EFI_SUCCESS           Successfully enumerate attached ATA devices.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EnumerateAttachedDevice (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE      *Instance
+  )
+{
+  EFI_STATUS                   Status;
+  PCI_TYPE00                   PciData;
+  UINT8                        ClassCode;
+
+  Status = EFI_SUCCESS;
+
+  Status = Instance->PciIo->Pci.Read (
+                                  Instance->PciIo,
+                                  EfiPciIoWidthUint8,
+                                  PCI_CLASSCODE_OFFSET,
+                                  sizeof (PciData.Hdr.ClassCode),
+                                  PciData.Hdr.ClassCode
+                                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ClassCode = PciData.Hdr.ClassCode[1];
+
+  switch (ClassCode) {
+    case PCI_CLASS_MASS_STORAGE_IDE :
+      //
+      // The ATA controller is working at IDE mode
+      //
+      Instance->Mode = EfiAtaIdeMode;
+
+      Status = IdeModeInitialization (Instance);
+      if (EFI_ERROR (Status)) {
+        Status = EFI_DEVICE_ERROR;
+        goto Done;
+      }
+      break;
+    case PCI_CLASS_MASS_STORAGE_SATADPA :
+      //
+      // The ATA controller is working at AHCI mode
+      //
+      Instance->Mode = EfiAtaAhciMode;
+
+      Status = AhciModeInitialization (Instance);
+
+      if (EFI_ERROR (Status)) {
+        Status = EFI_DEVICE_ERROR;
+        goto Done;
+      }
+
+      break;
+    default :
+      Status = EFI_UNSUPPORTED;
+  }
+
+Done:
+  return Status;
+}
+
+/**
+  Sends an ATA command to an ATA device that is attached to the ATA controller. This function
+  supports both blocking I/O and non-blocking I/O. The blocking I/O functionality is required,
+  and the non-blocking I/O functionality is optional.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               The port number of the ATA device to send the command.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device to send the command.
+                                     If there is no port multiplier, then specify 0.
+  @param[in, out] Packet             A pointer to the ATA command to send to the ATA device specified by Port
+                                     and PortMultiplierPort.
+  @param[in]      Event               If non-blocking I/O is not supported then Event is ignored, and blocking
+                                     I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                                     Event is not NULL and non blocking I/O is supported, then non-blocking
+                                     I/O is performed, and Event will be signaled when the ATA command completes.
+
+  @retval EFI_SUCCESS                The ATA command was sent by the host. For bi-directional commands,
+                                     InTransferLength bytes were transferred from InDataBuffer. For write and
+                                     bi-directional commands, OutTransferLength bytes were transferred by OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The ATA command was not executed. The number of bytes that could be transferred
+                                     is returned in InTransferLength. For write and bi-directional commands,
+                                     OutTransferLength bytes were transferred by OutDataBuffer.
+  @retval EFI_NOT_READY              The ATA command could not be sent because there are too many ATA commands
+                                     already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting to send the ATA command.
+  @retval EFI_INVALID_PARAMETER      Port, PortMultiplierPort, or the contents of Acb are invalid. The ATA
+                                     command was not sent, so no additional status information is available.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruPassThru (
+  IN     EFI_ATA_PASS_THRU_PROTOCOL       *This,
+  IN     UINT16                           Port,
+  IN     UINT16                           PortMultiplierPort,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *Packet,
+  IN     EFI_EVENT                        Event OPTIONAL
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+  EFI_IDENTIFY_DATA               *IdentifyData;
+  UINT64                          Capacity;
+  UINT32                          MaxSectorCount;
+  ATA_NONBLOCK_TASK               *Task;
+  EFI_TPL                         OldTpl;
+  UINT32                          BlockSize;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->InDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->OutDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->Asb, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Node = SearchDeviceInfoList (Instance, Port, PortMultiplierPort, EfiIdeHarddisk);
+
+  if (Node == NULL) {
+    Node = SearchDeviceInfoList(Instance, Port, PortMultiplierPort, EfiIdeCdrom);
+    if (Node == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // Check whether this device needs 48-bit addressing (ATAPI-6 ata device).
+  // Per ATA-6 spec, word83: bit15 is zero and bit14 is one.
+  // If bit10 is one, it means the ata device support 48-bit addressing.
+  //
+  DeviceInfo     = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+  IdentifyData   = DeviceInfo->IdentifyData;
+  MaxSectorCount = 0x100;
+  if ((IdentifyData->AtaData.command_set_supported_83 & (BIT10 | BIT15 | BIT14)) == 0x4400) {
+    Capacity = *((UINT64 *)IdentifyData->AtaData.maximum_lba_for_48bit_addressing);
+    if (Capacity > 0xFFFFFFF) {
+      //
+      // Capacity exceeds 120GB. 48-bit addressing is really needed
+      // In this case, the max sector count is 0x10000
+      //
+      MaxSectorCount = 0x10000;
+    }
+  }
+
+  BlockSize = 0x200;
+  if ((IdentifyData->AtaData.phy_logic_sector_support & (BIT14 | BIT15)) == BIT14) {
+    //
+    // Check logical block size
+    //
+    if ((IdentifyData->AtaData.phy_logic_sector_support & BIT12) != 0) {
+      BlockSize = (UINT32) (((IdentifyData->AtaData.logic_sector_size_hi << 16) | IdentifyData->AtaData.logic_sector_size_lo) * sizeof (UINT16));
+    }
+  }
+
+  //
+  // convert the transfer length from sector count to byte.
+  //
+  if (((Packet->Length & EFI_ATA_PASS_THRU_LENGTH_BYTES) == 0) &&
+       (Packet->InTransferLength != 0)) {
+    Packet->InTransferLength = Packet->InTransferLength * BlockSize;
+  }
+
+  //
+  // convert the transfer length from sector count to byte.
+  //
+  if (((Packet->Length & EFI_ATA_PASS_THRU_LENGTH_BYTES) == 0) &&
+       (Packet->OutTransferLength != 0)) {
+    Packet->OutTransferLength = Packet->OutTransferLength * BlockSize;
+  }
+
+  //
+  // If the data buffer described by InDataBuffer/OutDataBuffer and InTransferLength/OutTransferLength
+  // is too big to be transferred in a single command, then no data is transferred and EFI_BAD_BUFFER_SIZE
+  // is returned.
+  //
+  if (((Packet->InTransferLength != 0) && (Packet->InTransferLength > MaxSectorCount * BlockSize)) ||
+      ((Packet->OutTransferLength != 0) && (Packet->OutTransferLength > MaxSectorCount * BlockSize))) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  //
+  // For non-blocking mode, queue the Task into the list.
+  //
+  if (Event != NULL) {
+    Task = AllocateZeroPool (sizeof (ATA_NONBLOCK_TASK));
+    if (Task == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Task->Signature      = ATA_NONBLOCKING_TASK_SIGNATURE;
+    Task->Port           = Port;
+    Task->PortMultiplier = PortMultiplierPort;
+    Task->Packet         = Packet;
+    Task->Event          = Event;
+    Task->IsStart        = FALSE;
+    Task->RetryTimes     = DivU64x32(Packet->Timeout, 1000) + 1;
+    if (Packet->Timeout == 0) {
+      Task->InfiniteWait = TRUE;
+    } else {
+      Task->InfiniteWait = FALSE;
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    InsertTailList (&Instance->NonBlockingTaskList, &Task->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    return EFI_SUCCESS;
+  } else {
+    return AtaPassThruPassThruExecute (
+             Port,
+             PortMultiplierPort,
+             Packet,
+             Instance,
+             NULL
+             );
+  }
+}
+
+/**
+  Used to retrieve the list of legal port numbers for ATA devices on an ATA controller.
+  These can either be the list of ports where ATA devices are actually present or the
+  list of legal port numbers for the ATA controller. Regardless, the caller of this
+  function must probe the port number returned to see if an ATA device is actually
+  present at that location on the ATA controller.
+
+  The GetNextPort() function retrieves the port number on an ATA controller. If on input
+  Port is 0xFFFF, then the port number of the first port on the ATA controller is returned
+  in Port and EFI_SUCCESS is returned.
+
+  If Port is a port number that was returned on a previous call to GetNextPort(), then the
+  port number of the next port on the ATA controller is returned in Port, and EFI_SUCCESS
+  is returned. If Port is not 0xFFFF and Port was not returned on a previous call to
+  GetNextPort(), then EFI_INVALID_PARAMETER is returned.
+
+  If Port is the port number of the last port on the ATA controller, then EFI_NOT_FOUND is
+  returned.
+
+  @param[in]      This          A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in, out] Port          On input, a pointer to the port number on the ATA controller.
+                                On output, a pointer to the next port number on the ATA
+                                controller. An input value of 0xFFFF retrieves the first port
+                                number on the ATA controller.
+
+  @retval EFI_SUCCESS           The next port number on the ATA controller was returned in Port.
+  @retval EFI_NOT_FOUND         There are no more ports on this ATA controller.
+  @retval EFI_INVALID_PARAMETER Port is not 0xFFFF and Port was not returned on a previous call
+                                to GetNextPort().
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetNextPort (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN OUT UINT16                 *Port
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (Port == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Port == 0xFFFF) {
+    //
+    // If the Port is all 0xFF's, start to traverse the device list from the beginning
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if (DeviceInfo->Type == EfiIdeHarddisk) {
+        *Port = DeviceInfo->Port;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else if (*Port == Instance->PreviousPort) {
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeHarddisk) &&
+           (DeviceInfo->Port > *Port)){
+        *Port = DeviceInfo->Port;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else {
+    //
+    // Port is not equal to 0xFFFF and also not equal to previous return value
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+Exit:
+  //
+  // Update the PreviousPort and PreviousPortMultiplier.
+  //
+  Instance->PreviousPort = *Port;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to retrieve the list of legal port multiplier port numbers for ATA devices on a port of an ATA
+  controller. These can either be the list of port multiplier ports where ATA devices are actually
+  present on port or the list of legal port multiplier ports on that port. Regardless, the caller of this
+  function must probe the port number and port multiplier port number returned to see if an ATA
+  device is actually present.
+
+  The GetNextDevice() function retrieves the port multiplier port number of an ATA device
+  present on a port of an ATA controller.
+
+  If PortMultiplierPort points to a port multiplier port number value that was returned on a
+  previous call to GetNextDevice(), then the port multiplier port number of the next ATA device
+  on the port of the ATA controller is returned in PortMultiplierPort, and EFI_SUCCESS is
+  returned.
+
+  If PortMultiplierPort points to 0xFFFF, then the port multiplier port number of the first
+  ATA device on port of the ATA controller is returned in PortMultiplierPort and
+  EFI_SUCCESS is returned.
+
+  If PortMultiplierPort is not 0xFFFF and the value pointed to by PortMultiplierPort
+  was not returned on a previous call to GetNextDevice(), then EFI_INVALID_PARAMETER
+  is returned.
+
+  If PortMultiplierPort is the port multiplier port number of the last ATA device on the port of
+  the ATA controller, then EFI_NOT_FOUND is returned.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               The port number present on the ATA controller.
+  @param[in, out] PortMultiplierPort On input, a pointer to the port multiplier port number of an
+                                     ATA device present on the ATA controller.
+                                     If on input a PortMultiplierPort of 0xFFFF is specified,
+                                     then the port multiplier port number of the first ATA device
+                                     is returned. On output, a pointer to the port multiplier port
+                                     number of the next ATA device present on an ATA controller.
+
+  @retval EFI_SUCCESS                The port multiplier port number of the next ATA device on the port
+                                     of the ATA controller was returned in PortMultiplierPort.
+  @retval EFI_NOT_FOUND              There are no more ATA devices on this port of the ATA controller.
+  @retval EFI_INVALID_PARAMETER      PortMultiplierPort is not 0xFFFF, and PortMultiplierPort was not
+                                     returned on a previous call to GetNextDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetNextDevice (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port,
+  IN OUT UINT16                 *PortMultiplierPort
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (PortMultiplierPort == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*PortMultiplierPort == 0xFFFF) {
+    //
+    // If the PortMultiplierPort is all 0xFF's, start to traverse the device list from the beginning
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeHarddisk) &&
+           (DeviceInfo->Port == Port)){
+        *PortMultiplierPort = DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else if (*PortMultiplierPort == Instance->PreviousPortMultiplier) {
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeHarddisk) &&
+           (DeviceInfo->Port == Port) &&
+           (DeviceInfo->PortMultiplier > *PortMultiplierPort)){
+        *PortMultiplierPort = DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else {
+    //
+    // PortMultiplierPort is not equal to 0xFFFF and also not equal to previous return value
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+Exit:
+  //
+  // Update the PreviousPort and PreviousPortMultiplier.
+  //
+  Instance->PreviousPortMultiplier = *PortMultiplierPort;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to allocate and build a device path node for an ATA device on an ATA controller.
+
+  The BuildDevicePath() function allocates and builds a single device node for the ATA
+  device specified by Port and PortMultiplierPort. If the ATA device specified by Port and
+  PortMultiplierPort is not present on the ATA controller, then EFI_NOT_FOUND is returned.
+  If DevicePath is NULL, then EFI_INVALID_PARAMETER is returned. If there are not enough
+  resources to allocate the device path node, then EFI_OUT_OF_RESOURCES is returned.
+
+  Otherwise, DevicePath is allocated with the boot service AllocatePool(), the contents of
+  DevicePath are initialized to describe the ATA device specified by Port and PortMultiplierPort,
+  and EFI_SUCCESS is returned.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               Port specifies the port number of the ATA device for which a
+                                     device path node is to be allocated and built.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device for which a
+                                     device path node is to be allocated and built. If there is no
+                                     port multiplier, then specify 0.
+  @param[in, out] DevicePath         A pointer to a single device path node that describes the ATA
+                                     device specified by Port and PortMultiplierPort. This function
+                                     is responsible for allocating the buffer DevicePath with the
+                                     boot service AllocatePool(). It is the caller's responsibility
+                                     to free DevicePath when the caller is finished with DevicePath.
+  @retval EFI_SUCCESS                The device path node that describes the ATA device specified by
+                                     Port and PortMultiplierPort was allocated and returned in DevicePath.
+  @retval EFI_NOT_FOUND              The ATA device specified by Port and PortMultiplierPort does not
+                                     exist on the ATA controller.
+  @retval EFI_INVALID_PARAMETER      DevicePath is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruBuildDevicePath (
+  IN     EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN     UINT16                     Port,
+  IN     UINT16                     PortMultiplierPort,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL   **DevicePath
+  )
+{
+  EFI_DEV_PATH                    *DevicePathNode;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Node = SearchDeviceInfoList(Instance, Port, PortMultiplierPort, EfiIdeHarddisk);
+  if (Node == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    DevicePathNode = AllocateCopyPool (sizeof (ATAPI_DEVICE_PATH), &mAtapiDevicePathTemplate);
+    if (DevicePathNode == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    DevicePathNode->Atapi.PrimarySecondary = (UINT8) Port;
+    DevicePathNode->Atapi.SlaveMaster      = (UINT8) PortMultiplierPort;
+    DevicePathNode->Atapi.Lun              = 0;
+  } else {
+    DevicePathNode = AllocateCopyPool (sizeof (SATA_DEVICE_PATH), &mSataDevicePathTemplate);
+    if (DevicePathNode == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    DevicePathNode->Sata.HBAPortNumber            = Port;
+    DevicePathNode->Sata.PortMultiplierPortNumber = PortMultiplierPort;
+    DevicePathNode->Sata.Lun                      = 0;
+  }
+
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) DevicePathNode;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to translate a device path node to a port number and port multiplier port number.
+
+  The GetDevice() function determines the port and port multiplier port number associated with
+  the ATA device described by DevicePath. If DevicePath is a device path node type that the
+  ATA Pass Thru driver supports, then the ATA Pass Thru driver will attempt to translate the contents
+  DevicePath into a port number and port multiplier port number.
+
+  If this translation is successful, then that port number and port multiplier port number are returned
+  in Port and PortMultiplierPort, and EFI_SUCCESS is returned.
+
+  If DevicePath, Port, or PortMultiplierPort are NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If DevicePath is not a device path node type that the ATA Pass Thru driver supports, then
+  EFI_UNSUPPORTED is returned.
+
+  If DevicePath is a device path node type that the ATA Pass Thru driver supports, but there is not
+  a valid translation from DevicePath to a port number and port multiplier port number, then
+  EFI_NOT_FOUND is returned.
+
+  @param[in]  This                A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]  DevicePath          A pointer to the device path node that describes an ATA device on the
+                                  ATA controller.
+  @param[out] Port                On return, points to the port number of an ATA device on the ATA controller.
+  @param[out] PortMultiplierPort  On return, points to the port multiplier port number of an ATA device
+                                  on the ATA controller.
+
+  @retval EFI_SUCCESS             DevicePath was successfully translated to a port number and port multiplier
+                                  port number, and they were returned in Port and PortMultiplierPort.
+  @retval EFI_INVALID_PARAMETER   DevicePath is NULL.
+  @retval EFI_INVALID_PARAMETER   Port is NULL.
+  @retval EFI_INVALID_PARAMETER   PortMultiplierPort is NULL.
+  @retval EFI_UNSUPPORTED         This driver does not support the device path node type in DevicePath.
+  @retval EFI_NOT_FOUND           A valid translation from DevicePath to a port number and port multiplier
+                                  port number does not exist.
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetDevice (
+  IN  EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL   *DevicePath,
+  OUT UINT16                     *Port,
+  OUT UINT16                     *PortMultiplierPort
+  )
+{
+  EFI_DEV_PATH                    *DevicePathNode;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL || Port == NULL || PortMultiplierPort == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether the DevicePath belongs to SCSI_DEVICE_PATH or ATAPI_DEVICE_PATH
+  //
+  if ((DevicePath->Type != MESSAGING_DEVICE_PATH) ||
+      ((DevicePath->SubType != MSG_SATA_DP) &&
+      (DevicePath->SubType != MSG_ATAPI_DP)) ||
+      ((DevicePathNodeLength(DevicePath) != sizeof(ATAPI_DEVICE_PATH)) &&
+      (DevicePathNodeLength(DevicePath) != sizeof(SATA_DEVICE_PATH)))) {
+    return EFI_UNSUPPORTED;
+  }
+
+  DevicePathNode = (EFI_DEV_PATH *) DevicePath;
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    *Port               = DevicePathNode->Atapi.PrimarySecondary;
+    *PortMultiplierPort = DevicePathNode->Atapi.SlaveMaster;
+  } else {
+    *Port               = DevicePathNode->Sata.HBAPortNumber;
+    *PortMultiplierPort = DevicePathNode->Sata.PortMultiplierPortNumber;
+  }
+
+  Node = SearchDeviceInfoList(Instance, *Port, *PortMultiplierPort, EfiIdeHarddisk);
+
+  if (Node == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a specific port on the ATA controller. This operation also resets all the ATA devices
+  connected to the port.
+
+  The ResetChannel() function resets an a specific port on an ATA controller. This operation
+  resets all the ATA devices connected to that port. If this ATA controller does not support
+  a reset port operation, then EFI_UNSUPPORTED is returned.
+
+  If a device error occurs while executing that port reset operation, then EFI_DEVICE_ERROR is
+  returned.
+
+  If a timeout occurs during the execution of the port reset operation, then EFI_TIMEOUT is returned.
+
+  If the port reset operation is completed, then EFI_SUCCESS is returned.
+
+  @param[in]  This          A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]  Port          The port number on the ATA controller.
+
+  @retval EFI_SUCCESS       The ATA controller port was reset.
+  @retval EFI_UNSUPPORTED   The ATA controller does not support a port reset operation.
+  @retval EFI_DEVICE_ERROR  A device error occurred while attempting to reset the ATA port.
+  @retval EFI_TIMEOUT       A timeout occurred while attempting to reset the ATA port.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruResetPort (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port
+  )
+{
+  //
+  // Return success directly then upper layer driver could think reset port operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets an ATA device that is connected to an ATA controller.
+
+  The ResetDevice() function resets the ATA device specified by Port and PortMultiplierPort.
+  If this ATA controller does not support a device reset operation, then EFI_UNSUPPORTED is
+  returned.
+
+  If Port or PortMultiplierPort are not in a valid range for this ATA controller, then
+  EFI_INVALID_PARAMETER is returned.
+
+  If a device error occurs while executing that device reset operation, then EFI_DEVICE_ERROR
+  is returned.
+
+  If a timeout occurs during the execution of the device reset operation, then EFI_TIMEOUT is
+  returned.
+
+  If the device reset operation is completed, then EFI_SUCCESS is returned.
+
+  @param[in] This                A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in] Port                Port represents the port number of the ATA device to be reset.
+  @param[in] PortMultiplierPort  The port multiplier port number of the ATA device to reset.
+                                 If there is no port multiplier, then specify 0.
+  @retval EFI_SUCCESS            The ATA device specified by Port and PortMultiplierPort was reset.
+  @retval EFI_UNSUPPORTED        The ATA controller does not support a device reset operation.
+  @retval EFI_INVALID_PARAMETER  Port or PortMultiplierPort are invalid.
+  @retval EFI_DEVICE_ERROR       A device error occurred while attempting to reset the ATA device
+                                 specified by Port and PortMultiplierPort.
+  @retval EFI_TIMEOUT            A timeout occurred while attempting to reset the ATA device
+                                 specified by Port and PortMultiplierPort.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruResetDevice (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port,
+  IN UINT16                     PortMultiplierPort
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  Node = SearchDeviceInfoList (Instance, Port, PortMultiplierPort, EfiIdeHarddisk);
+
+  if (Node == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Return success directly then upper layer driver could think reset device operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Sumbit ATAPI request sense command.
+
+  @param[in] This            A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param[in] Target          The Target is an array of size TARGET_MAX_BYTES and it represents
+                             the id of the SCSI device to send the SCSI Request Packet. Each
+                             transport driver may choose to utilize a subset of this size to suit the needs
+                             of transport target representation. For example, a Fibre Channel driver
+                             may use only 8 bytes (WWN) to represent an FC target.
+  @param[in] Lun             The LUN of the SCSI device to send the SCSI Request Packet.
+  @param[in] SenseData       A pointer to store sense data.
+  @param[in] SenseDataLength The sense data length.
+  @param[in] Timeout         The timeout value to execute this cmd, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS        Send out the ATAPI packet command successfully.
+  @retval EFI_DEVICE_ERROR   The device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketRequestSense (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL         *This,
+  IN  UINT8                                   *Target,
+  IN  UINT64                                  Lun,
+  IN  VOID                                    *SenseData,
+  IN  UINT8                                   SenseDataLength,
+  IN  UINT64                                  Timeout
+  )
+{
+  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  Packet;
+  UINT8                                       Cdb[12];
+  EFI_STATUS                                  Status;
+
+  ZeroMem (&Packet, sizeof (EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, 12);
+
+  Cdb[0] = ATA_CMD_REQUEST_SENSE;
+  Cdb[4] = SenseDataLength;
+
+  Packet.Timeout          = Timeout;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = 12;
+  Packet.DataDirection    = EFI_EXT_SCSI_DATA_DIRECTION_READ;
+  Packet.InDataBuffer     = SenseData;
+  Packet.InTransferLength = SenseDataLength;
+
+  Status = ExtScsiPassThruPassThru (This, Target, Lun, &Packet, NULL);
+
+  return Status;
+}
+
+/**
+  Sends a SCSI Request Packet to a SCSI device that is attached to the SCSI channel. This function
+  supports both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the
+  nonblocking I/O functionality is optional.
+
+  @param  This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target  The Target is an array of size TARGET_MAX_BYTES and it represents
+                  the id of the SCSI device to send the SCSI Request Packet. Each
+                  transport driver may choose to utilize a subset of this size to suit the needs
+                  of transport target representation. For example, a Fibre Channel driver
+                  may use only 8 bytes (WWN) to represent an FC target.
+  @param  Lun     The LUN of the SCSI device to send the SCSI Request Packet.
+  @param  Packet  A pointer to the SCSI Request Packet to send to the SCSI device
+                  specified by Target and Lun.
+  @param  Event   If nonblocking I/O is not supported then Event is ignored, and blocking
+                  I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                  Event is not NULL and non blocking I/O is supported, then
+                  nonblocking I/O is performed, and Event will be signaled when the
+                  SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE   The SCSI Request Packet was not executed. The number of bytes that
+                                could be transferred is returned in InTransferLength. For write
+                                and bi-directional commands, OutTransferLength bytes were
+                                transferred by OutDataBuffer.
+  @retval EFI_NOT_READY         The SCSI Request Packet could not be sent because there are too many
+                                SCSI Request Packets already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_INVALID_PARAMETER Target, Lun, or the contents of ScsiRequestPacket are invalid.
+  @retval EFI_UNSUPPORTED       The command described by the SCSI Request Packet is not supported
+                                by the host adapter. This includes the case of Bi-directional SCSI
+                                commands not supported by the implementation. The SCSI Request
+                                Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruPassThru (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                    *This,
+  IN UINT8                                              *Target,
+  IN UINT64                                             Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET     *Packet,
+  IN EFI_EVENT                                          Event OPTIONAL
+  )
+{
+  EFI_STATUS                      Status;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  UINT8                           Port;
+  UINT8                           PortMultiplier;
+  EFI_ATA_HC_WORK_MODE            Mode;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+  BOOLEAN                         SenseReq;
+  EFI_SCSI_SENSE_DATA             *PtrSenseData;
+  UINTN                           SenseDataLen;
+  EFI_STATUS                      SenseStatus;
+
+  SenseDataLen = 0;
+  Instance     = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if ((Packet == NULL) || (Packet->Cdb == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Don't support variable length CDB
+  //
+  if ((Packet->CdbLength != 6) && (Packet->CdbLength != 10) &&
+      (Packet->CdbLength != 12) && (Packet->CdbLength != 16)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Packet->SenseDataLength != 0) && (Packet->SenseData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->InDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->OutDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->SenseData, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // For ATAPI device, doesn't support multiple LUN device.
+  //
+  if (Lun != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // The layout of Target array:
+  //  ________________________________________________________________________
+  // |       Byte 0        |       Byte 1        | ... | TARGET_MAX_BYTES - 1 |
+  // |_____________________|_____________________|_____|______________________|
+  // |                     | The port multiplier |     |                      |
+  // |   The port number   |    port number      | N/A |         N/A          |
+  // |_____________________|_____________________|_____|______________________|
+  //
+  // For ATAPI device, 2 bytes is enough to represent the location of SCSI device.
+  //
+  Port           = Target[0];
+  PortMultiplier = Target[1];
+
+  Node = SearchDeviceInfoList(Instance, Port, PortMultiplier, EfiIdeCdrom);
+  if (Node == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+  //
+  // ATA_CMD_IDENTIFY_DEVICE cmd is a ATA cmd but not a SCSI cmd.
+  // Normally it should NOT be passed down through ExtScsiPassThru protocol interface.
+  // But to response EFI_DISK_INFO.Identify() request from ScsiDisk, we should handle this command.
+  //
+  if (*((UINT8*)Packet->Cdb) == ATA_CMD_IDENTIFY_DEVICE) {
+    CopyMem (Packet->InDataBuffer, DeviceInfo->IdentifyData, sizeof (EFI_IDENTIFY_DATA));
+    //
+    // For IDENTIFY DEVICE cmd, we don't need to get sense data.
+    //
+    Packet->SenseDataLength = 0;
+    return EFI_SUCCESS;
+  }
+
+  Mode = Instance->Mode;
+  switch (Mode) {
+    case EfiAtaIdeMode:
+      //
+      // Reassign IDE mode io port registers' base addresses
+      //
+      Status = GetIdeRegisterIoAddr (Instance->PciIo, Instance->IdeRegisters);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      Status = AtaPacketCommandExecute (Instance->PciIo, &Instance->IdeRegisters[Port], Port, PortMultiplier, Packet);
+      break;
+    case EfiAtaAhciMode:
+      Status = AhciPacketCommandExecute (Instance->PciIo, &Instance->AhciRegisters, Port, PortMultiplier, Packet);
+      break;
+    default :
+      Status = EFI_DEVICE_ERROR;
+      break;
+  }
+
+  //
+  // If the cmd doesn't get executed correctly, then check sense data.
+  //
+  if (EFI_ERROR (Status) && (Packet->SenseDataLength != 0) && (*((UINT8*)Packet->Cdb) != ATA_CMD_REQUEST_SENSE)) {
+    PtrSenseData = AllocateAlignedPages (EFI_SIZE_TO_PAGES (sizeof (EFI_SCSI_SENSE_DATA)), This->Mode->IoAlign);
+    if (PtrSenseData == NULL) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    for (SenseReq = TRUE; SenseReq;) {
+      SenseStatus = AtaPacketRequestSense (
+                      This,
+                      Target,
+                      Lun,
+                      PtrSenseData,
+                      sizeof (EFI_SCSI_SENSE_DATA),
+                      Packet->Timeout
+                      );
+      if (EFI_ERROR (SenseStatus)) {
+        break;
+      }
+
+      CopyMem ((UINT8*)Packet->SenseData + SenseDataLen, PtrSenseData, sizeof (EFI_SCSI_SENSE_DATA));
+      SenseDataLen += sizeof (EFI_SCSI_SENSE_DATA);
+
+      //
+      // no more sense key or number of sense keys exceeds predefined,
+      // skip the loop.
+      //
+      if ((PtrSenseData->Sense_Key == EFI_SCSI_SK_NO_SENSE) ||
+          (SenseDataLen + sizeof (EFI_SCSI_SENSE_DATA) > Packet->SenseDataLength)) {
+        SenseReq = FALSE;
+      }
+    }
+    FreeAlignedPages (PtrSenseData, EFI_SIZE_TO_PAGES (sizeof (EFI_SCSI_SENSE_DATA)));
+  }
+  //
+  // Update the SenseDataLength field to the data length received.
+  //
+  Packet->SenseDataLength = (UINT8)SenseDataLen;
+  return Status;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs and LUNs for SCSI devices on a SCSI channel. These
+  can either be the list SCSI devices that are actually present on the SCSI channel, or the list of legal
+  Target Ids and LUNs for the SCSI channel. Regardless, the caller of this function must probe the
+  Target ID and LUN returned to see if a SCSI device is actually present at that location on the SCSI
+  channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target On input, a pointer to the Target ID (an array of size
+                 TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+  @param  Lun    On input, a pointer to the LUN of a SCSI device present on the SCSI
+                 channel. On output, a pointer to the LUN of the next SCSI device present
+                 on a SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID and LUN of the next SCSI device on the SCSI
+                                channel was returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER Target array is not all 0xF, and Target and Lun were
+                                not returned on a previous call to GetNextTargetLun().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetNextTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target,
+  IN OUT UINT64                          *Lun
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+  UINT8                           *Target8;
+  UINT16                          *Target16;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Target8  = *Target;
+  Target16 = (UINT16 *)*Target;
+
+  if (CompareMem(Target8, mScsiId, TARGET_MAX_BYTES) != 0) {
+    //
+    // For ATAPI device, we use 2 least significant bytes to represent the location of SCSI device.
+    // So the higher bytes in Target array should be 0xFF.
+    //
+    if (CompareMem (&Target8[2], &mScsiId[2], TARGET_MAX_BYTES - 2) != 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // When Target is not all 0xFF's, compare 2 least significant bytes with
+    // previous target id to see if it is returned by previous call.
+    //
+    if ((*Target16 != Instance->PreviousTargetId) ||
+        (*Lun != Instance->PreviousLun)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Traverse the whole device list to find the next cdrom closed to
+    // the device signified by Target[0] and Target[1].
+    //
+    // Note that we here use a tricky way to find the next cdrom :
+    // All ata devices are detected and inserted into the device list
+    // sequentially.
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeCdrom) &&
+         ((Target8[0] < DeviceInfo->Port) ||
+          ((Target8[0] == DeviceInfo->Port) &&
+           (Target8[1] < DeviceInfo->PortMultiplier)))) {
+        Target8[0] = (UINT8)DeviceInfo->Port;
+        Target8[1] = (UINT8)DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else {
+    //
+    // If the array is all 0xFF's, start to traverse the device list from the beginning
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if (DeviceInfo->Type == EfiIdeCdrom) {
+        Target8[0] = (UINT8)DeviceInfo->Port;
+        Target8[1] = (UINT8)DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  }
+
+Exit:
+  *Lun = 0;
+
+  //
+  // Update the PreviousTargetId.
+  //
+  Instance->PreviousTargetId = *Target16;
+  Instance->PreviousLun      = *Lun;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to allocate and build a device path node for a SCSI device on a SCSI channel.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target     The Target is an array of size TARGET_MAX_BYTES and it specifies the
+                     Target ID of the SCSI device for which a device path node is to be
+                     allocated and built. Transport drivers may chose to utilize a subset of
+                     this size to suit the representation of targets. For example, a Fibre
+                     Channel driver may use only 8 bytes (WWN) in the array to represent a
+                     FC target.
+  @param  Lun        The LUN of the SCSI device for which a device path node is to be
+                     allocated and built.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     specified by Target and Lun. This function is responsible for
+                     allocating the buffer DevicePath with the boot service
+                     AllocatePool(). It is the caller's responsibility to free
+                     DevicePath when the caller is finished with DevicePath.
+
+  @retval EFI_SUCCESS           The device path node that describes the SCSI device specified by
+                                Target and Lun was allocated and returned in
+                                DevicePath.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_NOT_FOUND         The SCSI devices specified by Target and Lun does not exist
+                                on the SCSI channel.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruBuildDevicePath (
+  IN     EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN     UINT8                              *Target,
+  IN     UINT64                             Lun,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL           **DevicePath
+  )
+{
+  EFI_DEV_PATH                    *DevicePathNode;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  UINT8                           Port;
+  UINT8                           PortMultiplier;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  Port           = Target[0];
+  PortMultiplier = Target[1];
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // can not build device path for the SCSI Host Controller.
+  //
+  if (Lun != 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (SearchDeviceInfoList(Instance, Port, PortMultiplier, EfiIdeCdrom) == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    DevicePathNode = AllocateCopyPool (sizeof (ATAPI_DEVICE_PATH), &mAtapiDevicePathTemplate);
+    if (DevicePathNode == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    DevicePathNode->Atapi.PrimarySecondary = Port;
+    DevicePathNode->Atapi.SlaveMaster      = PortMultiplier;
+    DevicePathNode->Atapi.Lun              = (UINT16) Lun;
+  } else {
+    DevicePathNode = AllocateCopyPool (sizeof (SATA_DEVICE_PATH), &mSataDevicePathTemplate);
+    if (DevicePathNode == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    DevicePathNode->Sata.HBAPortNumber            = Port;
+    DevicePathNode->Sata.PortMultiplierPortNumber = PortMultiplier;
+    DevicePathNode->Sata.Lun                      = (UINT16) Lun;
+  }
+
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) DevicePathNode;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to translate a device path node to a Target ID and LUN.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     on the SCSI channel.
+  @param  Target     A pointer to the Target Array which represents the ID of a SCSI device
+                     on the SCSI channel.
+  @param  Lun        A pointer to the LUN of a SCSI device on the SCSI channel.
+
+  @retval EFI_SUCCESS           DevicePath was successfully translated to a Target ID and
+                                LUN, and they were returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER DevicePath or Target or Lun is NULL.
+  @retval EFI_NOT_FOUND         A valid translation from DevicePath to a Target ID and LUN
+                                does not exist.
+  @retval EFI_UNSUPPORTED       This driver does not support the device path node type in
+                                 DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  OUT UINT8                              **Target,
+  OUT UINT64                             *Lun
+  )
+{
+  EFI_DEV_PATH                    *DevicePathNode;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL || Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Check whether the DevicePath belongs to SCSI_DEVICE_PATH
+  //
+  if ((DevicePath->Type != MESSAGING_DEVICE_PATH) ||
+      ((DevicePath->SubType != MSG_ATAPI_DP) &&
+      (DevicePath->SubType != MSG_SATA_DP)) ||
+      ((DevicePathNodeLength(DevicePath) != sizeof(ATAPI_DEVICE_PATH)) &&
+      (DevicePathNodeLength(DevicePath) != sizeof(SATA_DEVICE_PATH)))) {
+    return EFI_UNSUPPORTED;
+  }
+
+  SetMem (*Target, TARGET_MAX_BYTES, 0xFF);
+
+  DevicePathNode = (EFI_DEV_PATH *) DevicePath;
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    (*Target)[0] = (UINT8) DevicePathNode->Atapi.PrimarySecondary;
+    (*Target)[1] = (UINT8) DevicePathNode->Atapi.SlaveMaster;
+    *Lun         = (UINT8) DevicePathNode->Atapi.Lun;
+  } else {
+    (*Target)[0] = (UINT8) DevicePathNode->Sata.HBAPortNumber;
+    (*Target)[1] = (UINT8) DevicePathNode->Sata.PortMultiplierPortNumber;
+    *Lun         = (UINT8) DevicePathNode->Sata.Lun;
+  }
+
+  Node = SearchDeviceInfoList(Instance, (*Target)[0], (*Target)[1], EfiIdeCdrom);
+
+  if (Node == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (*Lun != 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a SCSI channel. This operation resets all the SCSI devices connected to the SCSI channel.
+
+  @param  This A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      The SCSI channel was reset.
+  @retval EFI_DEVICE_ERROR A device error occurred while attempting to reset the SCSI channel.
+  @retval EFI_TIMEOUT      A timeout occurred while attempting to reset the SCSI channel.
+  @retval EFI_UNSUPPORTED  The SCSI channel does not support a channel reset operation.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruResetChannel (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   *This
+  )
+{
+  //
+  // Return success directly then upper layer driver could think reset channel operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a SCSI logical unit that is connected to a SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target The Target is an array of size TARGET_MAX_BYTE and it represents the
+                 target port ID of the SCSI device containing the SCSI logical unit to
+                 reset. Transport drivers may chose to utilize a subset of this array to suit
+                 the representation of their targets.
+  @param  Lun    The LUN of the SCSI device to reset.
+
+  @retval EFI_SUCCESS           The SCSI device specified by Target and Lun was reset.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           A timeout occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+  @retval EFI_UNSUPPORTED       The SCSI channel does not support a target reset operation.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to reset the SCSI device
+                                 specified by Target and Lun.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruResetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN UINT8                              *Target,
+  IN UINT64                             Lun
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  UINT8                           Port;
+  UINT8                           PortMultiplier;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+  //
+  // For ATAPI device, doesn't support multiple LUN device.
+  //
+  if (Lun != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // The layout of Target array:
+  //  ________________________________________________________________________
+  // |       Byte 0        |       Byte 1        | ... | TARGET_MAX_BYTES - 1 |
+  // |_____________________|_____________________|_____|______________________|
+  // |                     | The port multiplier |     |                      |
+  // |   The port number   |    port number      | N/A |         N/A          |
+  // |_____________________|_____________________|_____|______________________|
+  //
+  // For ATAPI device, 2 bytes is enough to represent the location of SCSI device.
+  //
+  Port           = Target[0];
+  PortMultiplier = Target[1];
+
+  Node = SearchDeviceInfoList(Instance, Port, PortMultiplier, EfiIdeCdrom);
+  if (Node == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Return success directly then upper layer driver could think reset target LUN operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs for SCSI devices on a SCSI channel. These can either
+  be the list SCSI devices that are actually present on the SCSI channel, or the list of legal Target IDs
+  for the SCSI channel. Regardless, the caller of this function must probe the Target ID returned to
+  see if a SCSI device is actually present at that location on the SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target (TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID of the next SCSI device on the SCSI
+                                channel was returned in Target.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           Target array is not all 0xF, and Target was not
+                                returned on a previous call to GetNextTarget().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetNextTarget (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+  UINT8                           *Target8;
+  UINT16                          *Target16;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (Target == NULL || *Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Target8  = *Target;
+  Target16 = (UINT16 *)*Target;
+
+  if (CompareMem(Target8, mScsiId, TARGET_MAX_BYTES) != 0) {
+    //
+    // For ATAPI device, we use 2 least significant bytes to represent the location of SCSI device.
+    // So the higher bytes in Target array should be 0xFF.
+    //
+    if (CompareMem (&Target8[2], &mScsiId[2], TARGET_MAX_BYTES - 2) != 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // When Target is not all 0xFF's, compare 2 least significant bytes with
+    // previous target id to see if it is returned by previous call.
+    //
+    if (*Target16 != Instance->PreviousTargetId) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Traverse the whole device list to find the next cdrom closed to
+    // the device signified by Target[0] and Target[1].
+    //
+    // Note that we here use a tricky way to find the next cdrom :
+    // All ata devices are detected and inserted into the device list
+    // sequentially.
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeCdrom) &&
+         ((Target8[0] < DeviceInfo->Port) ||
+          ((Target8[0] == DeviceInfo->Port) &&
+           (Target8[1] < DeviceInfo->PortMultiplier)))) {
+        Target8[0] = (UINT8)DeviceInfo->Port;
+        Target8[1] = (UINT8)DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else {
+    //
+    // If the array is all 0xFF's, start to traverse the device list from the beginning
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if (DeviceInfo->Type == EfiIdeCdrom) {
+        Target8[0] = (UINT8)DeviceInfo->Port;
+        Target8[1] = (UINT8)DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  }
+
+Exit:
+  //
+  // Update the PreviousTargetId.
+  //
+  Instance->PreviousTargetId = *Target16;
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.h b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.h
new file mode 100644
index 0000000000..6f3407c01a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.h
@@ -0,0 +1,1300 @@
+/** @file
+  Header file for ATA/ATAPI PASS THRU driver.
+
+  Copyright (c) 2010 - 2012, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+#ifndef __ATA_ATAPI_PASS_THRU_H__
+#define __ATA_ATAPI_PASS_THRU_H__
+
+#include <Uefi.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/Atapi.h>
+#include <IndustryStandard/Scsi.h>
+
+#include <Protocol/PciIo.h>
+#include <Protocol/IdeControllerInit.h>
+#include <Protocol/AtaPassThru.h>
+#include <Protocol/ScsiPassThruExt.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/PciLib.h>
+#include <Library/PcdLib.h>
+#include <Library/TimerLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/DevicePathLib.h>
+
+#include "IdeMode.h"
+#include "AhciMode.h"
+
+extern EFI_DRIVER_BINDING_PROTOCOL  gAtaAtapiPassThruDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gAtaAtapiPassThruComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gAtaAtapiPassThruComponentName2;
+
+#define ATA_ATAPI_PASS_THRU_SIGNATURE  SIGNATURE_32 ('a', 'a', 'p', 't')
+#define ATA_ATAPI_DEVICE_SIGNATURE     SIGNATURE_32 ('a', 'd', 'e', 'v')
+#define ATA_NONBLOCKING_TASK_SIGNATURE  SIGNATURE_32 ('a', 't', 's', 'k')
+
+typedef struct _ATA_NONBLOCK_TASK ATA_NONBLOCK_TASK;
+
+typedef enum {
+  EfiAtaIdeMode,
+  EfiAtaAhciMode,
+  EfiAtaRaidMode,
+  EfiAtaUnknownMode
+} EFI_ATA_HC_WORK_MODE;
+
+typedef enum {
+  EfiIdeCdrom,                  /* ATAPI CDROM */
+  EfiIdeHarddisk,               /* Hard Disk */
+  EfiPortMultiplier,            /* Port Multiplier */
+  EfiIdeUnknown
+} EFI_ATA_DEVICE_TYPE;
+
+//
+// Ahci mode device info
+//
+typedef struct {
+  UINT32                            Signature;
+  LIST_ENTRY                        Link;
+
+  UINT16                            Port;
+  UINT16                            PortMultiplier;
+  EFI_ATA_DEVICE_TYPE               Type;
+
+  EFI_IDENTIFY_DATA                 *IdentifyData;
+} EFI_ATA_DEVICE_INFO;
+
+typedef struct {
+  UINT32                            Signature;
+
+  EFI_HANDLE                        ControllerHandle;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeControllerInit;
+
+  EFI_ATA_PASS_THRU_MODE            AtaPassThruMode;
+  EFI_ATA_PASS_THRU_PROTOCOL        AtaPassThru;
+  EFI_EXT_SCSI_PASS_THRU_MODE       ExtScsiPassThruMode;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   ExtScsiPassThru;
+
+  EFI_ATA_HC_WORK_MODE              Mode;
+
+  EFI_IDE_REGISTERS                 IdeRegisters[EfiIdeMaxChannel];
+  EFI_AHCI_REGISTERS                AhciRegisters;
+
+  //
+  // The attached device list
+  //
+  LIST_ENTRY                        DeviceList;
+  UINT64                            OriginalPciAttributes;
+
+  //
+  // For AtaPassThru protocol, using the following bytes to record the previous call in
+  // GetNextPort()/GetNextDevice().
+  //
+  UINT16                            PreviousPort;
+  UINT16                            PreviousPortMultiplier;
+  //
+  // For ExtScsiPassThru protocol, using the following bytes to record the previous call in
+  // GetNextTarget()/GetNextTargetLun().
+  //
+  UINT16                            PreviousTargetId;
+  UINT64                            PreviousLun;
+
+  //
+  // For Non-blocking.
+  //
+  EFI_EVENT                         TimerEvent;
+  LIST_ENTRY                        NonBlockingTaskList;
+} ATA_ATAPI_PASS_THRU_INSTANCE;
+
+//
+// Task for Non-blocking mode.
+//
+struct _ATA_NONBLOCK_TASK {
+  UINT32                            Signature;
+  LIST_ENTRY                        Link;
+
+  UINT16                            Port;
+  UINT16                            PortMultiplier;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  *Packet;
+  BOOLEAN                           IsStart;
+  EFI_EVENT                         Event;
+  UINT64                            RetryTimes;
+  BOOLEAN                           InfiniteWait;
+  VOID                              *Map;            // Pointer to map.
+  VOID                              *TableMap;       // Pointer to PRD table map.
+  EFI_ATA_DMA_PRD                   *MapBaseAddress; //  Pointer to range Base address for Map.
+  UINTN                             PageCount;       //  The page numbers used by PCIO freebuffer.
+};
+
+//
+// Timeout value which uses 100ns as a unit.
+// It means 3 second span.
+//
+#define ATA_ATAPI_TIMEOUT           EFI_TIMER_PERIOD_SECONDS(3)
+
+#define IS_ALIGNED(addr, size)      (((UINTN) (addr) & (size - 1)) == 0)
+
+#define ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      ATA_ATAPI_PASS_THRU_INSTANCE, \
+      AtaPassThru, \
+      ATA_ATAPI_PASS_THRU_SIGNATURE \
+      )
+
+#define EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      ATA_ATAPI_PASS_THRU_INSTANCE, \
+      ExtScsiPassThru, \
+      ATA_ATAPI_PASS_THRU_SIGNATURE \
+      )
+
+#define ATA_ATAPI_DEVICE_INFO_FROM_THIS(a) \
+  CR (a, \
+      EFI_ATA_DEVICE_INFO, \
+      Link, \
+      ATA_ATAPI_DEVICE_SIGNATURE \
+      );
+
+#define ATA_NON_BLOCK_TASK_FROM_ENTRY(a) \
+  CR (a, \
+      ATA_NONBLOCK_TASK, \
+      Link, \
+      ATA_NONBLOCKING_TASK_SIGNATURE \
+      );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Because ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN EFI_HANDLE                        Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL          *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  );
+
+/**
+  Traverse the attached ATA devices list to find out the device to access.
+
+  @param[in]  Instance            A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  Port                The port number of the ATA device to send the command.
+  @param[in]  PortMultiplierPort  The port multiplier port number of the ATA device to send the command.
+                                  If there is no port multiplier, then specify 0.
+  @param[in]  DeviceType          The device type of the ATA device.
+
+  @retval     The pointer to the data structure of the device info to access.
+
+**/
+LIST_ENTRY *
+EFIAPI
+SearchDeviceInfoList (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN  UINT16                         Port,
+  IN  UINT16                         PortMultiplier,
+  IN  EFI_ATA_DEVICE_TYPE            DeviceType
+  );
+
+/**
+  Allocate device info data structure to contain device info.
+  And insert the data structure to the tail of device list for tracing.
+
+  @param[in]  Instance            A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  Port                The port number of the ATA device to send the command.
+  @param[in]  PortMultiplierPort  The port multiplier port number of the ATA device to send the command.
+                                  If there is no port multiplier, then specify 0.
+  @param[in]  DeviceType          The device type of the ATA device.
+  @param[in]  IdentifyData        The data buffer to store the output of the IDENTIFY cmd.
+
+  @retval EFI_SUCCESS             Successfully insert the ata device to the tail of device list.
+  @retval EFI_OUT_OF_RESOURCES    Can not allocate enough resource for use.
+
+**/
+EFI_STATUS
+EFIAPI
+CreateNewDeviceInfo (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN  UINT16                         Port,
+  IN  UINT16                         PortMultiplier,
+  IN  EFI_ATA_DEVICE_TYPE            DeviceType,
+  IN  EFI_IDENTIFY_DATA              *IdentifyData
+  );
+
+/**
+  Destroy all attached ATA devices info.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+VOID
+EFIAPI
+DestroyDeviceInfoList (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance
+  );
+
+/**
+  Destroy all pending non blocking tasks.
+
+  @param[in]  Instance    A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  IsSigEvent  Indicate whether signal the task event when remove the
+                          task.
+
+**/
+VOID
+EFIAPI
+DestroyAsynTaskList (
+  IN ATA_ATAPI_PASS_THRU_INSTANCE *Instance,
+  IN BOOLEAN                       IsSigEvent
+  );
+
+/**
+  Enumerate all attached ATA devices at IDE mode or AHCI mode separately.
+
+  The function is designed to enumerate all attached ATA devices.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+  @retval EFI_SUCCESS           Successfully enumerate attached ATA devices.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EnumerateAttachedDevice (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE      *Instance
+  );
+
+/**
+  Call back funtion when the timer event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the
+                        Event.
+
+**/
+VOID
+EFIAPI
+AsyncNonBlockingTransferRoutine (
+  EFI_EVENT  Event,
+  VOID*      Context
+  );
+
+/**
+  Sends an ATA command to an ATA device that is attached to the ATA controller. This function
+  supports both blocking I/O and non-blocking I/O. The blocking I/O functionality is required,
+  and the non-blocking I/O functionality is optional.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               The port number of the ATA device to send the command.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device to send the command.
+                                     If there is no port multiplier, then specify 0.
+  @param[in, out] Packet             A pointer to the ATA command to send to the ATA device specified by Port
+                                     and PortMultiplierPort.
+  @param[in]      Event              If non-blocking I/O is not supported then Event is ignored, and blocking
+                                     I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                                     Event is not NULL and non blocking I/O is supported, then non-blocking
+                                     I/O is performed, and Event will be signaled when the ATA command completes.
+
+  @retval EFI_SUCCESS                The ATA command was sent by the host. For bi-directional commands,
+                                     InTransferLength bytes were transferred from InDataBuffer. For write and
+                                     bi-directional commands, OutTransferLength bytes were transferred by OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The ATA command was not executed. The number of bytes that could be transferred
+                                     is returned in InTransferLength. For write and bi-directional commands,
+                                     OutTransferLength bytes were transferred by OutDataBuffer.
+  @retval EFI_NOT_READY              The ATA command could not be sent because there are too many ATA commands
+                                     already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting to send the ATA command.
+  @retval EFI_INVALID_PARAMETER      Port, PortMultiplierPort, or the contents of Acb are invalid. The ATA
+                                     command was not sent, so no additional status information is available.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruPassThru (
+  IN     EFI_ATA_PASS_THRU_PROTOCOL       *This,
+  IN     UINT16                           Port,
+  IN     UINT16                           PortMultiplierPort,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *Packet,
+  IN     EFI_EVENT                        Event OPTIONAL
+  );
+
+/**
+  Used to retrieve the list of legal port numbers for ATA devices on an ATA controller.
+  These can either be the list of ports where ATA devices are actually present or the
+  list of legal port numbers for the ATA controller. Regardless, the caller of this
+  function must probe the port number returned to see if an ATA device is actually
+  present at that location on the ATA controller.
+
+  The GetNextPort() function retrieves the port number on an ATA controller. If on input
+  Port is 0xFFFF, then the port number of the first port on the ATA controller is returned
+  in Port and EFI_SUCCESS is returned.
+
+  If Port is a port number that was returned on a previous call to GetNextPort(), then the
+  port number of the next port on the ATA controller is returned in Port, and EFI_SUCCESS
+  is returned. If Port is not 0xFFFF and Port was not returned on a previous call to
+  GetNextPort(), then EFI_INVALID_PARAMETER is returned.
+
+  If Port is the port number of the last port on the ATA controller, then EFI_NOT_FOUND is
+  returned.
+
+  @param[in]      This          A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in, out] Port          On input, a pointer to the port number on the ATA controller.
+                                On output, a pointer to the next port number on the ATA
+                                controller. An input value of 0xFFFF retrieves the first port
+                                number on the ATA controller.
+
+  @retval EFI_SUCCESS           The next port number on the ATA controller was returned in Port.
+  @retval EFI_NOT_FOUND         There are no more ports on this ATA controller.
+  @retval EFI_INVALID_PARAMETER Port is not 0xFFFF and Port was not returned on a previous call
+                                to GetNextPort().
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetNextPort (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN OUT UINT16                 *Port
+  );
+
+/**
+  Used to retrieve the list of legal port multiplier port numbers for ATA devices on a port of an ATA
+  controller. These can either be the list of port multiplier ports where ATA devices are actually
+  present on port or the list of legal port multiplier ports on that port. Regardless, the caller of this
+  function must probe the port number and port multiplier port number returned to see if an ATA
+  device is actually present.
+
+  The GetNextDevice() function retrieves the port multiplier port number of an ATA device
+  present on a port of an ATA controller.
+
+  If PortMultiplierPort points to a port multiplier port number value that was returned on a
+  previous call to GetNextDevice(), then the port multiplier port number of the next ATA device
+  on the port of the ATA controller is returned in PortMultiplierPort, and EFI_SUCCESS is
+  returned.
+
+  If PortMultiplierPort points to 0xFFFF, then the port multiplier port number of the first
+  ATA device on port of the ATA controller is returned in PortMultiplierPort and
+  EFI_SUCCESS is returned.
+
+  If PortMultiplierPort is not 0xFFFF and the value pointed to by PortMultiplierPort
+  was not returned on a previous call to GetNextDevice(), then EFI_INVALID_PARAMETER
+  is returned.
+
+  If PortMultiplierPort is the port multiplier port number of the last ATA device on the port of
+  the ATA controller, then EFI_NOT_FOUND is returned.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               The port number present on the ATA controller.
+  @param[in, out] PortMultiplierPort On input, a pointer to the port multiplier port number of an
+                                     ATA device present on the ATA controller.
+                                     If on input a PortMultiplierPort of 0xFFFF is specified,
+                                     then the port multiplier port number of the first ATA device
+                                     is returned. On output, a pointer to the port multiplier port
+                                     number of the next ATA device present on an ATA controller.
+
+  @retval EFI_SUCCESS                The port multiplier port number of the next ATA device on the port
+                                     of the ATA controller was returned in PortMultiplierPort.
+  @retval EFI_NOT_FOUND              There are no more ATA devices on this port of the ATA controller.
+  @retval EFI_INVALID_PARAMETER      PortMultiplierPort is not 0xFFFF, and PortMultiplierPort was not
+                                     returned on a previous call to GetNextDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetNextDevice (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port,
+  IN OUT UINT16                 *PortMultiplierPort
+  );
+
+/**
+  Used to allocate and build a device path node for an ATA device on an ATA controller.
+
+  The BuildDevicePath() function allocates and builds a single device node for the ATA
+  device specified by Port and PortMultiplierPort. If the ATA device specified by Port and
+  PortMultiplierPort is not present on the ATA controller, then EFI_NOT_FOUND is returned.
+  If DevicePath is NULL, then EFI_INVALID_PARAMETER is returned. If there are not enough
+  resources to allocate the device path node, then EFI_OUT_OF_RESOURCES is returned.
+
+  Otherwise, DevicePath is allocated with the boot service AllocatePool(), the contents of
+  DevicePath are initialized to describe the ATA device specified by Port and PortMultiplierPort,
+  and EFI_SUCCESS is returned.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               Port specifies the port number of the ATA device for which a
+                                     device path node is to be allocated and built.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device for which a
+                                     device path node is to be allocated and built. If there is no
+                                     port multiplier, then specify 0.
+  @param[in, out] DevicePath         A pointer to a single device path node that describes the ATA
+                                     device specified by Port and PortMultiplierPort. This function
+                                     is responsible for allocating the buffer DevicePath with the
+                                     boot service AllocatePool(). It is the caller's responsibility
+                                     to free DevicePath when the caller is finished with DevicePath.
+  @retval EFI_SUCCESS                The device path node that describes the ATA device specified by
+                                     Port and PortMultiplierPort was allocated and returned in DevicePath.
+  @retval EFI_NOT_FOUND              The ATA device specified by Port and PortMultiplierPort does not
+                                     exist on the ATA controller.
+  @retval EFI_INVALID_PARAMETER      DevicePath is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruBuildDevicePath (
+  IN     EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN     UINT16                     Port,
+  IN     UINT16                     PortMultiplierPort,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL   **DevicePath
+  );
+
+/**
+  Used to translate a device path node to a port number and port multiplier port number.
+
+  The GetDevice() function determines the port and port multiplier port number associated with
+  the ATA device described by DevicePath. If DevicePath is a device path node type that the
+  ATA Pass Thru driver supports, then the ATA Pass Thru driver will attempt to translate the contents
+  DevicePath into a port number and port multiplier port number.
+
+  If this translation is successful, then that port number and port multiplier port number are returned
+  in Port and PortMultiplierPort, and EFI_SUCCESS is returned.
+
+  If DevicePath, Port, or PortMultiplierPort are NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If DevicePath is not a device path node type that the ATA Pass Thru driver supports, then
+  EFI_UNSUPPORTED is returned.
+
+  If DevicePath is a device path node type that the ATA Pass Thru driver supports, but there is not
+  a valid translation from DevicePath to a port number and port multiplier port number, then
+  EFI_NOT_FOUND is returned.
+
+  @param[in]  This                A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]  DevicePath          A pointer to the device path node that describes an ATA device on the
+                                  ATA controller.
+  @param[out] Port                On return, points to the port number of an ATA device on the ATA controller.
+  @param[out] PortMultiplierPort  On return, points to the port multiplier port number of an ATA device
+                                  on the ATA controller.
+
+  @retval EFI_SUCCESS             DevicePath was successfully translated to a port number and port multiplier
+                                  port number, and they were returned in Port and PortMultiplierPort.
+  @retval EFI_INVALID_PARAMETER   DevicePath is NULL.
+  @retval EFI_INVALID_PARAMETER   Port is NULL.
+  @retval EFI_INVALID_PARAMETER   PortMultiplierPort is NULL.
+  @retval EFI_UNSUPPORTED         This driver does not support the device path node type in DevicePath.
+  @retval EFI_NOT_FOUND           A valid translation from DevicePath to a port number and port multiplier
+                                  port number does not exist.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetDevice (
+  IN  EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL   *DevicePath,
+  OUT UINT16                     *Port,
+  OUT UINT16                     *PortMultiplierPort
+  );
+
+/**
+  Resets a specific port on the ATA controller. This operation also resets all the ATA devices
+  connected to the port.
+
+  The ResetChannel() function resets an a specific port on an ATA controller. This operation
+  resets all the ATA devices connected to that port. If this ATA controller does not support
+  a reset port operation, then EFI_UNSUPPORTED is returned.
+
+  If a device error occurs while executing that port reset operation, then EFI_DEVICE_ERROR is
+  returned.
+
+  If a timeout occurs during the execution of the port reset operation, then EFI_TIMEOUT is returned.
+
+  If the port reset operation is completed, then EFI_SUCCESS is returned.
+
+  @param[in]  This          A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]  Port          The port number on the ATA controller.
+
+  @retval EFI_SUCCESS       The ATA controller port was reset.
+  @retval EFI_UNSUPPORTED   The ATA controller does not support a port reset operation.
+  @retval EFI_DEVICE_ERROR  A device error occurred while attempting to reset the ATA port.
+  @retval EFI_TIMEOUT       A timeout occurred while attempting to reset the ATA port.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruResetPort (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port
+  );
+
+/**
+  Resets an ATA device that is connected to an ATA controller.
+
+  The ResetDevice() function resets the ATA device specified by Port and PortMultiplierPort.
+  If this ATA controller does not support a device reset operation, then EFI_UNSUPPORTED is
+  returned.
+
+  If Port or PortMultiplierPort are not in a valid range for this ATA controller, then
+  EFI_INVALID_PARAMETER is returned.
+
+  If a device error occurs while executing that device reset operation, then EFI_DEVICE_ERROR
+  is returned.
+
+  If a timeout occurs during the execution of the device reset operation, then EFI_TIMEOUT is
+  returned.
+
+  If the device reset operation is completed, then EFI_SUCCESS is returned.
+
+  @param[in] This                A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in] Port                Port represents the port number of the ATA device to be reset.
+  @param[in] PortMultiplierPort  The port multiplier port number of the ATA device to reset.
+                                 If there is no port multiplier, then specify 0.
+  @retval EFI_SUCCESS            The ATA device specified by Port and PortMultiplierPort was reset.
+  @retval EFI_UNSUPPORTED        The ATA controller does not support a device reset operation.
+  @retval EFI_INVALID_PARAMETER  Port or PortMultiplierPort are invalid.
+  @retval EFI_DEVICE_ERROR       A device error occurred while attempting to reset the ATA device
+                                 specified by Port and PortMultiplierPort.
+  @retval EFI_TIMEOUT            A timeout occurred while attempting to reset the ATA device
+                                 specified by Port and PortMultiplierPort.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruResetDevice (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port,
+  IN UINT16                     PortMultiplierPort
+  );
+
+/**
+  Sends a SCSI Request Packet to a SCSI device that is attached to the SCSI channel. This function
+  supports both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the
+  nonblocking I/O functionality is optional.
+
+  @param  This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target  The Target is an array of size TARGET_MAX_BYTES and it represents
+                  the id of the SCSI device to send the SCSI Request Packet. Each
+                  transport driver may choose to utilize a subset of this size to suit the needs
+                  of transport target representation. For example, a Fibre Channel driver
+                  may use only 8 bytes (WWN) to represent an FC target.
+  @param  Lun     The LUN of the SCSI device to send the SCSI Request Packet.
+  @param  Packet  A pointer to the SCSI Request Packet to send to the SCSI device
+                  specified by Target and Lun.
+  @param  Event   If nonblocking I/O is not supported then Event is ignored, and blocking
+                  I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                  Event is not NULL and non blocking I/O is supported, then
+                  nonblocking I/O is performed, and Event will be signaled when the
+                  SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE   The SCSI Request Packet was not executed. The number of bytes that
+                                could be transferred is returned in InTransferLength. For write
+                                and bi-directional commands, OutTransferLength bytes were
+                                transferred by OutDataBuffer.
+  @retval EFI_NOT_READY         The SCSI Request Packet could not be sent because there are too many
+                                SCSI Request Packets already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_INVALID_PARAMETER Target, Lun, or the contents of ScsiRequestPacket are invalid.
+  @retval EFI_UNSUPPORTED       The command described by the SCSI Request Packet is not supported
+                                by the host adapter. This includes the case of Bi-directional SCSI
+                                commands not supported by the implementation. The SCSI Request
+                                Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruPassThru (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                    *This,
+  IN UINT8                                              *Target,
+  IN UINT64                                             Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET     *Packet,
+  IN EFI_EVENT                                          Event OPTIONAL
+  );
+
+/**
+  Used to retrieve the list of legal Target IDs and LUNs for SCSI devices on a SCSI channel. These
+  can either be the list SCSI devices that are actually present on the SCSI channel, or the list of legal
+  Target Ids and LUNs for the SCSI channel. Regardless, the caller of this function must probe the
+  Target ID and LUN returned to see if a SCSI device is actually present at that location on the SCSI
+  channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target On input, a pointer to the Target ID (an array of size
+                 TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+  @param  Lun    On input, a pointer to the LUN of a SCSI device present on the SCSI
+                 channel. On output, a pointer to the LUN of the next SCSI device present
+                 on a SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID and LUN of the next SCSI device on the SCSI
+                                channel was returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER Target array is not all 0xF, and Target and Lun were
+                                not returned on a previous call to GetNextTargetLun().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetNextTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target,
+  IN OUT UINT64                          *Lun
+  );
+
+/**
+  Used to allocate and build a device path node for a SCSI device on a SCSI channel.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target     The Target is an array of size TARGET_MAX_BYTES and it specifies the
+                     Target ID of the SCSI device for which a device path node is to be
+                     allocated and built. Transport drivers may chose to utilize a subset of
+                     this size to suit the representation of targets. For example, a Fibre
+                     Channel driver may use only 8 bytes (WWN) in the array to represent a
+                     FC target.
+  @param  Lun        The LUN of the SCSI device for which a device path node is to be
+                     allocated and built.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     specified by Target and Lun. This function is responsible for
+                     allocating the buffer DevicePath with the boot service
+                     AllocatePool(). It is the caller's responsibility to free
+                     DevicePath when the caller is finished with DevicePath.
+
+  @retval EFI_SUCCESS           The device path node that describes the SCSI device specified by
+                                Target and Lun was allocated and returned in
+                                DevicePath.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_NOT_FOUND         The SCSI devices specified by Target and Lun does not exist
+                                on the SCSI channel.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruBuildDevicePath (
+  IN     EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN     UINT8                              *Target,
+  IN     UINT64                             Lun,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL           **DevicePath
+  );
+
+/**
+  Used to translate a device path node to a Target ID and LUN.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     on the SCSI channel.
+  @param  Target     A pointer to the Target Array which represents the ID of a SCSI device
+                     on the SCSI channel.
+  @param  Lun        A pointer to the LUN of a SCSI device on the SCSI channel.
+
+  @retval EFI_SUCCESS           DevicePath was successfully translated to a Target ID and
+                                LUN, and they were returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER DevicePath or Target or Lun is NULL.
+  @retval EFI_NOT_FOUND         A valid translation from DevicePath to a Target ID and LUN
+                                does not exist.
+  @retval EFI_UNSUPPORTED       This driver does not support the device path node type in
+                                 DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  OUT UINT8                              **Target,
+  OUT UINT64                             *Lun
+  );
+
+/**
+  Resets a SCSI channel. This operation resets all the SCSI devices connected to the SCSI channel.
+
+  @param  This A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      The SCSI channel was reset.
+  @retval EFI_DEVICE_ERROR A device error occurred while attempting to reset the SCSI channel.
+  @retval EFI_TIMEOUT      A timeout occurred while attempting to reset the SCSI channel.
+  @retval EFI_UNSUPPORTED  The SCSI channel does not support a channel reset operation.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruResetChannel (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   *This
+  );
+
+/**
+  Resets a SCSI logical unit that is connected to a SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target The Target is an array of size TARGET_MAX_BYTE and it represents the
+                 target port ID of the SCSI device containing the SCSI logical unit to
+                 reset. Transport drivers may chose to utilize a subset of this array to suit
+                 the representation of their targets.
+  @param  Lun    The LUN of the SCSI device to reset.
+
+  @retval EFI_SUCCESS           The SCSI device specified by Target and Lun was reset.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           A timeout occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+  @retval EFI_UNSUPPORTED       The SCSI channel does not support a target reset operation.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to reset the SCSI device
+                                 specified by Target and Lun.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruResetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN UINT8                              *Target,
+  IN UINT64                             Lun
+  );
+
+/**
+  Used to retrieve the list of legal Target IDs for SCSI devices on a SCSI channel. These can either
+  be the list SCSI devices that are actually present on the SCSI channel, or the list of legal Target IDs
+  for the SCSI channel. Regardless, the caller of this function must probe the Target ID returned to
+  see if a SCSI device is actually present at that location on the SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target (TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID of the next SCSI device on the SCSI
+                                channel was returned in Target.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           Target array is not all 0xF, and Target was not
+                                returned on a previous call to GetNextTarget().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetNextTarget (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target
+  );
+
+/**
+  Initialize ATA host controller at IDE mode.
+
+  The function is designed to initialize ATA host controller.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+EFI_STATUS
+EFIAPI
+IdeModeInitialization (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance
+  );
+
+/**
+  Initialize ATA host controller at AHCI mode.
+
+  The function is designed to initialize ATA host controller.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciModeInitialization (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance
+  );
+
+/**
+  Start a non data transfer on specific port.
+
+  @param[in]       PciIo               The PCI IO protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The non data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The non data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciNonDataTransfer (
+  IN     EFI_PCI_IO_PROTOCOL           *PciIo,
+  IN     EFI_AHCI_REGISTERS            *AhciRegisters,
+  IN     UINT8                         Port,
+  IN     UINT8                         PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND        *AtapiCommand OPTIONAL,
+  IN     UINT8                         AtapiCommandLength,
+  IN     EFI_ATA_COMMAND_BLOCK         *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock,
+  IN     UINT64                        Timeout,
+  IN     ATA_NONBLOCK_TASK             *Task
+  );
+
+/**
+  Start a DMA data transfer on specific port
+
+  @param[in]       Instance            The ATA_ATAPI_PASS_THRU_INSTANCE protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       Read                The transfer direction.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in, out]  MemoryAddr          The pointer to the data buffer.
+  @param[in]       DataCount           The data count to be transferred.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The DMA data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The DMA data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciDmaTransfer (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE *Instance,
+  IN     EFI_AHCI_REGISTERS           *AhciRegisters,
+  IN     UINT8                        Port,
+  IN     UINT8                        PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND       *AtapiCommand OPTIONAL,
+  IN     UINT8                        AtapiCommandLength,
+  IN     BOOLEAN                      Read,
+  IN     EFI_ATA_COMMAND_BLOCK        *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK         *AtaStatusBlock,
+  IN OUT VOID                         *MemoryAddr,
+  IN     UINT32                       DataCount,
+  IN     UINT64                       Timeout,
+  IN     ATA_NONBLOCK_TASK            *Task
+  );
+
+/**
+  Start a PIO data transfer on specific port.
+
+  @param[in]       PciIo               The PCI IO protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       Read                The transfer direction.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in, out]  MemoryAddr          The pointer to the data buffer.
+  @param[in]       DataCount           The data count to be transferred.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The PIO data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The PIO data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPioTransfer (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     EFI_AHCI_REGISTERS         *AhciRegisters,
+  IN     UINT8                      Port,
+  IN     UINT8                      PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND     *AtapiCommand OPTIONAL,
+  IN     UINT8                      AtapiCommandLength,
+  IN     BOOLEAN                    Read,
+  IN     EFI_ATA_COMMAND_BLOCK      *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK       *AtaStatusBlock,
+  IN OUT VOID                       *MemoryAddr,
+  IN     UINT32                     DataCount,
+  IN     UINT64                     Timeout,
+  IN     ATA_NONBLOCK_TASK          *Task
+  );
+
+/**
+  Send ATA command into device with NON_DATA protocol
+
+  @param[in]      PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE
+                                   data structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data
+                                   structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval  EFI_SUCCESS Reading succeed
+  @retval  EFI_ABORTED Command failed
+  @retval  EFI_DEVICE_ERROR Device status error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaNonDataCommandIn (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN     EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock,
+  IN     UINT64                    Timeout,
+  IN     ATA_NONBLOCK_TASK         *Task
+  );
+
+/**
+  Perform an ATA Udma operation (Read, ReadExt, Write, WriteExt).
+
+  @param[in]      Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data
+                                   structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]      Read             Flag used to determine the data transfer
+                                   direction. Read equals 1, means data transferred
+                                   from device to host;Read equals 0, means data
+                                   transferred from host to device.
+  @param[in]      DataBuffer       A pointer to the source buffer for the data.
+  @param[in]      DataLength       The length of  the data.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval EFI_SUCCESS          the operation is successful.
+  @retval EFI_OUT_OF_RESOURCES Build PRD table failed
+  @retval EFI_UNSUPPORTED      Unknown channel or operations command
+  @retval EFI_DEVICE_ERROR     Ata command execute failed
+
+**/
+EFI_STATUS
+EFIAPI
+AtaUdmaInOut (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     EFI_IDE_REGISTERS             *IdeRegisters,
+  IN     BOOLEAN                       Read,
+  IN     VOID                          *DataBuffer,
+  IN     UINT64                        DataLength,
+  IN     EFI_ATA_COMMAND_BLOCK         *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock,
+  IN     UINT64                        Timeout,
+  IN     ATA_NONBLOCK_TASK             *Task
+  );
+
+/**
+  This function is used to send out ATA commands conforms to the PIO Data In Protocol.
+
+  @param[in]      PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data
+                                   structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in, out] Buffer           A pointer to the source buffer for the data.
+  @param[in]      ByteCount        The length of  the data.
+  @param[in] Read                  Flag used to determine the data transfer direction.
+                                   Read equals 1, means data transferred from device
+                                   to host;Read equals 0, means data transferred
+                                   from host to device.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval EFI_SUCCESS      send out the ATA command and device send required data successfully.
+  @retval EFI_DEVICE_ERROR command sent failed.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPioDataInOut (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN OUT VOID                      *Buffer,
+  IN     UINT64                    ByteCount,
+  IN     BOOLEAN                   Read,
+  IN     EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock,
+  IN     UINT64                    Timeout,
+  IN     ATA_NONBLOCK_TASK         *Task
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.inf b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.inf
new file mode 100644
index 0000000000..82d5f7a46c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.inf
@@ -0,0 +1,78 @@
+## @file
+#  AtaAtapiPassThru driver to provide native IDE/AHCI mode support.
+#
+#  This driver installs AtaPassThru and ExtScsiPassThru protocol in each ide/sata controller
+#  to access to all attached Ata/Atapi devices.
+#
+#  Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = AtaAtapiPassThruDxe
+  MODULE_UNI_FILE                = AtaAtapiPassThruDxe.uni
+  FILE_GUID                      = 5E523CB4-D397-4986-87BD-A6DD8B22F455
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeAtaAtapiPassThru
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gAtaAtapiPassThruDriverBinding                         
+#  COMPONENT_NAME                =  gAtaAtapiPassThruComponentName
+#  COMPONENT_NAME2               =  gAtaAtapiPassThruComponentName2                         
+#
+#
+
+[Sources]
+  AtaAtapiPassThru.c
+  AtaAtapiPassThru.h
+  AhciMode.c
+  AhciMode.h
+  IdeMode.c
+  IdeMode.h
+  ComponentName.c
+  
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DevicePathLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiLib
+  BaseLib
+  UefiDriverEntryPoint
+  DebugLib
+  TimerLib
+  ReportStatusCodeLib
+  PcdLib
+
+[Protocols]
+  gEfiAtaPassThruProtocolGuid                   ## BY_START
+  gEfiExtScsiPassThruProtocolGuid               ## BY_START
+  gEfiIdeControllerInitProtocolGuid             ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiPciIoProtocolGuid                         ## TO_START
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAtaSmartEnable   ## SOMETIMES_CONSUMES
+
+# [Event]
+# EVENT_TYPE_PERIODIC_TIMER ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  AtaAtapiPassThruDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxe.uni b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxe.uni
new file mode 100644
index 0000000000..0285409ec5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxeExtra.uni b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxeExtra.uni
new file mode 100644
index 0000000000..d89350dfe3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/ComponentName.c b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/ComponentName.c
new file mode 100644
index 0000000000..c3f7738d45
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/ComponentName.c
@@ -0,0 +1,251 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for AtaAtapiPassThru driver.
+  
+  Copyright (c) 2010 - 2011, 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 "AtaAtapiPassThru.h"
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaAtapiPassThruDriverNameTable[] = {
+  { "eng;en", L"AtaAtapiPassThru Driver" },
+  { NULL , NULL }
+};
+
+//
+// Controller name table
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaAtapiPassThruIdeControllerNameTable[] = {
+  { "eng;en", L"IDE Controller" },
+  { NULL , NULL }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaAtapiPassThruAhciControllerNameTable[] = {
+  { "eng;en", L"AHCI Controller" },
+  { NULL , NULL }
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gAtaAtapiPassThruComponentName = {
+  AtaAtapiPassThruComponentNameGetDriverName,
+  AtaAtapiPassThruComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gAtaAtapiPassThruComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) AtaAtapiPassThruComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) AtaAtapiPassThruComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mAtaAtapiPassThruDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gAtaAtapiPassThruComponentName)
+           );
+}
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_UNICODE_STRING_TABLE      *ControllerNameTable;
+  VOID                          *Interface;
+  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance;
+
+  if (Language == NULL || ControllerName == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing Controller Handle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gAtaAtapiPassThruDriverBinding.DriverBindingHandle,
+             &gEfiIdeControllerInitProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // AtaPassThru and ExtScsiPassThru should also be installed at the controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiAtaPassThruProtocolGuid,
+                  &Interface,
+                  gAtaAtapiPassThruDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (Interface);
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    ControllerNameTable = mAtaAtapiPassThruIdeControllerNameTable;
+  } else if (Instance->Mode == EfiAtaAhciMode) {
+    ControllerNameTable = mAtaAtapiPassThruAhciControllerNameTable;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gAtaAtapiPassThruComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.c b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.c
new file mode 100644
index 0000000000..420ad273dc
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.c
@@ -0,0 +1,2890 @@
+/** @file
+  Header file for AHCI mode of ATA host controller.
+
+  Copyright (c) 2010 - 2015, 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 "AtaAtapiPassThru.h"
+
+/**
+  read a one-byte data from a IDE port.
+
+  @param  PciIo  A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port   The IDE Port number
+
+  @return  the one-byte data read from IDE port
+**/
+UINT8
+EFIAPI
+IdeReadPortB (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port
+  )
+{
+  UINT8 Data;
+
+  ASSERT (PciIo != NULL);
+
+  Data = 0;
+  //
+  // perform 1-byte data read from register
+  //
+  PciIo->Io.Read (
+              PciIo,
+              EfiPciIoWidthUint8,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              1,
+              &Data
+              );
+  return Data;
+}
+
+/**
+  write a 1-byte data to a specific IDE port.
+
+  @param  PciIo  A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port   The IDE port to be writen
+  @param  Data   The data to write to the port
+**/
+VOID
+EFIAPI
+IdeWritePortB (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINT8                 Data
+  )
+{
+  ASSERT (PciIo != NULL);
+
+  //
+  // perform 1-byte data write to register
+  //
+  PciIo->Io.Write (
+              PciIo,
+              EfiPciIoWidthUint8,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              1,
+              &Data
+              );
+}
+
+/**
+  write a 1-word data to a specific IDE port.
+
+  @param  PciIo  A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port   The IDE port to be writen
+  @param  Data   The data to write to the port
+**/
+VOID
+EFIAPI
+IdeWritePortW (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINT16                Data
+  )
+{
+  ASSERT (PciIo != NULL);
+
+  //
+  // perform 1-word data write to register
+  //
+  PciIo->Io.Write (
+              PciIo,
+              EfiPciIoWidthUint16,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              1,
+              &Data
+              );
+}
+
+/**
+  write a 2-word data to a specific IDE port.
+
+  @param  PciIo  A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port   The IDE port to be writen
+  @param  Data   The data to write to the port
+**/
+VOID
+EFIAPI
+IdeWritePortDW (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINT32                Data
+  )
+{
+  ASSERT (PciIo != NULL);
+
+  //
+  // perform 2-word data write to register
+  //
+  PciIo->Io.Write (
+              PciIo,
+              EfiPciIoWidthUint32,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              1,
+              &Data
+              );
+}
+
+/**
+  Write multiple words of data to the IDE data port.
+  Call the IO abstraction once to do the complete read,
+  not one word at a time
+
+  @param  PciIo      A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port       IO port to read
+  @param  Count      No. of UINT16's to read
+  @param  Buffer     Pointer to the data buffer for read
+
+**/
+VOID
+EFIAPI
+IdeWritePortWMultiple (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINTN                 Count,
+  IN  VOID                  *Buffer
+  )
+{
+  ASSERT (PciIo  != NULL);
+  ASSERT (Buffer != NULL);
+
+  //
+  // perform UINT16 data write to the FIFO
+  //
+  PciIo->Io.Write (
+              PciIo,
+              EfiPciIoWidthFifoUint16,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              Count,
+              (UINT16 *) Buffer
+              );
+
+}
+
+/**
+  Reads multiple words of data from the IDE data port.
+  Call the IO abstraction once to do the complete read,
+  not one word at a time
+
+  @param  PciIo    A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port     IO port to read
+  @param  Count    Number of UINT16's to read
+  @param  Buffer   Pointer to the data buffer for read
+
+**/
+VOID
+EFIAPI
+IdeReadPortWMultiple (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINTN                 Count,
+  IN  VOID                  *Buffer
+  )
+{
+  ASSERT (PciIo  != NULL);
+  ASSERT (Buffer != NULL);
+
+  //
+  // Perform UINT16 data read from FIFO
+  //
+  PciIo->Io.Read (
+              PciIo,
+              EfiPciIoWidthFifoUint16,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              Count,
+              (UINT16 *) Buffer
+              );
+
+}
+
+/**
+  This function is used to analyze the Status Register and print out
+  some debug information and if there is ERR bit set in the Status
+  Register, the Error Register's value is also be parsed and print out.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+**/
+VOID
+EFIAPI
+DumpAllIdeRegisters (
+  IN     EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN     EFI_IDE_REGISTERS        *IdeRegisters,
+  IN OUT EFI_ATA_STATUS_BLOCK     *AtaStatusBlock
+  )
+{
+  EFI_ATA_STATUS_BLOCK StatusBlock;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  ZeroMem (&StatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+  StatusBlock.AtaStatus          = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+  StatusBlock.AtaError           = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+  StatusBlock.AtaSectorCount     = IdeReadPortB (PciIo, IdeRegisters->SectorCount);
+  StatusBlock.AtaSectorCountExp  = IdeReadPortB (PciIo, IdeRegisters->SectorCount);
+  StatusBlock.AtaSectorNumber    = IdeReadPortB (PciIo, IdeRegisters->SectorNumber);
+  StatusBlock.AtaSectorNumberExp = IdeReadPortB (PciIo, IdeRegisters->SectorNumber);
+  StatusBlock.AtaCylinderLow     = IdeReadPortB (PciIo, IdeRegisters->CylinderLsb);
+  StatusBlock.AtaCylinderLowExp  = IdeReadPortB (PciIo, IdeRegisters->CylinderLsb);
+  StatusBlock.AtaCylinderHigh    = IdeReadPortB (PciIo, IdeRegisters->CylinderMsb);
+  StatusBlock.AtaCylinderHighExp = IdeReadPortB (PciIo, IdeRegisters->CylinderMsb);
+  StatusBlock.AtaDeviceHead      = IdeReadPortB (PciIo, IdeRegisters->Head);
+
+  if (AtaStatusBlock != NULL) {
+    //
+    // Dump the content of all ATA registers.
+    //
+    CopyMem (AtaStatusBlock, &StatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+  }
+
+  DEBUG_CODE_BEGIN ();
+  if ((StatusBlock.AtaStatus & ATA_STSREG_DWF) != 0) {
+    DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Write Fault\n", StatusBlock.AtaStatus));
+  }
+
+  if ((StatusBlock.AtaStatus & ATA_STSREG_CORR) != 0) {
+    DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Corrected Data\n", StatusBlock.AtaStatus));
+  }
+
+  if ((StatusBlock.AtaStatus & ATA_STSREG_ERR) != 0) {
+    if ((StatusBlock.AtaError & ATA_ERRREG_BBK) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Bad Block Detected\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_UNC) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Uncorrectable Data\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_MC) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Media Change\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_ABRT) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Abort\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_TK0NF) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Track 0 Not Found\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_AMNF) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Address Mark Not Found\n", StatusBlock.AtaError));
+    }
+  }
+  DEBUG_CODE_END ();
+}
+
+/**
+  This function is used to analyze the Status Register at the condition that BSY is zero.
+  if there is ERR bit set in the Status Register, then return error.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+
+  @retval EFI_SUCCESS       No err information in the Status Register.
+  @retval EFI_DEVICE_ERROR  Any err information in the Status Register.
+
+**/
+EFI_STATUS
+EFIAPI
+CheckStatusRegister (
+  IN  EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN  EFI_IDE_REGISTERS        *IdeRegisters
+  )
+{
+  UINT8           StatusRegister;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+
+  if ((StatusRegister & ATA_STSREG_BSY) == 0) {
+    if ((StatusRegister & (ATA_STSREG_ERR | ATA_STSREG_DWF | ATA_STSREG_CORR)) == 0) {
+      return EFI_SUCCESS;
+    } else {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to poll for the DRQ bit clear in the Status
+  Register. DRQ is cleared when the device is finished transferring data.
+  So this function is called after data transfer is finished.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS     DRQ bit clear within the time out.
+
+  @retval EFI_TIMEOUT     DRQ bit not clear within the time out.
+
+  @note
+  Read Status Register will clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRQClear (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  EFI_IDE_REGISTERS         *IdeRegisters,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   StatusRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+
+    //
+    // Wait for BSY == 0, then judge if DRQ is clear
+    //
+    if ((StatusRegister & ATA_STSREG_BSY) == 0) {
+      if ((StatusRegister & ATA_STSREG_DRQ) == ATA_STSREG_DRQ) {
+        return EFI_DEVICE_ERROR;
+      } else {
+        return EFI_SUCCESS;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+/**
+  This function is used to poll for the DRQ bit clear in the Alternate
+  Status Register. DRQ is cleared when the device is finished
+  transferring data. So this function is called after data transfer
+  is finished.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS     DRQ bit clear within the time out.
+
+  @retval EFI_TIMEOUT     DRQ bit not clear within the time out.
+  @note   Read Alternate Status Register will not clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRQClear2 (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   AltRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    AltRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+
+    //
+    // Wait for BSY == 0, then judge if DRQ is clear
+    //
+    if ((AltRegister & ATA_STSREG_BSY) == 0) {
+      if ((AltRegister & ATA_STSREG_DRQ) == ATA_STSREG_DRQ) {
+        return EFI_DEVICE_ERROR;
+      } else {
+        return EFI_SUCCESS;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the DRQ bit set in the
+  Status Register.
+  DRQ is set when the device is ready to transfer data. So this function
+  is called after the command is sent to the device and before required
+  data is transferred.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS          DRQ bit set within the time out.
+  @retval EFI_TIMEOUT          DRQ bit not set within the time out.
+  @retval EFI_ABORTED          DRQ bit not set caused by the command abort.
+
+  @note  Read Status Register will clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRQReady (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   StatusRegister;
+  UINT8   ErrorRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    //
+    // Read Status Register will clear interrupt
+    //
+    StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+
+    //
+    // Wait for BSY == 0, then judge if DRQ is clear or ERR is set
+    //
+    if ((StatusRegister & ATA_STSREG_BSY) == 0) {
+      if ((StatusRegister & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+        ErrorRegister = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+
+        if ((ErrorRegister & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+          return EFI_ABORTED;
+        }
+        return EFI_DEVICE_ERROR;
+      }
+
+      if ((StatusRegister & ATA_STSREG_DRQ) == ATA_STSREG_DRQ) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_NOT_READY;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+/**
+  This function is used to poll for the DRQ bit set in the Alternate Status Register.
+  DRQ is set when the device is ready to transfer data. So this function is called after
+  the command is sent to the device and before required data is transferred.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS           DRQ bit set within the time out.
+  @retval EFI_TIMEOUT           DRQ bit not set within the time out.
+  @retval EFI_ABORTED           DRQ bit not set caused by the command abort.
+  @note  Read Alternate Status Register will not clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRQReady2 (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   AltRegister;
+  UINT8   ErrorRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+
+  do {
+    //
+    // Read Alternate Status Register will not clear interrupt status
+    //
+    AltRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    //
+    // Wait for BSY == 0, then judge if DRQ is clear or ERR is set
+    //
+    if ((AltRegister & ATA_STSREG_BSY) == 0) {
+      if ((AltRegister & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+        ErrorRegister = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+
+        if ((ErrorRegister & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+          return EFI_ABORTED;
+        }
+        return EFI_DEVICE_ERROR;
+      }
+
+      if ((AltRegister & ATA_STSREG_DRQ) == ATA_STSREG_DRQ) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_NOT_READY;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the DRDY bit set in the Status Register. DRDY
+  bit is set when the device is ready to accept command. Most ATA commands must be
+  sent after DRDY set except the ATAPI Packet Command.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS         DRDY bit set within the time out.
+  @retval EFI_TIMEOUT         DRDY bit not set within the time out.
+
+  @note  Read Status Register will clear interrupt status.
+**/
+EFI_STATUS
+EFIAPI
+DRDYReady (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   StatusRegister;
+  UINT8   ErrorRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+    //
+    // Wait for BSY == 0, then judge if DRDY is set or ERR is set
+    //
+    if ((StatusRegister & ATA_STSREG_BSY) == 0) {
+      if ((StatusRegister & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+        ErrorRegister = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+
+        if ((ErrorRegister & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+          return EFI_ABORTED;
+        }
+        return EFI_DEVICE_ERROR;
+      }
+
+      if ((StatusRegister & ATA_STSREG_DRDY) == ATA_STSREG_DRDY) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_DEVICE_ERROR;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the DRDY bit set in the Alternate Status Register.
+  DRDY bit is set when the device is ready to accept command. Most ATA commands must
+  be sent after DRDY set except the ATAPI Packet Command.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS      DRDY bit set within the time out.
+  @retval EFI_TIMEOUT      DRDY bit not set within the time out.
+
+  @note  Read Alternate Status Register will clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRDYReady2 (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   AltRegister;
+  UINT8   ErrorRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    AltRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    //
+    // Wait for BSY == 0, then judge if DRDY is set or ERR is set
+    //
+    if ((AltRegister & ATA_STSREG_BSY) == 0) {
+      if ((AltRegister & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+        ErrorRegister = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+
+        if ((ErrorRegister & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+          return EFI_ABORTED;
+        }
+        return EFI_DEVICE_ERROR;
+      }
+
+      if ((AltRegister & ATA_STSREG_DRDY) == ATA_STSREG_DRDY) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_DEVICE_ERROR;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the BSY bit clear in the Status Register. BSY
+  is clear when the device is not busy. Every command must be sent after device is not busy.
+
+  @param PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS          BSY bit clear within the time out.
+  @retval EFI_TIMEOUT          BSY bit not clear within the time out.
+
+  @note Read Status Register will clear interrupt status.
+**/
+EFI_STATUS
+EFIAPI
+WaitForBSYClear (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   StatusRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+
+    if ((StatusRegister & ATA_STSREG_BSY) == 0x00) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the BSY bit clear in the Status Register. BSY
+  is clear when the device is not busy. Every command must be sent after device is not busy.
+
+  @param PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS          BSY bit clear within the time out.
+  @retval EFI_TIMEOUT          BSY bit not clear within the time out.
+
+  @note Read Status Register will clear interrupt status.
+**/
+EFI_STATUS
+EFIAPI
+WaitForBSYClear2 (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   AltStatusRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    AltStatusRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+
+    if ((AltStatusRegister & ATA_STSREG_BSY) == 0x00) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Get IDE i/o port registers' base addresses by mode.
+
+  In 'Compatibility' mode, use fixed addresses.
+  In Native-PCI mode, get base addresses from BARs in the PCI IDE controller's
+  Configuration Space.
+
+  The steps to get IDE i/o port registers' base addresses for each channel
+  as follows:
+
+  1. Examine the Programming Interface byte of the Class Code fields in PCI IDE
+  controller's Configuration Space to determine the operating mode.
+
+  2. a) In 'Compatibility' mode, use fixed addresses shown in the Table 1 below.
+   ___________________________________________
+  |           | Command Block | Control Block |
+  |  Channel  |   Registers   |   Registers   |
+  |___________|_______________|_______________|
+  |  Primary  |  1F0h - 1F7h  |  3F6h - 3F7h  |
+  |___________|_______________|_______________|
+  | Secondary |  170h - 177h  |  376h - 377h  |
+  |___________|_______________|_______________|
+
+  Table 1. Compatibility resource mappings
+
+  b) In Native-PCI mode, IDE registers are mapped into IO space using the BARs
+  in IDE controller's PCI Configuration Space, shown in the Table 2 below.
+   ___________________________________________________
+  |           |   Command Block   |   Control Block   |
+  |  Channel  |     Registers     |     Registers     |
+  |___________|___________________|___________________|
+  |  Primary  | BAR at offset 0x10| BAR at offset 0x14|
+  |___________|___________________|___________________|
+  | Secondary | BAR at offset 0x18| BAR at offset 0x1C|
+  |___________|___________________|___________________|
+
+  Table 2. BARs for Register Mapping
+
+  @param[in]      PciIo          Pointer to the EFI_PCI_IO_PROTOCOL instance
+  @param[in, out] IdeRegisters    Pointer to EFI_IDE_REGISTERS which is used to
+                                 store the IDE i/o port registers' base addresses
+
+  @retval EFI_UNSUPPORTED        Return this value when the BARs is not IO type
+  @retval EFI_SUCCESS            Get the Base address successfully
+  @retval Other                  Read the pci configureation data error
+
+**/
+EFI_STATUS
+EFIAPI
+GetIdeRegisterIoAddr (
+  IN     EFI_PCI_IO_PROTOCOL         *PciIo,
+  IN OUT EFI_IDE_REGISTERS           *IdeRegisters
+  )
+{
+  EFI_STATUS        Status;
+  PCI_TYPE00        PciData;
+  UINT16            CommandBlockBaseAddr;
+  UINT16            ControlBlockBaseAddr;
+  UINT16            BusMasterBaseAddr;
+
+  if ((PciIo == NULL) || (IdeRegisters == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        0,
+                        sizeof (PciData),
+                        &PciData
+                        );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  BusMasterBaseAddr    = (UINT16) ((PciData.Device.Bar[4] & 0x0000fff0));
+
+  if ((PciData.Hdr.ClassCode[0] & IDE_PRIMARY_OPERATING_MODE) == 0) {
+    CommandBlockBaseAddr = 0x1f0;
+    ControlBlockBaseAddr = 0x3f6;
+  } else {
+    //
+    // The BARs should be of IO type
+    //
+    if ((PciData.Device.Bar[0] & BIT0) == 0 ||
+        (PciData.Device.Bar[1] & BIT0) == 0) {
+      return EFI_UNSUPPORTED;
+    }
+
+    CommandBlockBaseAddr = (UINT16) (PciData.Device.Bar[0] & 0x0000fff8);
+    ControlBlockBaseAddr = (UINT16) ((PciData.Device.Bar[1] & 0x0000fffc) + 2);
+  }
+
+  //
+  // Calculate IDE primary channel I/O register base address.
+  //
+  IdeRegisters[EfiIdePrimary].Data              = CommandBlockBaseAddr;
+  IdeRegisters[EfiIdePrimary].ErrOrFeature      = (UINT16) (CommandBlockBaseAddr + 0x01);
+  IdeRegisters[EfiIdePrimary].SectorCount       = (UINT16) (CommandBlockBaseAddr + 0x02);
+  IdeRegisters[EfiIdePrimary].SectorNumber      = (UINT16) (CommandBlockBaseAddr + 0x03);
+  IdeRegisters[EfiIdePrimary].CylinderLsb       = (UINT16) (CommandBlockBaseAddr + 0x04);
+  IdeRegisters[EfiIdePrimary].CylinderMsb       = (UINT16) (CommandBlockBaseAddr + 0x05);
+  IdeRegisters[EfiIdePrimary].Head              = (UINT16) (CommandBlockBaseAddr + 0x06);
+  IdeRegisters[EfiIdePrimary].CmdOrStatus       = (UINT16) (CommandBlockBaseAddr + 0x07);
+  IdeRegisters[EfiIdePrimary].AltOrDev          = ControlBlockBaseAddr;
+  IdeRegisters[EfiIdePrimary].BusMasterBaseAddr = BusMasterBaseAddr;
+
+  if ((PciData.Hdr.ClassCode[0] & IDE_SECONDARY_OPERATING_MODE) == 0) {
+    CommandBlockBaseAddr = 0x170;
+    ControlBlockBaseAddr = 0x376;
+  } else {
+    //
+    // The BARs should be of IO type
+    //
+    if ((PciData.Device.Bar[2] & BIT0) == 0 ||
+        (PciData.Device.Bar[3] & BIT0) == 0) {
+      return EFI_UNSUPPORTED;
+    }
+
+    CommandBlockBaseAddr = (UINT16) (PciData.Device.Bar[2] & 0x0000fff8);
+    ControlBlockBaseAddr = (UINT16) ((PciData.Device.Bar[3] & 0x0000fffc) + 2);
+  }
+
+  //
+  // Calculate IDE secondary channel I/O register base address.
+  //
+  IdeRegisters[EfiIdeSecondary].Data              = CommandBlockBaseAddr;
+  IdeRegisters[EfiIdeSecondary].ErrOrFeature      = (UINT16) (CommandBlockBaseAddr + 0x01);
+  IdeRegisters[EfiIdeSecondary].SectorCount       = (UINT16) (CommandBlockBaseAddr + 0x02);
+  IdeRegisters[EfiIdeSecondary].SectorNumber      = (UINT16) (CommandBlockBaseAddr + 0x03);
+  IdeRegisters[EfiIdeSecondary].CylinderLsb       = (UINT16) (CommandBlockBaseAddr + 0x04);
+  IdeRegisters[EfiIdeSecondary].CylinderMsb       = (UINT16) (CommandBlockBaseAddr + 0x05);
+  IdeRegisters[EfiIdeSecondary].Head              = (UINT16) (CommandBlockBaseAddr + 0x06);
+  IdeRegisters[EfiIdeSecondary].CmdOrStatus       = (UINT16) (CommandBlockBaseAddr + 0x07);
+  IdeRegisters[EfiIdeSecondary].AltOrDev          = ControlBlockBaseAddr;
+  IdeRegisters[EfiIdeSecondary].BusMasterBaseAddr = (UINT16) (BusMasterBaseAddr + 0x8);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to implement the Soft Reset on the specified device. But,
+  the ATA Soft Reset mechanism is so strong a reset method that it will force
+  resetting on both devices connected to the same cable.
+
+  It is called by IdeBlkIoReset(), a interface function of Block
+  I/O protocol.
+
+  This function can also be used by the ATAPI device to perform reset when
+  ATAPI Reset command is failed.
+
+  @param PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS       Soft reset completes successfully.
+  @retval EFI_DEVICE_ERROR  Any step during the reset process is failed.
+
+  @note  The registers initial values after ATA soft reset are different
+         to the ATA device and ATAPI device.
+**/
+EFI_STATUS
+EFIAPI
+AtaSoftReset (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT8 DeviceControl;
+
+  DeviceControl = 0;
+  //
+  // disable Interrupt and set SRST bit to initiate soft reset
+  //
+  DeviceControl = ATA_CTLREG_SRST | ATA_CTLREG_IEN_L;
+
+  IdeWritePortB (PciIo, IdeRegisters->AltOrDev, DeviceControl);
+
+  //
+  // SRST should assert for at least 5 us, we use 10 us for
+  // better compatibility
+  //
+  MicroSecondDelay (10);
+
+  //
+  // Enable interrupt to support UDMA, and clear SRST bit
+  //
+  DeviceControl = 0;
+  IdeWritePortB (PciIo, IdeRegisters->AltOrDev, DeviceControl);
+
+  //
+  // Wait for at least 10 ms to check BSY status, we use 10 ms
+  // for better compatibility
+  //
+  MicroSecondDelay (10000);
+
+  //
+  // slave device needs at most 31ms to clear BSY
+  //
+  if (WaitForBSYClear (PciIo, IdeRegisters, Timeout) == EFI_TIMEOUT) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send ATA Ext command into device with NON_DATA protocol.
+
+  @param PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval  EFI_SUCCESS Reading succeed
+  @retval  EFI_DEVICE_ERROR Error executing commands on this device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaIssueCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  EFI_IDE_REGISTERS         *IdeRegisters,
+  IN  EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN  UINT64                    Timeout
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       DeviceHead;
+  UINT8       AtaCommand;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+  ASSERT (AtaCommandBlock != NULL);
+
+  DeviceHead = AtaCommandBlock->AtaDeviceHead;
+  AtaCommand = AtaCommandBlock->AtaCommand;
+
+  Status = WaitForBSYClear (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Select device (bit4), set LBA mode(bit6) (use 0xe0 for compatibility)
+  //
+  IdeWritePortB (PciIo, IdeRegisters->Head, (UINT8) (0xe0 | DeviceHead));
+
+  //
+  // set all the command parameters
+  // Before write to all the following registers, BSY and DRQ must be 0.
+  //
+  Status = DRQClear2 (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Fill the feature register, which is a two-byte FIFO. Need write twice.
+  //
+  IdeWritePortB (PciIo, IdeRegisters->ErrOrFeature, AtaCommandBlock->AtaFeaturesExp);
+  IdeWritePortB (PciIo, IdeRegisters->ErrOrFeature, AtaCommandBlock->AtaFeatures);
+
+  //
+  // Fill the sector count register, which is a two-byte FIFO. Need write twice.
+  //
+  IdeWritePortB (PciIo, IdeRegisters->SectorCount, AtaCommandBlock->AtaSectorCountExp);
+  IdeWritePortB (PciIo, IdeRegisters->SectorCount, AtaCommandBlock->AtaSectorCount);
+
+  //
+  // Fill the start LBA registers, which are also two-byte FIFO
+  //
+  IdeWritePortB (PciIo, IdeRegisters->SectorNumber, AtaCommandBlock->AtaSectorNumberExp);
+  IdeWritePortB (PciIo, IdeRegisters->SectorNumber, AtaCommandBlock->AtaSectorNumber);
+
+  IdeWritePortB (PciIo, IdeRegisters->CylinderLsb, AtaCommandBlock->AtaCylinderLowExp);
+  IdeWritePortB (PciIo, IdeRegisters->CylinderLsb, AtaCommandBlock->AtaCylinderLow);
+
+  IdeWritePortB (PciIo, IdeRegisters->CylinderMsb, AtaCommandBlock->AtaCylinderHighExp);
+  IdeWritePortB (PciIo, IdeRegisters->CylinderMsb, AtaCommandBlock->AtaCylinderHigh);
+
+  //
+  // Send command via Command Register
+  //
+  IdeWritePortB (PciIo, IdeRegisters->CmdOrStatus, AtaCommand);
+
+  //
+  // Stall at least 400 microseconds.
+  //
+  MicroSecondDelay (400);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to send out ATA commands conforms to the PIO Data In Protocol.
+
+  @param[in]      PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data
+                                   structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in, out] Buffer           A pointer to the source buffer for the data.
+  @param[in]      ByteCount        The length of the data.
+  @param[in]      Read             Flag used to determine the data transfer direction.
+                                   Read equals 1, means data transferred from device
+                                   to host;Read equals 0, means data transferred
+                                   from host to device.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval EFI_SUCCESS      send out the ATA command and device send required data successfully.
+  @retval EFI_DEVICE_ERROR command sent failed.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPioDataInOut (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN OUT VOID                      *Buffer,
+  IN     UINT64                    ByteCount,
+  IN     BOOLEAN                   Read,
+  IN     EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock,
+  IN     UINT64                    Timeout,
+  IN     ATA_NONBLOCK_TASK         *Task
+  )
+{
+  UINTN       WordCount;
+  UINTN       Increment;
+  UINT16      *Buffer16;
+  EFI_STATUS  Status;
+
+  if ((PciIo == NULL) || (IdeRegisters == NULL) || (Buffer == NULL) || (AtaCommandBlock == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Issue ATA command
+  //
+  Status = AtaIssueCommand (PciIo, IdeRegisters, AtaCommandBlock, Timeout);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  Buffer16 = (UINT16 *) Buffer;
+
+  //
+  // According to PIO data in protocol, host can perform a series of reads to
+  // the data register after each time device set DRQ ready;
+  // The data size of "a series of read" is command specific.
+  // For most ATA command, data size received from device will not exceed
+  // 1 sector, hence the data size for "a series of read" can be the whole data
+  // size of one command request.
+  // For ATA command such as Read Sector command, the data size of one ATA
+  // command request is often larger than 1 sector, according to the
+  // Read Sector command, the data size of "a series of read" is exactly 1
+  // sector.
+  // Here for simplification reason, we specify the data size for
+  // "a series of read" to 1 sector (256 words) if data size of one ATA command
+  // request is larger than 256 words.
+  //
+  Increment = 256;
+
+  //
+  // used to record bytes of currently transfered data
+  //
+  WordCount = 0;
+
+  while (WordCount < RShiftU64(ByteCount, 1)) {
+    //
+    // Poll DRQ bit set, data transfer can be performed only when DRQ is ready
+    //
+    Status = DRQReady2 (PciIo, IdeRegisters, Timeout);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+
+    //
+    // Get the byte count for one series of read
+    //
+    if ((WordCount + Increment) > RShiftU64(ByteCount, 1)) {
+      Increment = (UINTN)(RShiftU64(ByteCount, 1) - WordCount);
+    }
+
+    if (Read) {
+      IdeReadPortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        Increment,
+        Buffer16
+        );
+    } else {
+      IdeWritePortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        Increment,
+        Buffer16
+        );
+    }
+
+    Status = CheckStatusRegister (PciIo, IdeRegisters);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+
+    WordCount += Increment;
+    Buffer16  += Increment;
+  }
+
+  Status = DRQClear (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+Exit:
+  //
+  // Dump All Ide registers to ATA_STATUS_BLOCK
+  //
+  DumpAllIdeRegisters (PciIo, IdeRegisters, AtaStatusBlock);
+
+  //
+  // Not support the Non-blocking now,just do the blocking process.
+  //
+  return Status;
+}
+
+/**
+  Send ATA command into device with NON_DATA protocol
+
+  @param[in]      PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE
+                                   data structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data
+                                   structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval  EFI_SUCCESS Reading succeed
+  @retval  EFI_ABORTED Command failed
+  @retval  EFI_DEVICE_ERROR Device status error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaNonDataCommandIn (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN     EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock,
+  IN     UINT64                    Timeout,
+  IN     ATA_NONBLOCK_TASK         *Task
+  )
+{
+  EFI_STATUS  Status;
+
+  if ((PciIo == NULL) || (IdeRegisters == NULL) || (AtaCommandBlock == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Issue ATA command
+  //
+  Status = AtaIssueCommand (PciIo, IdeRegisters, AtaCommandBlock, Timeout);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  //
+  // Wait for command completion
+  //
+  Status = WaitForBSYClear (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  Status = CheckStatusRegister (PciIo, IdeRegisters);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+Exit:
+  //
+  // Dump All Ide registers to ATA_STATUS_BLOCK
+  //
+  DumpAllIdeRegisters (PciIo, IdeRegisters, AtaStatusBlock);
+
+  //
+  // Not support the Non-blocking now,just do the blocking process.
+  //
+  return Status;
+}
+
+/**
+  Wait for memory to be set.
+
+  @param[in]  PciIo           The PCI IO protocol instance.
+  @param[in]  IdeRegisters    A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]  Timeout         The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR  The memory is not set.
+  @retval EFI_TIMEOUT       The memory setting is time out.
+  @retval EFI_SUCCESS       The memory is correct set.
+
+**/
+EFI_STATUS
+AtaUdmStatusWait (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  EFI_IDE_REGISTERS         *IdeRegisters,
+  IN  UINT64                    Timeout
+ )
+{
+  UINT8                         RegisterValue;
+  EFI_STATUS                    Status;
+  UINT16                        IoPortForBmis;
+  UINT64                        Delay;
+  BOOLEAN                       InfiniteWait;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32 (Timeout, 1000) + 1;
+
+  do {
+    Status = CheckStatusRegister (PciIo, IdeRegisters);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    IoPortForBmis = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMIS_OFFSET);
+    RegisterValue = IdeReadPortB (PciIo, IoPortForBmis);
+    if (((RegisterValue & BMIS_ERROR) != 0) || (Timeout == 0)) {
+      DEBUG ((EFI_D_ERROR, "ATA UDMA operation fails\n"));
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    if ((RegisterValue & BMIS_INTERRUPT) != 0) {
+      Status = EFI_SUCCESS;
+      break;
+    }
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return Status;
+}
+
+/**
+  Check if the memory to be set.
+
+  @param[in]  PciIo           The PCI IO protocol instance.
+  @param[in]  Task            Optional. Pointer to the ATA_NONBLOCK_TASK
+                              used by non-blocking mode.
+  @param[in]  IdeRegisters    A pointer to EFI_IDE_REGISTERS data structure.
+
+  @retval EFI_DEVICE_ERROR  The memory setting met a issue.
+  @retval EFI_NOT_READY     The memory is not set.
+  @retval EFI_TIMEOUT       The memory setting is time out.
+  @retval EFI_SUCCESS       The memory is correct set.
+
+**/
+EFI_STATUS
+AtaUdmStatusCheck (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     ATA_NONBLOCK_TASK          *Task,
+  IN     EFI_IDE_REGISTERS          *IdeRegisters
+ )
+{
+  UINT8          RegisterValue;
+  UINT16         IoPortForBmis;
+  EFI_STATUS     Status;
+
+  Task->RetryTimes--;
+
+  Status = CheckStatusRegister (PciIo, IdeRegisters);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  IoPortForBmis = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMIS_OFFSET);
+  RegisterValue = IdeReadPortB (PciIo, IoPortForBmis);
+
+  if ((RegisterValue & BMIS_ERROR) != 0) {
+    DEBUG ((EFI_D_ERROR, "ATA UDMA operation fails\n"));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((RegisterValue & BMIS_INTERRUPT) != 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (!Task->InfiniteWait && (Task->RetryTimes == 0)) {
+    return EFI_TIMEOUT;
+  } else {
+    //
+    // The memory is not set.
+    //
+    return EFI_NOT_READY;
+  }
+}
+
+/**
+  Perform an ATA Udma operation (Read, ReadExt, Write, WriteExt).
+
+  @param[in]      Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data
+                                   structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]      Read             Flag used to determine the data transfer
+                                   direction. Read equals 1, means data transferred
+                                   from device to host;Read equals 0, means data
+                                   transferred from host to device.
+  @param[in]      DataBuffer       A pointer to the source buffer for the data.
+  @param[in]      DataLength       The length of  the data.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval EFI_SUCCESS          the operation is successful.
+  @retval EFI_OUT_OF_RESOURCES Build PRD table failed
+  @retval EFI_UNSUPPORTED      Unknown channel or operations command
+  @retval EFI_DEVICE_ERROR     Ata command execute failed
+
+**/
+EFI_STATUS
+EFIAPI
+AtaUdmaInOut (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     EFI_IDE_REGISTERS             *IdeRegisters,
+  IN     BOOLEAN                       Read,
+  IN     VOID                          *DataBuffer,
+  IN     UINT64                        DataLength,
+  IN     EFI_ATA_COMMAND_BLOCK         *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock,
+  IN     UINT64                        Timeout,
+  IN     ATA_NONBLOCK_TASK             *Task
+  )
+{
+  EFI_STATUS                    Status;
+  UINT16                        IoPortForBmic;
+  UINT16                        IoPortForBmis;
+  UINT16                        IoPortForBmid;
+
+  UINTN                         PrdTableSize;
+  EFI_PHYSICAL_ADDRESS          PrdTableMapAddr;
+  VOID                          *PrdTableMap;
+  EFI_PHYSICAL_ADDRESS          PrdTableBaseAddr;
+  EFI_ATA_DMA_PRD               *TempPrdBaseAddr;
+  UINTN                         PrdTableNum;
+
+  UINT8                         RegisterValue;
+  UINTN                         PageCount;
+  UINTN                         ByteCount;
+  UINTN                         ByteRemaining;
+  UINT8                         DeviceControl;
+
+  VOID                          *BufferMap;
+  EFI_PHYSICAL_ADDRESS          BufferMapAddress;
+  EFI_PCI_IO_PROTOCOL_OPERATION PciIoOperation;
+
+  UINT8                         DeviceHead;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  EFI_TPL                       OldTpl;
+
+  UINTN                         AlignmentMask;
+  UINTN                         RealPageCount;
+  EFI_PHYSICAL_ADDRESS          BaseAddr;
+  EFI_PHYSICAL_ADDRESS          BaseMapAddr;
+
+  Status        = EFI_SUCCESS;
+  PrdTableMap   = NULL;
+  BufferMap     = NULL;
+  PageCount     = 0;
+  RealPageCount = 0;
+  BaseAddr      = 0;
+  PciIo         = Instance->PciIo;
+
+  if ((PciIo == NULL) || (IdeRegisters == NULL) || (DataBuffer == NULL) || (AtaCommandBlock == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Before starting the Blocking BlockIO operation, push to finish all non-blocking
+  // BlockIO tasks.
+  // Delay 1ms to simulate the blocking time out checking.
+  //
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  while ((Task == NULL) && (!IsListEmpty (&Instance->NonBlockingTaskList))) {
+    AsyncNonBlockingTransferRoutine (NULL, Instance);
+    //
+    // Stall for 1 milliseconds.
+    //
+    MicroSecondDelay (1000);
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  //
+  // The data buffer should be even alignment
+  //
+  if (((UINTN)DataBuffer & 0x1) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Set relevant IO Port address.
+  //
+  IoPortForBmic = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMIC_OFFSET);
+  IoPortForBmis = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMIS_OFFSET);
+  IoPortForBmid = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMID_OFFSET);
+
+  //
+  // For Blocking mode, start the command.
+  // For non-blocking mode, when the command is not started, start it, otherwise
+  // go to check the status.
+  //
+  if (((Task != NULL) && (!Task->IsStart)) || (Task == NULL)) {
+    //
+    // Calculate the number of PRD entry.
+    // Every entry in PRD table can specify a 64K memory region.
+    //
+    PrdTableNum   = (UINTN)(RShiftU64(DataLength, 16) + 1);
+
+    //
+    // Make sure that the memory region of PRD table is not cross 64K boundary
+    //
+    PrdTableSize = PrdTableNum * sizeof (EFI_ATA_DMA_PRD);
+    if (PrdTableSize > 0x10000) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Allocate buffer for PRD table initialization.
+    // Note Ide Bus Master spec said the descriptor table must be aligned on a 4 byte
+    // boundary and the table cannot cross a 64K boundary in memory.
+    //
+    PageCount     = EFI_SIZE_TO_PAGES (PrdTableSize);
+    RealPageCount = PageCount + EFI_SIZE_TO_PAGES (SIZE_64KB);
+
+    //
+    // Make sure that PageCount plus EFI_SIZE_TO_PAGES (SIZE_64KB) does not overflow.
+    //
+    ASSERT (RealPageCount > PageCount);
+
+    Status    = PciIo->AllocateBuffer (
+                         PciIo,
+                         AllocateAnyPages,
+                         EfiBootServicesData,
+                         RealPageCount,
+                         (VOID **)&BaseAddr,
+                         0
+                         );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ByteCount = EFI_PAGES_TO_SIZE (RealPageCount);
+    Status    = PciIo->Map (
+                         PciIo,
+                         EfiPciIoOperationBusMasterCommonBuffer,
+                         (VOID*)(UINTN)BaseAddr,
+                         &ByteCount,
+                         &BaseMapAddr,
+                         &PrdTableMap
+                         );
+    if (EFI_ERROR (Status) || (ByteCount != EFI_PAGES_TO_SIZE (RealPageCount))) {
+      //
+      // If the data length actually mapped is not equal to the requested amount,
+      // it means the DMA operation may be broken into several discontinuous smaller chunks.
+      // Can't handle this case.
+      //
+      PciIo->FreeBuffer (PciIo, RealPageCount, (VOID*)(UINTN)BaseAddr);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ZeroMem ((VOID *) ((UINTN) BaseAddr), ByteCount);
+
+    //
+    // Calculate the 64K align address as PRD Table base address.
+    //
+    AlignmentMask    = SIZE_64KB - 1;
+    PrdTableBaseAddr = ((UINTN) BaseAddr + AlignmentMask) & ~AlignmentMask;
+    PrdTableMapAddr  = ((UINTN) BaseMapAddr + AlignmentMask) & ~AlignmentMask;
+
+    //
+    // Map the host address of DataBuffer to DMA master address.
+    //
+    if (Read) {
+      PciIoOperation = EfiPciIoOperationBusMasterWrite;
+    } else {
+      PciIoOperation = EfiPciIoOperationBusMasterRead;
+    }
+
+    ByteCount = (UINTN)DataLength;
+    Status    = PciIo->Map (
+                         PciIo,
+                         PciIoOperation,
+                         DataBuffer,
+                         &ByteCount,
+                         &BufferMapAddress,
+                         &BufferMap
+                         );
+    if (EFI_ERROR (Status) || (ByteCount != DataLength)) {
+      PciIo->Unmap (PciIo, PrdTableMap);
+      PciIo->FreeBuffer (PciIo, RealPageCount, (VOID*)(UINTN)BaseAddr);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // According to Ata spec, it requires the buffer address and size to be even.
+    //
+    ASSERT ((BufferMapAddress & 0x1) == 0);
+    ASSERT ((ByteCount & 0x1) == 0);
+
+    //
+    // Fill the PRD table with appropriate bus master address of data buffer and data length.
+    //
+    ByteRemaining   = ByteCount;
+    TempPrdBaseAddr = (EFI_ATA_DMA_PRD*)(UINTN)PrdTableBaseAddr;
+    while (ByteRemaining != 0) {
+      if (ByteRemaining <= 0x10000) {
+        TempPrdBaseAddr->RegionBaseAddr = (UINT32) ((UINTN) BufferMapAddress);
+        TempPrdBaseAddr->ByteCount      = (UINT16) ByteRemaining;
+        TempPrdBaseAddr->EndOfTable     = 0x8000;
+        break;
+      }
+
+      TempPrdBaseAddr->RegionBaseAddr = (UINT32) ((UINTN) BufferMapAddress);
+      TempPrdBaseAddr->ByteCount      = (UINT16) 0x0;
+
+      ByteRemaining    -= 0x10000;
+      BufferMapAddress += 0x10000;
+      TempPrdBaseAddr++;
+    }
+
+    //
+    // Start to enable the DMA operation
+    //
+    DeviceHead = AtaCommandBlock->AtaDeviceHead;
+
+    IdeWritePortB (PciIo, IdeRegisters->Head, (UINT8)(0xe0 | DeviceHead));
+
+    //
+    // Enable interrupt to support UDMA
+    //
+    DeviceControl = 0;
+    IdeWritePortB (PciIo, IdeRegisters->AltOrDev, DeviceControl);
+
+    //
+    // Read BMIS register and clear ERROR and INTR bit
+    //
+    RegisterValue  = IdeReadPortB(PciIo, IoPortForBmis);
+    RegisterValue |= (BMIS_INTERRUPT | BMIS_ERROR);
+    IdeWritePortB (PciIo, IoPortForBmis, RegisterValue);
+
+    //
+    // Set the base address to BMID register
+    //
+    IdeWritePortDW (PciIo, IoPortForBmid, (UINT32)PrdTableMapAddr);
+
+    //
+    // Set BMIC register to identify the operation direction
+    //
+    RegisterValue = IdeReadPortB(PciIo, IoPortForBmic);
+    if (Read) {
+      RegisterValue |= BMIC_NREAD;
+    } else {
+      RegisterValue &= ~((UINT8) BMIC_NREAD);
+    }
+    IdeWritePortB (PciIo, IoPortForBmic, RegisterValue);
+
+    if (Task != NULL) {
+      Task->Map            = BufferMap;
+      Task->TableMap       = PrdTableMap;
+      Task->MapBaseAddress = (EFI_ATA_DMA_PRD*)(UINTN)BaseAddr;
+      Task->PageCount      = RealPageCount;
+      Task->IsStart        = TRUE;
+    }
+
+    //
+    // Issue ATA command
+    //
+    Status = AtaIssueCommand (PciIo, IdeRegisters, AtaCommandBlock, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+
+    Status = CheckStatusRegister (PciIo, IdeRegisters);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+    //
+    // Set START bit of BMIC register
+    //
+    RegisterValue  = IdeReadPortB(PciIo, IoPortForBmic);
+    RegisterValue |= BMIC_START;
+    IdeWritePortB(PciIo, IoPortForBmic, RegisterValue);
+
+  }
+
+  //
+  // Check the INTERRUPT and ERROR bit of BMIS
+  //
+  if (Task != NULL) {
+    Status = AtaUdmStatusCheck (PciIo, Task, IdeRegisters);
+  } else {
+    Status = AtaUdmStatusWait (PciIo, IdeRegisters, Timeout);
+  }
+
+  //
+  // For blocking mode, clear registers and free buffers.
+  // For non blocking mode, when the related registers have been set or time
+  // out, or a error has been happened, it needs to clear the register and free
+  // buffer.
+  //
+  if ((Task == NULL) || Status != EFI_NOT_READY) {
+    //
+    // Read BMIS register and clear ERROR and INTR bit
+    //
+    RegisterValue  = IdeReadPortB (PciIo, IoPortForBmis);
+    RegisterValue |= (BMIS_INTERRUPT | BMIS_ERROR);
+    IdeWritePortB (PciIo, IoPortForBmis, RegisterValue);
+
+    //
+    // Read Status Register of IDE device to clear interrupt
+    //
+    RegisterValue  = IdeReadPortB(PciIo, IdeRegisters->CmdOrStatus);
+
+    //
+    // Clear START bit of BMIC register
+    //
+    RegisterValue  = IdeReadPortB(PciIo, IoPortForBmic);
+    RegisterValue &= ~((UINT8) BMIC_START);
+    IdeWritePortB (PciIo, IoPortForBmic, RegisterValue);
+
+    //
+    // Disable interrupt of Select device
+    //
+    DeviceControl  = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    DeviceControl |= ATA_CTLREG_IEN_L;
+    IdeWritePortB (PciIo, IdeRegisters->AltOrDev, DeviceControl);
+    //
+    // Stall for 10 milliseconds.
+    //
+    MicroSecondDelay (10000);
+
+  }
+
+Exit:
+  //
+  // Free all allocated resource
+  //
+  if ((Task == NULL) || Status != EFI_NOT_READY) {
+    if (Task != NULL) {
+      PciIo->Unmap (PciIo, Task->TableMap);
+      PciIo->FreeBuffer (PciIo, Task->PageCount, Task->MapBaseAddress);
+      PciIo->Unmap (PciIo, Task->Map);
+    } else {
+      PciIo->Unmap (PciIo, PrdTableMap);
+      PciIo->FreeBuffer (PciIo, RealPageCount, (VOID*)(UINTN)BaseAddr);
+      PciIo->Unmap (PciIo, BufferMap);
+    }
+
+    //
+    // Dump All Ide registers to ATA_STATUS_BLOCK
+    //
+    DumpAllIdeRegisters (PciIo, IdeRegisters, AtaStatusBlock);
+  }
+
+  return Status;
+}
+
+/**
+  This function reads the pending data in the device.
+
+  @param PciIo         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters  A pointer to EFI_IDE_REGISTERS data structure.
+
+  @retval EFI_SUCCESS   Successfully read.
+  @retval EFI_NOT_READY The BSY is set avoiding reading.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketReadPendingData (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  EFI_IDE_REGISTERS         *IdeRegisters
+  )
+{
+  UINT8     AltRegister;
+  UINT16    TempWordBuffer;
+
+  AltRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+  if ((AltRegister & ATA_STSREG_BSY) == ATA_STSREG_BSY) {
+    return EFI_NOT_READY;
+  }
+
+  if ((AltRegister & (ATA_STSREG_BSY | ATA_STSREG_DRQ)) == ATA_STSREG_DRQ) {
+    TempWordBuffer = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    while ((TempWordBuffer & (ATA_STSREG_BSY | ATA_STSREG_DRQ)) == ATA_STSREG_DRQ) {
+      IdeReadPortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        1,
+        &TempWordBuffer
+        );
+      TempWordBuffer = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is called by AtaPacketCommandExecute().
+  It is used to transfer data between host and device. The data direction is specified
+  by the fourth parameter.
+
+  @param PciIo         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters  A pointer to EFI_IDE_REGISTERS data structure.
+  @param Buffer        Buffer contained data transferred between host and device.
+  @param ByteCount     Data size in byte unit of the buffer.
+  @param Read          Flag used to determine the data transfer direction.
+                       Read equals 1, means data transferred from device to host;
+                       Read equals 0, means data transferred from host to device.
+  @param Timeout       Timeout value for wait DRQ ready before each data stream's transfer
+                       , uses 100ns as a unit.
+
+  @retval EFI_SUCCESS      data is transferred successfully.
+  @retval EFI_DEVICE_ERROR the device failed to transfer data.
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketReadWrite (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN OUT VOID                      *Buffer,
+  IN     UINT64                    ByteCount,
+  IN     BOOLEAN                   Read,
+  IN     UINT64                    Timeout
+  )
+{
+  UINT32      RequiredWordCount;
+  UINT32      ActualWordCount;
+  UINT32      WordCount;
+  EFI_STATUS  Status;
+  UINT16      *PtrBuffer;
+
+  //
+  // No data transfer is premitted.
+  //
+  if (ByteCount == 0) {
+    return EFI_SUCCESS;
+  }
+
+  PtrBuffer         = Buffer;
+  RequiredWordCount = (UINT32)RShiftU64(ByteCount, 1);
+  //
+  // ActuralWordCount means the word count of data really transferred.
+  //
+  ActualWordCount = 0;
+
+  while (ActualWordCount < RequiredWordCount) {
+    //
+    // before each data transfer stream, the host should poll DRQ bit ready,
+    // to see whether indicates device is ready to transfer data.
+    //
+    Status = DRQReady2 (PciIo, IdeRegisters, Timeout);
+    if (EFI_ERROR (Status)) {
+      return CheckStatusRegister (PciIo, IdeRegisters);
+    }
+
+    //
+    // get current data transfer size from Cylinder Registers.
+    //
+    WordCount  = IdeReadPortB (PciIo, IdeRegisters->CylinderMsb) << 8;
+    WordCount  = WordCount | IdeReadPortB (PciIo, IdeRegisters->CylinderLsb);
+    WordCount  = WordCount & 0xffff;
+    WordCount /= 2;
+
+    WordCount = MIN (WordCount, (RequiredWordCount - ActualWordCount));
+
+    if (Read) {
+      IdeReadPortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        WordCount,
+        PtrBuffer
+        );
+    } else {
+      IdeWritePortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        WordCount,
+        PtrBuffer
+        );
+    }
+
+    //
+    // read status register to check whether error happens.
+    //
+    Status = CheckStatusRegister (PciIo, IdeRegisters);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    PtrBuffer       += WordCount;
+    ActualWordCount += WordCount;
+  }
+
+  if (Read) {
+    //
+    // In the case where the drive wants to send more data than we need to read,
+    // the DRQ bit will be set and cause delays from DRQClear2().
+    // We need to read data from the drive until it clears DRQ so we can move on.
+    //
+    AtaPacketReadPendingData (PciIo, IdeRegisters);
+  }
+
+  //
+  // read status register to check whether error happens.
+  //
+  Status = CheckStatusRegister (PciIo, IdeRegisters);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // After data transfer is completed, normally, DRQ bit should clear.
+  //
+  Status = DRQClear (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+/**
+  This function is used to send out ATAPI commands conforms to the Packet Command
+  with PIO Data In Protocol.
+
+  @param[in] PciIo          Pointer to the EFI_PCI_IO_PROTOCOL instance
+  @param[in] IdeRegisters   Pointer to EFI_IDE_REGISTERS which is used to
+                            store the IDE i/o port registers' base addresses
+  @param[in] Channel        The channel number of device.
+  @param[in] Device         The device number of device.
+  @param[in] Packet         A pointer to EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET data structure.
+
+  @retval EFI_SUCCESS       send out the ATAPI packet command successfully
+                            and device sends data successfully.
+  @retval EFI_DEVICE_ERROR  the device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketCommandExecute (
+  IN  EFI_PCI_IO_PROTOCOL                           *PciIo,
+  IN  EFI_IDE_REGISTERS                             *IdeRegisters,
+  IN  UINT8                                         Channel,
+  IN  UINT8                                         Device,
+  IN  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet
+  )
+{
+  EFI_ATA_COMMAND_BLOCK       AtaCommandBlock;
+  EFI_STATUS                  Status;
+  UINT8                       Count;
+  UINT8                       PacketCommand[12];
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  //
+  // Fill ATAPI Command Packet according to CDB.
+  // For Atapi cmd, its length should be less than or equal to 12 bytes.
+  //
+  if (Packet->CdbLength > 12) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (PacketCommand, 12);
+  CopyMem (PacketCommand, Packet->Cdb, Packet->CdbLength);
+
+  //
+  // No OVL; No DMA
+  //
+  AtaCommandBlock.AtaFeatures = 0x00;
+  //
+  // set the transfersize to ATAPI_MAX_BYTE_COUNT to let the device
+  // determine how many data should be transferred.
+  //
+  AtaCommandBlock.AtaCylinderLow  = (UINT8) (ATAPI_MAX_BYTE_COUNT & 0x00ff);
+  AtaCommandBlock.AtaCylinderHigh = (UINT8) (ATAPI_MAX_BYTE_COUNT >> 8);
+  AtaCommandBlock.AtaDeviceHead   = (UINT8) (Device << 0x4);
+  AtaCommandBlock.AtaCommand      = ATA_CMD_PACKET;
+
+  IdeWritePortB (PciIo, IdeRegisters->Head, (UINT8)(0xe0 | (Device << 0x4)));
+  //
+  //  Disable interrupt
+  //
+  IdeWritePortB (PciIo, IdeRegisters->AltOrDev, ATA_DEFAULT_CTL);
+
+  //
+  // Issue ATA PACKET command firstly
+  //
+  Status = AtaIssueCommand (PciIo, IdeRegisters, &AtaCommandBlock, Packet->Timeout);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = DRQReady (PciIo, IdeRegisters, Packet->Timeout);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Send out ATAPI command packet
+  //
+  for (Count = 0; Count < 6; Count++) {
+    IdeWritePortW (PciIo, IdeRegisters->Data, *((UINT16*)PacketCommand + Count));
+    //
+    // Stall for 10 microseconds.
+    //
+    MicroSecondDelay (10);
+  }
+
+  //
+  // Read/Write the data of ATAPI Command
+  //
+  if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+    Status = AtaPacketReadWrite (
+               PciIo,
+               IdeRegisters,
+               Packet->InDataBuffer,
+               Packet->InTransferLength,
+               TRUE,
+               Packet->Timeout
+               );
+  } else {
+    Status = AtaPacketReadWrite (
+               PciIo,
+               IdeRegisters,
+               Packet->OutDataBuffer,
+               Packet->OutTransferLength,
+               FALSE,
+               Packet->Timeout
+               );
+  }
+
+  return Status;
+}
+
+
+/**
+  Set the calculated Best transfer mode to a detected device.
+
+  @param Instance               A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel                The channel number of device.
+  @param Device                 The device number of device.
+  @param TransferMode           A pointer to EFI_ATA_TRANSFER_MODE data structure.
+  @param AtaStatusBlock         A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS          Set transfer mode successfully.
+  @retval EFI_DEVICE_ERROR     Set transfer mode failed.
+  @retval EFI_OUT_OF_RESOURCES Allocate memory failed.
+
+**/
+EFI_STATUS
+EFIAPI
+SetDeviceTransferMode (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN     EFI_ATA_TRANSFER_MODE         *TransferMode,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand     = ATA_CMD_SET_FEATURES;
+  AtaCommandBlock.AtaDeviceHead  = (UINT8)(Device << 0x4);
+  AtaCommandBlock.AtaFeatures    = 0x03;
+  AtaCommandBlock.AtaSectorCount = *((UINT8 *)TransferMode);
+
+  //
+  // Send SET FEATURE command (sub command 0x03) to set pio mode.
+  //
+  Status = AtaNonDataCommandIn (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  Set drive parameters for devices not support PACKETS command.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param DriveParameters  A pointer to EFI_ATA_DRIVE_PARMS data structure.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS          Set drive parameter successfully.
+  @retval EFI_DEVICE_ERROR     Set drive parameter failed.
+  @retval EFI_OUT_OF_RESOURCES Allocate memory failed.
+
+**/
+EFI_STATUS
+EFIAPI
+SetDriveParameters (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN     EFI_ATA_DRIVE_PARMS           *DriveParameters,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand     = ATA_CMD_INIT_DRIVE_PARAM;
+  AtaCommandBlock.AtaSectorCount = DriveParameters->Sector;
+  AtaCommandBlock.AtaDeviceHead  = (UINT8) ((Device << 0x4) + DriveParameters->Heads);
+
+  //
+  // Send Init drive parameters
+  //
+  Status = AtaNonDataCommandIn (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  //
+  // Send Set Multiple parameters
+  //
+  AtaCommandBlock.AtaCommand     = ATA_CMD_SET_MULTIPLE_MODE;
+  AtaCommandBlock.AtaSectorCount = DriveParameters->MultipleSector;
+  AtaCommandBlock.AtaDeviceHead  = (UINT8)(Device << 0x4);
+
+  Status = AtaNonDataCommandIn (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  Send SMART Return Status command to check if the execution of SMART cmd is successful or not.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS     Successfully get the return status of S.M.A.R.T command execution.
+  @retval Others          Fail to get return status data.
+
+**/
+EFI_STATUS
+EFIAPI
+IdeAtaSmartReturnStatusCheck (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+  UINT8                   LBAMid;
+  UINT8                   LBAHigh;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+  AtaCommandBlock.AtaFeatures     = ATA_SMART_RETURN_STATUS;
+  AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+  AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+  AtaCommandBlock.AtaDeviceHead   = (UINT8) ((Device << 0x4) | 0xe0);
+
+  //
+  // Send S.M.A.R.T Read Return Status command to device
+  //
+  Status = AtaNonDataCommandIn (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_DISABLED)
+      );
+    return EFI_DEVICE_ERROR;
+  }
+
+  REPORT_STATUS_CODE (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_ENABLE)
+    );
+
+  LBAMid  = IdeReadPortB (Instance->PciIo, Instance->IdeRegisters[Channel].CylinderLsb);
+  LBAHigh = IdeReadPortB (Instance->PciIo, Instance->IdeRegisters[Channel].CylinderMsb);
+
+  if ((LBAMid == 0x4f) && (LBAHigh == 0xc2)) {
+    //
+    // The threshold exceeded condition is not detected by the device
+    //
+    DEBUG ((EFI_D_INFO, "The S.M.A.R.T threshold exceeded condition is not detected\n"));
+    REPORT_STATUS_CODE (
+          EFI_PROGRESS_CODE,
+          (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_UNDERTHRESHOLD)
+          );
+  } else if ((LBAMid == 0xf4) && (LBAHigh == 0x2c)) {
+    //
+    // The threshold exceeded condition is detected by the device
+    //
+    DEBUG ((EFI_D_INFO, "The S.M.A.R.T threshold exceeded condition is detected\n"));
+    REPORT_STATUS_CODE (
+         EFI_PROGRESS_CODE,
+         (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_OVERTHRESHOLD)
+         );
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Enable SMART command of the disk if supported.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param IdentifyData     A pointer to data buffer which is used to contain IDENTIFY data.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+**/
+VOID
+EFIAPI
+IdeAtaSmartSupport (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN     EFI_IDENTIFY_DATA             *IdentifyData,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+
+  //
+  // Detect if the device supports S.M.A.R.T.
+  //
+  if ((IdentifyData->AtaData.command_set_supported_82 & 0x0001) != 0x0001) {
+    //
+    // S.M.A.R.T is not supported by the device
+    //
+    DEBUG ((EFI_D_INFO, "S.M.A.R.T feature is not supported at [%a] channel [%a] device!\n",
+            (Channel == 1) ? "secondary" : "primary", (Device == 1) ? "slave" : "master"));
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_NOTSUPPORTED)
+      );
+  } else {
+    //
+    // Check if the feature is enabled. If not, then enable S.M.A.R.T.
+    //
+    if ((IdentifyData->AtaData.command_set_feature_enb_85 & 0x0001) != 0x0001) {
+
+      REPORT_STATUS_CODE (
+        EFI_PROGRESS_CODE,
+        (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_DISABLE)
+        );
+
+      ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+      AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+      AtaCommandBlock.AtaFeatures     = ATA_SMART_ENABLE_OPERATION;
+      AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+      AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+      AtaCommandBlock.AtaDeviceHead   = (UINT8) ((Device << 0x4) | 0xe0);
+
+      //
+      // Send S.M.A.R.T Enable command to device
+      //
+      Status = AtaNonDataCommandIn (
+                 Instance->PciIo,
+                 &Instance->IdeRegisters[Channel],
+                 &AtaCommandBlock,
+                 AtaStatusBlock,
+                 ATA_ATAPI_TIMEOUT,
+                 NULL
+                 );
+
+      if (!EFI_ERROR (Status)) {
+        //
+        // Send S.M.A.R.T AutoSave command to device
+        //
+        ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+        AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+        AtaCommandBlock.AtaFeatures     = 0xD2;
+        AtaCommandBlock.AtaSectorCount  = 0xF1;
+        AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+        AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+        AtaCommandBlock.AtaDeviceHead   = (UINT8) ((Device << 0x4) | 0xe0);
+
+        Status = AtaNonDataCommandIn (
+                   Instance->PciIo,
+                   &Instance->IdeRegisters[Channel],
+                   &AtaCommandBlock,
+                   AtaStatusBlock,
+                   ATA_ATAPI_TIMEOUT,
+                   NULL
+                   );
+        if (!EFI_ERROR (Status)) {
+          Status = IdeAtaSmartReturnStatusCheck (
+                     Instance,
+                     Channel,
+                     Device,
+                     AtaStatusBlock
+                     );
+        }
+      }
+    }
+
+    DEBUG ((EFI_D_INFO, "Enabled S.M.A.R.T feature at [%a] channel [%a] device!\n",
+           (Channel == 1) ? "secondary" : "primary", (Device == 1) ? "slave" : "master"));
+
+  }
+
+  return ;
+}
+
+
+/**
+  Sends out an ATA Identify Command to the specified device.
+
+  This function is called by DiscoverIdeDevice() during its device
+  identification. It sends out the ATA Identify Command to the
+  specified device. Only ATA device responses to this command. If
+  the command succeeds, it returns the Identify data structure which
+  contains information about the device. This function extracts the
+  information it needs to fill the IDE_BLK_IO_DEV data structure,
+  including device type, media block size, media capacity, and etc.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param Buffer           A pointer to data buffer which is used to contain IDENTIFY data.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS          Identify ATA device successfully.
+  @retval EFI_DEVICE_ERROR     ATA Identify Device Command failed or device is not ATA device.
+  @retval EFI_OUT_OF_RESOURCES Allocate memory failed.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaIdentify (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN OUT EFI_IDENTIFY_DATA             *Buffer,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS             Status;
+  EFI_ATA_COMMAND_BLOCK  AtaCommandBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand    = ATA_CMD_IDENTIFY_DRIVE;
+  AtaCommandBlock.AtaDeviceHead = (UINT8)(Device << 0x4);
+
+  Status = AtaPioDataInOut (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             Buffer,
+             sizeof (EFI_IDENTIFY_DATA),
+             TRUE,
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  This function is called by DiscoverIdeDevice() during its device
+  identification.
+  Its main purpose is to get enough information for the device media
+  to fill in the Media data structure of the Block I/O Protocol interface.
+
+  There are 5 steps to reach such objective:
+  1. Sends out the ATAPI Identify Command to the specified device.
+  Only ATAPI device responses to this command. If the command succeeds,
+  it returns the Identify data structure which filled with information
+  about the device. Since the ATAPI device contains removable media,
+  the only meaningful information is the device module name.
+  2. Sends out ATAPI Inquiry Packet Command to the specified device.
+  This command will return inquiry data of the device, which contains
+  the device type information.
+  3. Allocate sense data space for future use. We don't detect the media
+  presence here to improvement boot performance, especially when CD
+  media is present. The media detection will be performed just before
+  each BLK_IO read/write
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param Buffer           A pointer to data buffer which is used to contain IDENTIFY data.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS          Identify ATAPI device successfully.
+  @retval EFI_DEVICE_ERROR     ATA Identify Packet Device Command failed or device type
+                               is not supported by this IDE driver.
+  @retval EFI_OUT_OF_RESOURCES Allocate memory failed.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaIdentifyPacket (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN OUT EFI_IDENTIFY_DATA             *Buffer,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS             Status;
+  EFI_ATA_COMMAND_BLOCK  AtaCommandBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand    = ATA_CMD_IDENTIFY_DEVICE;
+  AtaCommandBlock.AtaDeviceHead = (UINT8)(Device << 0x4);
+
+  //
+  // Send ATAPI Identify Command to get IDENTIFY data.
+  //
+  Status = AtaPioDataInOut (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             (VOID *) Buffer,
+             sizeof (EFI_IDENTIFY_DATA),
+             TRUE,
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+
+/**
+  This function is used for detect whether the IDE device exists in the
+  specified Channel as the specified Device Number.
+
+  There is two IDE channels: one is Primary Channel, the other is
+  Secondary Channel.(Channel is the logical name for the physical "Cable".)
+  Different channel has different register group.
+
+  On each IDE channel, at most two IDE devices attach,
+  one is called Device 0 (Master device), the other is called Device 1
+  (Slave device). The devices on the same channel co-use the same register
+  group, so before sending out a command for a specified device via command
+  register, it is a must to select the current device to accept the command
+  by set the device number in the Head/Device Register.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeChannel       The channel number of device.
+
+  @retval EFI_SUCCESS successfully detects device.
+  @retval other       any failure during detection process will return this value.
+
+**/
+EFI_STATUS
+EFIAPI
+DetectAndConfigIdeDevice (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN  UINT8                         IdeChannel
+  )
+{
+  EFI_STATUS                        Status;
+  UINT8                             SectorCountReg;
+  UINT8                             LBALowReg;
+  UINT8                             LBAMidReg;
+  UINT8                             LBAHighReg;
+  EFI_ATA_DEVICE_TYPE               DeviceType;
+  UINT8                             IdeDevice;
+  EFI_IDE_REGISTERS                 *IdeRegisters;
+  EFI_IDENTIFY_DATA                 Buffer;
+
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeInit;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+
+  EFI_ATA_COLLECTIVE_MODE           *SupportedModes;
+  EFI_ATA_TRANSFER_MODE             TransferMode;
+  EFI_ATA_DRIVE_PARMS               DriveParameters;
+
+  IdeRegisters = &Instance->IdeRegisters[IdeChannel];
+  IdeInit      = Instance->IdeControllerInit;
+  PciIo        = Instance->PciIo;
+
+  for (IdeDevice = 0; IdeDevice < EfiIdeMaxDevice; IdeDevice++) {
+    //
+    // Send ATA Device Execut Diagnostic command.
+    // This command should work no matter DRDY is ready or not
+    //
+    IdeWritePortB (PciIo, IdeRegisters->CmdOrStatus, ATA_CMD_EXEC_DRIVE_DIAG);
+
+    Status = WaitForBSYClear (PciIo, IdeRegisters, 350000000);
+    if (EFI_ERROR (Status)) {
+      DEBUG((EFI_D_ERROR, "New detecting method: Send Execute Diagnostic Command: WaitForBSYClear: Status: %d\n", Status));
+      continue;
+    }
+
+    //
+    // Select Master or Slave device to get the return signature for ATA DEVICE DIAGNOSTIC cmd.
+    //
+    IdeWritePortB (PciIo, IdeRegisters->Head, (UINT8)((IdeDevice << 4) | 0xe0));
+    //
+    // Stall for 1 milliseconds.
+    //
+    MicroSecondDelay (1000);
+
+    SectorCountReg = IdeReadPortB (PciIo, IdeRegisters->SectorCount);
+    LBALowReg      = IdeReadPortB (PciIo, IdeRegisters->SectorNumber);
+    LBAMidReg      = IdeReadPortB (PciIo, IdeRegisters->CylinderLsb);
+    LBAHighReg     = IdeReadPortB (PciIo, IdeRegisters->CylinderMsb);
+
+    //
+    // Refer to ATA/ATAPI 4 Spec, section 9.1
+    //
+    if ((SectorCountReg == 0x1) && (LBALowReg == 0x1) && (LBAMidReg == 0x0) && (LBAHighReg == 0x0)) {
+      DeviceType = EfiIdeHarddisk;
+    } else if ((LBAMidReg == 0x14) && (LBAHighReg == 0xeb)) {
+      DeviceType = EfiIdeCdrom;
+    } else {
+      continue;
+    }
+
+    //
+    // Send IDENTIFY cmd to the device to test if it is really attached.
+    //
+    if (DeviceType == EfiIdeHarddisk) {
+      Status = AtaIdentify (Instance, IdeChannel, IdeDevice, &Buffer, NULL);
+      //
+      // if identifying ata device is failure, then try to send identify packet cmd.
+      //
+      if (EFI_ERROR (Status)) {
+        REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_PERIPHERAL_FIXED_MEDIA | EFI_P_EC_NOT_DETECTED));
+
+        DeviceType = EfiIdeCdrom;
+        Status     = AtaIdentifyPacket (Instance, IdeChannel, IdeDevice, &Buffer, NULL);
+      }
+    } else {
+      Status = AtaIdentifyPacket (Instance, IdeChannel, IdeDevice, &Buffer, NULL);
+      //
+      // if identifying atapi device is failure, then try to send identify cmd.
+      //
+      if (EFI_ERROR (Status)) {
+        DeviceType = EfiIdeHarddisk;
+        Status     = AtaIdentify (Instance, IdeChannel, IdeDevice, &Buffer, NULL);
+      }
+    }
+
+    if (EFI_ERROR (Status)) {
+      //
+      // No device is found at this port
+      //
+      continue;
+    }
+
+    DEBUG ((EFI_D_INFO, "[%a] channel [%a] [%a] device\n",
+            (IdeChannel == 1) ? "secondary" : "primary  ", (IdeDevice == 1) ? "slave " : "master",
+            DeviceType == EfiIdeCdrom ? "cdrom   " : "harddisk"));
+    //
+    // If the device is a hard disk, then try to enable S.M.A.R.T feature
+    //
+    if ((DeviceType == EfiIdeHarddisk) && PcdGetBool (PcdAtaSmartEnable)) {
+      IdeAtaSmartSupport (
+        Instance,
+        IdeChannel,
+        IdeDevice,
+        &Buffer,
+        NULL
+        );
+    }
+
+    //
+    // Submit identify data to IDE controller init driver
+    //
+    IdeInit->SubmitData (IdeInit, IdeChannel, IdeDevice, &Buffer);
+
+    //
+    // Now start to config ide device parameter and transfer mode.
+    //
+    Status = IdeInit->CalculateMode (
+                        IdeInit,
+                        IdeChannel,
+                        IdeDevice,
+                        &SupportedModes
+                        );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Calculate Mode Fail, Status = %r\n", Status));
+      continue;
+    }
+
+    //
+    // Set best supported PIO mode on this IDE device
+    //
+    if (SupportedModes->PioMode.Mode <= EfiAtaPioMode2) {
+      TransferMode.ModeCategory = EFI_ATA_MODE_DEFAULT_PIO;
+    } else {
+      TransferMode.ModeCategory = EFI_ATA_MODE_FLOW_PIO;
+    }
+
+    TransferMode.ModeNumber = (UINT8) (SupportedModes->PioMode.Mode);
+
+    if (SupportedModes->ExtModeCount == 0){
+      Status = SetDeviceTransferMode (Instance, IdeChannel, IdeDevice, &TransferMode, NULL);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Set transfer Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+    }
+
+    //
+    // Set supported DMA mode on this IDE device. Note that UDMA & MDMA cann't
+    // be set together. Only one DMA mode can be set to a device. If setting
+    // DMA mode operation fails, we can continue moving on because we only use
+    // PIO mode at boot time. DMA modes are used by certain kind of OS booting
+    //
+    if (SupportedModes->UdmaMode.Valid) {
+      TransferMode.ModeCategory = EFI_ATA_MODE_UDMA;
+      TransferMode.ModeNumber   = (UINT8) (SupportedModes->UdmaMode.Mode);
+      Status = SetDeviceTransferMode (Instance, IdeChannel, IdeDevice, &TransferMode, NULL);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Set transfer Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+    } else if (SupportedModes->MultiWordDmaMode.Valid) {
+      TransferMode.ModeCategory = EFI_ATA_MODE_MDMA;
+      TransferMode.ModeNumber   = (UINT8) SupportedModes->MultiWordDmaMode.Mode;
+      Status = SetDeviceTransferMode (Instance, IdeChannel, IdeDevice, &TransferMode, NULL);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Set transfer Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+    }
+
+    //
+    // Set Parameters for the device:
+    // 1) Init
+    // 2) Establish the block count for READ/WRITE MULTIPLE (EXT) command
+    //
+    if (DeviceType == EfiIdeHarddisk) {
+      //
+      // Init driver parameters
+      //
+      DriveParameters.Sector         = (UINT8) ((ATA5_IDENTIFY_DATA *)(&Buffer.AtaData))->sectors_per_track;
+      DriveParameters.Heads          = (UINT8) (((ATA5_IDENTIFY_DATA *)(&Buffer.AtaData))->heads - 1);
+      DriveParameters.MultipleSector = (UINT8) ((ATA5_IDENTIFY_DATA *)(&Buffer.AtaData))->multi_sector_cmd_max_sct_cnt;
+
+      Status = SetDriveParameters (Instance, IdeChannel, IdeDevice, &DriveParameters, NULL);
+    }
+
+    //
+    // Set IDE controller Timing Blocks in the PCI Configuration Space
+    //
+    IdeInit->SetTiming (IdeInit, IdeChannel, IdeDevice, SupportedModes);
+
+    //
+    // IDE controller and IDE device timing is configured successfully.
+    // Now insert the device into device list.
+    //
+    Status = CreateNewDeviceInfo (Instance, IdeChannel, IdeDevice, DeviceType, &Buffer);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if (DeviceType == EfiIdeHarddisk) {
+      REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_PERIPHERAL_FIXED_MEDIA | EFI_P_PC_ENABLE));
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialize ATA host controller at IDE mode.
+
+  The function is designed to initialize ATA host controller.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+EFI_STATUS
+EFIAPI
+IdeModeInitialization (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeInit;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  UINT8                             Channel;
+  UINT8                             IdeChannel;
+  BOOLEAN                           ChannelEnabled;
+  UINT8                             MaxDevices;
+
+  IdeInit = Instance->IdeControllerInit;
+  PciIo   = Instance->PciIo;
+  Channel = IdeInit->ChannelCount;
+
+  //
+  // Obtain IDE IO port registers' base addresses
+  //
+  Status = GetIdeRegisterIoAddr (PciIo, Instance->IdeRegisters);
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  for (IdeChannel = 0; IdeChannel < Channel; IdeChannel++) {
+    IdeInit->NotifyPhase (IdeInit, EfiIdeBeforeChannelEnumeration, IdeChannel);
+
+    //
+    // now obtain channel information fron IdeControllerInit protocol.
+    //
+    Status = IdeInit->GetChannelInfo (
+                        IdeInit,
+                        IdeChannel,
+                        &ChannelEnabled,
+                        &MaxDevices
+                        );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "[GetChannel, Status=%x]", Status));
+      continue;
+    }
+
+    if (!ChannelEnabled) {
+      continue;
+    }
+
+    ASSERT (MaxDevices <= 2);
+    //
+    // Now inform the IDE Controller Init Module.
+    //
+    IdeInit->NotifyPhase (IdeInit, EfiIdeBeforeChannelReset, IdeChannel);
+
+    //
+    // No reset channel function implemented.
+    //
+    IdeInit->NotifyPhase (IdeInit, EfiIdeAfterChannelReset, IdeChannel);
+
+    //
+    // Now inform the IDE Controller Init Module.
+    //
+    IdeInit->NotifyPhase (IdeInit, EfiIdeBusBeforeDevicePresenceDetection, IdeChannel);
+
+    //
+    // Detect all attached ATA devices and set the transfer mode for each device.
+    //
+    DetectAndConfigIdeDevice (Instance, IdeChannel);
+  }
+
+  //
+  // All configurations done! Notify IdeController to do post initialization
+  // work such as saving IDE controller PCI settings for S3 resume
+  //
+  IdeInit->NotifyPhase (IdeInit, EfiIdeBusPhaseMaximum, 0);
+
+ErrorExit:
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.h b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.h
new file mode 100644
index 0000000000..67fff64658
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.h
@@ -0,0 +1,204 @@
+/** @file
+  Header file for IDE mode of ATA host controller.
+  
+  Copyright (c) 2010 - 2011, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials                          
+  are licensed and made available under the terms and conditions of the BSD License         
+  which accompanies this distribution.  The full text of the license may be found at        
+  http://opensource.org/licenses/bsd-license.php                                            
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             
+
+**/
+#ifndef __ATA_HC_IDE_MODE_H__
+#define __ATA_HC_IDE_MODE_H__
+
+typedef enum {
+  EfiIdePrimary    = 0,
+  EfiIdeSecondary  = 1,
+  EfiIdeMaxChannel = 2
+} EFI_IDE_CHANNEL;
+
+typedef enum {
+  EfiIdeMaster    = 0,
+  EfiIdeSlave     = 1,
+  EfiIdeMaxDevice = 2
+} EFI_IDE_DEVICE;
+
+///
+/// PIO mode definition
+///
+typedef enum {
+  EfiAtaPioModeBelow2,
+  EfiAtaPioMode2,
+  EfiAtaPioMode3,
+  EfiAtaPioMode4
+} EFI_ATA_PIO_MODE;
+
+//
+// Multi word DMA definition
+//
+typedef enum {
+  EfiAtaMdmaMode0,
+  EfiAtaMdmaMode1,
+  EfiAtaMdmaMode2
+} EFI_ATA_MDMA_MODE;
+
+//
+// UDMA mode definition
+//
+typedef enum {
+  EfiAtaUdmaMode0,
+  EfiAtaUdmaMode1,
+  EfiAtaUdmaMode2,
+  EfiAtaUdmaMode3,
+  EfiAtaUdmaMode4,
+  EfiAtaUdmaMode5
+} EFI_ATA_UDMA_MODE;
+
+//
+// Bus Master Reg
+//
+#define BMIC_NREAD      BIT3
+#define BMIC_START      BIT0
+#define BMIS_INTERRUPT  BIT2
+#define BMIS_ERROR      BIT1
+
+#define BMIC_OFFSET    0x00
+#define BMIS_OFFSET    0x02
+#define BMID_OFFSET    0x04
+
+//
+// IDE transfer mode
+//
+#define EFI_ATA_MODE_DEFAULT_PIO 0x00
+#define EFI_ATA_MODE_FLOW_PIO    0x01
+#define EFI_ATA_MODE_MDMA        0x04
+#define EFI_ATA_MODE_UDMA        0x08
+
+typedef struct {
+  UINT32  RegionBaseAddr;
+  UINT16  ByteCount;
+  UINT16  EndOfTable;
+} EFI_ATA_DMA_PRD;
+
+typedef struct {
+  UINT8 ModeNumber   : 3;
+  UINT8 ModeCategory : 5;
+} EFI_ATA_TRANSFER_MODE;
+
+typedef struct {
+  UINT8 Sector;
+  UINT8 Heads;
+  UINT8 MultipleSector;
+} EFI_ATA_DRIVE_PARMS;
+
+//
+// IDE registers set
+//
+typedef struct {
+  UINT16                          Data;
+  UINT16                          ErrOrFeature;
+  UINT16                          SectorCount;
+  UINT16                          SectorNumber;
+  UINT16                          CylinderLsb;
+  UINT16                          CylinderMsb;
+  UINT16                          Head;
+  UINT16                          CmdOrStatus;
+  UINT16                          AltOrDev;
+
+  UINT16                          BusMasterBaseAddr;
+} EFI_IDE_REGISTERS;
+
+//
+// Bit definitions in Programming Interface byte of the Class Code field
+// in PCI IDE controller's Configuration Space
+//
+#define IDE_PRIMARY_OPERATING_MODE            BIT0
+#define IDE_PRIMARY_PROGRAMMABLE_INDICATOR    BIT1
+#define IDE_SECONDARY_OPERATING_MODE          BIT2
+#define IDE_SECONDARY_PROGRAMMABLE_INDICATOR  BIT3
+
+/**
+  Get IDE i/o port registers' base addresses by mode. 
+
+  In 'Compatibility' mode, use fixed addresses.
+  In Native-PCI mode, get base addresses from BARs in the PCI IDE controller's
+  Configuration Space.
+
+  The steps to get IDE i/o port registers' base addresses for each channel
+  as follows:
+
+  1. Examine the Programming Interface byte of the Class Code fields in PCI IDE
+  controller's Configuration Space to determine the operating mode.
+
+  2. a) In 'Compatibility' mode, use fixed addresses shown in the Table 1 below.
+   ___________________________________________
+  |           | Command Block | Control Block |
+  |  Channel  |   Registers   |   Registers   |
+  |___________|_______________|_______________|
+  |  Primary  |  1F0h - 1F7h  |  3F6h - 3F7h  |
+  |___________|_______________|_______________|
+  | Secondary |  170h - 177h  |  376h - 377h  |
+  |___________|_______________|_______________|
+
+  Table 1. Compatibility resource mappings
+  
+  b) In Native-PCI mode, IDE registers are mapped into IO space using the BARs
+  in IDE controller's PCI Configuration Space, shown in the Table 2 below.
+   ___________________________________________________
+  |           |   Command Block   |   Control Block   |
+  |  Channel  |     Registers     |     Registers     |
+  |___________|___________________|___________________|
+  |  Primary  | BAR at offset 0x10| BAR at offset 0x14|
+  |___________|___________________|___________________|
+  | Secondary | BAR at offset 0x18| BAR at offset 0x1C|
+  |___________|___________________|___________________|
+
+  Table 2. BARs for Register Mapping
+
+  @param[in]      PciIo          Pointer to the EFI_PCI_IO_PROTOCOL instance
+  @param[in, out] IdeRegisters   Pointer to EFI_IDE_REGISTERS which is used to
+                                 store the IDE i/o port registers' base addresses
+           
+  @retval EFI_UNSUPPORTED        Return this value when the BARs is not IO type
+  @retval EFI_SUCCESS            Get the Base address successfully
+  @retval Other                  Read the pci configureation data error
+
+**/
+EFI_STATUS
+EFIAPI
+GetIdeRegisterIoAddr (
+  IN     EFI_PCI_IO_PROTOCOL         *PciIo,
+  IN OUT EFI_IDE_REGISTERS           *IdeRegisters
+  );
+
+/**
+  This function is used to send out ATAPI commands conforms to the Packet Command 
+  with PIO Data In Protocol.
+
+  @param[in] PciIo          Pointer to the EFI_PCI_IO_PROTOCOL instance
+  @param[in] IdeRegisters   Pointer to EFI_IDE_REGISTERS which is used to
+                            store the IDE i/o port registers' base addresses
+  @param[in] Channel        The channel number of device.
+  @param[in] Device         The device number of device.
+  @param[in] Packet         A pointer to EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET data structure.
+
+  @retval EFI_SUCCESS       send out the ATAPI packet command successfully
+                            and device sends data successfully.
+  @retval EFI_DEVICE_ERROR  the device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketCommandExecute (
+  IN  EFI_PCI_IO_PROTOCOL                           *PciIo,
+  IN  EFI_IDE_REGISTERS                             *IdeRegisters,
+  IN  UINT8                                         Channel,
+  IN  UINT8                                         Device,
+  IN  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.c b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.c
new file mode 100644
index 0000000000..e7449f9714
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.c
@@ -0,0 +1,1718 @@
+/** @file
+  This file implements protocol interfaces for ATA bus driver.
+
+  This file implements protocol interfaces: Driver Binding protocol,
+  Block IO protocol and DiskInfo protocol.
+
+  Copyright (c) 2009 - 2015, 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 "AtaBus.h"
+
+//
+// ATA Bus Driver Binding Protocol Instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gAtaBusDriverBinding = {
+  AtaBusDriverBindingSupported,
+  AtaBusDriverBindingStart,
+  AtaBusDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// Template for ATA Child Device.
+//
+ATA_DEVICE gAtaDeviceTemplate = {
+  ATA_DEVICE_SIGNATURE,        // Signature
+  NULL,                        // Handle
+  {                            // BlockIo
+    EFI_BLOCK_IO_PROTOCOL_REVISION,
+    NULL,
+    AtaBlockIoReset,
+    AtaBlockIoReadBlocks,
+    AtaBlockIoWriteBlocks,
+    AtaBlockIoFlushBlocks
+  },
+  {                            // BlockIo2
+    NULL,
+    AtaBlockIoResetEx,
+    AtaBlockIoReadBlocksEx,
+    AtaBlockIoWriteBlocksEx,
+    AtaBlockIoFlushBlocksEx
+  },
+  {                            // BlockMedia
+    0,                         // MediaId
+    FALSE,                     // RemovableMedia
+    TRUE,                      // MediaPresent
+    FALSE,                     // LogicPartition
+    FALSE,                     // ReadOnly
+    FALSE,                     // WritingCache
+    0x200,                     // BlockSize
+    0,                         // IoAlign
+    0,                         // LastBlock
+    0,                         // LowestAlignedLba
+    1                          // LogicalBlocksPerPhysicalBlock
+  },
+  {                            // DiskInfo
+    EFI_DISK_INFO_IDE_INTERFACE_GUID,
+    AtaDiskInfoInquiry,
+    AtaDiskInfoIdentify,
+    AtaDiskInfoSenseData,
+    AtaDiskInfoWhichIde
+  },
+  NULL,                        // DevicePath
+  {
+    AtaStorageSecurityReceiveData,
+    AtaStorageSecuritySendData
+  },
+  NULL,                        // AtaBusDriverData
+  0,                           // Port
+  0,                           // PortMultiplierPort
+  { 0, },                      // Packet
+  {{ 0}, },                    // Acb
+  NULL,                        // Asb
+  FALSE,                       // UdmaValid
+  FALSE,                       // Lba48Bit
+  NULL,                        // IdentifyData
+  NULL,                        // ControllerNameTable
+  {L'\0', },                   // ModelName
+  {NULL, NULL},                // AtaTaskList
+  {NULL, NULL},                // AtaSubTaskList
+  FALSE                        // Abort
+};
+
+/**
+  Allocates an aligned buffer for ATA device.
+
+  This function allocates an aligned buffer for the ATA device to perform
+  ATA pass through operations. The alignment requirement is from ATA pass
+  through interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+  @param  BufferSize        The request buffer size.
+
+  @return A pointer to the aligned buffer or NULL if the allocation fails.
+
+**/
+VOID *
+AllocateAlignedBuffer (
+  IN ATA_DEVICE               *AtaDevice,
+  IN UINTN                    BufferSize
+  )
+{
+  return AllocateAlignedPages (EFI_SIZE_TO_PAGES (BufferSize), AtaDevice->AtaBusDriverData->AtaPassThru->Mode->IoAlign);
+}
+
+/**
+  Frees an aligned buffer for ATA device.
+
+  This function frees an aligned buffer for the ATA device to perform
+  ATA pass through operations.
+
+  @param  Buffer            The aligned buffer to be freed.
+  @param  BufferSize        The request buffer size.
+
+**/
+VOID
+FreeAlignedBuffer (
+  IN VOID                     *Buffer,
+  IN UINTN                    BufferSize
+  )
+{
+  if (Buffer != NULL) {
+    FreeAlignedPages (Buffer, EFI_SIZE_TO_PAGES (BufferSize));
+  }
+}
+
+
+/**
+  Release all the resources allocated for the ATA device.
+
+  This function releases all the resources allocated for the ATA device.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+**/
+VOID
+ReleaseAtaResources (
+  IN ATA_DEVICE  *AtaDevice
+  )
+{
+  ATA_BUS_ASYN_SUB_TASK *SubTask;
+  ATA_BUS_ASYN_TASK     *AtaTask;
+  LIST_ENTRY            *Entry;
+  LIST_ENTRY            *DelEntry;
+  EFI_TPL               OldTpl;
+
+  FreeUnicodeStringTable (AtaDevice->ControllerNameTable);
+  FreeAlignedBuffer (AtaDevice->Asb, sizeof (EFI_ATA_STATUS_BLOCK));
+  FreeAlignedBuffer (AtaDevice->IdentifyData, sizeof (ATA_IDENTIFY_DATA));
+  if (AtaDevice->DevicePath != NULL) {
+    FreePool (AtaDevice->DevicePath);
+  }
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  if (!IsListEmpty (&AtaDevice->AtaSubTaskList)) {
+    //
+    // Free the Subtask list.
+    //
+    for(Entry = AtaDevice->AtaSubTaskList.ForwardLink;
+        Entry != (&AtaDevice->AtaSubTaskList);
+       ) {
+      DelEntry = Entry;
+      Entry    = Entry->ForwardLink;
+      SubTask  = ATA_ASYN_SUB_TASK_FROM_ENTRY (DelEntry);
+
+      RemoveEntryList (DelEntry);
+      FreeAtaSubTask (SubTask);
+    }
+  }
+  if (!IsListEmpty (&AtaDevice->AtaTaskList)) {
+    //
+    // Free the Subtask list.
+    //
+    for(Entry = AtaDevice->AtaTaskList.ForwardLink;
+        Entry != (&AtaDevice->AtaTaskList);
+       ) {
+      DelEntry = Entry;
+      Entry    = Entry->ForwardLink;
+      AtaTask  = ATA_ASYN_TASK_FROM_ENTRY (DelEntry);
+
+      RemoveEntryList (DelEntry);
+      FreePool (AtaTask);
+    }
+  }
+  gBS->RestoreTPL (OldTpl);
+  FreePool (AtaDevice);
+}
+
+
+/**
+  Registers an ATA device.
+
+  This function allocates an ATA device structure for the ATA device specified by
+  Port and PortMultiplierPort if the ATA device is identified as a valid one.
+  Then it will create child handle and install Block IO and Disk Info protocol on
+  it.
+
+  @param  AtaBusDriverData      The parent ATA bus driver data structure.
+  @param  Port                  The port number of the ATA device.
+  @param  PortMultiplierPort    The port multiplier port number of the ATA device.
+
+  @retval EFI_SUCCESS           The ATA device is successfully registered.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough memory to allocate the ATA device
+                                and related data structures.
+  @return Others                Some error occurs when registering the ATA device.
+**/
+EFI_STATUS
+RegisterAtaDevice (
+  IN OUT ATA_BUS_DRIVER_DATA        *AtaBusDriverData,
+  IN     UINT16                     Port,
+  IN     UINT16                     PortMultiplierPort
+  )
+{
+  EFI_STATUS                        Status;
+  ATA_DEVICE                        *AtaDevice;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+  EFI_DEVICE_PATH_PROTOCOL          *NewDevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL          *RemainingDevicePath;
+  EFI_HANDLE                        DeviceHandle;
+
+  AtaDevice         = NULL;
+  NewDevicePathNode = NULL;
+  DevicePath        = NULL;
+  RemainingDevicePath = NULL;
+
+  //
+  // Build device path
+  //
+  AtaPassThru = AtaBusDriverData->AtaPassThru;
+  Status = AtaPassThru->BuildDevicePath (AtaPassThru, Port, PortMultiplierPort, &NewDevicePathNode);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  DevicePath = AppendDevicePathNode (AtaBusDriverData->ParentDevicePath, NewDevicePathNode);
+  if (DevicePath == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  DeviceHandle = NULL;
+  RemainingDevicePath = DevicePath;
+  Status = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &RemainingDevicePath, &DeviceHandle);
+  if (!EFI_ERROR (Status) && (DeviceHandle != NULL) && IsDevicePathEnd(RemainingDevicePath)) {
+    Status = EFI_ALREADY_STARTED;
+    FreePool (DevicePath);
+    goto Done;
+  }
+
+  //
+  // Allocate ATA device from the template.
+  //
+  AtaDevice = AllocateCopyPool (sizeof (ATA_DEVICE), &gAtaDeviceTemplate);
+  if (AtaDevice == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // Initializes ATA device structures and allocates the required buffer.
+  //
+  AtaDevice->BlockIo.Media      = &AtaDevice->BlockMedia;
+  AtaDevice->BlockIo2.Media     = &AtaDevice->BlockMedia;
+  AtaDevice->AtaBusDriverData   = AtaBusDriverData;
+  AtaDevice->DevicePath         = DevicePath;
+  AtaDevice->Port               = Port;
+  AtaDevice->PortMultiplierPort = PortMultiplierPort;
+  AtaDevice->Asb = AllocateAlignedBuffer (AtaDevice, sizeof (EFI_ATA_STATUS_BLOCK));
+  if (AtaDevice->Asb == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  AtaDevice->IdentifyData = AllocateAlignedBuffer (AtaDevice, sizeof (ATA_IDENTIFY_DATA));
+  if (AtaDevice->IdentifyData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // Initial Ata Task List
+  //
+  InitializeListHead (&AtaDevice->AtaTaskList);
+  InitializeListHead (&AtaDevice->AtaSubTaskList);
+
+  //
+  // Report Status Code to indicate the ATA device will be enabled
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_ENABLE),
+    AtaBusDriverData->ParentDevicePath
+    );
+
+  //
+  // Try to identify the ATA device via the ATA pass through command.
+  //
+  Status = DiscoverAtaDevice (AtaDevice);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Build controller name for Component Name (2) protocol.
+  //
+  Status = AddUnicodeString2 (
+             "eng",
+             gAtaBusComponentName.SupportedLanguages,
+             &AtaDevice->ControllerNameTable,
+             AtaDevice->ModelName,
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = AddUnicodeString2 (
+             "en",
+             gAtaBusComponentName2.SupportedLanguages,
+             &AtaDevice->ControllerNameTable,
+             AtaDevice->ModelName,
+             FALSE
+             );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Update to AHCI interface GUID based on device path node. The default one
+  // is IDE interface GUID copied from template.
+  //
+  if (NewDevicePathNode->SubType == MSG_SATA_DP) {
+    CopyGuid (&AtaDevice->DiskInfo.Interface, &gEfiDiskInfoAhciInterfaceGuid);
+  }
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &AtaDevice->Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  AtaDevice->DevicePath,
+                  &gEfiBlockIoProtocolGuid,
+                  &AtaDevice->BlockIo,
+                  &gEfiBlockIo2ProtocolGuid,
+                  &AtaDevice->BlockIo2,
+                  &gEfiDiskInfoProtocolGuid,
+                  &AtaDevice->DiskInfo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // See if the ata device support trust computing feature or not.
+  // If yes, then install Storage Security Protocol at the ata device handle.
+  //
+  if ((AtaDevice->IdentifyData->trusted_computing_support & BIT0) != 0) {
+    DEBUG ((EFI_D_INFO, "Found TCG support in Port %x PortMultiplierPort %x\n", Port, PortMultiplierPort));
+    Status = gBS->InstallProtocolInterface (
+                    &AtaDevice->Handle,
+                    &gEfiStorageSecurityCommandProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &AtaDevice->StorageSecurity
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    DEBUG ((EFI_D_INFO, "Successfully Install Storage Security Protocol on the ATA device\n"));
+  }
+
+  gBS->OpenProtocol (
+         AtaBusDriverData->Controller,
+         &gEfiAtaPassThruProtocolGuid,
+         (VOID **) &AtaPassThru,
+         AtaBusDriverData->DriverBindingHandle,
+         AtaDevice->Handle,
+         EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+         );
+
+Done:
+  if (NewDevicePathNode != NULL) {
+    FreePool (NewDevicePathNode);
+  }
+
+  if (EFI_ERROR (Status) && (AtaDevice != NULL)) {
+    ReleaseAtaResources (AtaDevice);
+    DEBUG ((EFI_D_ERROR | EFI_D_INIT, "Failed to initialize Port %x PortMultiplierPort %x, status = %r\n", Port, PortMultiplierPort, Status));
+  }
+  return Status;
+}
+
+
+/**
+  Unregisters an ATA device.
+
+  This function removes the protocols installed on the controller handle and
+  frees the resources allocated for the ATA device.
+
+  @param  This                  The pointer to EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The controller handle of the ATA device.
+  @param  Handle                The child handle.
+
+  @retval EFI_SUCCESS           The ATA device is successfully unregistered.
+  @return Others                Some error occurs when unregistering the ATA device.
+
+**/
+EFI_STATUS
+UnregisterAtaDevice (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  )
+{
+  EFI_STATUS                               Status;
+  EFI_BLOCK_IO_PROTOCOL                    *BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL                   *BlockIo2;
+  ATA_DEVICE                               *AtaDevice;
+  EFI_ATA_PASS_THRU_PROTOCOL               *AtaPassThru;
+  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *StorageSecurity;
+
+  BlockIo2             =     NULL;
+  BlockIo              =     NULL;
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlockIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // Locate BlockIo2 protocol
+    //
+    Status = gBS->OpenProtocol (
+                    Handle,
+                    &gEfiBlockIo2ProtocolGuid,
+                    (VOID **) &BlockIo2,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Get AtaDevice data.
+  //
+  if (BlockIo != NULL) {
+    AtaDevice = ATA_DEVICE_FROM_BLOCK_IO (BlockIo);
+  } else {
+    ASSERT (BlockIo2 != NULL);
+    AtaDevice = ATA_DEVICE_FROM_BLOCK_IO2 (BlockIo2);
+  }
+
+  //
+  // Close the child handle
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiAtaPassThruProtocolGuid,
+         This->DriverBindingHandle,
+         Handle
+         );
+
+  //
+  // The Ata Bus driver installs the BlockIo and BlockIo2 in the DriverBindingStart().
+  // Here should uninstall both of them.
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  AtaDevice->DevicePath,
+                  &gEfiBlockIoProtocolGuid,
+                  &AtaDevice->BlockIo,
+                  &gEfiBlockIo2ProtocolGuid,
+                  &AtaDevice->BlockIo2,
+                  &gEfiDiskInfoProtocolGuid,
+                  &AtaDevice->DiskInfo,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->OpenProtocol (
+          Controller,
+          &gEfiAtaPassThruProtocolGuid,
+          (VOID **) &AtaPassThru,
+          This->DriverBindingHandle,
+          Handle,
+          EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+          );
+    return Status;
+  }
+
+  //
+  // If Storage Security Command Protocol is installed, then uninstall this protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiStorageSecurityCommandProtocolGuid,
+                  (VOID **) &StorageSecurity,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->UninstallProtocolInterface (
+                    Handle,
+                    &gEfiStorageSecurityCommandProtocolGuid,
+                    &AtaDevice->StorageSecurity
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+        Controller,
+        &gEfiAtaPassThruProtocolGuid,
+        (VOID **) &AtaPassThru,
+        This->DriverBindingHandle,
+        Handle,
+        EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+        );
+      return Status;
+    }
+  }
+
+  ReleaseAtaResources (AtaDevice);
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_DEVICE_PATH_PROTOCOL          *ParentDevicePath;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+  UINT16                            Port;
+  UINT16                            PortMultiplierPort;
+
+  //
+  // Test EFI_ATA_PASS_THRU_PROTOCOL on controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiAtaPassThruProtocolGuid,
+                  (VOID **) &AtaPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Test to see if this ATA Pass Thru Protocol is for a LOGICAL channel
+  //
+  if ((AtaPassThru->Mode->Attributes & EFI_ATA_PASS_THRU_ATTRIBUTES_LOGICAL) == 0) {
+    //
+    // Close the I/O Abstraction(s) used to perform the supported test
+    //
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiAtaPassThruProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Test RemainingDevicePath is valid or not.
+  //
+  if ((RemainingDevicePath != NULL) && !IsDevicePathEnd (RemainingDevicePath)) {
+    Status = AtaPassThru->GetDevice (AtaPassThru, RemainingDevicePath, &Port, &PortMultiplierPort);
+    if (EFI_ERROR (Status)) {
+      //
+      // Close the I/O Abstraction(s) used to perform the supported test
+      //
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiAtaPassThruProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+      return Status;
+    }
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiAtaPassThruProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  return Status;
+}
+
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+  EFI_DEVICE_PATH_PROTOCOL          *ParentDevicePath;
+  ATA_BUS_DRIVER_DATA               *AtaBusDriverData;
+  UINT16                            Port;
+  UINT16                            PortMultiplierPort;
+
+  AtaBusDriverData = NULL;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Report Status Code to indicate ATA bus starts
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_INIT),
+    ParentDevicePath
+    );
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiAtaPassThruProtocolGuid,
+                  (VOID **) &AtaPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if ((EFI_ERROR (Status)) && (Status != EFI_ALREADY_STARTED)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Check EFI_ALREADY_STARTED to reuse the original ATA_BUS_DRIVER_DATA.
+  //
+  if (Status != EFI_ALREADY_STARTED) {
+    AtaBusDriverData = AllocateZeroPool (sizeof (ATA_BUS_DRIVER_DATA));
+    if (AtaBusDriverData == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ErrorExit;
+    }
+
+    AtaBusDriverData->AtaPassThru = AtaPassThru;
+    AtaBusDriverData->Controller  = Controller;
+    AtaBusDriverData->ParentDevicePath = ParentDevicePath;
+    AtaBusDriverData->DriverBindingHandle = This->DriverBindingHandle;
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Controller,
+                    &gEfiCallerIdGuid,
+                    AtaBusDriverData,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      goto ErrorExit;
+    }
+
+  } else {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &AtaBusDriverData,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      AtaBusDriverData = NULL;
+      goto ErrorExit;
+    }
+  }
+
+  //
+  // Report Status Code to indicate detecting devices on bus
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_DETECT),
+    ParentDevicePath
+    );
+
+  if (RemainingDevicePath == NULL) {
+    Port = 0xFFFF;
+    while (TRUE) {
+      Status = AtaPassThru->GetNextPort (AtaPassThru, &Port);
+      if (EFI_ERROR (Status)) {
+        //
+        // We cannot find more legal port then we are done.
+        //
+        break;
+      }
+
+      PortMultiplierPort = 0xFFFF;
+      while (TRUE) {
+        Status = AtaPassThru->GetNextDevice (AtaPassThru, Port, &PortMultiplierPort);
+        if (EFI_ERROR (Status)) {
+          //
+          // We cannot find more legal port multiplier port number for ATA device
+          // on the port, then we are done.
+          //
+          break;
+        }
+        RegisterAtaDevice (AtaBusDriverData, Port, PortMultiplierPort);
+      }
+    }
+    Status = EFI_SUCCESS;
+  } else if (!IsDevicePathEnd (RemainingDevicePath)) {
+    Status = AtaPassThru->GetDevice (AtaPassThru, RemainingDevicePath, &Port, &PortMultiplierPort);
+    if (!EFI_ERROR (Status)) {
+      Status = RegisterAtaDevice (AtaBusDriverData,Port, PortMultiplierPort);
+    }
+  }
+
+  return Status;
+
+ErrorExit:
+
+  if (AtaBusDriverData != NULL) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Controller,
+           &gEfiCallerIdGuid,
+           AtaBusDriverData,
+           NULL
+           );
+    FreePool (AtaBusDriverData);
+  }
+
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiAtaPassThruProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return Status;
+
+}
+
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  BOOLEAN                     AllChildrenStopped;
+  UINTN                       Index;
+  ATA_BUS_DRIVER_DATA         *AtaBusDriverData;
+
+  if (NumberOfChildren == 0) {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &AtaBusDriverData,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (!EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+            Controller,
+            &gEfiCallerIdGuid,
+            AtaBusDriverData,
+            NULL
+            );
+      FreePool (AtaBusDriverData);
+    }
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiAtaPassThruProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = UnregisterAtaDevice (This, Controller, ChildHandleBuffer[Index]);
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset the Block Device.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_STATUS      Status;
+  ATA_DEVICE      *AtaDevice;
+  EFI_TPL         OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  AtaDevice = ATA_DEVICE_FROM_BLOCK_IO (This);
+
+  Status = ResetAtaDevice (AtaDevice);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Read/Write BufferSize bytes from Lba from/into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context. Either be
+                              block I/O or block I/O2.
+  @param[in]       MediaId    The media ID that the read/write request is for.
+  @param[in]       Lba        The starting logical block address to be read/written.
+                              The caller is responsible for reading/writing to only
+                              legitimate locations.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[out]      Buffer     A pointer to the destination/source buffer for the data.
+  @param[in]       IsBlockIo2 Indicate the calling is from BlockIO or BlockIO2. TURE is
+                              from BlockIO2, FALSE is for BlockIO.
+  @param[in]       IsWrite    Indicates whether it is a write operation.
+
+  @retval EFI_SUCCESS           The data was read/written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be read/written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read/write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read/write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+BlockIoReadWrite (
+  IN     VOID                    *This,
+  IN     UINT32                  MediaId,
+  IN     EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN     *Token,
+  IN     UINTN                   BufferSize,
+  OUT    VOID                    *Buffer,
+  IN     BOOLEAN                 IsBlockIo2,
+  IN     BOOLEAN                 IsWrite
+  )
+{
+  ATA_DEVICE                        *AtaDevice;
+  EFI_STATUS                        Status;
+  EFI_TPL                           OldTpl;
+  EFI_BLOCK_IO_MEDIA                *Media;
+  UINTN                             BlockSize;
+  UINTN                             NumberOfBlocks;
+  UINTN                             IoAlign;
+
+  if (IsBlockIo2) {
+   Media     = ((EFI_BLOCK_IO2_PROTOCOL *) This)->Media;
+   AtaDevice = ATA_DEVICE_FROM_BLOCK_IO2 (This);
+  } else {
+   Media     = ((EFI_BLOCK_IO_PROTOCOL *) This)->Media;
+   AtaDevice = ATA_DEVICE_FROM_BLOCK_IO (This);
+  }
+
+  if (MediaId != Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  //
+  // Check parameters.
+  //
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    if ((Token != NULL) && (Token->Event != NULL)) {
+      Token->TransactionStatus = EFI_SUCCESS;
+      gBS->SignalEvent (Token->Event);
+    }
+    return EFI_SUCCESS;
+  }
+
+  BlockSize = Media->BlockSize;
+  if ((BufferSize % BlockSize) != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IoAlign = Media->IoAlign;
+  if (IoAlign > 0 && (((UINTN) Buffer & (IoAlign - 1)) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Invoke low level AtaDevice Access Routine.
+  //
+  Status = AccessAtaDevice (AtaDevice, Buffer, Lba, NumberOfBlocks, IsWrite, Token);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Lba        The starting Logical Block Address to read from
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the destination buffer for the data. The caller is
+                     responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  return BlockIoReadWrite ((VOID *) This, MediaId, Lba, NULL, BufferSize, Buffer, FALSE, FALSE);
+}
+
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    The media ID that the write request is for.
+  @param  Lba        The starting logical block address to be written. The caller is
+                     responsible for writing to only legitimate locations.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  )
+{
+  return BlockIoReadWrite ((VOID *) This, MediaId, Lba, NULL, BufferSize, Buffer, FALSE, TRUE);
+}
+
+
+/**
+  Flush the Block Device.
+
+  @param  This              Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  )
+{
+  //
+  // return directly
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Reset the Block Device.
+
+  @param[in]  This                 Indicates a pointer to the calling context.
+  @param[in]  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoResetEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_STATUS      Status;
+  ATA_DEVICE      *AtaDevice;
+  EFI_TPL         OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  AtaDevice = ATA_DEVICE_FROM_BLOCK_IO2 (This);
+
+  AtaTerminateNonBlockingTask (AtaDevice);
+
+  Status = ResetAtaDevice (AtaDevice);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    Id of the media, changes every time the media is replaced.
+  @param[in]       Lba        The starting Logical Block Address to read from.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[out]      Buffer     A pointer to the destination buffer for the data. The caller is
+                              responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The read request was queued if Event is not NULL.
+                                The data was read correctly from the device if
+                                the Event is NULL.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing
+                                the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of the
+                                intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack
+                                of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReadBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  return BlockIoReadWrite ((VOID *) This, MediaId, Lba, Token, BufferSize, Buffer, TRUE, FALSE);
+}
+
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    The media ID that the write request is for.
+  @param[in]       Lba        The starting logical block address to be written. The
+                              caller is responsible for writing to only legitimate
+                              locations.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[in]       Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoWriteBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  )
+{
+  return BlockIoReadWrite ((VOID *) This, MediaId, Lba, Token, BufferSize, Buffer, TRUE, TRUE);
+}
+
+
+/**
+  Flush the Block Device.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoFlushBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token
+  )
+{
+  //
+  // Signal event and return directly.
+  //
+  if (Token != NULL && Token->Event != NULL) {
+    Token->TransactionStatus = EFI_SUCCESS;
+    gBS->SignalEvent (Token->Event);
+  }
+  return EFI_SUCCESS;
+}
+/**
+  Provides inquiry information for the controller type.
+
+  This function is used by the IDE bus driver to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This             Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData      Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize  Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used by the IDE bus driver to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  )
+{
+  EFI_STATUS                      Status;
+  ATA_DEVICE                      *AtaDevice;
+
+  AtaDevice = ATA_DEVICE_FROM_DISK_INFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*IdentifyDataSize >= sizeof (ATA_IDENTIFY_DATA)) {
+    Status = EFI_SUCCESS;
+    CopyMem (IdentifyData, AtaDevice->IdentifyData, sizeof (ATA_IDENTIFY_DATA));
+  }
+  *IdentifyDataSize = sizeof (ATA_IDENTIFY_DATA);
+
+  return Status;
+}
+
+
+/**
+  Provides sense data information for the controller type.
+
+  This function is used by the IDE bus driver to get sense data.
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This             Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData        Pointer to the SenseData.
+  @param[in, out] SenseDataSize    Size of SenseData in bytes.
+  @param[out]     SenseDataNumber  Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function is used by the IDE bus driver to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  )
+{
+  ATA_DEVICE                   *AtaDevice;
+
+  AtaDevice       = ATA_DEVICE_FROM_DISK_INFO (This);
+  *IdeChannel     = AtaDevice->Port;
+  *IdeDevice      = AtaDevice->PortMultiplierPort;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send a security protocol command to a device that receives data and/or the result
+  of one or more commands sent by SendData.
+
+  The ReceiveData function sends a security protocol command to the given MediaId.
+  The security protocol command sent is defined by SecurityProtocolId and contains
+  the security protocol specific data SecurityProtocolSpecificData. The function
+  returns the data from the security protocol command in PayloadBuffer.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL IN command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED RECEIVE commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero.
+
+  If the PayloadBufferSize is zero, the security protocol command is sent using the
+  Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBufferSize is too small to store the available data from the security
+  protocol command, the function shall copy PayloadBufferSize bytes into the
+  PayloadBuffer and return EFI_WARN_BUFFER_TOO_SMALL.
+
+  If PayloadBuffer or PayloadTransferSize is NULL and PayloadBufferSize is non-zero,
+  the function shall return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function shall
+  return EFI_UNSUPPORTED. If there is no media in the device, the function returns
+  EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the device,
+  the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall
+  return EFI_SUCCESS. If the security protocol command completes with an error, the
+  function shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command. The caller is responsible for having
+                                       either implicit or explicit ownership of the buffer.
+  @param  PayloadTransferSize          A pointer to a buffer to store the size in bytes of the
+                                       data written to the payload data buffer.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL    The PayloadBufferSize was too small to store the available
+                                       data from the device. The PayloadBuffer contains the truncated data.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer or PayloadTransferSize is NULL and
+                                       PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaStorageSecurityReceiveData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  OUT VOID                                    *PayloadBuffer,
+  OUT UINTN                                   *PayloadTransferSize
+  )
+{
+  EFI_STATUS                       Status;
+  ATA_DEVICE                       *Private;
+  EFI_TPL                          OldTpl;
+
+  DEBUG ((EFI_D_INFO, "EFI Storage Security Protocol - Read\n"));
+  if ((PayloadBuffer == NULL || PayloadTransferSize == NULL) && PayloadBufferSize != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status  = EFI_SUCCESS;
+  Private = ATA_DEVICE_FROM_STORAGE_SECURITY (This);
+
+  if (MediaId != Private->BlockIo.Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (!Private->BlockIo.Media->MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Status = TrustTransferAtaDevice (
+             Private,
+             PayloadBuffer,
+             SecurityProtocolId,
+             SecurityProtocolSpecificData,
+             PayloadBufferSize,
+             FALSE,
+             Timeout,
+             PayloadTransferSize
+             );
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Send a security protocol command to a device.
+
+  The SendData function sends a security protocol command containing the payload
+  PayloadBuffer to the given MediaId. The security protocol command sent is
+  defined by SecurityProtocolId and contains the security protocol specific data
+  SecurityProtocolSpecificData. If the underlying protocol command requires a
+  specific padding for the command payload, the SendData function shall add padding
+  bytes to the command payload to satisfy the padding requirements.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL OUT command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED SEND commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero. If the PayloadBufferSize is zero, the security protocol command is
+  sent using the Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBuffer is NULL and PayloadBufferSize is non-zero, the function shall
+  return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function
+  shall return EFI_UNSUPPORTED. If there is no media in the device, the function
+  returns EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the
+  device, the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall return
+  EFI_SUCCESS. If the security protocol command completes with an error, the function
+  shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer is NULL and PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaStorageSecuritySendData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  IN VOID                                     *PayloadBuffer
+  )
+{
+  EFI_STATUS                       Status;
+  ATA_DEVICE                       *Private;
+  EFI_TPL                          OldTpl;
+
+  DEBUG ((EFI_D_INFO, "EFI Storage Security Protocol - Send\n"));
+  if ((PayloadBuffer == NULL) && (PayloadBufferSize != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status  = EFI_SUCCESS;
+  Private = ATA_DEVICE_FROM_STORAGE_SECURITY (This);
+
+  if (MediaId != Private->BlockIo.Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  Status = TrustTransferAtaDevice (
+             Private,
+             PayloadBuffer,
+             SecurityProtocolId,
+             SecurityProtocolSpecificData,
+             PayloadBufferSize,
+             TRUE,
+             Timeout,
+             NULL
+             );
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The user Entry Point for module AtaBus. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeAtaBus(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gAtaBusDriverBinding,
+             ImageHandle,
+             &gAtaBusComponentName,
+             &gAtaBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.h b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.h
new file mode 100644
index 0000000000..26ec70d30a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.h
@@ -0,0 +1,1081 @@
+/** @file
+  Master header file for ATA Bus Driver.
+
+  This file defines common data structures, macro definitions and some module
+  internal function header files.
+
+  Copyright (c) 2009 - 2015, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _ATA_BUS_H_
+#define _ATA_BUS_H_
+
+#include <Uefi.h>
+
+#include <Protocol/AtaPassThru.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/BlockIo2.h>
+#include <Protocol/DiskInfo.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/StorageSecurityCommand.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/TimerLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Atapi.h>
+
+//
+// Time out value for ATA pass through protocol
+//
+#define ATA_TIMEOUT                       EFI_TIMER_PERIOD_SECONDS (3)
+
+//
+// Maximum number of times to retry ATA command
+//
+#define MAX_RETRY_TIMES                   3
+
+//
+// The maximum total sectors count in 28 bit addressing mode
+//
+#define MAX_28BIT_ADDRESSING_CAPACITY     0xfffffff
+
+//
+// The maximum ATA transaction sector count in 28 bit addressing mode.
+//
+#define MAX_28BIT_TRANSFER_BLOCK_NUM      0x100
+
+//
+// The maximum ATA transaction sector count in 48 bit addressing mode.
+//
+//#define MAX_48BIT_TRANSFER_BLOCK_NUM      0x10000
+
+//
+// BugBug: if the TransferLength is equal with 0x10000 (the 48bit max length),
+// there is a bug that even the register interrupt bit has been sit, the buffer
+// seems not ready. Change the Maximum Sector Numbers to 0xFFFF to work round
+// this issue.
+//
+#define MAX_48BIT_TRANSFER_BLOCK_NUM      0xFFFF
+
+//
+// The maximum model name in ATA identify data
+//
+#define MAX_MODEL_NAME_LEN                40
+
+#define ATA_TASK_SIGNATURE                SIGNATURE_32 ('A', 'T', 'S', 'K')
+#define ATA_DEVICE_SIGNATURE              SIGNATURE_32 ('A', 'B', 'I', 'D')
+#define ATA_SUB_TASK_SIGNATURE            SIGNATURE_32 ('A', 'S', 'T', 'S')
+#define IS_ALIGNED(addr, size)            (((UINTN) (addr) & (size - 1)) == 0)
+
+//
+// ATA bus data structure for ATA controller
+//
+typedef struct {
+  EFI_ATA_PASS_THRU_PROTOCOL  *AtaPassThru;
+  EFI_HANDLE                  Controller;
+  EFI_DEVICE_PATH_PROTOCOL    *ParentDevicePath;
+  EFI_HANDLE                  DriverBindingHandle;
+} ATA_BUS_DRIVER_DATA;
+
+//
+// ATA device data structure for each child device
+//
+typedef struct {
+  UINT32                                Signature;
+
+  EFI_HANDLE                            Handle;
+  EFI_BLOCK_IO_PROTOCOL                 BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL                BlockIo2;
+  EFI_BLOCK_IO_MEDIA                    BlockMedia;
+  EFI_DISK_INFO_PROTOCOL                DiskInfo;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL StorageSecurity;
+
+  ATA_BUS_DRIVER_DATA                   *AtaBusDriverData;
+  UINT16                                Port;
+  UINT16                                PortMultiplierPort;
+
+  //
+  // Buffer for the execution of ATA pass through protocol
+  //
+  EFI_ATA_PASS_THRU_COMMAND_PACKET      Packet;
+  EFI_ATA_COMMAND_BLOCK                 Acb;
+  EFI_ATA_STATUS_BLOCK                  *Asb;
+
+  BOOLEAN                               UdmaValid;
+  BOOLEAN                               Lba48Bit;
+
+  //
+  // Cached data for ATA identify data
+  //
+  ATA_IDENTIFY_DATA                     *IdentifyData;
+
+  EFI_UNICODE_STRING_TABLE              *ControllerNameTable;
+  CHAR16                                ModelName[MAX_MODEL_NAME_LEN + 1];
+
+  LIST_ENTRY                            AtaTaskList;
+  LIST_ENTRY                            AtaSubTaskList;
+  BOOLEAN                               Abort;
+} ATA_DEVICE;
+
+//
+// Sub-Task for the non blocking I/O
+//
+typedef struct {
+  UINT32                            Signature;
+  ATA_DEVICE                        *AtaDevice;
+  EFI_BLOCK_IO2_TOKEN               *Token;
+  UINTN                             *UnsignalledEventCount;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  Packet;
+  BOOLEAN                           *IsError;// Indicate whether meeting error during source allocation for new task.
+  LIST_ENTRY                        TaskEntry;
+} ATA_BUS_ASYN_SUB_TASK;
+
+//
+// Task for the non blocking I/O
+//
+typedef struct {
+  UINT32                            Signature;
+  EFI_BLOCK_IO2_TOKEN               *Token;
+  ATA_DEVICE                        *AtaDevice;
+  UINT8                             *Buffer;
+  EFI_LBA                           StartLba;
+  UINTN                             NumberOfBlocks;
+  BOOLEAN                           IsWrite;
+  LIST_ENTRY                        TaskEntry;
+} ATA_BUS_ASYN_TASK;
+
+#define ATA_DEVICE_FROM_BLOCK_IO(a)         CR (a, ATA_DEVICE, BlockIo, ATA_DEVICE_SIGNATURE)
+#define ATA_DEVICE_FROM_BLOCK_IO2(a)        CR (a, ATA_DEVICE, BlockIo2, ATA_DEVICE_SIGNATURE)
+#define ATA_DEVICE_FROM_DISK_INFO(a)        CR (a, ATA_DEVICE, DiskInfo, ATA_DEVICE_SIGNATURE)
+#define ATA_DEVICE_FROM_STORAGE_SECURITY(a) CR (a, ATA_DEVICE, StorageSecurity, ATA_DEVICE_SIGNATURE)
+#define ATA_ASYN_SUB_TASK_FROM_ENTRY(a)     CR (a, ATA_BUS_ASYN_SUB_TASK, TaskEntry, ATA_SUB_TASK_SIGNATURE)
+#define ATA_ASYN_TASK_FROM_ENTRY(a)         CR (a, ATA_BUS_ASYN_TASK, TaskEntry, ATA_TASK_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL        gAtaBusDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL        gAtaBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL       gAtaBusComponentName2;
+
+/**
+  Allocates an aligned buffer for ATA device.
+
+  This function allocates an aligned buffer for the ATA device to perform
+  ATA pass through operations. The alignment requirement is from ATA pass
+  through interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+  @param  BufferSize        The request buffer size.
+
+  @return A pointer to the aligned buffer or NULL if the allocation fails.
+
+**/
+VOID *
+AllocateAlignedBuffer (
+  IN ATA_DEVICE               *AtaDevice,
+  IN UINTN                    BufferSize
+  );
+
+/**
+  Frees an aligned buffer for ATA device.
+
+  This function frees an aligned buffer for the ATA device to perform
+  ATA pass through operations.
+
+  @param  Buffer            The aligned buffer to be freed.
+  @param  BufferSize        The request buffer size.
+
+**/
+VOID
+FreeAlignedBuffer (
+  IN VOID                     *Buffer,
+  IN UINTN                    BufferSize
+  );
+
+/**
+  Free SubTask.
+
+  @param[in, out]  Task      Pointer to task to be freed.
+
+**/
+VOID
+EFIAPI
+FreeAtaSubTask (
+  IN OUT ATA_BUS_ASYN_SUB_TASK  *Task
+  );
+
+/**
+  Wrapper for EFI_ATA_PASS_THRU_PROTOCOL.ResetDevice().
+
+  This function wraps the ResetDevice() invocation for ATA pass through function
+  for an ATA device.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @return The return status from EFI_ATA_PASS_THRU_PROTOCOL.PassThru().
+
+**/
+EFI_STATUS
+ResetAtaDevice (
+  IN ATA_DEVICE                           *AtaDevice
+  );
+
+
+/**
+  Discovers whether it is a valid ATA device.
+
+  This function issues ATA_CMD_IDENTIFY_DRIVE command to the ATA device to identify it.
+  If the command is executed successfully, it then identifies it and initializes
+  the Media information in Block IO protocol interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @retval EFI_SUCCESS       The device is successfully identified and Media information
+                            is correctly initialized.
+  @return others            Some error occurs when discovering the ATA device.
+
+**/
+EFI_STATUS
+DiscoverAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice
+  );
+
+/**
+  Read or write a number of blocks from ATA device.
+
+  This function performs ATA pass through transactions to read/write data from/to
+  ATA device. It may separate the read/write request into several ATA pass through
+  transactions.
+
+  @param[in, out]  AtaDevice       The ATA child device involved for the operation.
+  @param[in, out]  Buffer          The pointer to the current transaction buffer.
+  @param[in]       StartLba        The starting logical block address to be accessed.
+  @param[in]       NumberOfBlocks  The block number or sector count of the transfer.
+  @param[in]       IsWrite         Indicates whether it is a write operation.
+  @param[in, out]  Token           A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+AccessAtaDevice(
+  IN OUT ATA_DEVICE                 *AtaDevice,
+  IN OUT UINT8                      *Buffer,
+  IN EFI_LBA                        StartLba,
+  IN UINTN                          NumberOfBlocks,
+  IN BOOLEAN                        IsWrite,
+  IN OUT EFI_BLOCK_IO2_TOKEN        *Token
+  );
+
+/**
+  Trust transfer data from/to ATA device.
+
+  This function performs one ATA pass through transaction to do a trust transfer from/to
+  ATA device. It chooses the appropriate ATA command and protocol to invoke PassThru
+  interface of ATA pass through.
+
+  @param  AtaDevice                    The ATA child device involved for the operation.
+  @param  Buffer                       The pointer to the current transaction buffer.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  TransferLength               The block number or sector count of the transfer.
+  @param  IsTrustSend                  Indicates whether it is a trust send operation or not.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  TransferLengthOut            A pointer to a buffer to store the size in bytes of the data
+                                       written to the buffer. Ignore it when IsTrustSend is TRUE.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+EFIAPI
+TrustTransferAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice,
+  IN OUT VOID                       *Buffer,
+  IN UINT8                          SecurityProtocolId,
+  IN UINT16                         SecurityProtocolSpecificData,
+  IN UINTN                          TransferLength,
+  IN BOOLEAN                        IsTrustSend,
+  IN UINT64                         Timeout,
+  OUT UINTN                         *TransferLengthOut
+  );
+
+//
+// Protocol interface prototypes
+//
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+/**
+  Reset the Block Device.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Lba        The starting Logical Block Address to read from
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the destination buffer for the data. The caller is
+                     responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    The media ID that the write request is for.
+  @param  Lba        The starting logical block address to be written. The caller is
+                     responsible for writing to only legitimate locations.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  );
+
+
+/**
+  Flush the Block Device.
+
+  @param  This              Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  );
+
+/**
+  Reset the Block Device throught Block I/O2 protocol.
+
+  @param[in]  This                 Indicates a pointer to the calling context.
+  @param[in]  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoResetEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    Id of the media, changes every time the media is replaced.
+  @param[in]       Lba        The starting Logical Block Address to read from.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[out]      Buffer     A pointer to the destination buffer for the data. The caller is
+                              responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The read request was queued if Event is not NULL.
+                                The data was read correctly from the device if
+                                the Event is NULL.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing
+                                the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of the
+                                intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack
+                                of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReadBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    The media ID that the write request is for.
+  @param[in]       Lba        The starting logical block address to be written. The
+                              caller is responsible for writing to only legitimate
+                              locations.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[in]       Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoWriteBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  );
+
+/**
+  Flush the Block Device.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoFlushBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token
+  );
+
+/**
+  Terminate any in-flight non-blocking I/O requests by signaling an EFI_ABORTED
+  in the TransactionStatus member of the EFI_BLOCK_IO2_TOKEN for the non-blocking
+  I/O. After that it is safe to free any Token or Buffer data structures that
+  were allocated to initiate the non-blockingI/O requests that were in-flight for
+  this device.
+
+  @param[in]  AtaDevice     The ATA child device involved for the operation.
+
+**/
+VOID
+EFIAPI
+AtaTerminateNonBlockingTask (
+  IN ATA_DEVICE               *AtaDevice
+  );
+
+/**
+  Provides inquiry information for the controller type.
+
+  This function is used by the IDE bus driver to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This             Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData      Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize  Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  );
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used by the IDE bus driver to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  );
+
+
+/**
+  Provides sense data information for the controller type.
+
+  This function is used by the IDE bus driver to get sense data.
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This             Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData        Pointer to the SenseData.
+  @param[in, out] SenseDataSize    Size of SenseData in bytes.
+  @param[out]     SenseDataNumber  Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  );
+
+
+/**
+  This function is used by the IDE bus driver to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  );
+
+/**
+  Send a security protocol command to a device that receives data and/or the result
+  of one or more commands sent by SendData.
+
+  The ReceiveData function sends a security protocol command to the given MediaId.
+  The security protocol command sent is defined by SecurityProtocolId and contains
+  the security protocol specific data SecurityProtocolSpecificData. The function
+  returns the data from the security protocol command in PayloadBuffer.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL IN command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED RECEIVE commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero.
+
+  If the PayloadBufferSize is zero, the security protocol command is sent using the
+  Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBufferSize is too small to store the available data from the security
+  protocol command, the function shall copy PayloadBufferSize bytes into the
+  PayloadBuffer and return EFI_WARN_BUFFER_TOO_SMALL.
+
+  If PayloadBuffer or PayloadTransferSize is NULL and PayloadBufferSize is non-zero,
+  the function shall return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function shall
+  return EFI_UNSUPPORTED. If there is no media in the device, the function returns
+  EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the device,
+  the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall
+  return EFI_SUCCESS. If the security protocol command completes with an error, the
+  function shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command. The caller is responsible for having
+                                       either implicit or explicit ownership of the buffer.
+  @param  PayloadTransferSize          A pointer to a buffer to store the size in bytes of the
+                                       data written to the payload data buffer.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL    The PayloadBufferSize was too small to store the available
+                                       data from the device. The PayloadBuffer contains the truncated data.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer or PayloadTransferSize is NULL and
+                                       PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaStorageSecurityReceiveData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  OUT VOID                                    *PayloadBuffer,
+  OUT UINTN                                   *PayloadTransferSize
+  );
+
+/**
+  Send a security protocol command to a device.
+
+  The SendData function sends a security protocol command containing the payload
+  PayloadBuffer to the given MediaId. The security protocol command sent is
+  defined by SecurityProtocolId and contains the security protocol specific data
+  SecurityProtocolSpecificData. If the underlying protocol command requires a
+  specific padding for the command payload, the SendData function shall add padding
+  bytes to the command payload to satisfy the padding requirements.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL OUT command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED SEND commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero. If the PayloadBufferSize is zero, the security protocol command is
+  sent using the Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBuffer is NULL and PayloadBufferSize is non-zero, the function shall
+  return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function
+  shall return EFI_UNSUPPORTED. If there is no media in the device, the function
+  returns EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the
+  device, the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall return
+  EFI_SUCCESS. If the security protocol command completes with an error, the function
+  shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer is NULL and PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaStorageSecuritySendData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  IN VOID                                     *PayloadBuffer
+  );
+
+/**
+  Send TPer Reset command to reset eDrive to lock all protected bands.
+  Typically, there are 2 mechanism for resetting eDrive. They are:
+  1. TPer Reset through IEEE 1667 protocol.
+  2. TPer Reset through native TCG protocol.
+  This routine will detect what protocol the attached eDrive comform to, TCG or
+  IEEE 1667 protocol. Then send out TPer Reset command separately.
+
+  @param[in] AtaDevice    ATA_DEVICE pointer.
+
+**/
+VOID
+InitiateTPerReset (
+  IN   ATA_DEVICE       *AtaDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.inf b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.inf
new file mode 100644
index 0000000000..4aab75bab7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.inf
@@ -0,0 +1,77 @@
+## @file
+#  ATA Bus driver to enumerate and identfy ATA devices.
+#
+#  This driver follows UEFI driver model and layers on ATA Pass Thru protocol defined
+#  in UEFI spec 2.2. It installs Block IO and Disk Info protocol for each ATA device
+#  it enumerates and identifies successfully.
+#
+#  Copyright (c) 2009 - 2015, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = AtaBusDxe
+  MODULE_UNI_FILE                = AtaBusDxe.uni
+  FILE_GUID                      = 19DF145A-B1D4-453f-8507-38816676D7F6
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeAtaBus
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gAtaBusDriverBinding
+#  COMPONENT_NAME                =  gAtaBusComponentName
+#  COMPONENT_NAME2               =  gAtaBusComponentName2
+#
+#
+
+[Sources]
+  AtaBus.h
+  AtaBus.c
+  AtaPassThruExecute.c
+  ComponentName.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  DevicePathLib
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiLib
+  BaseLib
+  UefiDriverEntryPoint
+  DebugLib
+  TimerLib
+  ReportStatusCodeLib
+
+[Guids]
+  gEfiDiskInfoIdeInterfaceGuid                  ## SOMETIMES_PRODUCES ## UNDEFINED
+  gEfiDiskInfoAhciInterfaceGuid                 ## SOMETIMES_PRODUCES ## UNDEFINED
+
+[Protocols]
+  gEfiDiskInfoProtocolGuid                      ## BY_START
+  gEfiBlockIoProtocolGuid                       ## BY_START
+  gEfiBlockIo2ProtocolGuid                      ## BY_START
+  ## TO_START
+  ## BY_START
+  gEfiDevicePathProtocolGuid
+  gEfiAtaPassThruProtocolGuid                   ## TO_START
+  gEfiStorageSecurityCommandProtocolGuid        ## BY_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  AtaBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.uni b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.uni
new file mode 100644
index 0000000000..45a8be7713
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxeExtra.uni b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxeExtra.uni
new file mode 100644
index 0000000000..269bbf9780
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaPassThruExecute.c b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaPassThruExecute.c
new file mode 100644
index 0000000000..a3008f92d9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaPassThruExecute.c
@@ -0,0 +1,1072 @@
+/** @file
+  This file implements ATA pass through transaction for ATA bus driver.
+
+  This file implements the low level execution of ATA pass through transaction.
+  It transforms the high level identity, read/write, reset command to ATA pass
+  through command and protocol.
+
+  NOTE: This file also implements the StorageSecurityCommandProtocol(SSP). For input
+  parameter SecurityProtocolSpecificData, ATA spec has no explicitly definition
+  for Security Protocol Specific layout. This implementation uses big endian for
+  Cylinder register.
+
+  Copyright (c) 2009 - 2013, 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 "AtaBus.h"
+
+#define ATA_CMD_TRUST_NON_DATA    0x5B
+#define ATA_CMD_TRUST_RECEIVE     0x5C
+#define ATA_CMD_TRUST_RECEIVE_DMA 0x5D
+#define ATA_CMD_TRUST_SEND        0x5E
+#define ATA_CMD_TRUST_SEND_DMA    0x5F
+
+//
+// Look up table (UdmaValid, IsWrite) for EFI_ATA_PASS_THRU_CMD_PROTOCOL
+//
+EFI_ATA_PASS_THRU_CMD_PROTOCOL mAtaPassThruCmdProtocols[][2] = {
+  {
+    EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN,
+    EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_OUT
+  },
+  {
+    EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_IN,
+    EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_OUT,
+  }
+};
+
+//
+// Look up table (UdmaValid, Lba48Bit, IsIsWrite) for ATA_CMD
+//
+UINT8 mAtaCommands[][2][2] = {
+  {
+    {
+      ATA_CMD_READ_SECTORS,            // 28-bit LBA; PIO read
+      ATA_CMD_WRITE_SECTORS            // 28-bit LBA; PIO write
+    },
+    {
+      ATA_CMD_READ_SECTORS_EXT,        // 48-bit LBA; PIO read
+      ATA_CMD_WRITE_SECTORS_EXT        // 48-bit LBA; PIO write
+    }
+  },
+  {
+    {
+      ATA_CMD_READ_DMA,                // 28-bit LBA; DMA read
+      ATA_CMD_WRITE_DMA                // 28-bit LBA; DMA write
+    },
+    {
+      ATA_CMD_READ_DMA_EXT,            // 48-bit LBA; DMA read
+      ATA_CMD_WRITE_DMA_EXT            // 48-bit LBA; DMA write
+    }
+  }
+};
+
+//
+// Look up table (UdmaValid, IsTrustSend) for ATA_CMD
+//
+UINT8 mAtaTrustCommands[2][2] = {
+  {
+    ATA_CMD_TRUST_RECEIVE,            // PIO read
+    ATA_CMD_TRUST_SEND                // PIO write
+  },
+  {
+    ATA_CMD_TRUST_RECEIVE_DMA,        // DMA read
+    ATA_CMD_TRUST_SEND_DMA            // DMA write
+  }
+};
+
+
+//
+// Look up table (Lba48Bit) for maximum transfer block number
+//
+UINTN mMaxTransferBlockNumber[] = {
+  MAX_28BIT_TRANSFER_BLOCK_NUM,
+  MAX_48BIT_TRANSFER_BLOCK_NUM
+};
+
+
+/**
+  Wrapper for EFI_ATA_PASS_THRU_PROTOCOL.PassThru().
+
+  This function wraps the PassThru() invocation for ATA pass through function
+  for an ATA device. It assembles the ATA pass through command packet for ATA
+  transaction.
+
+  @param[in, out]  AtaDevice   The ATA child device involved for the operation.
+  @param[in, out]  TaskPacket  Pointer to a Pass Thru Command Packet. Optional,
+                               if it is NULL, blocking mode, and use the packet
+                               in AtaDevice. If it is not NULL, non blocking mode,
+                               and pass down this Packet.
+  @param[in, out]  Event       If Event is NULL, then blocking I/O is performed.
+                               If Event is not NULL and non-blocking I/O is
+                               supported,then non-blocking I/O is performed,
+                               and Event will be signaled when the write
+                               request is completed.
+
+  @return The return status from EFI_ATA_PASS_THRU_PROTOCOL.PassThru().
+
+**/
+EFI_STATUS
+AtaDevicePassThru (
+  IN OUT ATA_DEVICE                       *AtaDevice,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *TaskPacket, OPTIONAL
+  IN OUT EFI_EVENT                        Event OPTIONAL
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_ATA_PASS_THRU_PROTOCOL              *AtaPassThru;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET        *Packet;
+
+  //
+  // Assemble packet. If it is non blocking mode, the Ata driver should keep each
+  // subtask and clean them when the event is signaled.
+  //
+  if (TaskPacket != NULL) {
+    Packet = TaskPacket;
+    Packet->Asb = AllocateAlignedBuffer (AtaDevice, sizeof (EFI_ATA_STATUS_BLOCK));
+    if (Packet->Asb == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    CopyMem (Packet->Asb, AtaDevice->Asb, sizeof (EFI_ATA_STATUS_BLOCK));
+    Packet->Acb = AllocateCopyPool (sizeof (EFI_ATA_COMMAND_BLOCK), &AtaDevice->Acb);
+  } else {
+    Packet = &AtaDevice->Packet;
+    Packet->Asb = AtaDevice->Asb;
+    Packet->Acb = &AtaDevice->Acb;
+  }
+
+  AtaPassThru = AtaDevice->AtaBusDriverData->AtaPassThru;
+
+  Status = AtaPassThru->PassThru (
+                          AtaPassThru,
+                          AtaDevice->Port,
+                          AtaDevice->PortMultiplierPort,
+                          Packet,
+                          Event
+                          );
+  //
+  // Ensure ATA pass through caller and callee have the same
+  // interpretation of ATA pass through protocol.
+  //
+  ASSERT (Status != EFI_INVALID_PARAMETER);
+  ASSERT (Status != EFI_BAD_BUFFER_SIZE);
+
+  return Status;
+}
+
+
+/**
+  Wrapper for EFI_ATA_PASS_THRU_PROTOCOL.ResetDevice().
+
+  This function wraps the ResetDevice() invocation for ATA pass through function
+  for an ATA device.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @return The return status from EFI_ATA_PASS_THRU_PROTOCOL.PassThru().
+
+**/
+EFI_STATUS
+ResetAtaDevice (
+  IN ATA_DEVICE                           *AtaDevice
+  )
+{
+  EFI_ATA_PASS_THRU_PROTOCOL              *AtaPassThru;
+
+  AtaPassThru = AtaDevice->AtaBusDriverData->AtaPassThru;
+
+  //
+  // Report Status Code to indicate reset happens
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_RESET),
+    AtaDevice->AtaBusDriverData->ParentDevicePath
+    );
+
+  return AtaPassThru->ResetDevice (
+                        AtaPassThru,
+                        AtaDevice->Port,
+                        AtaDevice->PortMultiplierPort
+                        );
+}
+
+
+/**
+  Prints ATA model name to ATA device structure.
+
+  This function converts ATA device model name from ATA identify data
+  to a string in ATA device structure. It needs to change the character
+  order in the original model name string.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+**/
+VOID
+PrintAtaModelName (
+  IN OUT ATA_DEVICE  *AtaDevice
+  )
+{
+  UINTN   Index;
+  CHAR8   *Source;
+  CHAR16  *Destination;
+
+  Source = AtaDevice->IdentifyData->ModelName;
+  Destination = AtaDevice->ModelName;
+
+  //
+  // Swap the byte order in the original module name.
+  //
+  for (Index = 0; Index < MAX_MODEL_NAME_LEN; Index += 2) {
+    Destination[Index]      = Source[Index + 1];
+    Destination[Index + 1]  = Source[Index];
+  }
+  AtaDevice->ModelName[MAX_MODEL_NAME_LEN] = L'\0';
+}
+
+
+/**
+  Gets ATA device Capacity according to ATA 6.
+
+  This function returns the capacity of the ATA device if it follows
+  ATA 6 to support 48 bit addressing.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @return The capacity of the ATA device or 0 if the device does not support
+          48-bit addressing defined in ATA 6.
+
+**/
+EFI_LBA
+GetAtapi6Capacity (
+  IN ATA_DEVICE                 *AtaDevice
+  )
+{
+  EFI_LBA                       Capacity;
+  EFI_LBA                       TmpLba;
+  UINTN                         Index;
+  ATA_IDENTIFY_DATA             *IdentifyData;
+
+  IdentifyData = AtaDevice->IdentifyData;
+  if ((IdentifyData->command_set_supported_83 & BIT10) == 0) {
+    //
+    // The device doesn't support 48 bit addressing
+    //
+    return 0;
+  }
+
+  //
+  // 48 bit address feature set is supported, get maximum capacity
+  //
+  Capacity = 0;
+  for (Index = 0; Index < 4; Index++) {
+    //
+    // Lower byte goes first: word[100] is the lowest word, word[103] is highest
+    //
+    TmpLba = IdentifyData->maximum_lba_for_48bit_addressing[Index];
+    Capacity |= LShiftU64 (TmpLba, 16 * Index);
+  }
+
+  return Capacity;
+}
+
+
+/**
+  Identifies ATA device via the Identify data.
+
+  This function identifies the ATA device and initializes the Media information in
+  Block IO protocol interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @retval EFI_UNSUPPORTED   The device is not a valid ATA device (hard disk).
+  @retval EFI_SUCCESS       The device is successfully identified and Media information
+                            is correctly initialized.
+
+**/
+EFI_STATUS
+IdentifyAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice
+  )
+{
+  ATA_IDENTIFY_DATA                 *IdentifyData;
+  EFI_BLOCK_IO_MEDIA                *BlockMedia;
+  EFI_LBA                           Capacity;
+  UINT16                            PhyLogicSectorSupport;
+  UINT16                            UdmaMode;
+
+  IdentifyData = AtaDevice->IdentifyData;
+
+  if ((IdentifyData->config & BIT15) != 0) {
+    //
+    // This is not an hard disk
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  DEBUG ((EFI_D_INFO, "AtaBus - Identify Device: Port %x PortMultiplierPort %x\n", AtaDevice->Port, AtaDevice->PortMultiplierPort));
+
+  //
+  // Check whether the WORD 88 (supported UltraDMA by drive) is valid
+  //
+  if ((IdentifyData->field_validity & BIT2) != 0) {
+    UdmaMode = IdentifyData->ultra_dma_mode;
+    if ((UdmaMode & (BIT0 | BIT1 | BIT2 | BIT3 | BIT4 | BIT5 | BIT6)) != 0) {
+      //
+      // If BIT0~BIT6 is selected, then UDMA is supported
+      //
+      AtaDevice->UdmaValid = TRUE;
+    }
+  }
+
+  Capacity = GetAtapi6Capacity (AtaDevice);
+  if (Capacity > MAX_28BIT_ADDRESSING_CAPACITY) {
+    //
+    // Capacity exceeds 120GB. 48-bit addressing is really needed
+    //
+    AtaDevice->Lba48Bit = TRUE;
+  } else {
+    //
+    // This is a hard disk <= 120GB capacity, treat it as normal hard disk
+    //
+    Capacity = ((UINT32)IdentifyData->user_addressable_sectors_hi << 16) | IdentifyData->user_addressable_sectors_lo;
+    AtaDevice->Lba48Bit = FALSE;
+  }
+
+  //
+  // Block Media Information:
+  //
+  BlockMedia = &AtaDevice->BlockMedia;
+  BlockMedia->LastBlock = Capacity - 1;
+  BlockMedia->IoAlign = AtaDevice->AtaBusDriverData->AtaPassThru->Mode->IoAlign;
+  //
+  // Check whether Long Physical Sector Feature is supported
+  //
+  PhyLogicSectorSupport = IdentifyData->phy_logic_sector_support;
+  if ((PhyLogicSectorSupport & (BIT14 | BIT15)) == BIT14) {
+    //
+    // Check whether one physical block contains multiple physical blocks
+    //
+    if ((PhyLogicSectorSupport & BIT13) != 0) {
+      BlockMedia->LogicalBlocksPerPhysicalBlock = (UINT32) (1 << (PhyLogicSectorSupport & 0x000f));
+      //
+      // Check lowest alignment of logical blocks within physical block
+      //
+      if ((IdentifyData->alignment_logic_in_phy_blocks & (BIT14 | BIT15)) == BIT14) {
+        BlockMedia->LowestAlignedLba = (EFI_LBA) ((BlockMedia->LogicalBlocksPerPhysicalBlock - ((UINT32)IdentifyData->alignment_logic_in_phy_blocks & 0x3fff)) %
+          BlockMedia->LogicalBlocksPerPhysicalBlock);
+      }
+    }
+    //
+    // Check logical block size
+    //
+    if ((PhyLogicSectorSupport & BIT12) != 0) {
+      BlockMedia->BlockSize = (UINT32) (((IdentifyData->logic_sector_size_hi << 16) | IdentifyData->logic_sector_size_lo) * sizeof (UINT16));
+    }
+    AtaDevice->BlockIo.Revision = EFI_BLOCK_IO_PROTOCOL_REVISION2;
+  }
+  //
+  // Get ATA model name from identify data structure.
+  //
+  PrintAtaModelName (AtaDevice);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Discovers whether it is a valid ATA device.
+
+  This function issues ATA_CMD_IDENTIFY_DRIVE command to the ATA device to identify it.
+  If the command is executed successfully, it then identifies it and initializes
+  the Media information in Block IO protocol interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @retval EFI_SUCCESS       The device is successfully identified and Media information
+                            is correctly initialized.
+  @return others            Some error occurs when discovering the ATA device.
+
+**/
+EFI_STATUS
+DiscoverAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_ATA_COMMAND_BLOCK             *Acb;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  *Packet;
+  UINTN                             Retry;
+
+  //
+  // Prepare for ATA command block.
+  //
+  Acb = ZeroMem (&AtaDevice->Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
+  Acb->AtaCommand = ATA_CMD_IDENTIFY_DRIVE;
+  Acb->AtaDeviceHead = (UINT8) (BIT7 | BIT6 | BIT5 | (AtaDevice->PortMultiplierPort << 4));
+
+  //
+  // Prepare for ATA pass through packet.
+  //
+  Packet = ZeroMem (&AtaDevice->Packet, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
+  Packet->InDataBuffer = AtaDevice->IdentifyData;
+  Packet->InTransferLength = sizeof (ATA_IDENTIFY_DATA);
+  Packet->Protocol = EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN;
+  Packet->Length   = EFI_ATA_PASS_THRU_LENGTH_BYTES | EFI_ATA_PASS_THRU_LENGTH_SECTOR_COUNT;
+  Packet->Timeout  = ATA_TIMEOUT;
+
+  Retry = MAX_RETRY_TIMES;
+  do {
+    Status = AtaDevicePassThru (AtaDevice, NULL, NULL);
+    if (!EFI_ERROR (Status)) {
+      //
+      // The command is issued successfully
+      //
+      Status = IdentifyAtaDevice (AtaDevice);
+      return Status;
+    }
+  } while (Retry-- > 0);
+
+  return Status;
+}
+
+/**
+  Transfer data from ATA device.
+
+  This function performs one ATA pass through transaction to transfer data from/to
+  ATA device. It chooses the appropriate ATA command and protocol to invoke PassThru
+  interface of ATA pass through.
+
+  @param[in, out]  AtaDevice       The ATA child device involved for the operation.
+  @param[in, out]  TaskPacket      Pointer to a Pass Thru Command Packet. Optional,
+                                   if it is NULL, blocking mode, and use the packet
+                                   in AtaDevice. If it is not NULL, non blocking mode,
+                                   and pass down this Packet.
+  @param[in, out]  Buffer          The pointer to the current transaction buffer.
+  @param[in]       StartLba        The starting logical block address to be accessed.
+  @param[in]       TransferLength  The block number or sector count of the transfer.
+  @param[in]       IsWrite         Indicates whether it is a write operation.
+  @param[in]       Event           If Event is NULL, then blocking I/O is performed.
+                                   If Event is not NULL and non-blocking I/O is
+                                   supported,then non-blocking I/O is performed,
+                                   and Event will be signaled when the write
+                                   request is completed.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+TransferAtaDevice (
+  IN OUT ATA_DEVICE                       *AtaDevice,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *TaskPacket, OPTIONAL
+  IN OUT VOID                             *Buffer,
+  IN EFI_LBA                              StartLba,
+  IN UINT32                               TransferLength,
+  IN BOOLEAN                              IsWrite,
+  IN EFI_EVENT                            Event OPTIONAL
+  )
+{
+  EFI_ATA_COMMAND_BLOCK             *Acb;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  *Packet;
+
+  //
+  // Ensure AtaDevice->UdmaValid, AtaDevice->Lba48Bit and IsWrite are valid boolean values
+  //
+  ASSERT ((UINTN) AtaDevice->UdmaValid < 2);
+  ASSERT ((UINTN) AtaDevice->Lba48Bit < 2);
+  ASSERT ((UINTN) IsWrite < 2);
+  //
+  // Prepare for ATA command block.
+  //
+  Acb = ZeroMem (&AtaDevice->Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
+  Acb->AtaCommand = mAtaCommands[AtaDevice->UdmaValid][AtaDevice->Lba48Bit][IsWrite];
+  Acb->AtaSectorNumber = (UINT8) StartLba;
+  Acb->AtaCylinderLow = (UINT8) RShiftU64 (StartLba, 8);
+  Acb->AtaCylinderHigh = (UINT8) RShiftU64 (StartLba, 16);
+  Acb->AtaDeviceHead = (UINT8) (BIT7 | BIT6 | BIT5 | (AtaDevice->PortMultiplierPort << 4));
+  Acb->AtaSectorCount = (UINT8) TransferLength;
+  if (AtaDevice->Lba48Bit) {
+    Acb->AtaSectorNumberExp = (UINT8) RShiftU64 (StartLba, 24);
+    Acb->AtaCylinderLowExp = (UINT8) RShiftU64 (StartLba, 32);
+    Acb->AtaCylinderHighExp = (UINT8) RShiftU64 (StartLba, 40);
+    Acb->AtaSectorCountExp = (UINT8) (TransferLength >> 8);
+  } else {
+    Acb->AtaDeviceHead = (UINT8) (Acb->AtaDeviceHead | RShiftU64 (StartLba, 24));
+  }
+
+  //
+  // Prepare for ATA pass through packet.
+  //
+  if (TaskPacket != NULL) {
+    Packet = ZeroMem (TaskPacket, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
+  } else {
+    Packet = ZeroMem (&AtaDevice->Packet, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
+  }
+
+  if (IsWrite) {
+    Packet->OutDataBuffer = Buffer;
+    Packet->OutTransferLength = TransferLength;
+  } else {
+    Packet->InDataBuffer = Buffer;
+    Packet->InTransferLength = TransferLength;
+  }
+
+  Packet->Protocol = mAtaPassThruCmdProtocols[AtaDevice->UdmaValid][IsWrite];
+  Packet->Length = EFI_ATA_PASS_THRU_LENGTH_SECTOR_COUNT;
+  //
+  // |------------------------|-----------------|------------------------|-----------------|
+  // | ATA PIO Transfer Mode  |  Transfer Rate  | ATA DMA Transfer Mode  |  Transfer Rate  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 0       |  3.3Mbytes/sec  | Single-word DMA Mode 0 |  2.1Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 1       |  5.2Mbytes/sec  | Single-word DMA Mode 1 |  4.2Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 2       |  8.3Mbytes/sec  | Single-word DMA Mode 2 |  8.4Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 3       | 11.1Mbytes/sec  | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 4       | 16.6Mbytes/sec  | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  //
+  // As AtaBus is used to manage ATA devices, we have to use the lowest transfer rate to
+  // calculate the possible maximum timeout value for each read/write operation.
+  // The timout value is rounded up to nearest integar and here an additional 30s is added
+  // to follow ATA spec in which it mentioned that the device may take up to 30s to respond
+  // commands in the Standby/Idle mode.
+  //
+  if (AtaDevice->UdmaValid) {
+    //
+    // Calculate the maximum timeout value for DMA read/write operation.
+    //
+    Packet->Timeout  = EFI_TIMER_PERIOD_SECONDS (DivU64x32 (MultU64x32 (TransferLength, AtaDevice->BlockMedia.BlockSize), 2100000) + 31);
+  } else {
+    //
+    // Calculate the maximum timeout value for PIO read/write operation
+    //
+    Packet->Timeout  = EFI_TIMER_PERIOD_SECONDS (DivU64x32 (MultU64x32 (TransferLength, AtaDevice->BlockMedia.BlockSize), 3300000) + 31);
+  }
+
+  return AtaDevicePassThru (AtaDevice, TaskPacket, Event);
+}
+
+/**
+  Free SubTask.
+
+  @param[in, out]  Task      Pointer to task to be freed.
+
+**/
+VOID
+EFIAPI
+FreeAtaSubTask (
+  IN OUT ATA_BUS_ASYN_SUB_TASK  *Task
+  )
+{
+  if (Task->Packet.Asb != NULL) {
+    FreeAlignedBuffer (Task->Packet.Asb, sizeof (EFI_ATA_STATUS_BLOCK));
+  }
+  if (Task->Packet.Acb != NULL) {
+    FreePool (Task->Packet.Acb);
+  }
+
+  FreePool (Task);
+}
+
+/**
+  Terminate any in-flight non-blocking I/O requests by signaling an EFI_ABORTED
+  in the TransactionStatus member of the EFI_BLOCK_IO2_TOKEN for the non-blocking
+  I/O. After that it is safe to free any Token or Buffer data structures that
+  were allocated to initiate the non-blockingI/O requests that were in-flight for
+  this device.
+
+  @param[in]  AtaDevice     The ATA child device involved for the operation.
+
+**/
+VOID
+EFIAPI
+AtaTerminateNonBlockingTask (
+  IN ATA_DEVICE               *AtaDevice
+  )
+{
+  BOOLEAN               SubTaskEmpty;
+  EFI_TPL               OldTpl;
+  ATA_BUS_ASYN_TASK     *AtaTask;
+  LIST_ENTRY            *Entry;
+  LIST_ENTRY            *List;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  //
+  // Abort all executing tasks from now.
+  //
+  AtaDevice->Abort = TRUE;
+
+  List = &AtaDevice->AtaTaskList;
+  for (Entry = GetFirstNode (List); !IsNull (List, Entry);) {
+    AtaTask  = ATA_ASYN_TASK_FROM_ENTRY (Entry);
+    AtaTask->Token->TransactionStatus = EFI_ABORTED;
+    gBS->SignalEvent (AtaTask->Token->Event);
+
+    Entry = RemoveEntryList (Entry);
+    FreePool (AtaTask);
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  do {
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    //
+    // Wait for executing subtasks done.
+    //
+    SubTaskEmpty = IsListEmpty (&AtaDevice->AtaSubTaskList);
+    gBS->RestoreTPL (OldTpl);
+  } while (!SubTaskEmpty);
+
+  //
+  // Aborting operation has been done. From now on, don't need to abort normal operation.
+  //  
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  AtaDevice->Abort = FALSE;
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Call back funtion when the event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the
+                        Event.
+
+**/
+VOID
+EFIAPI
+AtaNonBlockingCallBack (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  )
+{
+  ATA_BUS_ASYN_SUB_TASK *Task;
+  ATA_BUS_ASYN_TASK     *AtaTask;
+  ATA_DEVICE            *AtaDevice;
+  LIST_ENTRY            *Entry;
+  EFI_STATUS            Status;
+
+  Task = (ATA_BUS_ASYN_SUB_TASK *) Context;
+  gBS->CloseEvent (Event);
+
+  AtaDevice = Task->AtaDevice;
+
+  //
+  // Check the command status.
+  // If there is error during the sub task source allocation, the error status
+  // should be returned to the caller directly, so here the Task->Token may already
+  // be deleted by the caller and no need to update the status.
+  //
+  if ((!(*Task->IsError)) && ((Task->Packet.Asb->AtaStatus & 0x01) == 0x01)) {
+    Task->Token->TransactionStatus = EFI_DEVICE_ERROR;
+  }
+
+  if (AtaDevice->Abort) {
+    Task->Token->TransactionStatus = EFI_ABORTED;
+  }
+
+  DEBUG ((
+    EFI_D_BLKIO,
+    "NON-BLOCKING EVENT FINISHED!- STATUS = %r\n",
+    Task->Token->TransactionStatus
+    ));
+
+  //
+  // Reduce the SubEventCount, till it comes to zero.
+  //
+  (*Task->UnsignalledEventCount) --;
+  DEBUG ((EFI_D_BLKIO, "UnsignalledEventCount = %d\n", *Task->UnsignalledEventCount));
+
+  //
+  // Remove the SubTask from the Task list.
+  //
+  RemoveEntryList (&Task->TaskEntry);
+  if ((*Task->UnsignalledEventCount) == 0) {
+    //
+    // All Sub tasks are done, then signal the upper layer event.
+    // Except there is error during the sub task source allocation.
+    //
+    if (!(*Task->IsError)) {
+      gBS->SignalEvent (Task->Token->Event);
+      DEBUG ((EFI_D_BLKIO, "Signal the upper layer event!\n"));
+    }
+
+    FreePool (Task->UnsignalledEventCount);
+    FreePool (Task->IsError);
+
+
+    //
+    // Finish all subtasks and move to the next task in AtaTaskList.
+    //
+    if (!IsListEmpty (&AtaDevice->AtaTaskList)) {
+      Entry   = GetFirstNode (&AtaDevice->AtaTaskList);
+      AtaTask = ATA_ASYN_TASK_FROM_ENTRY (Entry);
+      DEBUG ((EFI_D_BLKIO, "Start to embark a new Ata Task\n"));
+      DEBUG ((EFI_D_BLKIO, "AtaTask->NumberOfBlocks = %x; AtaTask->Token=%x\n", AtaTask->NumberOfBlocks, AtaTask->Token));
+      Status = AccessAtaDevice (
+                 AtaTask->AtaDevice,
+                 AtaTask->Buffer,
+                 AtaTask->StartLba,
+                 AtaTask->NumberOfBlocks,
+                 AtaTask->IsWrite,
+                 AtaTask->Token
+                 );
+      if (EFI_ERROR (Status)) {
+        AtaTask->Token->TransactionStatus = Status;
+        gBS->SignalEvent (AtaTask->Token->Event);
+      }
+      RemoveEntryList (Entry);
+      FreePool (AtaTask);
+    }
+  }
+
+  DEBUG ((
+    EFI_D_BLKIO,
+    "PACKET INFO: Write=%s, Length=%x, LowCylinder=%x, HighCylinder=%x, SectionNumber=%x\n",
+    Task->Packet.OutDataBuffer != NULL ? L"YES" : L"NO",
+    Task->Packet.OutDataBuffer != NULL ? Task->Packet.OutTransferLength : Task->Packet.InTransferLength,
+    Task->Packet.Acb->AtaCylinderLow,
+    Task->Packet.Acb->AtaCylinderHigh,
+    Task->Packet.Acb->AtaSectorCount
+    ));
+
+  //
+  // Free the buffer of SubTask.
+  //
+  FreeAtaSubTask (Task);
+}
+
+/**
+  Read or write a number of blocks from ATA device.
+
+  This function performs ATA pass through transactions to read/write data from/to
+  ATA device. It may separate the read/write request into several ATA pass through
+  transactions.
+
+  @param[in, out]  AtaDevice       The ATA child device involved for the operation.
+  @param[in, out]  Buffer          The pointer to the current transaction buffer.
+  @param[in]       StartLba        The starting logical block address to be accessed.
+  @param[in]       NumberOfBlocks  The block number or sector count of the transfer.
+  @param[in]       IsWrite         Indicates whether it is a write operation.
+  @param[in, out]  Token           A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+AccessAtaDevice(
+  IN OUT ATA_DEVICE                 *AtaDevice,
+  IN OUT UINT8                      *Buffer,
+  IN EFI_LBA                        StartLba,
+  IN UINTN                          NumberOfBlocks,
+  IN BOOLEAN                        IsWrite,
+  IN OUT EFI_BLOCK_IO2_TOKEN        *Token
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             MaxTransferBlockNumber;
+  UINTN                             TransferBlockNumber;
+  UINTN                             BlockSize;
+  ATA_BUS_ASYN_SUB_TASK             *SubTask;
+  UINTN                             *EventCount;
+  UINTN                             TempCount;
+  ATA_BUS_ASYN_TASK                 *AtaTask;
+  EFI_EVENT                         SubEvent;
+  UINTN                             Index;
+  BOOLEAN                           *IsError;
+  EFI_TPL                           OldTpl;
+
+  TempCount  = 0;
+  Status     = EFI_SUCCESS;
+  EventCount = NULL;
+  IsError    = NULL;
+  Index      = 0;
+  SubTask    = NULL;
+  SubEvent   = NULL;
+  AtaTask    = NULL;
+
+  //
+  // Ensure AtaDevice->Lba48Bit is a valid boolean value
+  //
+  ASSERT ((UINTN) AtaDevice->Lba48Bit < 2);
+  MaxTransferBlockNumber = mMaxTransferBlockNumber[AtaDevice->Lba48Bit];
+  BlockSize              = AtaDevice->BlockMedia.BlockSize;
+
+  //
+  // Initial the return status and shared account for Non Blocking.
+  //
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+    if (!IsListEmpty (&AtaDevice->AtaSubTaskList)) {
+      AtaTask = AllocateZeroPool (sizeof (ATA_BUS_ASYN_TASK));
+      if (AtaTask == NULL) {
+        gBS->RestoreTPL (OldTpl);
+        return EFI_OUT_OF_RESOURCES;
+      }
+      AtaTask->AtaDevice      = AtaDevice;
+      AtaTask->Buffer         = Buffer;
+      AtaTask->IsWrite        = IsWrite;
+      AtaTask->NumberOfBlocks = NumberOfBlocks;
+      AtaTask->Signature      = ATA_TASK_SIGNATURE;
+      AtaTask->StartLba       = StartLba;
+      AtaTask->Token          = Token;
+
+      InsertTailList (&AtaDevice->AtaTaskList, &AtaTask->TaskEntry);
+      gBS->RestoreTPL (OldTpl);
+      return EFI_SUCCESS;
+    }
+    gBS->RestoreTPL (OldTpl);
+
+    Token->TransactionStatus = EFI_SUCCESS;
+    EventCount = AllocateZeroPool (sizeof (UINTN));
+    if (EventCount == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    IsError = AllocateZeroPool (sizeof (BOOLEAN));
+    if (IsError == NULL) {
+      FreePool (EventCount);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    DEBUG ((EFI_D_BLKIO, "Allocation IsError Addr=%x\n", IsError));
+    *IsError = FALSE;
+    TempCount   = (NumberOfBlocks + MaxTransferBlockNumber - 1) / MaxTransferBlockNumber;
+    *EventCount = TempCount;
+    DEBUG ((EFI_D_BLKIO, "AccessAtaDevice, NumberOfBlocks=%x\n", NumberOfBlocks));
+    DEBUG ((EFI_D_BLKIO, "AccessAtaDevice, MaxTransferBlockNumber=%x\n", MaxTransferBlockNumber));
+    DEBUG ((EFI_D_BLKIO, "AccessAtaDevice, EventCount=%x\n", TempCount));
+  } else {
+    while (!IsListEmpty (&AtaDevice->AtaTaskList) || !IsListEmpty (&AtaDevice->AtaSubTaskList)) {
+      //
+      // Stall for 100us.
+      //
+      MicroSecondDelay (100);
+    }
+  }
+
+  do {
+    if (NumberOfBlocks > MaxTransferBlockNumber) {
+      TransferBlockNumber = MaxTransferBlockNumber;
+      NumberOfBlocks     -= MaxTransferBlockNumber;
+    } else  {
+      TransferBlockNumber = NumberOfBlocks;
+      NumberOfBlocks      = 0;
+    }
+
+    //
+    // Create sub event for the sub ata task. Non-blocking mode.
+    //
+    if ((Token != NULL) && (Token->Event != NULL)) {
+      SubTask  = NULL;
+      SubEvent = NULL;
+
+      SubTask = AllocateZeroPool (sizeof (ATA_BUS_ASYN_SUB_TASK));
+      if (SubTask == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto EXIT;
+      }
+
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+      SubTask->UnsignalledEventCount = EventCount;
+      SubTask->Signature             = ATA_SUB_TASK_SIGNATURE;
+      SubTask->AtaDevice             = AtaDevice;
+      SubTask->Token                 = Token;
+      SubTask->IsError               = IsError;
+      InsertTailList (&AtaDevice->AtaSubTaskList, &SubTask->TaskEntry);
+      gBS->RestoreTPL (OldTpl);
+
+      Status = gBS->CreateEvent (
+                      EVT_NOTIFY_SIGNAL,
+                      TPL_NOTIFY,
+                      AtaNonBlockingCallBack,
+                      SubTask,
+                      &SubEvent
+                      );
+      //
+      // If resource allocation fail, the un-signalled event count should equal to
+      // the original one minus the unassigned subtasks number.
+      //
+      if (EFI_ERROR (Status)) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto EXIT;
+      }
+
+      Status = TransferAtaDevice (AtaDevice, &SubTask->Packet, Buffer, StartLba, (UINT32) TransferBlockNumber, IsWrite, SubEvent);
+    } else {
+      //
+      // Blocking Mode.
+      //
+      DEBUG ((EFI_D_BLKIO, "Blocking AccessAtaDevice, TransferBlockNumber=%x; StartLba = %x\n", TransferBlockNumber, StartLba));
+      Status = TransferAtaDevice (AtaDevice, NULL, Buffer, StartLba, (UINT32) TransferBlockNumber, IsWrite, NULL);
+    }
+
+    if (EFI_ERROR (Status)) {
+      goto EXIT;
+    }
+
+    Index++;
+    StartLba += TransferBlockNumber;
+    Buffer   += TransferBlockNumber * BlockSize;
+  } while (NumberOfBlocks > 0);
+
+EXIT:
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    //
+    // Release resource at non-blocking mode.
+    //
+    if (EFI_ERROR (Status)) {
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+      Token->TransactionStatus = Status;
+      *EventCount = (*EventCount) - (TempCount - Index);
+      *IsError    = TRUE;
+
+      if (*EventCount == 0) {
+        FreePool (EventCount);
+        FreePool (IsError);
+      }
+
+      if (SubTask != NULL) {
+        RemoveEntryList (&SubTask->TaskEntry);
+        FreeAtaSubTask (SubTask);
+      }
+
+      if (SubEvent != NULL) {
+        gBS->CloseEvent (SubEvent);
+      }
+      gBS->RestoreTPL (OldTpl);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Trust transfer data from/to ATA device.
+
+  This function performs one ATA pass through transaction to do a trust transfer from/to
+  ATA device. It chooses the appropriate ATA command and protocol to invoke PassThru
+  interface of ATA pass through.
+
+  @param  AtaDevice                    The ATA child device involved for the operation.
+  @param  Buffer                       The pointer to the current transaction buffer.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  TransferLength               The block number or sector count of the transfer.
+  @param  IsTrustSend                  Indicates whether it is a trust send operation or not.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  TransferLengthOut            A pointer to a buffer to store the size in bytes of the data
+                                       written to the buffer. Ignore it when IsTrustSend is TRUE.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+EFIAPI
+TrustTransferAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice,
+  IN OUT VOID                       *Buffer,
+  IN UINT8                          SecurityProtocolId,
+  IN UINT16                         SecurityProtocolSpecificData,
+  IN UINTN                          TransferLength,
+  IN BOOLEAN                        IsTrustSend,
+  IN UINT64                         Timeout,
+  OUT UINTN                         *TransferLengthOut
+  )
+{
+  EFI_ATA_COMMAND_BLOCK             *Acb;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  *Packet;
+  EFI_STATUS                        Status;
+  VOID                              *NewBuffer;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+
+  //
+  // Ensure AtaDevice->UdmaValid and IsTrustSend are valid boolean values
+  //
+  ASSERT ((UINTN) AtaDevice->UdmaValid < 2);
+  ASSERT ((UINTN) IsTrustSend < 2);
+  //
+  // Prepare for ATA command block.
+  //
+  Acb = ZeroMem (&AtaDevice->Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
+  if (TransferLength == 0) {
+    Acb->AtaCommand    = ATA_CMD_TRUST_NON_DATA;
+  } else {
+    Acb->AtaCommand    = mAtaTrustCommands[AtaDevice->UdmaValid][IsTrustSend];
+  }
+  Acb->AtaFeatures      = SecurityProtocolId;
+  Acb->AtaSectorCount   = (UINT8) (TransferLength / 512);
+  Acb->AtaSectorNumber  = (UINT8) ((TransferLength / 512) >> 8);
+  //
+  // NOTE: ATA Spec has no explicitly definition for Security Protocol Specific layout.
+  // Here use big endian for Cylinder register.
+  //
+  Acb->AtaCylinderHigh  = (UINT8) SecurityProtocolSpecificData;
+  Acb->AtaCylinderLow   = (UINT8) (SecurityProtocolSpecificData >> 8);
+  Acb->AtaDeviceHead    = (UINT8) (BIT7 | BIT6 | BIT5 | (AtaDevice->PortMultiplierPort << 4));
+
+  //
+  // Prepare for ATA pass through packet.
+  //
+  Packet = ZeroMem (&AtaDevice->Packet, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
+  if (TransferLength == 0) {
+    Packet->InTransferLength  = 0;
+    Packet->OutTransferLength = 0;
+    Packet->Protocol = EFI_ATA_PASS_THRU_PROTOCOL_ATA_NON_DATA;
+  } else if (IsTrustSend) {
+    //
+    // Check the alignment of the incoming buffer prior to invoking underlying ATA PassThru
+    //
+    AtaPassThru = AtaDevice->AtaBusDriverData->AtaPassThru;
+    if ((AtaPassThru->Mode->IoAlign > 1) && !IS_ALIGNED (Buffer, AtaPassThru->Mode->IoAlign)) {
+      NewBuffer = AllocateAlignedBuffer (AtaDevice, TransferLength);
+      if (NewBuffer == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      CopyMem (NewBuffer, Buffer, TransferLength);
+      FreePool (Buffer);
+      Buffer = NewBuffer;
+    }
+    Packet->OutDataBuffer = Buffer;
+    Packet->OutTransferLength = (UINT32) TransferLength;
+    Packet->Protocol = mAtaPassThruCmdProtocols[AtaDevice->UdmaValid][IsTrustSend];
+  } else {
+    Packet->InDataBuffer = Buffer;
+    Packet->InTransferLength = (UINT32) TransferLength;
+    Packet->Protocol = mAtaPassThruCmdProtocols[AtaDevice->UdmaValid][IsTrustSend];
+  }
+  Packet->Length   = EFI_ATA_PASS_THRU_LENGTH_BYTES;
+  Packet->Timeout  = Timeout;
+
+  Status = AtaDevicePassThru (AtaDevice, NULL, NULL);
+  if (TransferLengthOut != NULL) {
+    if (! IsTrustSend) {
+      *TransferLengthOut = Packet->InTransferLength;
+    }
+  }
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/ComponentName.c
new file mode 100644
index 0000000000..ff9d727e6c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/ComponentName.c
@@ -0,0 +1,238 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for ConPlatform driver.
+  
+  Copyright (c) 2009 - 2011, 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 "AtaBus.h"
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaBusDriverNameTable[] = {
+  { "eng;en", L"ATA Bus Driver" },
+  { NULL , NULL }
+};
+
+//
+// Controller name table
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaBusControllerNameTable[] = {
+  { "eng;en", L"ATA Controller" },
+  { NULL , NULL }
+};
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gAtaBusComponentName = {
+  AtaBusComponentNameGetDriverName,
+  AtaBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gAtaBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) AtaBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) AtaBusComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mAtaBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gAtaBusComponentName)
+           );
+}
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                Status;
+  EFI_BLOCK_IO_PROTOCOL     *BlockIo;
+  ATA_DEVICE                *AtaDevice;
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+
+  //
+  // Make sure this driver is currently managing ControllHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gAtaBusDriverBinding.DriverBindingHandle,
+             &gEfiAtaPassThruProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ControllerNameTable = mAtaBusControllerNameTable;
+  if (ChildHandle != NULL) {
+    Status = EfiTestChildHandle (
+               ControllerHandle,
+               ChildHandle,
+               &gEfiAtaPassThruProtocolGuid
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Get the child context
+    //
+    Status = gBS->OpenProtocol (
+                    ChildHandle,
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlockIo,
+                    gAtaBusDriverBinding.DriverBindingHandle,
+                    ChildHandle,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+    AtaDevice = ATA_DEVICE_FROM_BLOCK_IO (BlockIo);
+    ControllerNameTable =AtaDevice->ControllerNameTable;
+  }
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gAtaBusComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBus.c b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBus.c
new file mode 100644
index 0000000000..cad5ad7d44
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBus.c
@@ -0,0 +1,1498 @@
+/** @file
+  This file implements I2C IO Protocol which enables the user to manipulate a single 
+  I2C device independent of the host controller and I2C design.
+
+  Copyright (c) 2013 - 2015, 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 "I2cDxe.h"
+
+//
+//  EFI_DRIVER_BINDING_PROTOCOL instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gI2cBusDriverBinding = {
+  I2cBusDriverSupported,
+  I2cBusDriverStart,
+  I2cBusDriverStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// Template for I2C Bus Child Device.
+//
+I2C_DEVICE_CONTEXT gI2cDeviceContextTemplate = {
+  I2C_DEVICE_SIGNATURE,
+  NULL,
+  {                     // I2cIo Protocol
+    I2cBusQueueRequest, // QueueRequest
+    NULL,               // DeviceGuid
+    0,                  // DeviceIndex
+    0,                  // HardwareRevision
+    NULL                // I2cControllerCapabilities
+  },
+  NULL,                 // DevicePath
+  NULL,                 // I2cDevice
+  NULL,                 // I2cBusContext
+};
+
+//
+// Template for controller device path node.
+//
+CONTROLLER_DEVICE_PATH gControllerDevicePathTemplate = {
+  {
+    HARDWARE_DEVICE_PATH,
+    HW_CONTROLLER_DP,
+    {
+      (UINT8) (sizeof (CONTROLLER_DEVICE_PATH)),
+      (UINT8) ((sizeof (CONTROLLER_DEVICE_PATH)) >> 8)
+    }
+  },
+  0
+};
+
+//
+// Template for vendor device path node.
+//
+VENDOR_DEVICE_PATH gVendorDevicePathTemplate = {
+  {
+    HARDWARE_DEVICE_PATH,
+    HW_VENDOR_DP,
+    {
+      (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+      (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+    }
+  },
+  { 0x0, 0x0, 0x0, { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 }}
+};
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mI2cBusDriverNameTable[] = {
+  { "eng;en", (CHAR16 *) L"I2C Bus Driver" },
+  { NULL , NULL }
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gI2cBusComponentName = {
+  (EFI_COMPONENT_NAME_GET_DRIVER_NAME) I2cBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME_GET_CONTROLLER_NAME) I2cBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gI2cBusComponentName2 = {
+  I2cBusComponentNameGetDriverName,
+  I2cBusComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mI2cBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This != &gI2cBusComponentName2)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Check if the child of I2C controller has been created.
+
+  @param[in] This                         A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.   
+  @param[in] Controller                   I2C controller handle.
+  @param[in] RemainingDevicePath          A pointer to the remaining portion of a device path.
+  @param[in] RemainingHasControllerNode   Indicate if RemainingDevicePath contains CONTROLLER_DEVICE_PATH.
+  @param[in] RemainingControllerNumber    Controller number in CONTROLLER_DEVICE_PATH.
+  
+  @retval EFI_SUCCESS                     The child of I2C controller is not created.
+  @retval Others                          The child of I2C controller has been created or other errors happen.
+
+**/
+EFI_STATUS
+CheckRemainingDevicePath (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath,
+  IN BOOLEAN                      RemainingHasControllerNode,
+  IN UINT32                       RemainingControllerNumber
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_DEVICE_PATH_PROTOCOL                *SystemDevicePath;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY     *OpenInfoBuffer;
+  UINTN                                   EntryCount;
+  UINTN                                   Index;
+  BOOLEAN                                 SystemHasControllerNode;
+  UINT32                                  SystemControllerNumber;  
+
+  SystemHasControllerNode = FALSE;
+  SystemControllerNumber    = 0;
+  
+  Status = gBS->OpenProtocolInformation (
+                  Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  &OpenInfoBuffer,
+                  &EntryCount
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  for (Index = 0; Index < EntryCount; Index++) {
+    if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+      Status = gBS->OpenProtocol (
+                      OpenInfoBuffer[Index].ControllerHandle,
+                      &gEfiDevicePathProtocolGuid,
+                      (VOID **) &SystemDevicePath,
+                      This->DriverBindingHandle,
+                      Controller,
+                      EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                      );
+      if (!EFI_ERROR (Status)) {
+        //
+        // Find vendor device path node and compare
+        //    
+        while (!IsDevicePathEnd (SystemDevicePath)) {
+          if ((DevicePathType (SystemDevicePath) == HARDWARE_DEVICE_PATH) &&
+              (DevicePathSubType (SystemDevicePath) == HW_VENDOR_DP)) {
+            //
+            // Check if vendor device path is same between system device path and remaining device path
+            //
+            if (CompareMem (SystemDevicePath, RemainingDevicePath, sizeof (VENDOR_DEVICE_PATH)) == 0) {
+              //
+              // Get controller node appended after vendor node
+              //
+              SystemDevicePath = NextDevicePathNode (SystemDevicePath);
+              if ((DevicePathType (SystemDevicePath) == HARDWARE_DEVICE_PATH) &&
+                  (DevicePathSubType (SystemDevicePath) == HW_CONTROLLER_DP)) {
+                SystemHasControllerNode = TRUE;
+                SystemControllerNumber    = ((CONTROLLER_DEVICE_PATH *) SystemDevicePath)->ControllerNumber;
+              } else {
+                SystemHasControllerNode = FALSE;
+                SystemControllerNumber    = 0;
+              }
+              if (((SystemHasControllerNode)  && (!RemainingHasControllerNode) && (SystemControllerNumber == 0)) ||
+                  ((!SystemHasControllerNode) && (RemainingHasControllerNode)  && (RemainingControllerNumber == 0)) ||
+                  ((SystemHasControllerNode)  && (RemainingHasControllerNode)  && (SystemControllerNumber == RemainingControllerNumber)) ||
+                  ((!SystemHasControllerNode) && (!RemainingHasControllerNode))) {
+                  DEBUG ((EFI_D_ERROR, "This I2C device has been already started.\n"));
+                  Status = EFI_UNSUPPORTED;
+                  break;
+              }
+            }
+          }
+          SystemDevicePath = NextDevicePathNode (SystemDevicePath);
+        }
+        if (EFI_ERROR (Status)) {
+          break;
+        }
+      }
+    }
+  }
+  FreePool (OpenInfoBuffer);
+  return Status;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_I2C_ENUMERATE_PROTOCOL          *I2cEnumerate;
+  EFI_I2C_HOST_PROTOCOL               *I2cHost;
+  EFI_DEVICE_PATH_PROTOCOL            *ParentDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL            *DevPathNode;
+  BOOLEAN                             RemainingHasControllerNode;
+  UINT32                              RemainingControllerNumber;
+
+  RemainingHasControllerNode = FALSE;
+  RemainingControllerNumber    = 0;
+
+  //
+  //  Determine if the I2c Enumerate Protocol is available
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cEnumerateProtocolGuid,
+                  (VOID **) &I2cEnumerate,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if ((EFI_ERROR (Status)) && (Status != EFI_ALREADY_STARTED)) {
+    return Status;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiI2cEnumerateProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if ((EFI_ERROR (Status)) && (Status != EFI_ALREADY_STARTED)) {
+    return Status;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+
+  if ((RemainingDevicePath != NULL) && !IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // Check if the first node of RemainingDevicePath is a hardware vendor device path
+    //
+    if ((DevicePathType (RemainingDevicePath) != HARDWARE_DEVICE_PATH) ||
+        (DevicePathSubType (RemainingDevicePath) != HW_VENDOR_DP)) {
+      return EFI_UNSUPPORTED;
+    }
+    //
+    // Check if the second node of RemainingDevicePath is a controller node
+    //
+    DevPathNode = NextDevicePathNode (RemainingDevicePath);
+    if (!IsDevicePathEnd (DevPathNode)) {
+      if ((DevicePathType (DevPathNode) != HARDWARE_DEVICE_PATH) ||
+          (DevicePathSubType (DevPathNode) != HW_CONTROLLER_DP)) {
+        return EFI_UNSUPPORTED;
+      } else {
+        RemainingHasControllerNode = TRUE;
+        RemainingControllerNumber    = ((CONTROLLER_DEVICE_PATH *) DevPathNode)->ControllerNumber;
+      }
+    }
+  }
+
+  //
+  // Determine if the I2C Host Protocol is available
+  //
+  Status = gBS->OpenProtocol ( 
+                  Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  (VOID **) &I2cHost,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiI2cHostProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+
+
+  if (Status == EFI_ALREADY_STARTED) {
+    if ((RemainingDevicePath == NULL) || 
+        ((RemainingDevicePath != NULL) && IsDevicePathEnd (RemainingDevicePath))) {
+      //
+      // If RemainingDevicePath is NULL or is the End of Device Path Node, return EFI_SUCCESS.
+      //
+      Status = EFI_SUCCESS;
+    } else {
+      //
+      // Test if the child with the RemainingDevicePath has already been created.
+      //    
+      Status = CheckRemainingDevicePath (
+                 This,
+                 Controller,
+                 RemainingDevicePath,
+                 RemainingHasControllerNode,
+                 RemainingControllerNumber
+                 );
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_I2C_ENUMERATE_PROTOCOL *I2cEnumerate;
+  EFI_I2C_HOST_PROTOCOL      *I2cHost;
+  I2C_BUS_CONTEXT            *I2cBusContext;
+  EFI_STATUS                 Status;
+  EFI_DEVICE_PATH_PROTOCOL   *ParentDevicePath;
+
+  I2cBusContext     = NULL;
+  ParentDevicePath  = NULL;
+  I2cEnumerate      = NULL;
+  I2cHost           = NULL;
+
+  //
+  //  Determine if the I2C controller is available
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  (VOID**)&I2cHost,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    DEBUG ((EFI_D_ERROR, "I2cBus: open I2C host error, Status = %r\n", Status));
+    return Status;
+  }
+
+  if (Status == EFI_ALREADY_STARTED) {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &I2cBusContext,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "I2cBus: open private protocol error, Status = %r.\n", Status));
+      return Status;
+    }
+  }
+
+  //
+  //  Get the I2C bus enumeration API
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cEnumerateProtocolGuid,
+                  (VOID**)&I2cEnumerate,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    DEBUG ((EFI_D_ERROR, "I2cBus: open I2C enumerate error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  Status = gBS->OpenProtocol (
+                   Controller,
+                   &gEfiDevicePathProtocolGuid,
+                   (VOID **) &ParentDevicePath,
+                   This->DriverBindingHandle,
+                   Controller,
+                   EFI_OPEN_PROTOCOL_BY_DRIVER
+                   );
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    DEBUG ((EFI_D_ERROR, "I2cBus: open device path error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  if ((RemainingDevicePath != NULL) && IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // If RemainingDevicePath is the End of Device Path Node,
+    // don't create any child device and return EFI_SUCESS
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Allocate the buffer for I2C_BUS_CONTEXT if it is not allocated before.
+  //
+  if (I2cBusContext == NULL) {
+    //
+    //  Allocate the I2C context structure for the current I2C controller
+    //
+    I2cBusContext = AllocateZeroPool (sizeof (I2C_BUS_CONTEXT));
+    if (I2cBusContext == NULL) {
+      DEBUG ((EFI_D_ERROR, "I2cBus: there is no enough memory to allocate.\n"));
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Error;
+    }
+    
+    /*
+       +----------------+
+    .->| I2C_BUS_CONTEXT|<----- This file Protocol (gEfiCallerIdGuid) installed on I2C Controller handle
+    |  +----------------+
+    |
+    |  +----------------------------+
+    |  | I2C_DEVICE_CONTEXT         |
+    `--|                            |
+       |                            |
+       | I2C IO Protocol Structure  | <----- I2C IO Protocol
+       |                            |
+       +----------------------------+
+    
+    */
+    I2cBusContext->I2cHost      = I2cHost;
+    I2cBusContext->I2cEnumerate = I2cEnumerate;
+    //
+    // Parent controller used to create children
+    //
+    I2cBusContext->Controller   = Controller;
+    //
+    // Parent controller device path used to create children device path
+    //
+    I2cBusContext->ParentDevicePath = ParentDevicePath;
+    
+    I2cBusContext->DriverBindingHandle = This->DriverBindingHandle;
+    
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Controller,
+                    &gEfiCallerIdGuid,
+                    I2cBusContext,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "I2cBus: install private protocol error, Status = %r.\n", Status));
+      goto Error;
+    }
+  }
+
+  //
+  //  Start the driver
+  //
+  Status = RegisterI2cDevice (I2cBusContext, Controller, RemainingDevicePath);
+
+  return Status;
+
+Error:
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cBus: Start() function failed, Status = %r\n", Status));
+    if (ParentDevicePath != NULL) {
+      gBS->CloseProtocol ( 
+            Controller,
+            &gEfiDevicePathProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+    }
+
+    if (I2cHost != NULL) {
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiI2cHostProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+    }
+
+    if (I2cEnumerate != NULL) {
+      gBS->CloseProtocol ( 
+            Controller,
+            &gEfiI2cEnumerateProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+    }
+    
+    if (I2cBusContext != NULL) {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      &Controller,
+                      gEfiCallerIdGuid,
+                      I2cBusContext,
+                      NULL
+                      );
+      FreePool (I2cBusContext);
+    }
+  }
+
+  //
+  //  Return the operation status.
+  //
+  return Status;
+}
+
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Controller,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  I2C_BUS_CONTEXT             *I2cBusContext;
+  EFI_STATUS                  Status;
+  BOOLEAN                     AllChildrenStopped;
+  UINTN                       Index;
+
+  if (NumberOfChildren == 0) {
+    gBS->CloseProtocol ( 
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiI2cHostProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiI2cEnumerateProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &I2cBusContext,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (!EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+            Controller,
+            &gEfiCallerIdGuid,
+            I2cBusContext,
+            NULL
+            );
+      //
+      // No more child now, free bus context data.
+      //
+      FreePool (I2cBusContext);
+    }
+    return Status;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = UnRegisterI2cDevice (This, Controller, ChildHandleBuffer[Index]);
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Enumerate the I2C bus
+
+  This routine walks the platform specific data describing the
+  I2C bus to create the I2C devices where driver GUIDs were
+  specified.
+
+  @param[in] I2cBusContext            Address of an I2C_BUS_CONTEXT structure
+  @param[in] Controller               Handle to the controller
+  @param[in] RemainingDevicePath      A pointer to the remaining portion of a device path.
+
+  @retval EFI_SUCCESS       The bus is successfully configured
+
+**/
+EFI_STATUS
+RegisterI2cDevice (
+  IN I2C_BUS_CONTEXT            *I2cBusContext,
+  IN EFI_HANDLE                 Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL   *RemainingDevicePath
+  )
+{
+  I2C_DEVICE_CONTEXT                *I2cDeviceContext;
+  EFI_STATUS                        Status;
+  CONST EFI_I2C_DEVICE              *Device;
+  CONST EFI_I2C_DEVICE              *TempDevice;
+  UINT32                            RemainingPathDeviceIndex;
+  EFI_DEVICE_PATH_PROTOCOL          *DevPathNode;
+  BOOLEAN                           BuildControllerNode;
+  UINTN                             Count;
+
+  Status                   = EFI_SUCCESS;
+  BuildControllerNode      = TRUE;
+
+  //
+  // Default DeviceIndex
+  //
+  RemainingPathDeviceIndex = 0;
+  
+  //
+  // Determine the controller number in Controller Node Device Path when RemainingDevicePath is not NULL.
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if there is a controller node appended after vendor node
+    //    
+    DevPathNode = NextDevicePathNode (RemainingDevicePath);
+    if ((DevicePathType (DevPathNode) == HARDWARE_DEVICE_PATH) &&
+        (DevicePathSubType(DevPathNode) == HW_CONTROLLER_DP)) {
+      //
+      // RemainingDevicePath != NULL and RemainingDevicePath contains Controller Node,
+      // add Controller Node to Device Path on child handle.
+      //
+      RemainingPathDeviceIndex = ((CONTROLLER_DEVICE_PATH *) DevPathNode)->ControllerNumber;
+    } else {
+      //
+      // RemainingDevicePath != NULL and RemainingDevicePath does not contain Controller Node,
+      // do not add controller node to Device Path on child handle.
+      //
+      BuildControllerNode = FALSE;
+    }
+  }
+
+  //
+  //  Walk the list of I2C devices on this bus
+  //
+  Device = NULL;
+  while (TRUE) {
+    //
+    //  Get the next I2C device
+    //
+    Status = I2cBusContext->I2cEnumerate->Enumerate (I2cBusContext->I2cEnumerate, &Device);
+    if (EFI_ERROR (Status) || Device == NULL) {
+      if (RemainingDevicePath != NULL) {
+        Status = EFI_NOT_FOUND;
+      } else {
+        Status = EFI_SUCCESS;
+      }
+      break;
+    }
+
+    //
+    //  Determine if the device info is valid
+    //
+    if ((Device->DeviceGuid == NULL) || (Device->SlaveAddressCount == 0) || (Device->SlaveAddressArray == NULL)) {
+      DEBUG ((EFI_D_ERROR, "Invalid EFI_I2C_DEVICE reported by I2c Enumerate protocol.\n"));
+      continue;
+    }
+
+    if (RemainingDevicePath == NULL) {
+      if (Device->DeviceIndex == 0) {
+        //
+        // Determine if the controller node is necessary when controller number is zero in I2C device
+        //
+        TempDevice = NULL;
+        Count      = 0;
+        while (TRUE) {
+          //
+          //  Get the next I2C device
+          //
+          Status = I2cBusContext->I2cEnumerate->Enumerate (I2cBusContext->I2cEnumerate, &TempDevice);
+          if (EFI_ERROR (Status) || TempDevice == NULL) {
+            Status = EFI_SUCCESS;
+            break;
+          }
+          if (CompareGuid (Device->DeviceGuid, TempDevice->DeviceGuid)) {
+            Count++;
+          }
+        }
+        if (Count == 1) {
+          //
+          // RemainingDevicePath == NULL and only DeviceIndex 0 is present on the I2C bus,
+          // do not add Controller Node to Device Path on child handle.
+          //
+          BuildControllerNode = FALSE;
+        }
+      }
+    } else {
+      //
+      // Find I2C device reported in Remaining Device Path
+      //
+      if ((!CompareGuid (&((VENDOR_DEVICE_PATH *)RemainingDevicePath)->Guid, Device->DeviceGuid)) ||
+          (RemainingPathDeviceIndex != Device->DeviceIndex)) {
+        continue; 
+      }
+    }
+
+    //
+    // Build the device context for current I2C device.
+    //
+    I2cDeviceContext = NULL;
+    I2cDeviceContext = AllocateCopyPool (sizeof (I2C_DEVICE_CONTEXT), &gI2cDeviceContextTemplate);
+    ASSERT (I2cDeviceContext != NULL);
+    if (I2cDeviceContext == NULL) {
+      continue;
+    }
+
+    //
+    //  Initialize the specific device context
+    //
+    I2cDeviceContext->I2cBusContext          = I2cBusContext;
+    I2cDeviceContext->I2cDevice              = Device;
+    I2cDeviceContext->I2cIo.DeviceGuid       = Device->DeviceGuid;
+    I2cDeviceContext->I2cIo.DeviceIndex      = Device->DeviceIndex;
+    I2cDeviceContext->I2cIo.HardwareRevision = Device->HardwareRevision;
+    I2cDeviceContext->I2cIo.I2cControllerCapabilities = I2cBusContext->I2cHost->I2cControllerCapabilities;
+
+    //
+    //  Build the device path
+    //
+    Status = I2cBusDevicePathAppend (I2cDeviceContext, BuildControllerNode);
+    ASSERT_EFI_ERROR (Status);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    //  Install the protocol
+    //
+    Status = gBS->InstallMultipleProtocolInterfaces (
+              &I2cDeviceContext->Handle,
+              &gEfiI2cIoProtocolGuid,
+              &I2cDeviceContext->I2cIo,
+              &gEfiDevicePathProtocolGuid,
+              I2cDeviceContext->DevicePath,
+              NULL );
+    if (EFI_ERROR (Status)) {
+      //
+      // Free resources for this I2C device
+      //
+      ReleaseI2cDeviceContext (I2cDeviceContext);
+      continue;
+    }
+    
+    //
+    // Create the child handle
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiI2cHostProtocolGuid,
+                    (VOID **) &I2cBusContext->I2cHost,
+                    I2cBusContext->DriverBindingHandle,
+                    I2cDeviceContext->Handle,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+    if (EFI_ERROR (Status)) {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      I2cDeviceContext->Handle,
+                      &gEfiDevicePathProtocolGuid,
+                      I2cDeviceContext->DevicePath,
+                      &gEfiI2cIoProtocolGuid,
+                      &I2cDeviceContext->I2cIo,
+                      NULL
+                      );
+      //
+      // Free resources for this I2C device
+      //
+      ReleaseI2cDeviceContext (I2cDeviceContext);
+      continue;      
+    }
+
+    if (RemainingDevicePath != NULL) {
+      //
+      // Child has been created successfully
+      //
+      break;
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Queue an I2C transaction for execution on the I2C device.
+
+  This routine must be called at or below TPL_NOTIFY.  For synchronous
+  requests this routine must be called at or below TPL_CALLBACK.
+
+  This routine queues an I2C transaction to the I2C controller for
+  execution on the I2C bus.
+
+  When Event is NULL, QueueRequest() operates synchronously and returns
+  the I2C completion status as its return value.
+
+  When Event is not NULL, QueueRequest() synchronously returns EFI_SUCCESS
+  indicating that the asynchronous I2C transaction was queued.  The values
+  above are returned in the buffer pointed to by I2cStatus upon the
+  completion of the I2C transaction when I2cStatus is not NULL.
+
+  The upper layer driver writer provides the following to the platform
+  vendor:
+  
+  1.  Vendor specific GUID for the I2C part
+  2.  Guidance on proper construction of the slave address array when the
+      I2C device uses more than one slave address.  The I2C bus protocol
+      uses the SlaveAddressIndex to perform relative to physical address
+      translation to access the blocks of hardware within the I2C device.
+
+  @param[in] This               Pointer to an EFI_I2C_IO_PROTOCOL structure.
+  @param[in] SlaveAddressIndex  Index value into an array of slave addresses
+                                for the I2C device.  The values in the array
+                                are specified by the board designer, with the
+                                third party I2C device driver writer providing
+                                the slave address order.
+
+                                For devices that have a single slave address,
+                                this value must be zero.  If the I2C device
+                                uses more than one slave address then the
+                                third party (upper level) I2C driver writer
+                                needs to specify the order of entries in the
+                                slave address array.
+
+                                \ref ThirdPartyI2cDrivers "Third Party I2C
+                                Drivers" section in I2cMaster.h.
+  @param[in] Event              Event to signal for asynchronous transactions,
+                                NULL for synchronous transactions
+  @param[in] RequestPacket      Pointer to an EFI_I2C_REQUEST_PACKET structure
+                                describing the I2C transaction
+  @param[out] I2cStatus         Optional buffer to receive the I2C transaction
+                                completion status
+
+  @retval EFI_SUCCESS           The asynchronous transaction was successfully
+                                queued when Event is not NULL.
+  @retval EFI_SUCCESS           The transaction completed successfully when
+                                Event is NULL.
+  @retval EFI_BAD_BUFFER_SIZE   The RequestPacket->LengthInBytes value is too
+                                large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the
+                                transaction.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_NO_MAPPING        The EFI_I2C_HOST_PROTOCOL could not set the
+                                bus configuration required to access this I2C
+                                device.
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the slave
+                                address selected by SlaveAddressIndex.
+                                EFI_DEVICE_ERROR will be returned if the
+                                controller cannot distinguish when the NACK
+                                occurred.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C transaction
+  @retval EFI_UNSUPPORTED       The controller does not support the requested
+                                transaction.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusQueueRequest (
+  IN CONST EFI_I2C_IO_PROTOCOL  *This,
+  IN UINTN                      SlaveAddressIndex,
+  IN EFI_EVENT                  Event               OPTIONAL,
+  IN EFI_I2C_REQUEST_PACKET     *RequestPacket,
+  OUT EFI_STATUS                *I2cStatus          OPTIONAL
+  )
+{
+  CONST EFI_I2C_DEVICE        *I2cDevice;
+  I2C_BUS_CONTEXT             *I2cBusContext;
+  CONST EFI_I2C_HOST_PROTOCOL *I2cHost;
+  I2C_DEVICE_CONTEXT          *I2cDeviceContext;
+  EFI_STATUS                  Status;
+
+  if (RequestPacket == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  //  Validate the I2C slave index
+  //
+  I2cDeviceContext = I2C_DEVICE_CONTEXT_FROM_PROTOCOL (This);
+  I2cDevice        = I2cDeviceContext->I2cDevice;
+  if ( SlaveAddressIndex >= I2cDevice->SlaveAddressCount ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  //  Locate the I2c Host Protocol to queue request
+  //
+  I2cBusContext = I2cDeviceContext->I2cBusContext;
+  I2cHost       = I2cBusContext->I2cHost;
+
+  //
+  //  Start the I2C operation
+  //
+  Status = I2cHost->QueueRequest (
+                      I2cHost,
+                      I2cDevice->I2cBusConfiguration,
+                      I2cDevice->SlaveAddressArray [SlaveAddressIndex],
+                      Event,
+                      RequestPacket,
+                      I2cStatus
+                      );
+
+  return Status;
+}
+
+/**
+  Release all the resources allocated for the I2C device.
+
+  This function releases all the resources allocated for the I2C device.
+
+  @param  I2cDeviceContext         The I2C child device involved for the operation.
+
+**/
+VOID
+ReleaseI2cDeviceContext (
+  IN I2C_DEVICE_CONTEXT          *I2cDeviceContext
+  )
+{
+  if (I2cDeviceContext == NULL) {
+    return;
+  }
+  
+  if (I2cDeviceContext->DevicePath != NULL) {
+    FreePool (I2cDeviceContext->DevicePath);
+  }
+
+  FreePool (I2cDeviceContext);
+}
+
+/**
+  Unregister an I2C device.
+
+  This function removes the protocols installed on the controller handle and
+  frees the resources allocated for the I2C device.
+
+  @param  This                  The pointer to EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The controller handle of the I2C device.
+  @param  Handle                The child handle.
+
+  @retval EFI_SUCCESS           The I2C device is successfully unregistered.
+  @return Others                Some error occurs when unregistering the I2C device.
+
+**/
+EFI_STATUS
+UnRegisterI2cDevice (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  )
+{
+  EFI_STATUS                  Status;
+  I2C_DEVICE_CONTEXT          *I2cDeviceContext;
+  EFI_I2C_IO_PROTOCOL         *I2cIo;
+  EFI_I2C_HOST_PROTOCOL       *I2cHost;
+
+  I2cIo = NULL;
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiI2cIoProtocolGuid,
+                  (VOID **) &I2cIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get I2c device context data.
+  //
+  I2cDeviceContext = I2C_DEVICE_CONTEXT_FROM_PROTOCOL (I2cIo);
+
+  //
+  // Close the child handle
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiI2cHostProtocolGuid,
+         This->DriverBindingHandle,
+         Handle
+         );
+
+  //
+  // The I2C Bus driver installs the I2C Io and Device Path Protocol in the DriverBindingStart().
+  // Here should uninstall them.
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  I2cDeviceContext->DevicePath,
+                  &gEfiI2cIoProtocolGuid,
+                  &I2cDeviceContext->I2cIo,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Keep parent and child relationship
+    //
+    gBS->OpenProtocol (
+          Controller,
+          &gEfiI2cHostProtocolGuid,
+          (VOID **) &I2cHost,
+          This->DriverBindingHandle,
+          Handle,
+          EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+          );
+    return Status;
+  }
+  
+  //
+  // Free resources for this I2C device
+  //
+  ReleaseI2cDeviceContext (I2cDeviceContext);
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Create a path for the I2C device
+
+  Append the I2C slave path to the I2C master controller path.
+
+  @param[in] I2cDeviceContext           Address of an I2C_DEVICE_CONTEXT structure.
+  @param[in] BuildControllerNode        Flag to build controller node in device path.
+
+  @retval EFI_SUCCESS           The I2C device path is built successfully.
+  @return Others                It is failed to built device path.
+
+**/
+EFI_STATUS
+I2cBusDevicePathAppend (
+  IN I2C_DEVICE_CONTEXT     *I2cDeviceContext,
+  IN BOOLEAN                BuildControllerNode
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *PreviousDevicePath;
+  
+  PreviousDevicePath = NULL;
+
+  //
+  // Build vendor device path
+  //  
+  CopyMem (&gVendorDevicePathTemplate.Guid, I2cDeviceContext->I2cDevice->DeviceGuid, sizeof (EFI_GUID));
+  I2cDeviceContext->DevicePath                    = AppendDevicePathNode (
+                                                      I2cDeviceContext->I2cBusContext->ParentDevicePath,
+                                                      (EFI_DEVICE_PATH_PROTOCOL *) &gVendorDevicePathTemplate
+                                                      );
+  ASSERT (I2cDeviceContext->DevicePath != NULL);
+  if (I2cDeviceContext->DevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  if ((BuildControllerNode) && (I2cDeviceContext->DevicePath != NULL)) {
+    //
+    // Build the final I2C device path with controller node
+    //
+    PreviousDevicePath = I2cDeviceContext->DevicePath;
+    gControllerDevicePathTemplate.ControllerNumber = I2cDeviceContext->I2cDevice->DeviceIndex; 
+    I2cDeviceContext->DevicePath          = AppendDevicePathNode (
+                                              I2cDeviceContext->DevicePath,
+                                              (EFI_DEVICE_PATH_PROTOCOL *) &gControllerDevicePathTemplate
+                                              );
+    gBS->FreePool (PreviousDevicePath);
+    ASSERT (I2cDeviceContext->DevicePath != NULL);
+    if (I2cDeviceContext->DevicePath == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The user entry point for the I2C bus module. The user code starts with
+  this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2cBus(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gI2cBusDriverBinding,
+             NULL,
+             &gI2cBusComponentName,
+             &gI2cBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  
+  return Status;
+}
+
+/**
+  This is the unload handle for I2C bus module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusUnload (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HANDLE                        *DeviceHandleBuffer;
+  UINTN                             DeviceHandleCount;
+  UINTN                             Index;
+  EFI_COMPONENT_NAME_PROTOCOL       *ComponentName;
+  EFI_COMPONENT_NAME2_PROTOCOL      *ComponentName2;
+
+  //
+  // Get the list of all I2C Controller handles in the handle database.
+  // If there is an error getting the list, then the unload
+  // operation fails.
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiI2cHostProtocolGuid,
+                  NULL,
+                  &DeviceHandleCount,
+                  &DeviceHandleBuffer
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Disconnect the driver specified by Driver BindingHandle from all
+    // the devices in the handle database.
+    //
+    for (Index = 0; Index < DeviceHandleCount; Index++) {
+      Status = gBS->DisconnectController (
+                      DeviceHandleBuffer[Index],
+                      gI2cBusDriverBinding.DriverBindingHandle,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+    }
+  }
+
+  //
+  // Uninstall all the protocols installed in the driver entry point
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  gI2cBusDriverBinding.DriverBindingHandle,
+                  &gEfiDriverBindingProtocolGuid,
+                  &gI2cBusDriverBinding,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Note we have to one by one uninstall the following protocols.
+  // It's because some of them are optionally installed based on
+  // the following PCD settings.
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnosticsDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentNameDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnostics2Disable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentName2Disable
+  //
+  Status = gBS->HandleProtocol (
+                  gI2cBusDriverBinding.DriverBindingHandle,
+                  &gEfiComponentNameProtocolGuid,
+                  (VOID **) &ComponentName
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           gI2cBusDriverBinding.DriverBindingHandle,
+           &gEfiComponentNameProtocolGuid,
+           ComponentName
+           );
+  }
+
+  Status = gBS->HandleProtocol (
+                  gI2cBusDriverBinding.DriverBindingHandle,
+                  &gEfiComponentName2ProtocolGuid,
+                  (VOID **) &ComponentName2
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           gI2cBusDriverBinding.DriverBindingHandle,
+           &gEfiComponentName2ProtocolGuid,
+           ComponentName2
+           );
+  }
+
+  Status = EFI_SUCCESS;
+
+Done:
+  //
+  // Free the buffer containing the list of handles from the handle database
+  //
+  if (DeviceHandleBuffer != NULL) {
+    gBS->FreePool (DeviceHandleBuffer);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.inf b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.inf
new file mode 100644
index 0000000000..4dbe2f5e68
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.inf
@@ -0,0 +1,58 @@
+## @file
+#  This driver enumerates I2C devices on I2C bus and produce I2C IO Protocol on I2C devices.
+#
+#  Copyright (c) 2013 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = I2cBusDxe
+  MODULE_UNI_FILE                = I2cBusDxe.uni
+  FILE_GUID                      = 0C34B372-2622-4A13-A46E-BFD0DEB48BFF
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeI2cBus
+  UNLOAD_IMAGE                   = I2cBusUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  I2cDxe.h
+  I2cBus.c
+
+[LibraryClasses]
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[Protocols]
+  gEfiI2cIoProtocolGuid                             ## BY_START
+  ## BY_START
+  ## TO_START
+  gEfiDevicePathProtocolGuid
+  gEfiI2cEnumerateProtocolGuid                      ## TO_START
+  gEfiI2cHostProtocolGuid                           ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  I2cBusDxeExtra.uni
+
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.uni
new file mode 100644
index 0000000000..f0a1f5e650
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxeExtra.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxeExtra.uni
new file mode 100644
index 0000000000..13381e6282
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.c b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.c
new file mode 100644
index 0000000000..fb985f96e2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.c
@@ -0,0 +1,75 @@
+/** @file
+  This file implements the entrypoint and unload function for I2C DXE module.
+
+  Copyright (c) 2013, 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 "I2cDxe.h"
+
+/**
+  The user Entry Point for I2C module. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2c(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = InitializeI2cHost ( ImageHandle, SystemTable );
+  if ( !EFI_ERROR ( Status ))
+  {
+    Status = InitializeI2cBus ( ImageHandle, SystemTable );
+  }
+  return Status;
+}
+
+/**
+  This is the unload handle for I2C module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cUnload (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+
+  //
+  //  Disconnect the drivers
+  //
+  Status = I2cBusUnload ( ImageHandle );
+  if ( !EFI_ERROR ( Status )) {
+    Status = I2cHostUnload ( ImageHandle );
+  }
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.h b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.h
new file mode 100644
index 0000000000..1490d4283a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.h
@@ -0,0 +1,1097 @@
+/** @file
+  Private data structures for the I2C DXE driver.
+
+  This file defines common data structures, macro definitions and some module
+  internal function header files.
+
+  Copyright (c) 2013, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef __I2C_DXE_H__
+#define __I2C_DXE_H__
+
+#include <Uefi.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/TimerLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+
+#include <Protocol/DriverBinding.h>
+#include <Protocol/I2cEnumerate.h>
+#include <Protocol/I2cHost.h>
+#include <Protocol/I2cIo.h>
+#include <Protocol/I2cMaster.h>
+#include <Protocol/I2cBusConfigurationManagement.h>
+#include <Protocol/LoadedImage.h>
+
+#define I2C_DEVICE_SIGNATURE          SIGNATURE_32 ('I', '2', 'C', 'D')
+#define I2C_HOST_SIGNATURE            SIGNATURE_32 ('I', '2', 'C', 'H')
+#define I2C_REQUEST_SIGNATURE         SIGNATURE_32 ('I', '2', 'C', 'R')
+
+//
+// Synchronize access to the list of requests
+//
+#define TPL_I2C_SYNC                  TPL_NOTIFY
+
+//
+//  I2C bus context
+//
+typedef struct {
+  EFI_I2C_ENUMERATE_PROTOCOL       *I2cEnumerate;
+  EFI_I2C_HOST_PROTOCOL            *I2cHost;
+  EFI_HANDLE                       Controller;
+  EFI_DEVICE_PATH_PROTOCOL         *ParentDevicePath;
+  EFI_HANDLE                       DriverBindingHandle;  
+} I2C_BUS_CONTEXT;
+
+//
+// I2C device context
+//
+typedef struct {
+  //
+  // Structure identification
+  //
+  UINT32                        Signature;
+
+  //
+  // I2c device handle
+  //
+  EFI_HANDLE                    Handle;
+
+  //
+  // Upper level API to support the I2C device I/O
+  //
+  EFI_I2C_IO_PROTOCOL           I2cIo;
+
+  //
+  // Device path for this device
+  //
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+
+  //
+  // Platform specific data for this device
+  //
+  CONST EFI_I2C_DEVICE          *I2cDevice;
+
+  //
+  // Context for the common I/O support including the
+  // lower level API to the host controller.
+  //
+  I2C_BUS_CONTEXT               *I2cBusContext;
+} I2C_DEVICE_CONTEXT;
+
+#define I2C_DEVICE_CONTEXT_FROM_PROTOCOL(a) CR (a, I2C_DEVICE_CONTEXT, I2cIo, I2C_DEVICE_SIGNATURE)
+
+//
+// I2C Request
+//
+typedef struct {
+  //
+  // Signature
+  //
+  UINT32                            Signature;
+  
+  //
+  // Next request in the pending request list
+  //
+  LIST_ENTRY                        Link;
+
+  //
+  // I2C bus configuration for the operation
+  //
+  UINTN                             I2cBusConfiguration;
+
+  //
+  // I2C slave address for the operation
+  //
+  UINTN                             SlaveAddress;
+
+  //
+  // Event to set for asynchronous operations, NULL for
+  // synchronous operations
+  //
+  EFI_EVENT                         Event;
+
+  //
+  // I2C operation description
+  //
+  EFI_I2C_REQUEST_PACKET            *RequestPacket;
+
+  //
+  // Optional buffer to receive the I2C operation completion status
+  //
+  EFI_STATUS                        *Status;
+} I2C_REQUEST;
+
+#define I2C_REQUEST_FROM_ENTRY(a)         CR (a, I2C_REQUEST, Link, I2C_REQUEST_SIGNATURE);
+
+//
+// I2C host context
+//
+typedef struct {
+  //
+  // Structure identification
+  //
+  UINTN Signature;
+
+  //
+  // Current I2C bus configuration
+  //
+  UINTN I2cBusConfiguration;
+
+  //
+  // I2C bus configuration management event
+  //
+  EFI_EVENT I2cBusConfigurationEvent;
+
+  //
+  // I2C operation completion event
+  //
+  EFI_EVENT I2cEvent;
+
+  //
+  // I2C operation and I2C bus configuration management status
+  //
+  EFI_STATUS Status;
+
+  //
+  // I2C bus configuration management operation pending
+  //
+  BOOLEAN I2cBusConfigurationManagementPending;
+
+  //
+  // I2C request list maintained by I2C Host
+  //
+  LIST_ENTRY                  RequestList;
+
+  //
+  // Upper level API
+  //
+  EFI_I2C_HOST_PROTOCOL       I2cHost;
+
+  //
+  // I2C bus configuration management protocol
+  //
+  EFI_I2C_BUS_CONFIGURATION_MANAGEMENT_PROTOCOL *I2cBusConfigurationManagement;
+
+  //
+  // Lower level API for I2C master (controller)
+  //
+  EFI_I2C_MASTER_PROTOCOL *I2cMaster;
+} I2C_HOST_CONTEXT;
+
+#define I2C_HOST_CONTEXT_FROM_PROTOCOL(a) CR (a, I2C_HOST_CONTEXT, I2cHost, I2C_HOST_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_COMPONENT_NAME_PROTOCOL    gI2cBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL   gI2cBusComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL    gI2cBusDriverBinding;
+
+extern EFI_COMPONENT_NAME_PROTOCOL    gI2cHostComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL   gI2cHostComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL    gI2cHostDriverBinding;
+
+/**
+  Start the I2C driver
+
+  This routine allocates the necessary resources for the driver.
+
+  This routine is called by I2cBusDriverStart to complete the driver
+  initialization.
+
+  @param[in] I2cBus                   Address of an I2C_BUS_CONTEXT structure
+  @param[in] Controller               Handle to the controller
+  @param[in] RemainingDevicePath      A pointer to the remaining portion of a device path.
+
+  @retval EFI_SUCCESS                 Driver API properly initialized
+  
+**/
+EFI_STATUS
+RegisterI2cDevice (
+  IN I2C_BUS_CONTEXT           *I2cBus,
+  IN EFI_HANDLE                 Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath
+  );
+
+/**
+  Unregister an I2C device.
+
+  This function removes the protocols installed on the controller handle and
+  frees the resources allocated for the I2C device.
+
+  @param  This                  The pointer to EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The controller handle of the I2C device.
+  @param  Handle                The child handle.
+
+  @retval EFI_SUCCESS           The I2C device is successfully unregistered.
+  @return Others                Some error occurs when unregistering the I2C device.
+
+**/
+EFI_STATUS
+UnRegisterI2cDevice (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  );
+
+/**
+  Create a path for the I2C device
+
+  Append the I2C slave path to the I2C master controller path.
+
+  @param[in] I2cDeviceContext           Address of an I2C_DEVICE_CONTEXT structure.
+  @param[in] BuildControllerNode        Flag to build controller node in device path.
+
+  @retval EFI_SUCCESS           The I2C device path is built successfully.
+  @return Others                It is failed to built device path.
+
+**/
+EFI_STATUS
+I2cBusDevicePathAppend (
+  IN I2C_DEVICE_CONTEXT     *I2cDeviceContext,
+  IN BOOLEAN                BuildControllerNode
+  );
+
+/**
+  Queue an I2C transaction for execution on the I2C device.
+
+  This routine must be called at or below TPL_NOTIFY.  For synchronous
+  requests this routine must be called at or below TPL_CALLBACK.
+
+  This routine queues an I2C transaction to the I2C controller for
+  execution on the I2C bus.
+
+  When Event is NULL, QueueRequest() operates synchronously and returns
+  the I2C completion status as its return value.
+
+  When Event is not NULL, QueueRequest() synchronously returns EFI_SUCCESS
+  indicating that the asynchronous I2C transaction was queued.  The values
+  above are returned in the buffer pointed to by I2cStatus upon the
+  completion of the I2C transaction when I2cStatus is not NULL.
+
+  The upper layer driver writer provides the following to the platform
+  vendor:
+  
+  1.  Vendor specific GUID for the I2C part
+  2.  Guidance on proper construction of the slave address array when the
+      I2C device uses more than one slave address.  The I2C bus protocol
+      uses the SlaveAddressIndex to perform relative to physical address
+      translation to access the blocks of hardware within the I2C device.
+
+  @param[in] This               Pointer to an EFI_I2C_IO_PROTOCOL structure.
+  @param[in] SlaveAddressIndex  Index value into an array of slave addresses
+                                for the I2C device.  The values in the array
+                                are specified by the board designer, with the
+                                third party I2C device driver writer providing
+                                the slave address order.
+
+                                For devices that have a single slave address,
+                                this value must be zero.  If the I2C device
+                                uses more than one slave address then the
+                                third party (upper level) I2C driver writer
+                                needs to specify the order of entries in the
+                                slave address array.
+
+                                \ref ThirdPartyI2cDrivers "Third Party I2C
+                                Drivers" section in I2cMaster.h.
+  @param[in] Event              Event to signal for asynchronous transactions,
+                                NULL for synchronous transactions
+  @param[in] RequestPacket      Pointer to an EFI_I2C_REQUEST_PACKET structure
+                                describing the I2C transaction
+  @param[out] I2cStatus         Optional buffer to receive the I2C transaction
+                                completion status
+
+  @retval EFI_SUCCESS           The asynchronous transaction was successfully
+                                queued when Event is not NULL.
+  @retval EFI_SUCCESS           The transaction completed successfully when
+                                Event is NULL.
+  @retval EFI_ABORTED           The request did not complete because the driver
+                                binding Stop() routine was called.
+  @retval EFI_BAD_BUFFER_SIZE   The RequestPacket->LengthInBytes value is too
+                                large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the
+                                transaction.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_NOT_FOUND         Reserved bit set in the SlaveAddress parameter
+  @retval EFI_NO_MAPPING        The EFI_I2C_HOST_PROTOCOL could not set the
+                                bus configuration required to access this I2C
+                                device.
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the slave
+                                address selected by SlaveAddressIndex.
+                                EFI_DEVICE_ERROR will be returned if the
+                                controller cannot distinguish when the NACK
+                                occurred.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C transaction
+  @retval EFI_UNSUPPORTED       The controller does not support the requested
+                                transaction.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusQueueRequest (
+  IN CONST EFI_I2C_IO_PROTOCOL  *This,
+  IN UINTN                      SlaveAddressIndex,
+  IN EFI_EVENT                  Event               OPTIONAL,
+  IN EFI_I2C_REQUEST_PACKET     *RequestPacket,
+  OUT EFI_STATUS                *I2cStatus          OPTIONAL
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Controller,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+  
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL                    *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  The user entry point for the I2C bus module. The user code starts with
+  this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2cBus(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  );
+
+/**
+  This is the unload handle for I2C bus module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusUnload (
+  IN EFI_HANDLE             ImageHandle
+  );
+
+/**
+  Release all the resources allocated for the I2C device.
+
+  This function releases all the resources allocated for the I2C device.
+
+  @param  I2cDeviceContext         The I2C child device involved for the operation.
+
+**/
+VOID
+ReleaseI2cDeviceContext (
+  IN I2C_DEVICE_CONTEXT          *I2cDeviceContext
+  );
+
+/**
+  Complete the current request
+
+  @param[in] I2cHost  Address of an I2C_HOST_CONTEXT structure.
+  @param[in] Status   Status of the I<sub>2</sub>C operation.
+
+  @return This routine returns the input status value.
+
+**/
+EFI_STATUS
+I2cHostRequestComplete (
+  I2C_HOST_CONTEXT *I2cHost,
+  EFI_STATUS       Status
+  );
+
+/**
+  Enable access to the I2C bus configuration
+
+  @param[in] I2cHostContext     Address of an I2C_HOST_CONTEXT structure
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_ABORTED           The request did not complete because the driver
+                                was shutdown.
+  @retval EFI_BAD_BUFFER_SIZE   The WriteBytes or ReadBytes buffer size is too large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the operation.
+                                This could indicate the slave device is not present.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_NO_MAPPING        Invalid I2cBusConfiguration value
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the
+                                slave address.  EFI_DEVICE_ERROR may also be
+                                returned if the controller can not distinguish
+                                when the NACK occurred.
+  @retval EFI_NOT_FOUND         I2C slave address exceeds maximum address
+  @retval EFI_NOT_READY         I2C bus is busy or operation pending, wait for
+                                the event and then read status.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C operation
+  @retval EFI_TIMEOUT           The transaction did not complete within an internally
+                                specified timeout period.
+
+**/
+EFI_STATUS
+I2cHostRequestEnable (
+  I2C_HOST_CONTEXT *I2cHost
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  );
+  
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+  
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL                    *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Handle the bus available event
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] Event    Address of an EFI_EVENT handle
+  @param[in] Context  Address of an I2C_HOST_CONTEXT structure
+
+**/
+VOID
+EFIAPI
+I2cHostRequestCompleteEvent (
+  IN EFI_EVENT Event,
+  IN VOID *Context
+  );
+
+/**
+  Handle the I2C bus configuration available event
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] Event    Address of an EFI_EVENT handle
+  @param[in] Context  Address of an I2C_HOST_CONTEXT structure
+
+**/
+VOID
+EFIAPI
+I2cHostI2cBusConfigurationAvailable (
+  IN EFI_EVENT Event,
+  IN VOID *Context
+  );
+
+/**
+  Queue an I2C operation for execution on the I2C controller.
+
+  This routine must be called at or below TPL_NOTIFY.  For synchronous
+  requests this routine must be called at or below TPL_CALLBACK.
+
+  N.B. The typical consumers of this API are the I2C bus driver and
+  on rare occasions the I2C test application.  Extreme care must be
+  taken by other consumers of this API to prevent confusing the
+  third party I2C drivers due to a state change at the I2C device
+  which the third party I2C drivers did not initiate.  I2C platform
+  drivers may use this API within these guidelines.
+
+  This layer uses the concept of I2C bus configurations to describe
+  the I2C bus.  An I2C bus configuration is defined as a unique
+  setting of the multiplexers and switches in the I2C bus which
+  enable access to one or more I2C devices.  When using a switch
+  to divide a bus, due to speed differences, the I2C platform layer
+  would define an I2C bus configuration for the I2C devices on each
+  side of the switch.  When using a multiplexer, the I2C platform
+  layer defines an I2C bus configuration for each of the selector
+  values required to control the multiplexer.  See Figure 1 in the
+  <a href="http://www.nxp.com/documents/user_manual/UM10204.pdf">I<sup>2</sup>C
+  Specification</a> for a complex I2C bus configuration.
+
+  The I2C host driver processes all operations in FIFO order.  Prior to
+  performing the operation, the I2C host driver calls the I2C platform
+  driver to reconfigure the switches and multiplexers in the I2C bus
+  enabling access to the specified I2C device.  The I2C platform driver
+  also selects the maximum bus speed for the device.  After the I2C bus
+  is configured, the I2C host driver calls the I2C port driver to
+  initialize the I2C controller and start the I2C operation.
+
+  @param[in] This             Address of an EFI_I2C_HOST_PROTOCOL instance.
+  @param[in] I2cBusConfiguration  I2C bus configuration to access the I2C
+                                  device.
+  @param[in] SlaveAddress     Address of the device on the I2C bus.
+  @param[in] Event            Event to set for asynchronous operations,
+                              NULL for synchronous operations
+  @param[in] RequestPacket    Address of an EFI_I2C_REQUEST_PACKET
+                              structure describing the I2C operation
+  @param[out] I2cStatus       Optional buffer to receive the I2C operation
+                              completion status
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_ABORTED           The request did not complete because the driver
+                                was shutdown.
+  @retval EFI_BAD_BUFFER_SIZE   The WriteBytes or ReadBytes buffer size is too large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the operation.
+                                This could indicate the slave device is not present.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_INVALID_PARAMETER TPL is too high
+  @retval EFI_NO_MAPPING        Invalid I2cBusConfiguration value
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the
+                                slave address.  EFI_DEVICE_ERROR may also be
+                                returned if the controller can not distinguish
+                                when the NACK occurred.
+  @retval EFI_NOT_FOUND         I2C slave address exceeds maximum address
+  @retval EFI_NOT_READY         I2C bus is busy or operation pending, wait for
+                                the event and then read status pointed to by
+                                the request packet.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C operation
+  @retval EFI_TIMEOUT           The transaction did not complete within an internally
+                                specified timeout period.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostQueueRequest (
+  IN CONST EFI_I2C_HOST_PROTOCOL  *This,
+  IN UINTN                        I2cBusConfiguration,
+  IN UINTN                        SlaveAddress,
+  IN EFI_EVENT                    Event            OPTIONAL,
+  IN EFI_I2C_REQUEST_PACKET       *RequestPacket,
+  OUT EFI_STATUS                  *I2cStatus       OPTIONAL
+  );
+
+/**
+  The user Entry Point for I2C host module. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2cHost(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  );
+
+/**
+  This is the unload handle for I2C host module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostUnload (
+  IN EFI_HANDLE             ImageHandle
+  );
+
+#endif  //  __I2C_DXE_H__
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.inf b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.inf
new file mode 100644
index 0000000000..5cd53b2fc4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.inf
@@ -0,0 +1,67 @@
+## @file
+#  I2c Dxe driver includes both I2c Bus and Host functionality.
+#
+#  This driver produce I2C Host Protocol on I2C controller handle, enumerate I2C
+#  devices on I2C bus and produce I2C IO Protocol on I2C devices.
+#
+#  Copyright (c) 2013 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = I2cDxe
+  MODULE_UNI_FILE                = I2cDxe.uni
+  FILE_GUID                      = ECA2AE9E-7594-4901-871C-449DA1A11660
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeI2c
+  UNLOAD_IMAGE                   = I2cUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  I2cDxe.c
+  I2cDxe.h
+  I2cHost.c
+  I2cBus.c
+
+[LibraryClasses]
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[Protocols]
+  gEfiI2cIoProtocolGuid                             ## BY_START
+  ## BY_START
+  ## TO_START
+  gEfiI2cHostProtocolGuid
+  ## BY_START
+  ## TO_START
+  gEfiDevicePathProtocolGuid
+  gEfiI2cMasterProtocolGuid                         ## TO_START
+  gEfiI2cEnumerateProtocolGuid                      ## TO_START
+  gEfiI2cBusConfigurationManagementProtocolGuid     ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  I2cDxeExtra.uni
+
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.uni
new file mode 100644
index 0000000000..03f9776d1f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxeExtra.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxeExtra.uni
new file mode 100644
index 0000000000..9496661f9e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHost.c b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHost.c
new file mode 100644
index 0000000000..68dd931887
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHost.c
@@ -0,0 +1,1228 @@
+/** @file
+  This file implements I2C Host Protocol which provides callers with the ability to 
+  do I/O transactions to all of the devices on the I2C bus.
+
+  Copyright (c) 2014, Hewlett-Packard Development Company, L.P.<BR>
+  Copyright (c) 2013 - 2015, 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 "I2cDxe.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gI2cHostDriverBinding = {
+  I2cHostDriverSupported,
+  I2cHostDriverStart,
+  I2cHostDriverStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mI2cHostDriverNameTable[] = {
+  { "eng;en", L"I2c Host Driver" },
+  { NULL , NULL }
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gI2cHostComponentName = {
+  (EFI_COMPONENT_NAME_GET_DRIVER_NAME) I2cHostComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME_GET_CONTROLLER_NAME) I2cHostComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gI2cHostComponentName2 = {
+  I2cHostComponentNameGetDriverName,
+  I2cHostComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mI2cHostDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This != &gI2cHostComponentName2)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL                    *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_I2C_MASTER_PROTOCOL                       *I2cMaster;
+  EFI_I2C_BUS_CONFIGURATION_MANAGEMENT_PROTOCOL *I2cBusConfigurationManagement;
+  EFI_STATUS                                    Status;
+
+  //
+  //  Locate I2C Bus Configuration Management Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cBusConfigurationManagementProtocolGuid,
+                  (VOID **)&I2cBusConfigurationManagement,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close the protocol because we don't use it here
+  //
+  gBS->CloseProtocol (
+                  Controller,
+                  &gEfiI2cBusConfigurationManagementProtocolGuid,
+                  This->DriverBindingHandle,
+                  Controller
+                  );
+
+  //
+  //  Locate I2C Master Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cMasterProtocolGuid,
+                  (VOID **)&I2cMaster,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  )
+{
+  EFI_STATUS                                          Status;
+  EFI_I2C_MASTER_PROTOCOL                             *I2cMaster;
+  EFI_I2C_BUS_CONFIGURATION_MANAGEMENT_PROTOCOL       *I2cBusConfigurationManagement;
+  I2C_HOST_CONTEXT                                    *I2cHostContext;
+
+  I2cMaster                     = NULL;
+  I2cHostContext                = NULL;
+  I2cBusConfigurationManagement = NULL;
+
+  //
+  // Locate I2C Bus Configuration Management Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cBusConfigurationManagementProtocolGuid,
+                  (VOID **)&I2cBusConfigurationManagement,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: Open I2C bus configuration error, Status = %r\n", Status));
+    return Status;
+  }
+
+  //
+  // Locate I2C Master Protocol
+  //
+  Status = gBS->OpenProtocol ( 
+                  Controller,
+                  &gEfiI2cMasterProtocolGuid,
+                  (VOID **)&I2cMaster,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: Open I2C master error, Status = %r\n", Status));
+    goto Exit;
+  }
+
+  //
+  // Allocate the I2C Host Context structure
+  //
+  I2cHostContext = AllocateZeroPool (sizeof (I2C_HOST_CONTEXT));
+  if (I2cHostContext == NULL) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: there is no enough memory to allocate.\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+
+  //
+  // Initialize the context structure for the current I2C Controller
+  //
+  I2cHostContext->Signature                     = I2C_HOST_SIGNATURE;
+  I2cHostContext->I2cMaster                     = I2cMaster;
+  I2cHostContext->I2cBusConfigurationManagement = I2cBusConfigurationManagement;
+  I2cHostContext->I2cBusConfiguration           = (UINTN) -1;
+  InitializeListHead(&I2cHostContext->RequestList);
+
+  //
+  // Reset the controller
+  //
+  Status = I2cMaster->Reset (I2cMaster);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: I2C controller reset failed!\n"));
+    goto Exit;
+  }
+
+  //
+  // Create the I2C transaction complete event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_I2C_SYNC,
+                  I2cHostRequestCompleteEvent,
+                  I2cHostContext,
+                  &I2cHostContext->I2cEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: create complete event error, Status = %r\n", Status));
+    goto Exit;
+  }
+
+  //
+  // Get the bus management event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_I2C_SYNC,
+                  I2cHostI2cBusConfigurationAvailable,
+                  I2cHostContext,
+                  &I2cHostContext->I2cBusConfigurationEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: create bus available event error, Status = %r\n", Status));
+    goto Exit;
+  }  
+
+  //
+  // Build the I2C host protocol for the current I2C controller
+  //
+  I2cHostContext->I2cHost.QueueRequest              = I2cHostQueueRequest;
+  I2cHostContext->I2cHost.I2cControllerCapabilities = I2cMaster->I2cControllerCapabilities;
+
+  //
+  //  Install the driver protocol
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  &I2cHostContext->I2cHost,
+                  NULL
+                  );
+Exit:
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: Start() function failed, Status = %r\n", Status));
+    if (I2cBusConfigurationManagement != NULL) {
+      gBS->CloseProtocol (
+                      Controller,
+                      &gEfiI2cBusConfigurationManagementProtocolGuid,
+                      This->DriverBindingHandle,
+                      Controller
+                      );      
+    }
+    
+    if ((I2cHostContext != NULL) && (I2cHostContext->I2cEvent != NULL)) {
+      gBS->CloseEvent (I2cHostContext->I2cEvent);
+      I2cHostContext->I2cEvent = NULL;
+    }
+
+    if ((I2cHostContext != NULL) && (I2cHostContext->I2cBusConfigurationEvent != NULL)) {
+      gBS->CloseEvent (I2cHostContext->I2cBusConfigurationEvent);
+      I2cHostContext->I2cBusConfigurationEvent = NULL;
+    }
+
+    //
+    //  Release the context structure upon failure
+    //
+    if (I2cHostContext != NULL) {
+      FreePool (I2cHostContext);
+    }
+  }
+
+  //
+  //  Return the operation status.
+  //
+  return Status;
+}
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;  
+  I2C_HOST_CONTEXT            *I2cHostContext;
+  EFI_I2C_HOST_PROTOCOL       *I2cHost;
+  EFI_TPL                     TplPrevious;
+
+  TplPrevious = EfiGetCurrentTpl ();
+  if (TplPrevious > TPL_I2C_SYNC) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: TPL %d is too high in Stop.\n", TplPrevious));
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  (VOID **) &I2cHost,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  I2cHostContext = I2C_HOST_CONTEXT_FROM_PROTOCOL (I2cHost);
+
+  //
+  // Raise TPL for critical section
+  //
+  TplPrevious = gBS->RaiseTPL (TPL_I2C_SYNC);
+  
+  //
+  // If there is pending request or pending bus configuration, do not stop
+  //
+  Status = EFI_DEVICE_ERROR;
+  if (( !I2cHostContext->I2cBusConfigurationManagementPending )
+    && IsListEmpty (&I2cHostContext->RequestList)) {
+    
+    //
+    //  Remove the I2C host protocol
+    //
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    Controller,
+                    &gEfiI2cHostProtocolGuid,
+                    I2cHost,
+                    NULL
+                    );
+  }
+  
+  //
+  // Leave critical section
+  //
+  gBS->RestoreTPL (TplPrevious);
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiI2cBusConfigurationManagementProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+
+    //
+    // Release I2c Host resources
+    //
+    if (I2cHostContext->I2cBusConfigurationEvent != NULL) {
+      gBS->CloseEvent (I2cHostContext->I2cBusConfigurationEvent);
+      I2cHostContext->I2cBusConfigurationEvent = NULL;
+    }
+    
+    if (I2cHostContext->I2cEvent != NULL) {
+      gBS->CloseEvent (I2cHostContext->I2cEvent);
+      I2cHostContext->I2cEvent = NULL;
+    }
+    
+    FreePool (I2cHostContext);
+  }
+
+  //
+  //  Return the stop status
+  //
+  return Status;
+}
+
+/**
+  Handle the I2C bus configuration available event
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] Event    Address of an EFI_EVENT handle
+  @param[in] Context  Address of an I2C_HOST_CONTEXT structure
+
+**/
+VOID
+EFIAPI
+I2cHostI2cBusConfigurationAvailable (
+  IN EFI_EVENT Event,
+  IN VOID *Context
+  )
+{
+  I2C_HOST_CONTEXT            *I2cHostContext;
+  EFI_I2C_MASTER_PROTOCOL     *I2cMaster;
+  I2C_REQUEST                 *I2cRequest;
+  LIST_ENTRY                  *EntryHeader;
+  LIST_ENTRY                  *Entry;
+  EFI_STATUS                  Status;
+
+  //
+  // Mark this I2C bus configuration management operation as complete
+  //
+  I2cHostContext = (I2C_HOST_CONTEXT *)Context;
+  I2cMaster      = I2cHostContext->I2cMaster;
+  ASSERT (I2cMaster != NULL);
+  //
+  // Clear flag to indicate I2C bus configuration is finished
+  //
+  I2cHostContext->I2cBusConfigurationManagementPending = FALSE;
+
+  //
+  //  Validate the completion status
+  //
+  if (EFI_ERROR (I2cHostContext->Status)) {
+    //
+    // Setting I2C bus configuration failed before
+    //
+    I2cHostRequestComplete (I2cHostContext, I2cHostContext->Status);
+
+    //
+    // Unknown I2C bus configuration
+    // Force next operation to enable the I2C bus configuration
+    //
+    I2cHostContext->I2cBusConfiguration = (UINTN) -1;
+    
+    //
+    // Do not continue current I2C request
+    //
+    return;
+  }
+
+  //
+  // Get the first request in the link with FIFO order
+  //
+  EntryHeader = &I2cHostContext->RequestList;
+  Entry = GetFirstNode (EntryHeader);
+  I2cRequest = I2C_REQUEST_FROM_ENTRY (Entry);
+
+  //
+  // Update the I2C bus configuration of the current I2C request
+  //
+  I2cHostContext->I2cBusConfiguration = I2cRequest->I2cBusConfiguration;
+
+  //
+  // Start an I2C operation on the host, the status is returned by I2cHostContext->Status
+  //
+  Status = I2cMaster->StartRequest ( 
+                        I2cMaster,
+                        I2cRequest->SlaveAddress,
+                        I2cRequest->RequestPacket,
+                        I2cHostContext->I2cEvent,
+                        &I2cHostContext->Status
+                        );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG((DEBUG_ERROR, "I2cHostI2cBusConfigurationAvailable: Error starting I2C operation, %r\n", Status));
+  }
+}
+
+/**
+  Complete the current request
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] I2cHostContext  Address of an I2C_HOST_CONTEXT structure.
+  @param[in] Status          Status of the I2C operation.
+
+  @return This routine returns the input status value.
+
+**/
+EFI_STATUS
+I2cHostRequestComplete (
+  I2C_HOST_CONTEXT *I2cHostContext,
+  EFI_STATUS       Status
+  )
+{
+  I2C_REQUEST *I2cRequest;
+  LIST_ENTRY  *EntryHeader;
+  LIST_ENTRY  *Entry;
+
+  //
+  // Remove the current I2C request from the list
+  //
+  EntryHeader = &I2cHostContext->RequestList;
+  Entry = GetFirstNode (EntryHeader);
+  I2cRequest = I2C_REQUEST_FROM_ENTRY (Entry);
+
+  //
+  // Save the status for QueueRequest
+  //
+  if ( NULL != I2cRequest->Status ) {
+    *I2cRequest->Status = Status;
+  }
+
+  //
+  //  Notify the user of the I2C request completion
+  //
+  if ( NULL != I2cRequest->Event ) {
+    gBS->SignalEvent (I2cRequest->Event);
+  }
+
+  //
+  // Done with this request, remove the current request from list
+  //
+  RemoveEntryList (&I2cRequest->Link);
+  FreePool (I2cRequest->RequestPacket);
+  FreePool (I2cRequest);
+
+  //
+  // If there is more I2C request, start next one
+  //
+  if(!IsListEmpty (EntryHeader)) {
+    I2cHostRequestEnable (I2cHostContext);
+  }
+  
+  return Status;
+}
+
+/**
+  Handle the bus available event
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] Event    Address of an EFI_EVENT handle
+  @param[in] Context  Address of an I2C_HOST_CONTEXT structure
+
+**/
+VOID
+EFIAPI
+I2cHostRequestCompleteEvent (
+  IN EFI_EVENT Event,
+  IN VOID *Context
+  )
+{
+  I2C_HOST_CONTEXT *I2cHostContext;
+
+  //
+  // Handle the completion event
+  //
+  I2cHostContext = (I2C_HOST_CONTEXT *)Context;
+  I2cHostRequestComplete (I2cHostContext, I2cHostContext->Status);
+}
+
+/**
+  Enable access to the I2C bus configuration
+
+  @param[in] I2cHostContext     Address of an I2C_HOST_CONTEXT structure
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_ABORTED           The request did not complete because the driver
+                                was shutdown.
+  @retval EFI_BAD_BUFFER_SIZE   The WriteBytes or ReadBytes buffer size is too large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the operation.
+                                This could indicate the slave device is not present.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_NO_MAPPING        Invalid I2cBusConfiguration value
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the
+                                slave address.  EFI_DEVICE_ERROR may also be
+                                returned if the controller can not distinguish
+                                when the NACK occurred.
+  @retval EFI_NOT_FOUND         I2C slave address exceeds maximum address
+  @retval EFI_NOT_READY         I2C bus is busy or operation pending, wait for
+                                the event and then read status.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C operation
+  @retval EFI_TIMEOUT           The transaction did not complete within an internally
+                                specified timeout period.
+
+**/
+EFI_STATUS
+I2cHostRequestEnable (
+  I2C_HOST_CONTEXT *I2cHostContext
+  )
+{
+  UINTN                                                 I2cBusConfiguration;
+  CONST EFI_I2C_BUS_CONFIGURATION_MANAGEMENT_PROTOCOL   *I2cBusConfigurationManagement;
+  I2C_REQUEST                                           *I2cRequest;
+  EFI_STATUS                                            Status;
+  EFI_TPL                                               TplPrevious;
+  LIST_ENTRY                                            *EntryHeader;
+  LIST_ENTRY                                            *Entry;
+
+  //
+  //  Assume pending request
+  //
+  Status = EFI_NOT_READY;
+
+  I2cBusConfigurationManagement = I2cHostContext->I2cBusConfigurationManagement;
+
+  //
+  //  Validate the I2c bus configuration
+  //
+  EntryHeader = &I2cHostContext->RequestList;
+  Entry       = GetFirstNode (EntryHeader);
+  I2cRequest = I2C_REQUEST_FROM_ENTRY (Entry);
+
+  I2cBusConfiguration = I2cRequest->I2cBusConfiguration;
+
+  if (I2cHostContext->I2cBusConfiguration != I2cBusConfiguration ) {
+    //
+    // Set flag to indicate I2C bus configuration is in progress
+    //
+    I2cHostContext->I2cBusConfigurationManagementPending = TRUE;
+    //
+    //  Update bus configuration for this device's requesting bus configuration
+    //
+    Status = I2cBusConfigurationManagement->EnableI2cBusConfiguration (
+                I2cBusConfigurationManagement,
+                I2cBusConfiguration,
+                I2cHostContext->I2cBusConfigurationEvent,
+                &I2cHostContext->Status
+                );
+  } else {
+    //
+    //  I2C bus configuration is same, no need change configuration and start I2c transaction directly
+    //
+    TplPrevious = gBS->RaiseTPL ( TPL_I2C_SYNC );
+
+    //
+    //  Same I2C bus configuration
+    //
+    I2cHostContext->Status = EFI_SUCCESS;
+    I2cHostI2cBusConfigurationAvailable (I2cHostContext->I2cBusConfigurationEvent, I2cHostContext);
+
+    //
+    //  Release the thread synchronization
+    //
+    gBS->RestoreTPL ( TplPrevious );
+  }
+  return Status;
+}
+
+/**
+  Queue an I2C operation for execution on the I2C controller.
+
+  This routine must be called at or below TPL_NOTIFY.  For synchronous
+  requests this routine must be called at or below TPL_CALLBACK.
+
+  N.B. The typical consumers of this API are the I2C bus driver and
+  on rare occasions the I2C test application.  Extreme care must be
+  taken by other consumers of this API to prevent confusing the
+  third party I2C drivers due to a state change at the I2C device
+  which the third party I2C drivers did not initiate.  I2C platform
+  drivers may use this API within these guidelines.
+
+  This layer uses the concept of I2C bus configurations to describe
+  the I2C bus.  An I2C bus configuration is defined as a unique
+  setting of the multiplexers and switches in the I2C bus which
+  enable access to one or more I2C devices.  When using a switch
+  to divide a bus, due to speed differences, the I2C platform layer
+  would define an I2C bus configuration for the I2C devices on each
+  side of the switch.  When using a multiplexer, the I2C platform
+  layer defines an I2C bus configuration for each of the selector
+  values required to control the multiplexer.  See Figure 1 in the
+  <a href="http://www.nxp.com/documents/user_manual/UM10204.pdf">I<sup>2</sup>C
+  Specification</a> for a complex I2C bus configuration.
+
+  The I2C host driver processes all operations in FIFO order.  Prior to
+  performing the operation, the I2C host driver calls the I2C platform
+  driver to reconfigure the switches and multiplexers in the I2C bus
+  enabling access to the specified I2C device.  The I2C platform driver
+  also selects the maximum bus speed for the device.  After the I2C bus
+  is configured, the I2C host driver calls the I2C port driver to
+  initialize the I2C controller and start the I2C operation.
+
+  @param[in] This             Address of an EFI_I2C_HOST_PROTOCOL instance.
+  @param[in] I2cBusConfiguration  I2C bus configuration to access the I2C
+                                  device.
+  @param[in] SlaveAddress     Address of the device on the I2C bus.
+  @param[in] Event            Event to set for asynchronous operations,
+                              NULL for synchronous operations
+  @param[in] RequestPacket    Address of an EFI_I2C_REQUEST_PACKET
+                              structure describing the I2C operation
+  @param[out] I2cStatus       Optional buffer to receive the I2C operation
+                              completion status
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_BAD_BUFFER_SIZE   The WriteBytes or ReadBytes buffer size is too large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the operation.
+                                This could indicate the slave device is not present.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_INVALID_PARAMETER TPL is too high
+  @retval EFI_NO_MAPPING        Invalid I2cBusConfiguration value
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the
+                                slave address.  EFI_DEVICE_ERROR may also be
+                                returned if the controller can not distinguish
+                                when the NACK occurred.
+  @retval EFI_NOT_FOUND         I2C slave address exceeds maximum address
+  @retval EFI_NOT_READY         I2C bus is busy or operation pending, wait for
+                                the event and then read status pointed to by
+                                the request packet.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C operation
+  @retval EFI_TIMEOUT           The transaction did not complete within an internally
+                                specified timeout period.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostQueueRequest (
+  IN CONST EFI_I2C_HOST_PROTOCOL  *This,
+  IN UINTN                        I2cBusConfiguration,
+  IN UINTN                        SlaveAddress,
+  IN EFI_EVENT                    Event            OPTIONAL,
+  IN EFI_I2C_REQUEST_PACKET       *RequestPacket,
+  OUT EFI_STATUS                  *I2cStatus       OPTIONAL
+  )
+{
+  EFI_STATUS        Status;
+  EFI_EVENT         SyncEvent;
+  EFI_TPL           TplPrevious;
+  I2C_REQUEST       *I2cRequest;
+  I2C_HOST_CONTEXT  *I2cHostContext;
+  BOOLEAN           FirstRequest;
+  UINTN             RequestPacketSize;
+  UINTN             StartBit;
+
+  SyncEvent    = NULL;
+  FirstRequest = FALSE;
+  Status       = EFI_SUCCESS;
+
+  if (RequestPacket == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  if ((SlaveAddress & I2C_ADDRESSING_10_BIT) != 0) {
+    //
+    // 10-bit address, bits 0-9 are used for 10-bit I2C slave addresses,
+    // bits 10-30 are reserved bits and must be zero
+    //
+    StartBit = 10;
+  } else {
+    //
+    // 7-bit address, Bits 0-6 are used for 7-bit I2C slave addresses,
+    // bits 7-30 are reserved bits and must be zero
+    //
+    StartBit = 7;
+  }
+
+  if (BitFieldRead32 ((UINT32)SlaveAddress, StartBit, 30) != 0) {
+    //
+    // Reserved bit set in the SlaveAddress parameter
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  I2cHostContext = I2C_HOST_CONTEXT_FROM_PROTOCOL (This);
+
+  if (Event == NULL) {
+    //
+    // For synchronous transaction, register an event used to wait for finishing synchronous transaction
+    //
+    Status = gBS->CreateEvent ( 
+                0,
+                TPL_I2C_SYNC,
+                NULL,
+                NULL,
+                &SyncEvent
+                );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+ 
+  //
+  // TPL should be at or below TPL_NOTIFY.
+  // For synchronous requests this routine must be called at or below TPL_CALLBACK.
+  //
+  TplPrevious = EfiGetCurrentTpl ();
+  if ((TplPrevious > TPL_I2C_SYNC) || ((Event == NULL) && (TplPrevious > TPL_CALLBACK))) {
+    DEBUG ((EFI_D_ERROR, "ERROR - TPL %d is too high!\n", TplPrevious));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Allocate the request structure
+  //
+  I2cRequest = AllocateZeroPool (sizeof (I2C_REQUEST));
+  if (I2cRequest == NULL) {
+    DEBUG ((EFI_D_ERROR, "WARNING - Failed to allocate I2C_REQUEST!\n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Initialize the request
+  //
+  I2cRequest->Signature           = I2C_REQUEST_SIGNATURE;
+  I2cRequest->I2cBusConfiguration = I2cBusConfiguration;
+  I2cRequest->SlaveAddress        = SlaveAddress;
+  I2cRequest->Event               = (Event == NULL) ? SyncEvent : Event;
+  I2cRequest->Status              = I2cStatus;
+
+  //
+  // Copy request packet into private buffer, as RequestPacket may be freed during asynchronous transaction
+  //
+  RequestPacketSize = sizeof (UINTN) + RequestPacket->OperationCount * sizeof (EFI_I2C_OPERATION);
+  I2cRequest->RequestPacket = AllocateZeroPool (RequestPacketSize);
+  ASSERT (I2cRequest->RequestPacket != NULL);
+  CopyMem (I2cRequest->RequestPacket, RequestPacket, RequestPacketSize);
+
+  //
+  // Synchronize with the other threads
+  //
+  gBS->RaiseTPL ( TPL_I2C_SYNC );
+  
+  FirstRequest = IsListEmpty (&I2cHostContext->RequestList);
+  
+  //
+  // Insert new I2C request in the list
+  //
+  InsertTailList (&I2cHostContext->RequestList, &I2cRequest->Link);
+
+  //
+  // Release the thread synchronization
+  //
+  gBS->RestoreTPL (TplPrevious);
+  
+  if (FirstRequest) {
+    //
+    // Start the first I2C request, then the subsequent of I2C request will continue
+    //
+    Status = I2cHostRequestEnable (I2cHostContext);
+  }
+
+  if (Event != NULL) {
+    //
+    // For asynchronous, return EFI_SUCCESS indicating that the asynchronously I2C transaction was queued.
+    // No real I2C operation status in I2cStatus
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // For synchronous transaction, wait for the operation completion
+  //
+  do {
+    Status = gBS->CheckEvent (SyncEvent);
+  } while (Status == EFI_NOT_READY);
+
+  //
+  // Get the I2C operation status
+  //
+  Status = I2cHostContext->Status;
+
+  //
+  // Return the I2C operation status
+  //
+  if (I2cStatus != NULL) {
+    *I2cStatus = Status;
+  }
+
+  //
+  // Close the event if necessary
+  //
+  if (SyncEvent != NULL) {
+    gBS->CloseEvent (SyncEvent);
+  }
+
+  return Status;
+}
+
+/**
+  The user Entry Point for I2C host module. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2cHost(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gI2cHostDriverBinding,
+             ImageHandle,
+             &gI2cHostComponentName,
+             &gI2cHostComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
+
+/**
+  This is the unload handle for I2C host module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostUnload (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HANDLE                        *DeviceHandleBuffer;
+  UINTN                             DeviceHandleCount;
+  UINTN                             Index;
+  EFI_COMPONENT_NAME_PROTOCOL       *ComponentName;
+  EFI_COMPONENT_NAME2_PROTOCOL      *ComponentName2;
+
+  //
+  // Get the list of all I2C Controller handles in the handle database.
+  // If there is an error getting the list, then the unload
+  // operation fails.
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiI2cHostProtocolGuid,
+                  NULL,
+                  &DeviceHandleCount,
+                  &DeviceHandleBuffer
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Disconnect the driver specified by ImageHandle from all
+    // the devices in the handle database.
+    //
+    for (Index = 0; Index < DeviceHandleCount; Index++) {
+      Status = gBS->DisconnectController (
+                      DeviceHandleBuffer[Index],
+                      ImageHandle,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+    }
+  }
+
+  //
+  // Uninstall all the protocols installed in the driver entry point
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  gI2cHostDriverBinding.DriverBindingHandle,
+                  &gEfiDriverBindingProtocolGuid,
+                  &gI2cHostDriverBinding,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Note we have to one by one uninstall the following protocols.
+  // It's because some of them are optionally installed based on
+  // the following PCD settings.
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnosticsDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentNameDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnostics2Disable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentName2Disable
+  //
+  Status = gBS->HandleProtocol (
+                  gI2cHostDriverBinding.DriverBindingHandle,
+                  &gEfiComponentNameProtocolGuid,
+                  (VOID **) &ComponentName
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           gI2cHostDriverBinding.DriverBindingHandle,
+           &gEfiComponentNameProtocolGuid,
+           ComponentName
+           );
+  }
+
+  Status = gBS->HandleProtocol (
+                  gI2cHostDriverBinding.DriverBindingHandle,
+                  &gEfiComponentName2ProtocolGuid,
+                  (VOID **) &ComponentName2
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           gI2cHostDriverBinding.DriverBindingHandle,
+           &gEfiComponentName2ProtocolGuid,
+           ComponentName2
+           );
+  }
+
+  Status = EFI_SUCCESS;
+
+Done:
+  //
+  // Free the buffer containing the list of handles from the handle database
+  //
+  if (DeviceHandleBuffer != NULL) {
+    gBS->FreePool (DeviceHandleBuffer);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.inf b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.inf
new file mode 100644
index 0000000000..2fb1085c6d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.inf
@@ -0,0 +1,55 @@
+## @file
+#  This driver produce I2C Host Protocol on I2C controller handle.
+#
+#  Copyright (c) 2013 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = I2cHostDxe
+  MODULE_UNI_FILE                = I2cHostDxe.uni
+  FILE_GUID                      = CDEC3671-816E-43DC-A002-DCD645229338
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeI2cHost
+  UNLOAD_IMAGE                   = I2cHostUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  I2cDxe.h
+  I2cHost.c
+
+[LibraryClasses]
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[Protocols]
+  gEfiI2cHostProtocolGuid                           ## BY_START
+  gEfiI2cMasterProtocolGuid                         ## TO_START
+  gEfiI2cBusConfigurationManagementProtocolGuid     ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  I2cHostDxeExtra.uni
+
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.uni
new file mode 100644
index 0000000000..a5a804303a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxeExtra.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxeExtra.uni
new file mode 100644
index 0000000000..d1f60d4f09
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.c
new file mode 100644
index 0000000000..ff7bbef278
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.c
@@ -0,0 +1,180 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for IsaBusDxe driver.
+  
+  Copyright (c) 2015, 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 "ComponentName.h"
+#include <Library/UefiLib.h>
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE     mIsaBusDriverNameTable[] = {
+  { "eng;en", L"PI ISA BUS Driver" },
+  { NULL , NULL }
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gIsaBusComponentName = {
+  IsaBusComponentNameGetDriverName,
+  IsaBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gIsaBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) IsaBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) IsaBusComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mIsaBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gIsaBusComponentName)
+           );
+}
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.h
new file mode 100644
index 0000000000..f533971833
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.h
@@ -0,0 +1,151 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for IsaBusDxe driver.
+  
+  Copyright (c) 2015, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials                          
+  are licensed and made available under the terms and conditions of the BSD License         
+  which accompanies this distribution.  The full text of the license may be found at        
+  http://opensource.org/licenses/bsd-license.php                                            
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             
+
+**/
+
+#ifndef _ISA_BUS_COMPONENT_NAME_H_
+#define _ISA_BUS_COMPONENT_NAME_H_
+
+#include <Uefi.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+
+extern EFI_COMPONENT_NAME_PROTOCOL  gIsaBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gIsaBusComponentName2;
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.c b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.c
new file mode 100644
index 0000000000..7067c6e6f0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.c
@@ -0,0 +1,461 @@
+/** @file
+  This file consumes the ISA Host Controller protocol produced by the ISA Host
+  Controller and installs the ISA Host Controller Service Binding protocol
+  on the ISA Host Controller's handle.
+
+  Copyright (c) 2015, 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 "IsaBusDxe.h"
+#include "ComponentName.h"
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided, 
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This 
+                                   parameter is ignored by device drivers, and is optional for bus 
+                                   drivers. For bus drivers, if this parameter is not NULL, then 
+                                   the bus driver must determine if the bus controller specified 
+                                   by ControllerHandle and the child controller specified 
+                                   by RemainingDevicePath are both supported by this 
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+IsaBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  VOID                              *Instance;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiIsaHcProtocolGuid,
+                  &Instance,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+      Controller,
+      &gEfiIsaHcProtocolGuid,
+      This->DriverBindingHandle,
+      Controller
+      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  &Instance,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+      Controller,
+      &gEfiDevicePathProtocolGuid,
+      This->DriverBindingHandle,
+      Controller
+      );
+  }
+
+  return Status;
+}
+
+ISA_BUS_CHILD_PRIVATE_DATA mIsaBusChildPrivateTemplate = {
+  ISA_BUS_CHILD_PRIVATE_DATA_SIGNATURE,
+  FALSE
+};
+
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. 
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned in ChildHandle. 
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                      then a new handle is created. If it is a pointer to an existing UEFI handle, 
+                      then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusCreateChild (
+  IN     EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN OUT EFI_HANDLE                    *ChildHandle
+  )
+{
+  EFI_STATUS                           Status;
+  ISA_BUS_PRIVATE_DATA                 *Private;
+  EFI_ISA_HC_PROTOCOL                  *IsaHc;
+  ISA_BUS_CHILD_PRIVATE_DATA           *Child;
+
+  Private = ISA_BUS_PRIVATE_DATA_FROM_THIS (This);
+
+  Child = AllocateCopyPool (sizeof (mIsaBusChildPrivateTemplate), &mIsaBusChildPrivateTemplate);
+  if (Child == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiIsaHcProtocolGuid, Private->IsaHc,
+                  &gEfiCallerIdGuid,      Child,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (Child);
+    return Status;
+  }
+
+  return gBS->OpenProtocol (
+                Private->IsaHcHandle,
+                &gEfiIsaHcProtocolGuid,
+                (VOID **) &IsaHc,
+                gIsaBusDriverBinding.DriverBindingHandle,
+                *ChildHandle,
+                EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                );
+}
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                       ChildHandle
+  )
+{
+  EFI_STATUS                           Status;
+  ISA_BUS_PRIVATE_DATA                 *Private;
+  EFI_ISA_HC_PROTOCOL                  *IsaHc;
+  ISA_BUS_CHILD_PRIVATE_DATA           *Child;
+
+  Private = ISA_BUS_PRIVATE_DATA_FROM_THIS (This);
+
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiCallerIdGuid,
+                  (VOID **) &Child,
+                  gIsaBusDriverBinding.DriverBindingHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ASSERT (Child->Signature == ISA_BUS_CHILD_PRIVATE_DATA_SIGNATURE);
+
+  if (Child->InDestroying) {
+    return EFI_SUCCESS;
+  }
+
+  Child->InDestroying = TRUE;
+  Status = gBS->CloseProtocol (
+                  Private->IsaHcHandle,
+                  &gEfiIsaHcProtocolGuid,
+                  gIsaBusDriverBinding.DriverBindingHandle,
+                  ChildHandle
+                  );
+  ASSERT_EFI_ERROR (Status);
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    ChildHandle,
+                    &gEfiIsaHcProtocolGuid, Private->IsaHc,
+                    &gEfiCallerIdGuid,      Child,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+             Private->IsaHcHandle,
+             &gEfiIsaHcProtocolGuid,
+             (VOID **) &IsaHc,
+             gIsaBusDriverBinding.DriverBindingHandle,
+             ChildHandle,
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    Child->InDestroying = FALSE;
+  } else {
+    FreePool (Child);
+  }
+
+  return Status;
+}
+
+ISA_BUS_PRIVATE_DATA   mIsaBusPrivateTemplate = {
+  ISA_BUS_PRIVATE_DATA_SIGNATURE,
+  {
+    IsaBusCreateChild,
+    IsaBusDestroyChild
+  }
+};
+
+/**
+  Starts a device controller or a bus controller.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This 
+                                   parameter is ignored by device drivers, and is optional for bus 
+                                   drivers. For a bus driver, if this parameter is NULL, then handles 
+                                   for all the children of Controller are created by this driver.  
+                                   If this parameter is not NULL and the first Device Path Node is 
+                                   not the End of Device Path Node, then only the handle for the 
+                                   child device specified by the first Device Path Node of 
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is 
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath;
+  ISA_BUS_PRIVATE_DATA              *Private;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiIsaHcProtocolGuid,
+                  (VOID **) &mIsaBusPrivateTemplate.IsaHc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiIsaHcProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    return Status;
+  }
+
+  Private = AllocateCopyPool (sizeof (mIsaBusPrivateTemplate), &mIsaBusPrivateTemplate);
+  ASSERT (Private != NULL);
+
+  Private->IsaHcHandle = Controller;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiIsaHcServiceBindingProtocolGuid, &Private->ServiceBinding,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Stops a device controller or a bus controller.
+  
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must 
+                                support a bus specific I/O protocol for the driver 
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL 
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                         Status;
+  EFI_SERVICE_BINDING_PROTOCOL       *ServiceBinding;
+  ISA_BUS_PRIVATE_DATA               *Private;
+  UINTN                              Index;
+  BOOLEAN                            AllChildrenStopped;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiIsaHcServiceBindingProtocolGuid,
+                  (VOID **) &ServiceBinding,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Private = ISA_BUS_PRIVATE_DATA_FROM_THIS (ServiceBinding);
+
+  if (NumberOfChildren == 0) {
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    Controller,
+                    &gEfiIsaHcServiceBindingProtocolGuid, &Private->ServiceBinding,
+                    NULL
+                    );
+    if (!EFI_ERROR (Status)) {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiDevicePathProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiIsaHcProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      FreePool (Private);
+    }
+
+    return Status;
+  }
+
+  AllChildrenStopped = TRUE;
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+    Status = ServiceBinding->DestroyChild (ServiceBinding, ChildHandleBuffer[Index]);
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  return AllChildrenStopped ? EFI_SUCCESS : EFI_DEVICE_ERROR;
+}
+
+//
+// ISA Bus Driver Binding Protocol Instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gIsaBusDriverBinding = {
+  IsaBusDriverBindingSupported,
+  IsaBusDriverBindingStart,
+  IsaBusDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+/**
+  Entry point of the IsaBusDxe driver.
+  
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+**/
+EFI_STATUS
+EFIAPI
+InitializeIsaBus (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS           Status;
+  
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gIsaBusDriverBinding,
+             ImageHandle,
+             &gIsaBusComponentName,
+             &gIsaBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.h b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.h
new file mode 100644
index 0000000000..86f6f56a61
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.h
@@ -0,0 +1,46 @@
+/** @file
+  Header file for the ISA BUS driver.
+
+  Copyright (c) 2015, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+
+**/
+
+#ifndef _ISA_BUS_H_
+#define _ISA_BUS_H_
+
+#include <Uefi.h>
+#include <Protocol/IsaHc.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/ServiceBinding.h>
+
+typedef struct {
+  UINT32                          Signature;
+  EFI_SERVICE_BINDING_PROTOCOL    ServiceBinding;
+  EFI_ISA_HC_PROTOCOL             *IsaHc;       ///< ISA HC protocol produced by the ISA Host Controller driver
+  EFI_HANDLE                      IsaHcHandle;  ///< ISA HC handle created by the ISA Host Controller driver
+} ISA_BUS_PRIVATE_DATA;
+#define ISA_BUS_PRIVATE_DATA_SIGNATURE    SIGNATURE_32 ('_', 'i', 's', 'b')
+#define ISA_BUS_PRIVATE_DATA_FROM_THIS(a) CR (a, ISA_BUS_PRIVATE_DATA, ServiceBinding, ISA_BUS_PRIVATE_DATA_SIGNATURE)
+
+typedef struct {
+  UINT32                          Signature;
+  BOOLEAN                         InDestroying; ///< Flag to avoid DestroyChild() re-entry.
+} ISA_BUS_CHILD_PRIVATE_DATA;
+#define ISA_BUS_CHILD_PRIVATE_DATA_SIGNATURE    SIGNATURE_32 ('_', 'i', 's', 'c')
+
+extern EFI_DRIVER_BINDING_PROTOCOL gIsaBusDriverBinding;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.inf b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.inf
new file mode 100644
index 0000000000..1d9e22ec4c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.inf
@@ -0,0 +1,65 @@
+## @file
+#  ISA Bus driver to manage the child devices attached to the ISA Host Controller.
+#
+#  This driver follows UEFI driver model and layers on ISA HC protocol defined
+#  in PI spec 1.2.1. It consumes the ISA Host Controller protocol produced by
+#  the ISA Host Controller and installs the ISA Host Controller Service Binding
+#  protocol on the ISA Host Controller's handle.
+#
+#  Copyright (c) 2015, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = IsaBusDxe
+  MODULE_UNI_FILE                = IsaBusDxe.uni
+  FILE_GUID                      = DCBE6D66-D928-4138-8041-358F35CBCF80
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeIsaBus
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gIsaBusDriverBinding
+#  COMPONENT_NAME                =  gIsaBusComponentName
+#  COMPONENT_NAME2               =  gIsaBusComponentName2
+#
+
+[Sources]
+  IsaBusDxe.h
+  IsaBusDxe.c
+  ComponentName.h
+  ComponentName.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  BaseLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiLib
+  DebugLib
+
+[Protocols]
+  ## CONSUMES 
+  ## PRODUCES
+  gEfiIsaHcProtocolGuid                 
+  gEfiIsaHcServiceBindingProtocolGuid   ## PRODUCES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  IsaBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.uni b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.uni
new file mode 100644
index 0000000000..5025c25300
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxeExtra.uni b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxeExtra.uni
new file mode 100644
index 0000000000..44354a0f99
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.c
new file mode 100644
index 0000000000..3181b9da5b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.c
@@ -0,0 +1,225 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for EHCI driver.
+
+Copyright (c) 2006 - 2011, 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 "Ehci.h"
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gEhciComponentName = {
+  EhciComponentNameGetDriverName,
+  EhciComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gEhciComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) EhciComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) EhciComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mEhciDriverNameTable[] = {
+  { "eng;en", L"Usb Ehci Driver" },
+  { NULL , NULL }
+};
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+EhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mEhciDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gEhciComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+EhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS           Status;
+  USB2_HC_DEV          *EhciDev;
+  EFI_USB2_HC_PROTOCOL *Usb2Hc;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gEhciDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  gEhciDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  EhciDev = EHC_FROM_THIS (Usb2Hc);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           EhciDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gEhciComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.h
new file mode 100644
index 0000000000..1a17d0bb49
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.h
@@ -0,0 +1,147 @@
+/** @file
+
+  This file contains the delarations for componet name routines.
+
+Copyright (c) 2008 - 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _COMPONENT_NAME_H_
+#define _COMPONENT_NAME_H_
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+EhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+EhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.c
new file mode 100644
index 0000000000..4e9e05f0e4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.c
@@ -0,0 +1,2106 @@
+/** @file  
+  The Ehci controller driver.
+
+  EhciDxe driver is responsible for managing the behavior of EHCI controller. 
+  It implements the interfaces of monitoring the status of all ports and transferring 
+  Control, Bulk, Interrupt and Isochronous requests to Usb2.0 device.
+
+  Note that EhciDxe driver is enhanced to guarantee that the EHCI controller get attached
+  to the EHCI controller before a UHCI or OHCI driver attaches to the companion UHCI or 
+  OHCI controller.  This way avoids the control transfer on a shared port between EHCI 
+  and companion host controller when UHCI or OHCI gets attached earlier than EHCI and a 
+  USB 2.0 device inserts.
+
+Copyright (c) 2006 - 2015, 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 "Ehci.h"
+
+//
+// Two arrays used to translate the EHCI port state (change)
+// to the UEFI protocol's port state (change).
+//
+USB_PORT_STATE_MAP  mUsbPortStateMap[] = {
+  {PORTSC_CONN,     USB_PORT_STAT_CONNECTION},
+  {PORTSC_ENABLED,  USB_PORT_STAT_ENABLE},
+  {PORTSC_SUSPEND,  USB_PORT_STAT_SUSPEND},
+  {PORTSC_OVERCUR,  USB_PORT_STAT_OVERCURRENT},
+  {PORTSC_RESET,    USB_PORT_STAT_RESET},
+  {PORTSC_POWER,    USB_PORT_STAT_POWER},
+  {PORTSC_OWNER,    USB_PORT_STAT_OWNER}
+};
+
+USB_PORT_STATE_MAP  mUsbPortChangeMap[] = {
+  {PORTSC_CONN_CHANGE,    USB_PORT_STAT_C_CONNECTION},
+  {PORTSC_ENABLE_CHANGE,  USB_PORT_STAT_C_ENABLE},
+  {PORTSC_OVERCUR_CHANGE, USB_PORT_STAT_C_OVERCURRENT}
+};
+
+EFI_DRIVER_BINDING_PROTOCOL
+gEhciDriverBinding = {
+  EhcDriverBindingSupported,
+  EhcDriverBindingStart,
+  EhcDriverBindingStop,
+  0x30,
+  NULL,
+  NULL
+};
+
+/**
+  Retrieves the capability of root hub ports.
+
+  @param  This                  This EFI_USB_HC_PROTOCOL instance.
+  @param  MaxSpeed              Max speed supported by the controller.
+  @param  PortNumber            Number of the root hub ports.
+  @param  Is64BitCapable        Whether the controller supports 64-bit memory
+                                addressing.
+
+  @retval EFI_SUCCESS           Host controller capability were retrieved successfully.
+  @retval EFI_INVALID_PARAMETER Either of the three capability pointer is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetCapability (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT UINT8                 *MaxSpeed,
+  OUT UINT8                 *PortNumber,
+  OUT UINT8                 *Is64BitCapable
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+
+  if ((MaxSpeed == NULL) || (PortNumber == NULL) || (Is64BitCapable == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (EHC_TPL);
+  Ehc             = EHC_FROM_THIS (This);
+
+  *MaxSpeed       = EFI_USB_SPEED_HIGH;
+  *PortNumber     = (UINT8) (Ehc->HcStructParams & HCSP_NPORTS);
+  *Is64BitCapable = (UINT8) (Ehc->HcCapParams & HCCP_64BIT);
+
+  DEBUG ((EFI_D_INFO, "EhcGetCapability: %d ports, 64 bit %d\n", *PortNumber, *Is64BitCapable));
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Provides software reset for the USB host controller.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  Attributes            A bit mask of the reset operation to perform.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcReset (
+  IN EFI_USB2_HC_PROTOCOL *This,
+  IN UINT16               Attributes
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  UINT32                  DbgCtrlStatus;
+
+  Ehc = EHC_FROM_THIS (This);
+
+  if (Ehc->DevicePath != NULL) {
+    //
+    // Report Status Code to indicate reset happens
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_IO_BUS_USB | EFI_IOB_PC_RESET),
+      Ehc->DevicePath
+      );
+  }
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+
+  switch (Attributes) {
+  case EFI_USB_HC_RESET_GLOBAL:
+  //
+  // Flow through, same behavior as Host Controller Reset
+  //
+  case EFI_USB_HC_RESET_HOST_CONTROLLER:
+    //
+    // Host Controller must be Halt when Reset it
+    //
+    if (Ehc->DebugPortNum != 0) {
+      DbgCtrlStatus = EhcReadDbgRegister(Ehc, 0);
+      if ((DbgCtrlStatus & (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) == (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) {
+        Status = EFI_SUCCESS;
+        goto ON_EXIT;
+      }
+    }
+
+    if (!EhcIsHalt (Ehc)) {
+      Status = EhcHaltHC (Ehc, EHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        Status = EFI_DEVICE_ERROR;
+        goto ON_EXIT;
+      }
+    }
+
+    //
+    // Clean up the asynchronous transfers, currently only
+    // interrupt supports asynchronous operation.
+    //
+    EhciDelAllAsyncIntTransfers (Ehc);
+    EhcAckAllInterrupt (Ehc);
+    EhcFreeSched (Ehc);
+
+    Status = EhcResetHC (Ehc, EHC_RESET_TIMEOUT);
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+
+    Status = EhcInitHC (Ehc);
+    break;
+
+  case EFI_USB_HC_RESET_GLOBAL_WITH_DEBUG:
+  case EFI_USB_HC_RESET_HOST_WITH_DEBUG:
+    Status = EFI_UNSUPPORTED;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "EhcReset: exit status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Retrieve the current state of the USB host controller.
+
+  @param  This                   This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Variable to return the current host controller
+                                 state.
+
+  @retval EFI_SUCCESS            Host controller state was returned in State.
+  @retval EFI_INVALID_PARAMETER  State is NULL.
+  @retval EFI_DEVICE_ERROR       An error was encountered while attempting to
+                                 retrieve the host controller's current state.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetState (
+  IN   EFI_USB2_HC_PROTOCOL  *This,
+  OUT  EFI_USB_HC_STATE      *State
+  )
+{
+  EFI_TPL                 OldTpl;
+  USB2_HC_DEV             *Ehc;
+
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+  Ehc     = EHC_FROM_THIS (This);
+
+  if (EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT)) {
+    *State = EfiUsbHcStateHalt;
+  } else {
+    *State = EfiUsbHcStateOperational;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  DEBUG ((EFI_D_INFO, "EhcGetState: current state %d\n", *State));
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Sets the USB host controller to a specific state.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                 The state of the host controller that will be set.
+
+  @retval EFI_SUCCESS           The USB host controller was successfully placed
+                                in the state specified by State.
+  @retval EFI_INVALID_PARAMETER State is invalid.
+  @retval EFI_DEVICE_ERROR      Failed to set the state due to device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcSetState (
+  IN EFI_USB2_HC_PROTOCOL *This,
+  IN EFI_USB_HC_STATE     State
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  EFI_USB_HC_STATE        CurState;
+
+  Status = EhcGetState (This, &CurState);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (CurState == State) {
+    return EFI_SUCCESS;
+  }
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+  Ehc     = EHC_FROM_THIS (This);
+
+  switch (State) {
+  case EfiUsbHcStateHalt:
+    Status = EhcHaltHC (Ehc, EHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateOperational:
+    if (EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_SYS_ERROR)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    //
+    // Software must not write a one to this field unless the host controller
+    // is in the Halted state. Doing so will yield undefined results.
+    // refers to Spec[EHCI1.0-2.3.1]
+    //
+    if (!EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    Status = EhcRunHC (Ehc, EHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateSuspend:
+    Status = EFI_UNSUPPORTED;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG ((EFI_D_INFO, "EhcSetState: exit status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            The root hub port to retrieve the state from.
+                                This value is zero-based.
+  @param  PortStatus            Variable to receive the port state.
+
+  @retval EFI_SUCCESS           The status of the USB root hub port specified.
+                                by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetRootHubPortStatus (
+  IN   EFI_USB2_HC_PROTOCOL  *This,
+  IN   UINT8                 PortNumber,
+  OUT  EFI_USB_PORT_STATUS   *PortStatus
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  UINTN                   Index;
+  UINTN                   MapSize;
+  EFI_STATUS              Status;
+  UINT32                  DbgCtrlStatus;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl    = gBS->RaiseTPL (EHC_TPL);
+
+  Ehc       = EHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset                        = (UINT32) (EHC_PORT_STAT_OFFSET + (4 * PortNumber));
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  if ((Ehc->DebugPortNum != 0) && (PortNumber == (Ehc->DebugPortNum - 1))) {
+    DbgCtrlStatus = EhcReadDbgRegister(Ehc, 0);
+    if ((DbgCtrlStatus & (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) == (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) {
+      goto ON_EXIT;
+    }
+  }
+
+  State                         = EhcReadOpReg (Ehc, Offset);
+
+  //
+  // Identify device speed. If in K state, it is low speed.
+  // If the port is enabled after reset, the device is of
+  // high speed. The USB bus driver should retrieve the actual
+  // port speed after reset.
+  //
+  if (EHC_BIT_IS_SET (State, PORTSC_LINESTATE_K)) {
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+
+  } else if (EHC_BIT_IS_SET (State, PORTSC_ENABLED)) {
+    PortStatus->PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+  }
+
+  //
+  // Convert the EHCI port/port change state to UEFI status
+  //
+  MapSize = sizeof (mUsbPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (EHC_BIT_IS_SET (State, mUsbPortStateMap[Index].HwState)) {
+      PortStatus->PortStatus = (UINT16) (PortStatus->PortStatus | mUsbPortStateMap[Index].UefiState);
+    }
+  }
+
+  MapSize = sizeof (mUsbPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (EHC_BIT_IS_SET (State, mUsbPortChangeMap[Index].HwState)) {
+      PortStatus->PortChangeStatus = (UINT16) (PortStatus->PortChangeStatus | mUsbPortChangeMap[Index].UefiState);
+    }
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcSetRootHubPortFeature (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  IN  UINT8                 PortNumber,
+  IN  EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+
+  OldTpl    = gBS->RaiseTPL (EHC_TPL);
+  Ehc       = EHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset  = (UINT32) (EHC_PORT_STAT_OFFSET + (4 * PortNumber));
+  State   = EhcReadOpReg (Ehc, Offset);
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bit
+  //
+  State &= ~PORTSC_CHANGE_MASK;
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Sofeware can't set this bit, Port can only be enable by
+    // EHCI as a part of the reset and enable
+    //
+    State |= PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    State |= PORTSC_SUSPEND;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // Make sure Host Controller not halt before reset it
+    //
+    if (EhcIsHalt (Ehc)) {
+      Status = EhcRunHC (Ehc, EHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_INFO, "EhcSetRootHubPortFeature :failed to start HC - %r\n", Status));
+        break;
+      }
+    }
+
+    //
+    // Set one to PortReset bit must also set zero to PortEnable bit
+    //
+    State |= PORTSC_RESET;
+    State &= ~PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // Set port power bit when PPC is 1
+    //
+    if ((Ehc->HcCapParams & HCSP_PPC) == HCSP_PPC) {
+      State |= PORTSC_POWER;
+      EhcWriteOpReg (Ehc, Offset, State);
+    }
+    break;
+
+  case EfiUsbPortOwner:
+    State |= PORTSC_OWNER;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "EhcSetRootHubPortFeature: exit status %r\n", Status));
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Specifies the root hub port whose feature is
+                                requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was cleared
+                                for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcClearRootHubPortFeature (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  IN  UINT8                 PortNumber,
+  IN  EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+
+  OldTpl    = gBS->RaiseTPL (EHC_TPL);
+  Ehc       = EHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset  = EHC_PORT_STAT_OFFSET + (4 * PortNumber);
+  State   = EhcReadOpReg (Ehc, Offset);
+  State &= ~PORTSC_CHANGE_MASK;
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Clear PORT_ENABLE feature means disable port.
+    //
+    State &= ~PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    //
+    // A write of zero to this bit is ignored by the host
+    // controller. The host controller will unconditionally
+    // set this bit to a zero when:
+    //   1. software sets the Forct Port Resume bit to a zero from a one.
+    //   2. software sets the Port Reset bit to a one frome a zero.
+    //
+    State &= ~PORSTSC_RESUME;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // Clear PORT_RESET means clear the reset signal.
+    //
+    State &= ~PORTSC_RESET;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortOwner:
+    //
+    // Clear port owner means this port owned by EHC
+    //
+    State &= ~PORTSC_OWNER;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortConnectChange:
+    //
+    // Clear connect status change
+    //
+    State |= PORTSC_CONN_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortEnableChange:
+    //
+    // Clear enable status change
+    //
+    State |= PORTSC_ENABLE_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortOverCurrentChange:
+    //
+    // Clear PortOverCurrent change
+    //
+    State |= PORTSC_OVERCUR_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // Clear port power bit when PPC is 1
+    //
+    if ((Ehc->HcCapParams & HCSP_PPC) == HCSP_PPC) {
+      State &= ~PORTSC_POWER;
+      EhcWriteOpReg (Ehc, Offset, State);
+    }
+    break;
+  case EfiUsbPortSuspendChange:
+  case EfiUsbPortResetChange:
+    //
+    // Not supported or not related operation
+    //
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "EhcClearRootHubPortFeature: exit status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Submits control transfer to a target USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         The target device address.
+  @param  DeviceSpeed           Target device speed.
+  @param  MaximumPacketLength   Maximum packet size the default control transfer
+                                endpoint is capable of sending or receiving.
+  @param  Request               USB device request to send.
+  @param  TransferDirection     Specifies the data direction for the data stage
+  @param  Data                  Data buffer to be transmitted or received from USB
+                                device.
+  @param  DataLength            The size (in bytes) of the data buffer.
+  @param  TimeOut               Indicates the maximum timeout, in millisecond.
+  @param  Translator            Transaction translator to be used by this device.
+  @param  TransferResult        Return the result of this control transfer.
+
+  @retval EFI_SUCCESS           Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcControlTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  EFI_USB_DEVICE_REQUEST              *Request,
+  IN  EFI_USB_DATA_DIRECTION              TransferDirection,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  URB                     *Urb;
+  EFI_TPL                 OldTpl;
+  UINT8                   Endpoint;
+  EFI_STATUS              Status;
+
+  //
+  // Validate parameters
+  //
+  if ((Request == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbDataIn) &&
+      (TransferDirection != EfiUsbDataOut) &&
+      (TransferDirection != EfiUsbNoData)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection == EfiUsbNoData) &&
+      ((Data != NULL) || (*DataLength != 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) &&
+     ((Data == NULL) || (*DataLength == 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8)  && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (EHC_TPL);
+  Ehc             = EHC_FROM_THIS (This);
+
+  Status          = EFI_DEVICE_ERROR;
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    DEBUG ((EFI_D_ERROR, "EhcControlTransfer: HC halted at entrance\n"));
+
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  //
+  // Encode the direction in address, although default control
+  // endpoint is bidirectional. EhcCreateUrb expects this
+  // combination of Ep addr and its direction.
+  //
+  Endpoint = (UINT8) (0 | ((TransferDirection == EfiUsbDataIn) ? 0x80 : 0));
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          Endpoint,
+          DeviceSpeed,
+          0,
+          MaximumPacketLength,
+          Translator,
+          EHC_CTRL_TRANSFER,
+          Request,
+          Data,
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcControlTransfer: failed to create URB"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToAsync (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromAsync (Ehc, Urb->Qh);
+
+  //
+  // Get the status from URB. The result is updated in EhcCheckUrbResult
+  // which is called by EhcExecTransfer
+  //
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+  EhcFreeUrb (Ehc, Urb);
+
+ON_EXIT:
+  Ehc->PciIo->Flush (Ehc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcControlTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support bulk
+                                transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of
+                                sending or receiving.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data to transmit
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to
+                                use of the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which
+                                the transfer is allowed to complete.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcBulkTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  UINT8                               DataBuffersNumber,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  URB                     *Urb;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+
+  //
+  // Validate the parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (Data[0] == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 0) && (*DataToggle != 1)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((EFI_USB_SPEED_HIGH == DeviceSpeed) && (MaximumPacketLength > 512))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (EHC_TPL);
+  Ehc             = EHC_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    DEBUG ((EFI_D_ERROR, "EhcBulkTransfer: HC is halted\n"));
+
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          *DataToggle,
+          MaximumPacketLength,
+          Translator,
+          EHC_BULK_TRANSFER,
+          NULL,
+          Data[0],
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcBulkTransfer: failed to create URB\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToAsync (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromAsync (Ehc, Urb->Qh);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+  *DataToggle     = Urb->DataToggle;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+  EhcFreeUrb (Ehc, Urb);
+
+ON_EXIT:
+  Ehc->PciIo->Flush (Ehc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcBulkTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+
+/**
+  Submits an asynchronous interrupt transfer to an
+  interrupt endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+  @param  IsNewTransfer         If TRUE, to submit an new asynchronous interrupt
+                                transfer If FALSE, to remove the specified
+                                asynchronous interrupt.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  PollingInterval       The he interval, in milliseconds, that the transfer
+                                is polled.
+  @param  DataLength            The length of data to receive at the rate specified
+                                by  PollingInterval.
+  @param  Translator            Transaction translator to use.
+  @param  CallBackFunction      Function to call at the rate specified by
+                                PollingInterval.
+  @param  Context               Context to CallBackFunction.
+
+  @retval EFI_SUCCESS           The request has been successfully submitted or canceled.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request failed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcAsyncInterruptTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                  * This,
+  IN  UINT8                                 DeviceAddress,
+  IN  UINT8                                 EndPointAddress,
+  IN  UINT8                                 DeviceSpeed,
+  IN  UINTN                                 MaximumPacketLength,
+  IN  BOOLEAN                               IsNewTransfer,
+  IN  OUT UINT8                             *DataToggle,
+  IN  UINTN                                 PollingInterval,
+  IN  UINTN                                 DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR    * Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK       CallBackFunction,
+  IN  VOID                                  *Context OPTIONAL
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  URB                     *Urb;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  UINT8                   *Data;
+
+  //
+  // Validate parameters
+  //
+  if (!EHCI_IS_DATAIN (EndPointAddress)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsNewTransfer) {
+    if (DataLength == 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((*DataToggle != 1) && (*DataToggle != 0)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((PollingInterval > 255) || (PollingInterval < 1)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+  Ehc     = EHC_FROM_THIS (This);
+
+  //
+  // Delete Async interrupt transfer request. DataToggle will return
+  // the next data toggle to use.
+  //
+  if (!IsNewTransfer) {
+    Status = EhciDelAsyncIntTransfer (Ehc, DeviceAddress, EndPointAddress, DataToggle);
+
+    DEBUG ((EFI_D_INFO, "EhcAsyncInterruptTransfer: remove old transfer - %r\n", Status));
+    goto ON_EXIT;
+  }
+
+  Status = EFI_SUCCESS;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    DEBUG ((EFI_D_ERROR, "EhcAsyncInterruptTransfer: HC is halt\n"));
+    EhcAckAllInterrupt (Ehc);
+
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  Data = AllocatePool (DataLength);
+
+  if (Data == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcAsyncInterruptTransfer: failed to allocate buffer\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          *DataToggle,
+          MaximumPacketLength,
+          Translator,
+          EHC_INT_TRANSFER_ASYNC,
+          NULL,
+          Data,
+          DataLength,
+          CallBackFunction,
+          Context,
+          PollingInterval
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcAsyncInterruptTransfer: failed to create URB\n"));
+
+    gBS->FreePool (Data);
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  //
+  // New asynchronous transfer must inserted to the head.
+  // Check the comments in EhcMoniteAsyncRequests
+  //
+  EhcLinkQhToPeriod (Ehc, Urb->Qh);
+  InsertHeadList (&Ehc->AsyncIntTransfers, &Urb->UrbList);
+
+ON_EXIT:
+  Ehc->PciIo->Flush (Ehc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits synchronous interrupt transfer to an interrupt endpoint
+  of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+                                of sending or receiving.
+  @param  Data                  Buffer of data that will be transmitted to  USB
+                                device or received from USB device.
+  @param  DataLength            On input, the size, in bytes, of the data buffer; On
+                                output, the number of bytes transferred.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  TimeOut               Maximum time, in second, to complete.
+  @param  Translator            Transaction translator to use.
+  @param  TransferResult        Variable to receive the transfer result.
+
+  @return EFI_SUCCESS           The transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER Some parameters are invalid.
+  @return EFI_TIMEOUT           The transfer failed due to timeout.
+  @return EFI_DEVICE_ERROR      The failed due to host controller or device error
+
+**/
+EFI_STATUS
+EFIAPI
+EhcSyncInterruptTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  URB                     *Urb;
+  EFI_STATUS              Status;
+
+  //
+  // Validates parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8))  ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((DeviceSpeed == EFI_USB_SPEED_HIGH) && (MaximumPacketLength > 3072))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (EHC_TPL);
+  Ehc             = EHC_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    DEBUG ((EFI_D_ERROR, "EhcSyncInterruptTransfer: HC is halt\n"));
+
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          *DataToggle,
+          MaximumPacketLength,
+          Translator,
+          EHC_INT_TRANSFER_SYNC,
+          NULL,
+          Data,
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcSyncInterruptTransfer: failed to create URB\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToPeriod (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromPeriod (Ehc, Urb->Qh);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+  *DataToggle     = Urb->DataToggle;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+ON_EXIT:
+  Ehc->PciIo->Flush (Ehc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcSyncInterruptTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+
+/**
+  Submits isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  TransferResult       Variable to receive the transfer result.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer is unsupported.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcIsochronousTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  UINT8                               DataBuffersNumber,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Submits Async isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  IsochronousCallBack  Function to be called when the transfer complete.
+  @param  Context              Context passed to the call back function as
+                               parameter.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcAsyncIsochronousTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  UINT8                               DataBuffersNumber,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     IsochronousCallBack,
+  IN  VOID                                *Context
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Entry point for EFI drivers.
+
+  @param  ImageHandle       EFI_HANDLE.
+  @param  SystemTable       EFI_SYSTEM_TABLE.
+
+  @return EFI_SUCCESS       Success.
+          EFI_DEVICE_ERROR  Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gEhciDriverBinding,
+           ImageHandle,
+           &gEhciComponentName,
+           &gEhciComponentName2
+           );
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has Usb2HcProtocol installed will
+  be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  EFI_STATUS              Status;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  USB_CLASSC              UsbClassCReg;
+
+  //
+  // Test whether there is PCI IO Protocol attached on the controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (USB_CLASSC) / sizeof (UINT8),
+                        &UsbClassCReg
+                        );
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Test whether the controller belongs to Ehci type
+  //
+  if ((UsbClassCReg.BaseCode != PCI_CLASS_SERIAL) || (UsbClassCReg.SubClassCode != PCI_CLASS_SERIAL_USB)
+      || ((UsbClassCReg.ProgInterface != PCI_IF_EHCI) && (UsbClassCReg.ProgInterface != PCI_IF_UHCI) && (UsbClassCReg.ProgInterface != PCI_IF_OHCI))) {
+
+    Status = EFI_UNSUPPORTED;
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+/**
+  Get the usb debug port related information.
+
+  @param  Ehc                The EHCI device.
+
+  @retval RETURN_SUCCESS     Get debug port number, bar and offset successfully.
+  @retval Others             The usb host controller does not supported usb debug port capability.
+
+**/
+EFI_STATUS
+EhcGetUsbDebugPortInfo (
+  IN  USB2_HC_DEV     *Ehc
+ )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT16              PciStatus;
+  UINT8               CapabilityPtr;
+  UINT8               CapabilityId;
+  UINT16              DebugPort;
+  EFI_STATUS          Status;
+
+  ASSERT (Ehc->PciIo != NULL);
+  PciIo = Ehc->PciIo;
+
+  //
+  // Detect if the EHCI host controller support Capaility Pointer.
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_PRIMARY_STATUS_OFFSET,
+                        sizeof (UINT16),
+                        &PciStatus
+                        );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((PciStatus & EFI_PCI_STATUS_CAPABILITY) == 0) {
+    //
+    // The Pci Device Doesn't Support Capability Pointer.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get Pointer To Capability List
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CAPBILITY_POINTER_OFFSET,
+                        1,
+                        &CapabilityPtr
+                        );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Find Capability ID 0xA, Which Is For Debug Port
+  //
+  while (CapabilityPtr != 0) {
+    Status = PciIo->Pci.Read (
+                          PciIo,
+                          EfiPciIoWidthUint8,
+                          CapabilityPtr,
+                          1,
+                          &CapabilityId
+                          );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (CapabilityId == EHC_DEBUG_PORT_CAP_ID) {
+      break;
+    }
+
+    Status = PciIo->Pci.Read (
+                          PciIo,
+                          EfiPciIoWidthUint8,
+                          CapabilityPtr + 1,
+                          1,
+                          &CapabilityPtr
+                          );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // No Debug Port Capability Found
+  //
+  if (CapabilityPtr == 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get The Base Address Of Debug Port Register In Debug Port Capability Register
+  //
+  Status = PciIo->Pci.Read (
+                        Ehc->PciIo,
+                        EfiPciIoWidthUint8,
+                        CapabilityPtr + 2,
+                        sizeof (UINT16),
+                        &DebugPort
+                        );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Ehc->DebugPortOffset = DebugPort & 0x1FFF;
+  Ehc->DebugPortBarNum = (UINT8)((DebugPort >> 13) - 1);
+  Ehc->DebugPortNum    = (UINT8)((Ehc->HcStructParams & 0x00F00000) >> 20);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Create and initialize a USB2_HC_DEV.
+
+  @param  PciIo                  The PciIo on this device.
+  @param  DevicePath             The device path of host controller.
+  @param  OriginalPciAttributes  Original PCI attributes.
+
+  @return  The allocated and initialized USB2_HC_DEV structure if created,
+           otherwise NULL.
+
+**/
+USB2_HC_DEV *
+EhcCreateUsb2Hc (
+  IN EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN UINT64                    OriginalPciAttributes
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_STATUS              Status;
+
+  Ehc = AllocateZeroPool (sizeof (USB2_HC_DEV));
+
+  if (Ehc == NULL) {
+    return NULL;
+  }
+
+  //
+  // Init EFI_USB2_HC_PROTOCOL interface and private data structure
+  //
+  Ehc->Signature                        = USB2_HC_DEV_SIGNATURE;
+
+  Ehc->Usb2Hc.GetCapability             = EhcGetCapability;
+  Ehc->Usb2Hc.Reset                     = EhcReset;
+  Ehc->Usb2Hc.GetState                  = EhcGetState;
+  Ehc->Usb2Hc.SetState                  = EhcSetState;
+  Ehc->Usb2Hc.ControlTransfer           = EhcControlTransfer;
+  Ehc->Usb2Hc.BulkTransfer              = EhcBulkTransfer;
+  Ehc->Usb2Hc.AsyncInterruptTransfer    = EhcAsyncInterruptTransfer;
+  Ehc->Usb2Hc.SyncInterruptTransfer     = EhcSyncInterruptTransfer;
+  Ehc->Usb2Hc.IsochronousTransfer       = EhcIsochronousTransfer;
+  Ehc->Usb2Hc.AsyncIsochronousTransfer  = EhcAsyncIsochronousTransfer;
+  Ehc->Usb2Hc.GetRootHubPortStatus      = EhcGetRootHubPortStatus;
+  Ehc->Usb2Hc.SetRootHubPortFeature     = EhcSetRootHubPortFeature;
+  Ehc->Usb2Hc.ClearRootHubPortFeature   = EhcClearRootHubPortFeature;
+  Ehc->Usb2Hc.MajorRevision             = 0x2;
+  Ehc->Usb2Hc.MinorRevision             = 0x0;
+
+  Ehc->PciIo                 = PciIo;
+  Ehc->DevicePath            = DevicePath;
+  Ehc->OriginalPciAttributes = OriginalPciAttributes;
+
+  InitializeListHead (&Ehc->AsyncIntTransfers);
+
+  Ehc->HcStructParams = EhcReadCapRegister (Ehc, EHC_HCSPARAMS_OFFSET);
+  Ehc->HcCapParams    = EhcReadCapRegister (Ehc, EHC_HCCPARAMS_OFFSET);
+  Ehc->CapLen         = EhcReadCapRegister (Ehc, EHC_CAPLENGTH_OFFSET) & 0x0FF;
+
+  DEBUG ((EFI_D_INFO, "EhcCreateUsb2Hc: capability length %d\n", Ehc->CapLen));
+
+  //
+  // EHCI Controllers with a CapLen of 0 are ignored.
+  //
+  if (Ehc->CapLen == 0) {
+    gBS->FreePool (Ehc);
+    return NULL;
+  }
+  
+  EhcGetUsbDebugPortInfo (Ehc);
+
+  //
+  // Create AsyncRequest Polling Timer
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  EhcMonitorAsyncRequests,
+                  Ehc,
+                  &Ehc->PollTimer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->FreePool (Ehc);
+    return NULL;
+  }
+
+  return Ehc;
+}
+
+/**
+  One notified function to stop the Host Controller when gBS->ExitBootServices() called.
+
+  @param  Event                   Pointer to this event
+  @param  Context                 Event handler private data
+
+**/
+VOID
+EFIAPI
+EhcExitBootService (
+  EFI_EVENT                      Event,
+  VOID                           *Context
+  )
+
+{
+  USB2_HC_DEV   *Ehc;
+
+  Ehc = (USB2_HC_DEV *) Context;
+
+  //
+  // Reset the Host Controller
+  //
+  EhcResetHC (Ehc, EHC_RESET_TIMEOUT);
+}
+
+
+/**
+  Starting the Usb EHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          supports this device.
+  @return EFI_UNSUPPORTED      do not support this device.
+  @return EFI_DEVICE_ERROR     cannot be started due to device Error.
+  @return EFI_OUT_OF_RESOURCES cannot allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  EFI_STATUS              Status;
+  USB2_HC_DEV             *Ehc;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  EFI_PCI_IO_PROTOCOL     *Instance;
+  UINT64                  Supports;
+  UINT64                  OriginalPciAttributes;
+  BOOLEAN                 PciAttributesSaved;
+  USB_CLASSC              UsbClassCReg;
+  EFI_HANDLE              *HandleBuffer;
+  UINTN                   NumberOfHandles;
+  UINTN                   Index;
+  UINTN                   CompanionSegmentNumber;
+  UINTN                   CompanionBusNumber;
+  UINTN                   CompanionDeviceNumber;
+  UINTN                   CompanionFunctionNumber;
+  UINTN                   EhciSegmentNumber;
+  UINTN                   EhciBusNumber;
+  UINTN                   EhciDeviceNumber;
+  UINTN                   EhciFunctionNumber;
+  UINT32                  State;
+  EFI_DEVICE_PATH_PROTOCOL  *HcDevicePath;
+
+  //
+  // Open the PciIo Protocol, then enable the USB host controller
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open Device Path Protocol for on USB host controller
+  //
+  HcDevicePath = NULL;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &HcDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  PciAttributesSaved = FALSE;
+  //
+  // Save original PCI attributes
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &OriginalPciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto CLOSE_PCIIO;
+  }
+  PciAttributesSaved = TRUE;
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status = PciIo->Attributes (
+                      PciIo,
+                      EfiPciIoAttributeOperationEnable,
+                      Supports,
+                      NULL
+                      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to enable controller\n"));
+    goto CLOSE_PCIIO;
+  }
+
+  //
+  // Get the Pci device class code.
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (USB_CLASSC) / sizeof (UINT8),
+                        &UsbClassCReg
+                        );
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+    goto CLOSE_PCIIO;
+  }
+  //
+  // Determine if the device is UHCI or OHCI host controller or not. If yes, then find out the 
+  // companion usb ehci host controller and force EHCI driver get attached to it before
+  // UHCI or OHCI driver attaches to UHCI or OHCI host controller.
+  //
+  if ((UsbClassCReg.ProgInterface == PCI_IF_UHCI || UsbClassCReg.ProgInterface == PCI_IF_OHCI) &&
+       (UsbClassCReg.BaseCode == PCI_CLASS_SERIAL) && 
+       (UsbClassCReg.SubClassCode == PCI_CLASS_SERIAL_USB)) {
+    Status = PciIo->GetLocation (
+                    PciIo,
+                    &CompanionSegmentNumber,
+                    &CompanionBusNumber,
+                    &CompanionDeviceNumber,
+                    &CompanionFunctionNumber
+                    );
+    if (EFI_ERROR (Status)) {
+      goto CLOSE_PCIIO;
+    }
+
+    Status = gBS->LocateHandleBuffer (
+                    ByProtocol,
+                    &gEfiPciIoProtocolGuid,
+                    NULL,
+                    &NumberOfHandles,
+                    &HandleBuffer
+                    );
+    if (EFI_ERROR (Status)) {
+      goto CLOSE_PCIIO;
+    }
+
+    for (Index = 0; Index < NumberOfHandles; Index++) {
+      //
+      // Get the device path on this handle
+      //
+      Status = gBS->HandleProtocol (
+                    HandleBuffer[Index],
+                    &gEfiPciIoProtocolGuid,
+                    (VOID **)&Instance
+                    );
+      ASSERT_EFI_ERROR (Status);
+
+      Status = Instance->Pci.Read (
+                    Instance,
+                    EfiPciIoWidthUint8,
+                    PCI_CLASSCODE_OFFSET,
+                    sizeof (USB_CLASSC) / sizeof (UINT8),
+                    &UsbClassCReg
+                    );
+
+      if (EFI_ERROR (Status)) {
+        Status = EFI_UNSUPPORTED;
+        goto CLOSE_PCIIO;
+      }
+
+      if ((UsbClassCReg.ProgInterface == PCI_IF_EHCI) &&
+           (UsbClassCReg.BaseCode == PCI_CLASS_SERIAL) && 
+           (UsbClassCReg.SubClassCode == PCI_CLASS_SERIAL_USB)) {
+        Status = Instance->GetLocation (
+                    Instance,
+                    &EhciSegmentNumber,
+                    &EhciBusNumber,
+                    &EhciDeviceNumber,
+                    &EhciFunctionNumber
+                    );
+        if (EFI_ERROR (Status)) {
+          goto CLOSE_PCIIO;
+        }
+        //
+        // Currently, the judgment on the companion usb host controller is through the
+        // same bus number, which may vary on different platform.
+        //
+        if (EhciBusNumber == CompanionBusNumber) {
+          gBS->CloseProtocol (
+                    Controller,
+                    &gEfiPciIoProtocolGuid,
+                    This->DriverBindingHandle,
+                    Controller
+                    );
+          EhcDriverBindingStart(This, HandleBuffer[Index], NULL);
+        }
+      }
+    }
+    Status = EFI_NOT_FOUND;
+    goto CLOSE_PCIIO;
+  }
+
+  //
+  // Create then install USB2_HC_PROTOCOL
+  //
+  Ehc = EhcCreateUsb2Hc (PciIo, HcDevicePath, OriginalPciAttributes);
+
+  if (Ehc == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to create USB2_HC\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto CLOSE_PCIIO;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &Ehc->Usb2Hc
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to install USB2_HC Protocol\n"));
+    goto FREE_POOL;
+  }
+
+  //
+  // Robustnesss improvement such as for Duet platform
+  // Default is not required.
+  //
+  if (FeaturePcdGet (PcdTurnOffUsbLegacySupport)) {
+    EhcClearLegacySupport (Ehc);
+  }
+
+  if (Ehc->DebugPortNum != 0) {
+    State = EhcReadDbgRegister(Ehc, 0);
+    if ((State & (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) != (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) {
+      EhcResetHC (Ehc, EHC_RESET_TIMEOUT);
+    }
+  }
+
+  Status = EhcInitHC (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to init host controller\n"));
+    goto UNINSTALL_USBHC;
+  }
+
+  //
+  // Start the asynchronous interrupt monitor
+  //
+  Status = gBS->SetTimer (Ehc->PollTimer, TimerPeriodic, EHC_ASYNC_POLL_INTERVAL);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to start async interrupt monitor\n"));
+
+    EhcHaltHC (Ehc, EHC_GENERIC_TIMEOUT);
+    goto UNINSTALL_USBHC;
+  }
+
+  //
+  // Create event to stop the HC when exit boot service.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  EhcExitBootService,
+                  Ehc,
+                  &gEfiEventExitBootServicesGuid,
+                  &Ehc->ExitBootServiceEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto UNINSTALL_USBHC;
+  }
+
+  //
+  // Install the component name protocol, don't fail the start
+  // because of something for display.
+  //
+  AddUnicodeString2 (
+    "eng",
+    gEhciComponentName.SupportedLanguages,
+    &Ehc->ControllerNameTable,
+    L"Enhanced Host Controller (USB 2.0)",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gEhciComponentName2.SupportedLanguages,
+    &Ehc->ControllerNameTable,
+    L"Enhanced Host Controller (USB 2.0)",
+    FALSE
+    );
+
+
+  DEBUG ((EFI_D_INFO, "EhcDriverBindingStart: EHCI started for controller @ %p\n", Controller));
+  return EFI_SUCCESS;
+
+UNINSTALL_USBHC:
+  gBS->UninstallProtocolInterface (
+         Controller,
+         &gEfiUsb2HcProtocolGuid,
+         &Ehc->Usb2Hc
+         );
+
+FREE_POOL:
+  EhcFreeSched (Ehc);
+  gBS->CloseEvent (Ehc->PollTimer);
+  gBS->FreePool (Ehc);
+
+CLOSE_PCIIO:
+  if (PciAttributesSaved) {
+    //
+    // Restore original PCI attributes
+    //
+    PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSet,
+                    OriginalPciAttributes,
+                    NULL
+                    );
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS          Success.
+  @return EFI_DEVICE_ERROR     Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  )
+{
+  EFI_STATUS            Status;
+  EFI_USB2_HC_PROTOCOL  *Usb2Hc;
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  USB2_HC_DEV           *Ehc;
+
+  //
+  // Test whether the Controller handler passed in is a valid
+  // Usb controller handle that should be supported, if not,
+  // return the error status directly
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Ehc   = EHC_FROM_THIS (Usb2Hc);
+  PciIo = Ehc->PciIo;
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  Usb2Hc
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Stop AsyncRequest Polling timer then stop the EHCI driver
+  // and uninstall the EHCI protocl.
+  //
+  gBS->SetTimer (Ehc->PollTimer, TimerCancel, EHC_ASYNC_POLL_INTERVAL);
+  EhcHaltHC (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (Ehc->PollTimer != NULL) {
+    gBS->CloseEvent (Ehc->PollTimer);
+  }
+
+  if (Ehc->ExitBootServiceEvent != NULL) {
+    gBS->CloseEvent (Ehc->ExitBootServiceEvent);
+  }
+
+  EhcFreeSched (Ehc);
+
+  if (Ehc->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (Ehc->ControllerNameTable);
+  }
+
+  //
+  // Disable routing of all ports to EHCI controller, so all ports are 
+  // routed back to the UHCI or OHCI controller.
+  //
+  EhcClearOpRegBit (Ehc, EHC_CONFIG_FLAG_OFFSET, CONFIGFLAG_ROUTE_EHC);
+
+  //
+  // Restore original PCI attributes
+  //
+  PciIo->Attributes (
+                  PciIo,
+                  EfiPciIoAttributeOperationSet,
+                  Ehc->OriginalPciAttributes,
+                  NULL
+                  );
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  FreePool (Ehc);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.h
new file mode 100644
index 0000000000..7177658092
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.h
@@ -0,0 +1,248 @@
+/** @file
+
+  Provides some data struct used by EHCI controller driver.
+
+Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_H_
+#define _EFI_EHCI_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/Usb2HostController.h>
+#include <Protocol/PciIo.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Pci.h>
+
+typedef struct _USB2_HC_DEV  USB2_HC_DEV;
+
+#include "UsbHcMem.h"
+#include "EhciReg.h"
+#include "EhciUrb.h"
+#include "EhciSched.h"
+#include "EhciDebug.h"
+#include "ComponentName.h"
+
+//
+// EHC timeout experience values
+//
+
+#define EHC_1_MICROSECOND            1
+#define EHC_1_MILLISECOND            (1000 * EHC_1_MICROSECOND)
+#define EHC_1_SECOND                 (1000 * EHC_1_MILLISECOND)
+
+//
+// EHCI register operation timeout, set by experience
+//
+#define EHC_RESET_TIMEOUT            (1 * EHC_1_SECOND)
+#define EHC_GENERIC_TIMEOUT          (10 * EHC_1_MILLISECOND)
+
+//
+// Wait for roothub port power stable, refers to Spec[EHCI1.0-2.3.9]
+//
+#define EHC_ROOT_PORT_RECOVERY_STALL (20 * EHC_1_MILLISECOND)
+
+//
+// Sync and Async transfer polling interval, set by experience,
+// and the unit of Async is 100us, means 1ms as interval.
+//
+#define EHC_SYNC_POLL_INTERVAL       (1 * EHC_1_MILLISECOND)
+#define EHC_ASYNC_POLL_INTERVAL      EFI_TIMER_PERIOD_MILLISECONDS(1)
+
+//
+// EHCI debug port control status register bit definition
+//
+#define USB_DEBUG_PORT_IN_USE        BIT10
+#define USB_DEBUG_PORT_ENABLE        BIT28
+#define USB_DEBUG_PORT_OWNER         BIT30
+
+//
+// EHC raises TPL to TPL_NOTIFY to serialize all its operations
+// to protect shared data structures.
+//
+#define  EHC_TPL                     TPL_NOTIFY
+
+//
+//Iterate through the doule linked list. NOT delete safe
+//
+#define EFI_LIST_FOR_EACH(Entry, ListHead)    \
+  for(Entry = (ListHead)->ForwardLink; Entry != (ListHead); Entry = Entry->ForwardLink)
+
+//
+//Iterate through the doule linked list. This is delete-safe.
+//Don't touch NextEntry
+//
+#define EFI_LIST_FOR_EACH_SAFE(Entry, NextEntry, ListHead)            \
+  for(Entry = (ListHead)->ForwardLink, NextEntry = Entry->ForwardLink;\
+      Entry != (ListHead); Entry = NextEntry, NextEntry = Entry->ForwardLink)
+
+#define EFI_LIST_CONTAINER(Entry, Type, Field) BASE_CR(Entry, Type, Field)
+
+
+#define EHC_LOW_32BIT(Addr64)     ((UINT32)(((UINTN)(Addr64)) & 0XFFFFFFFF))
+#define EHC_HIGH_32BIT(Addr64)    ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+#define EHC_BIT_IS_SET(Data, Bit) ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define EHC_REG_BIT_IS_SET(Ehc, Offset, Bit) \
+          (EHC_BIT_IS_SET(EhcReadOpReg ((Ehc), (Offset)), (Bit)))
+
+#define USB2_HC_DEV_SIGNATURE  SIGNATURE_32 ('e', 'h', 'c', 'i')
+#define EHC_FROM_THIS(a)       CR(a, USB2_HC_DEV, Usb2Hc, USB2_HC_DEV_SIGNATURE)
+
+struct _USB2_HC_DEV {
+  UINTN                     Signature;
+  EFI_USB2_HC_PROTOCOL      Usb2Hc;
+
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  UINT64                    OriginalPciAttributes;
+  USBHC_MEM_POOL            *MemPool;
+
+  //
+  // Schedule data shared between asynchronous and periodic
+  // transfers:
+  // ShortReadStop, as its name indicates, is used to terminate
+  // the short read except the control transfer. EHCI follows
+  // the alternative next QTD point when a short read happens.
+  // For control transfer, even the short read happens, try the
+  // status stage.
+  //
+  EHC_QTD                  *ShortReadStop;
+  EFI_EVENT                 PollTimer;
+
+  //
+  // ExitBootServicesEvent is used to stop the EHC DMA operation 
+  // after exit boot service.
+  //
+  EFI_EVENT                 ExitBootServiceEvent;
+
+  //
+  // Asynchronous(bulk and control) transfer schedule data:
+  // ReclaimHead is used as the head of the asynchronous transfer
+  // list. It acts as the reclamation header.
+  //
+  EHC_QH                   *ReclaimHead;
+
+  //
+  // Peroidic (interrupt) transfer schedule data:
+  //
+  VOID                      *PeriodFrame;     // the buffer pointed by this pointer is used to store pci bus address of the QH descriptor.
+  VOID                      *PeriodFrameHost; // the buffer pointed by this pointer is used to store host memory address of the QH descriptor.
+  VOID                      *PeriodFrameMap;
+
+  EHC_QH                    *PeriodOne;
+  LIST_ENTRY                AsyncIntTransfers;
+
+  //
+  // EHCI configuration data
+  //
+  UINT32                    HcStructParams; // Cache of HC structure parameter, EHC_HCSPARAMS_OFFSET
+  UINT32                    HcCapParams;    // Cache of HC capability parameter, HCCPARAMS
+  UINT32                    CapLen;         // Capability length
+
+  //
+  // Misc
+  //
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+
+  //
+  // EHCI debug port info
+  //
+  UINT16                    DebugPortOffset; // The offset of debug port mmio register
+  UINT8                     DebugPortBarNum; // The bar number of debug port mmio register
+  UINT8                     DebugPortNum;    // The port number of usb debug port
+};
+
+
+extern EFI_DRIVER_BINDING_PROTOCOL      gEhciDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL      gEhciComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL     gEhciComponentName2;
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has Usb2HcProtocol installed will
+  be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+/**
+  Starting the Usb EHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          supports this device.
+  @return EFI_UNSUPPORTED      do not support this device.
+  @return EFI_DEVICE_ERROR     cannot be started due to device Error.
+  @return EFI_OUT_OF_RESOURCES cannot allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS          Success.
+  @return EFI_DEVICE_ERROR     Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.c
new file mode 100644
index 0000000000..76368b4748
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.c
@@ -0,0 +1,258 @@
+/** @file
+
+  This file provides the information dump support for EHCI when in debug mode.
+
+Copyright (c) 2007 - 2013, 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 "Ehci.h"
+
+/**
+  Dump the status byte in QTD/QH to a more friendly format.
+
+  @param  State    The state in the QTD/QH.
+
+**/
+VOID
+EhcDumpStatus (
+  IN UINT32               State
+  )
+{
+  if (EHC_BIT_IS_SET (State, QTD_STAT_DO_PING)) {
+    DEBUG ((EFI_D_VERBOSE, "  Do_Ping"));
+  } else {
+    DEBUG ((EFI_D_VERBOSE, "  Do_Out"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_DO_CS)) {
+    DEBUG ((EFI_D_VERBOSE, "  Do_CS"));
+  } else {
+    DEBUG ((EFI_D_VERBOSE, "  Do_SS"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_TRANS_ERR)) {
+    DEBUG ((EFI_D_VERBOSE, "  Transfer_Error"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_BABBLE_ERR)) {
+    DEBUG ((EFI_D_VERBOSE, "  Babble_Error"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_BUFF_ERR)) {
+    DEBUG ((EFI_D_VERBOSE, "  Buffer_Error"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_HALTED)) {
+    DEBUG ((EFI_D_VERBOSE, "  Halted"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_ACTIVE)) {
+    DEBUG ((EFI_D_VERBOSE, "  Active"));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "\n"));
+}
+
+
+/**
+  Dump the fields of a QTD.
+
+  @param  Qtd      The QTD to dump.
+  @param  Msg      The message to print before the dump.
+
+**/
+VOID
+EhcDumpQtd (
+  IN EHC_QTD              *Qtd,
+  IN CHAR8                *Msg
+  )
+{
+  QTD_HW                  *QtdHw;
+  UINTN                   Index;
+
+  if (Msg != NULL) {
+    DEBUG ((EFI_D_VERBOSE, Msg));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "Queue TD @ 0x%p, data length %d\n", Qtd, (UINT32)Qtd->DataLen));
+
+  QtdHw = &Qtd->QtdHw;
+
+  DEBUG ((EFI_D_VERBOSE, "Next QTD     : %x\n", QtdHw->NextQtd));
+  DEBUG ((EFI_D_VERBOSE, "AltNext QTD  : %x\n", QtdHw->AltNext));
+  DEBUG ((EFI_D_VERBOSE, "Status       : %x\n", QtdHw->Status));
+  EhcDumpStatus (QtdHw->Status);
+
+  if (QtdHw->Pid == QTD_PID_SETUP) {
+    DEBUG ((EFI_D_VERBOSE, "PID          : Setup\n"));
+
+  } else if (QtdHw->Pid == QTD_PID_INPUT) {
+    DEBUG ((EFI_D_VERBOSE, "PID          : IN\n"));
+
+  } else if (QtdHw->Pid == QTD_PID_OUTPUT) {
+    DEBUG ((EFI_D_VERBOSE, "PID          : OUT\n"));
+
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "Error Count  : %d\n", QtdHw->ErrCnt));
+  DEBUG ((EFI_D_VERBOSE, "Current Page : %d\n", QtdHw->CurPage));
+  DEBUG ((EFI_D_VERBOSE, "IOC          : %d\n", QtdHw->Ioc));
+  DEBUG ((EFI_D_VERBOSE, "Total Bytes  : %d\n", QtdHw->TotalBytes));
+  DEBUG ((EFI_D_VERBOSE, "Data Toggle  : %d\n", QtdHw->DataToggle));
+
+  for (Index = 0; Index < 5; Index++) {
+    DEBUG ((EFI_D_VERBOSE, "Page[%d]      : 0x%x\n", (UINT32)Index, QtdHw->Page[Index]));
+  }
+}
+
+
+/**
+  Dump the queue head.
+
+  @param  Qh       The queue head to dump.
+  @param  Msg      The message to print before the dump.
+  @param  DumpBuf  Whether to dump the memory buffer of the associated QTD.
+
+**/
+VOID
+EhcDumpQh (
+  IN EHC_QH               *Qh,
+  IN CHAR8                *Msg,
+  IN BOOLEAN              DumpBuf
+  )
+{
+  EHC_QTD                 *Qtd;
+  QH_HW                   *QhHw;
+  LIST_ENTRY              *Entry;
+  UINTN                   Index;
+
+  if (Msg != NULL) {
+    DEBUG ((EFI_D_VERBOSE, Msg));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "Queue head @ 0x%p, interval %ld, next qh %p\n",
+                                Qh, (UINT64)Qh->Interval, Qh->NextQh));
+
+  QhHw = &Qh->QhHw;
+
+  DEBUG ((EFI_D_VERBOSE, "Hoziontal link: %x\n", QhHw->HorizonLink));
+  DEBUG ((EFI_D_VERBOSE, "Device address: %d\n", QhHw->DeviceAddr));
+  DEBUG ((EFI_D_VERBOSE, "Inactive      : %d\n", QhHw->Inactive));
+  DEBUG ((EFI_D_VERBOSE, "EP number     : %d\n", QhHw->EpNum));
+  DEBUG ((EFI_D_VERBOSE, "EP speed      : %d\n", QhHw->EpSpeed));
+  DEBUG ((EFI_D_VERBOSE, "DT control    : %d\n", QhHw->DtCtrl));
+  DEBUG ((EFI_D_VERBOSE, "Reclaim head  : %d\n", QhHw->ReclaimHead));
+  DEBUG ((EFI_D_VERBOSE, "Max packet len: %d\n", QhHw->MaxPacketLen));
+  DEBUG ((EFI_D_VERBOSE, "Ctrl EP       : %d\n", QhHw->CtrlEp));
+  DEBUG ((EFI_D_VERBOSE, "Nak reload    : %d\n", QhHw->NakReload));
+
+  DEBUG ((EFI_D_VERBOSE, "SMask         : %x\n", QhHw->SMask));
+  DEBUG ((EFI_D_VERBOSE, "CMask         : %x\n", QhHw->CMask));
+  DEBUG ((EFI_D_VERBOSE, "Hub address   : %d\n", QhHw->HubAddr));
+  DEBUG ((EFI_D_VERBOSE, "Hub port      : %d\n", QhHw->PortNum));
+  DEBUG ((EFI_D_VERBOSE, "Multiplier    : %d\n", QhHw->Multiplier));
+
+  DEBUG ((EFI_D_VERBOSE, "Cur QTD       : %x\n", QhHw->CurQtd));
+
+  DEBUG ((EFI_D_VERBOSE, "Next QTD      : %x\n", QhHw->NextQtd));
+  DEBUG ((EFI_D_VERBOSE, "AltNext QTD   : %x\n", QhHw->AltQtd));
+  DEBUG ((EFI_D_VERBOSE, "Status        : %x\n", QhHw->Status));
+
+  EhcDumpStatus (QhHw->Status);
+
+  if (QhHw->Pid == QTD_PID_SETUP) {
+    DEBUG ((EFI_D_VERBOSE, "PID           : Setup\n"));
+
+  } else if (QhHw->Pid == QTD_PID_INPUT) {
+    DEBUG ((EFI_D_VERBOSE, "PID           : IN\n"));
+
+  } else if (QhHw->Pid == QTD_PID_OUTPUT) {
+    DEBUG ((EFI_D_VERBOSE, "PID           : OUT\n"));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "Error Count   : %d\n", QhHw->ErrCnt));
+  DEBUG ((EFI_D_VERBOSE, "Current Page  : %d\n", QhHw->CurPage));
+  DEBUG ((EFI_D_VERBOSE, "IOC           : %d\n", QhHw->Ioc));
+  DEBUG ((EFI_D_VERBOSE, "Total Bytes   : %d\n", QhHw->TotalBytes));
+  DEBUG ((EFI_D_VERBOSE, "Data Toggle   : %d\n", QhHw->DataToggle));
+
+  for (Index = 0; Index < 5; Index++) {
+    DEBUG ((EFI_D_VERBOSE, "Page[%d]       : 0x%x\n", Index, QhHw->Page[Index]));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "\n"));
+
+  EFI_LIST_FOR_EACH (Entry, &Qh->Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+    EhcDumpQtd (Qtd, NULL);
+
+    if (DumpBuf && (Qtd->DataLen != 0)) {
+      EhcDumpBuf (Qtd->Data, Qtd->DataLen);
+    }
+  }
+}
+
+
+/**
+  Dump the buffer in the form of hex.
+
+  @param  Buf      The buffer to dump.
+  @param  Len      The length of buffer.
+
+**/
+VOID
+EhcDumpBuf (
+  IN UINT8                *Buf,
+  IN UINTN                Len
+  )
+{
+  UINTN                   Index;
+
+  for (Index = 0; Index < Len; Index++) {
+    if (Index % 16 == 0) {
+      DEBUG ((EFI_D_VERBOSE,"\n"));
+    }
+
+    DEBUG ((EFI_D_VERBOSE, "%02x ", Buf[Index]));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "\n"));
+}
+
+/**
+  Dump the EHCI status registers.
+
+  @param  Ehc    USB EHCI Host Controller instance
+
+**/
+VOID
+EhcDumpRegs (
+  IN  USB2_HC_DEV         *Ehc
+  )
+{
+  UINT8   Index;
+
+  DEBUG ((EFI_D_VERBOSE, "  EHC_CAPLENGTH_OFFSET   = 0x%08x\n", EhcReadCapRegister (Ehc, EHC_CAPLENGTH_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_HCSPARAMS_OFFSET   = 0x%08x\n", EhcReadCapRegister (Ehc, EHC_HCSPARAMS_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_HCCPARAMS_OFFSET   = 0x%08x\n", EhcReadCapRegister (Ehc, EHC_HCCPARAMS_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_USBCMD_OFFSET      = 0x%08x\n", EhcReadOpReg (Ehc, EHC_USBCMD_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_USBSTS_OFFSET      = 0x%08x\n", EhcReadOpReg (Ehc, EHC_USBSTS_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_USBINTR_OFFSET     = 0x%08x\n", EhcReadOpReg (Ehc, EHC_USBINTR_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_FRINDEX_OFFSET     = 0x%08x\n", EhcReadOpReg (Ehc, EHC_FRINDEX_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_CTRLDSSEG_OFFSET   = 0x%08x\n", EhcReadOpReg (Ehc,  EHC_CTRLDSSEG_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_FRAME_BASE_OFFSET  = 0x%08x\n", EhcReadOpReg (Ehc,  EHC_FRAME_BASE_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_ASYNC_HEAD_OFFSET  = 0x%08x\n", EhcReadOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_CONFIG_FLAG_OFFSET = 0x%08x\n", EhcReadOpReg (Ehc, EHC_CONFIG_FLAG_OFFSET)));
+  for (Index = 0; Index < (UINT8) (Ehc->HcStructParams & HCSP_NPORTS); Index++) {
+    DEBUG ((EFI_D_VERBOSE, "  EHC_PORT_STAT_OFFSET(%d)  = 0x%08x\n", Index, EhcReadOpReg (Ehc, EHC_PORT_STAT_OFFSET + (4 * Index))));
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.h
new file mode 100644
index 0000000000..bc84bb7864
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.h
@@ -0,0 +1,75 @@
+/** @file
+
+  This file contains the definination for host controller debug support routines.
+
+Copyright (c) 2007 - 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_DEBUG_H_
+#define _EFI_EHCI_DEBUG_H_
+
+
+/**
+  Dump the fields of a QTD.
+
+  @param  Qtd      The QTD to dump.
+  @param  Msg      The message to print before the dump.
+
+**/
+VOID
+EhcDumpQtd (
+  IN EHC_QTD              *Qtd,
+  IN CHAR8                *Msg
+  );
+
+
+/**
+  Dump the queue head.
+
+  @param  Qh       The queue head to dump.
+  @param  Msg      The message to print before the dump.
+  @param  DumpBuf  Whether to dump the memory buffer of the associated QTD.
+
+**/
+VOID
+EhcDumpQh (
+  IN EHC_QH               *Qh,
+  IN CHAR8                *Msg,
+  IN BOOLEAN              DumpBuf
+  );
+
+
+/**
+  Dump the buffer in the form of hex.
+
+  @param  Buf      The buffer to dump.
+  @param  Len      The length of buffer.
+
+**/
+VOID
+EhcDumpBuf (
+  IN UINT8                *Buf,
+  IN UINTN                Len
+  );
+
+
+/**
+  Dump the EHCI status registers.
+
+  @param  Ehc    USB EHCI Host Controller instance
+
+**/
+VOID
+EhcDumpRegs (
+  IN  USB2_HC_DEV         *Ehc
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.inf b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.inf
new file mode 100644
index 0000000000..238923e828
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.inf
@@ -0,0 +1,90 @@
+## @file
+#  The EhciDxe driver is responsible for managing the behavior of EHCI controller.
+#  It implements the interfaces of monitoring the status of all ports and transferring 
+#  Control, Bulk, Interrupt and Isochronous requests to Usb2.0 device.
+#
+#  Note that EhciDxe driver is enhanced to guarantee that the EHCI controller get attached
+#  to the EHCI controller before the UHCI driver attaches to the companion UHCI controller. 
+#  This way avoids the control transfer on a shared port between EHCI and companion host
+#  controller when UHCI gets attached earlier than EHCI and a USB 2.0 device inserts.
+#
+#  Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = EhciDxe
+  MODULE_UNI_FILE                = EhciDxe.uni
+  FILE_GUID                      = BDFE430E-8F2A-4db0-9991-6F856594777E
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = EhcDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gEhciDriverBinding                        
+#  COMPONENT_NAME                =  gEhciComponentName
+#  COMPONENT_NAME2               =  gEhciComponentName2
+#
+
+[Sources]
+  UsbHcMem.h
+  EhciUrb.c
+  EhciReg.h
+  UsbHcMem.c
+  EhciSched.c
+  EhciDebug.c
+  EhciReg.c
+  EhciDebug.h
+  ComponentName.c
+  ComponentName.h
+  EhciUrb.h
+  Ehci.h
+  EhciSched.h
+  Ehci.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdTurnOffUsbLegacySupport  ## CONSUMES
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  PcdLib
+  ReportStatusCodeLib
+
+[Guids]
+  gEfiEventExitBootServicesGuid                 ## SOMETIMES_CONSUMES ## Event
+
+[Protocols]
+  gEfiPciIoProtocolGuid                         ## TO_START
+  gEfiUsb2HcProtocolGuid                        ## BY_START
+
+# [Event]
+# EVENT_TYPE_PERIODIC_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  EhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.uni b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.uni
new file mode 100644
index 0000000000..0ddb7ab705
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxeExtra.uni
new file mode 100644
index 0000000000..0039424c15
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.c
new file mode 100644
index 0000000000..88a66aee71
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.c
@@ -0,0 +1,658 @@
+/** @file
+
+  The EHCI register operation routines.
+
+Copyright (c) 2007 - 2012, 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 "Ehci.h"
+
+
+/**
+  Read EHCI capability register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       Capability register address.
+
+  @return The register content read.
+  @retval If err, return 0xffff.
+
+**/
+UINT32
+EhcReadCapRegister (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  Status = Ehc->PciIo->Mem.Read (
+                             Ehc->PciIo,
+                             EfiPciIoWidthUint32,
+                             EHC_BAR_INDEX,
+                             (UINT64) Offset,
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcReadCapRegister: Pci Io read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Read EHCI debug port register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       Debug port register offset.
+
+  @return The register content read.
+  @retval If err, return 0xffff.
+
+**/
+UINT32
+EhcReadDbgRegister (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  Status = Ehc->PciIo->Mem.Read (
+                             Ehc->PciIo,
+                             EfiPciIoWidthUint32,
+                             Ehc->DebugPortBarNum,
+                             (UINT64) (Ehc->DebugPortOffset + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcReadDbgRegister: Pci Io read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFF;
+  }
+
+  return Data;
+}
+
+
+/**
+  Read EHCI Operation register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The operation register offset.
+
+  @return The register content read.
+  @retval If err, return 0xffff.
+
+**/
+UINT32
+EhcReadOpReg (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Ehc->CapLen != 0);
+
+  Status = Ehc->PciIo->Mem.Read (
+                             Ehc->PciIo,
+                             EfiPciIoWidthUint32,
+                             EHC_BAR_INDEX,
+                             (UINT64) (Ehc->CapLen + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcReadOpReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFF;
+  }
+
+  return Data;
+}
+
+
+/**
+  Write  the data to the EHCI operation register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       EHCI operation register offset.
+  @param  Data         The data to write.
+
+**/
+VOID
+EhcWriteOpReg (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Ehc->CapLen != 0);
+
+  Status = Ehc->PciIo->Mem.Write (
+                             Ehc->PciIo,
+                             EfiPciIoWidthUint32,
+                             EHC_BAR_INDEX,
+                             (UINT64) (Ehc->CapLen + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcWriteOpReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+EhcSetOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = EhcReadOpReg (Ehc, Offset);
+  Data |= Bit;
+  EhcWriteOpReg (Ehc, Offset, Data);
+}
+
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to clear.
+
+**/
+VOID
+EhcClearOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = EhcReadOpReg (Ehc, Offset);
+  Data &= ~Bit;
+  EhcWriteOpReg (Ehc, Offset, Data);
+}
+
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to become set (or clear).
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operation register.
+  @param  Bit          The bit of the register to wait for.
+  @param  WaitToSet    Wait the bit to set or clear.
+  @param  Timeout      The time to wait before abort (in millisecond).
+
+  @retval EFI_SUCCESS  The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT  The time out occurred.
+
+**/
+EFI_STATUS
+EhcWaitOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit,
+  IN BOOLEAN              WaitToSet,
+  IN UINT32               Timeout
+  )
+{
+  UINT32                  Index;
+
+  for (Index = 0; Index < Timeout / EHC_SYNC_POLL_INTERVAL + 1; Index++) {
+    if (EHC_REG_BIT_IS_SET (Ehc, Offset, Bit) == WaitToSet) {
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (EHC_SYNC_POLL_INTERVAL);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+
+/**
+  Add support for UEFI Over Legacy (UoL) feature, stop
+  the legacy USB SMI support.
+
+  @param  Ehc          The EHCI device.
+
+**/
+VOID
+EhcClearLegacySupport (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  UINT32                    ExtendCap;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  UINT32                    Value;
+  UINT32                    TimeOut;
+
+  DEBUG ((EFI_D_INFO, "EhcClearLegacySupport: called to clear legacy support\n"));
+
+  PciIo     = Ehc->PciIo;
+  ExtendCap = (Ehc->HcCapParams >> 8) & 0xFF;
+
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap + 0x4, 1, &Value);
+
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+  Value |= (0x1 << 24);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+
+  TimeOut = 40;
+  while (TimeOut-- != 0) {
+    gBS->Stall (500);
+
+    PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+
+    if ((Value & 0x01010000) == 0x01000000) {
+      break;
+    }
+  }
+
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap + 0x4, 1, &Value);
+}
+
+
+
+/**
+  Set door bell and wait it to be ACKed by host controller.
+  This function is used to synchronize with the hardware.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  Synchronized with the hardware.
+  @retval EFI_TIMEOUT  Time out happened while waiting door bell to set.
+
+**/
+EFI_STATUS
+EhcSetAndWaitDoorBell (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+  UINT32                  Data;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_IAAD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_IAA, TRUE, Timeout);
+
+  //
+  // ACK the IAA bit in USBSTS register. Make sure other
+  // interrupt bits are not ACKed. These bits are WC (Write Clean).
+  //
+  Data  = EhcReadOpReg (Ehc, EHC_USBSTS_OFFSET);
+  Data &= ~USBSTS_INTACK_MASK;
+  Data |= USBSTS_IAA;
+
+  EhcWriteOpReg (Ehc, EHC_USBSTS_OFFSET, Data);
+
+  return Status;
+}
+
+
+/**
+  Clear all the interrutp status bits, these bits
+  are Write-Clean.
+
+  @param  Ehc          The EHCI device.
+
+**/
+VOID
+EhcAckAllInterrupt (
+  IN  USB2_HC_DEV         *Ehc
+  )
+{
+  EhcWriteOpReg (Ehc, EHC_USBSTS_OFFSET, USBSTS_INTACK_MASK);
+}
+
+
+/**
+  Enable the periodic schedule then wait EHC to
+  actually enable it.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The periodical schedule is enabled.
+  @retval EFI_TIMEOUT  Time out happened while enabling periodic schedule.
+
+**/
+EFI_STATUS
+EhcEnablePeriodSchd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_PERIOD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_PERIOD_ENABLED, TRUE, Timeout);
+  return Status;
+}
+
+
+/**
+  Disable periodic schedule.
+
+  @param  Ehc               The EHCI device.
+  @param  Timeout           Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS       Periodic schedule is disabled.
+  @retval EFI_DEVICE_ERROR  Fail to disable periodic schedule.
+
+**/
+EFI_STATUS
+EhcDisablePeriodSchd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_PERIOD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_PERIOD_ENABLED, FALSE, Timeout);
+  return Status;
+}
+
+
+
+/**
+  Enable asynchrounous schedule.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @retval EFI_SUCCESS  The EHCI asynchronous schedule is enabled.
+  @return Others       Failed to enable the asynchronous scheudle.
+
+**/
+EFI_STATUS
+EhcEnableAsyncSchd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_ASYNC);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_ASYNC_ENABLED, TRUE, Timeout);
+  return Status;
+}
+
+
+
+/**
+  Disable asynchrounous schedule.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The asynchronous schedule is disabled.
+  @return Others       Failed to disable the asynchronous schedule.
+
+**/
+EFI_STATUS
+EhcDisableAsyncSchd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS  Status;
+
+  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_ASYNC);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_ASYNC_ENABLED, FALSE, Timeout);
+  return Status;
+}
+
+
+
+/**
+  Whether Ehc is halted.
+
+  @param  Ehc          The EHCI device.
+
+  @retval TRUE         The controller is halted.
+  @retval FALSE        It isn't halted.
+
+**/
+BOOLEAN
+EhcIsHalt (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  return EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT);
+}
+
+
+/**
+  Whether system error occurred.
+
+  @param  Ehc          The EHCI device.
+
+  @return TRUE         System error happened.
+  @return FALSE        No system error.
+
+**/
+BOOLEAN
+EhcIsSysError (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  return EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_SYS_ERROR);
+}
+
+
+/**
+  Reset the host controller.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The host controller is reset.
+  @return Others       Failed to reset the host.
+
+**/
+EFI_STATUS
+EhcResetHC (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Host can only be reset when it is halt. If not so, halt it
+  //
+  if (!EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT)) {
+    Status = EhcHaltHC (Ehc, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RESET);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RESET, FALSE, Timeout);
+  return Status;
+}
+
+
+/**
+  Halt the host controller.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @retval EFI_SUCCESS  The EHCI is halt.
+  @retval EFI_TIMEOUT  Failed to halt the controller before Timeout.
+
+**/
+EFI_STATUS
+EhcHaltHC (
+  IN USB2_HC_DEV         *Ehc,
+  IN UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT, TRUE, Timeout);
+  return Status;
+}
+
+
+/**
+  Set the EHCI to run.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @retval EFI_SUCCESS  The EHCI is running.
+  @return Others       Failed to set the EHCI to run.
+
+**/
+EFI_STATUS
+EhcRunHC (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT, FALSE, Timeout);
+  return Status;
+}
+
+
+/**
+  Initialize the HC hardware.
+  EHCI spec lists the five things to do to initialize the hardware:
+  1. Program CTRLDSSEGMENT
+  2. Set USBINTR to enable interrupts
+  3. Set periodic list base
+  4. Set USBCMD, interrupt threshold, frame list size etc
+  5. Write 1 to CONFIGFLAG to route all ports to EHCI
+
+  @param  Ehc          The EHCI device.
+
+  @return EFI_SUCCESS  The EHCI has come out of halt state.
+  @return EFI_TIMEOUT  Time out happened.
+
+**/
+EFI_STATUS
+EhcInitHC (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  EFI_STATUS              Status;
+  UINT32                  Index;
+
+  // This ASSERT crashes the BeagleBoard. There is some issue in the USB stack.
+  // This ASSERT needs to be removed so the BeagleBoard will boot. When we fix
+  // the USB stack we can put this ASSERT back in
+  // ASSERT (EhcIsHalt (Ehc));
+
+  //
+  // Allocate the periodic frame and associated memeory
+  // management facilities if not already done.
+  //
+  if (Ehc->PeriodFrame != NULL) {
+    EhcFreeSched (Ehc);
+  }
+
+  Status = EhcInitSched (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // 1. Clear USBINTR to disable all the interrupt. UEFI works by polling
+  //
+  EhcWriteOpReg (Ehc, EHC_USBINTR_OFFSET, 0);
+
+  //
+  // 2. Start the Host Controller
+  //
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+
+  //
+  // 3. Power up all ports if EHCI has Port Power Control (PPC) support
+  //
+  if (Ehc->HcStructParams & HCSP_PPC) {
+    for (Index = 0; Index < (UINT8) (Ehc->HcStructParams & HCSP_NPORTS); Index++) {
+      EhcSetOpRegBit (Ehc, (UINT32) (EHC_PORT_STAT_OFFSET + (4 * Index)), PORTSC_POWER);
+    }
+  }
+
+  //
+  // Wait roothub port power stable
+  //
+  gBS->Stall (EHC_ROOT_PORT_RECOVERY_STALL);
+
+  //
+  // 4. Set all ports routing to EHC
+  //
+  EhcSetOpRegBit (Ehc, EHC_CONFIG_FLAG_OFFSET, CONFIGFLAG_ROUTE_EHC);
+
+  Status = EhcEnablePeriodSchd (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcInitHC: failed to enable period schedule\n"));
+    return Status;
+  }
+
+  Status = EhcEnableAsyncSchd (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcInitHC: failed to enable async schedule\n"));
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.h
new file mode 100644
index 0000000000..2347ee125f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.h
@@ -0,0 +1,363 @@
+/** @file
+
+  This file contains the definination for host controller register operation routines.
+
+Copyright (c) 2007 - 2012, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_REG_H_
+#define _EFI_EHCI_REG_H_
+
+//
+// EHCI register offset
+//
+
+
+//
+// Capability register offset
+//
+#define EHC_CAPLENGTH_OFFSET    0    // Capability register length offset
+#define EHC_HCSPARAMS_OFFSET    0x04 // Structural Parameters 04-07h
+#define EHC_HCCPARAMS_OFFSET    0x08 // Capability parameters offset
+
+//
+// Capability register bit definition
+//
+#define HCSP_NPORTS             0x0F // Number of root hub port
+#define HCSP_PPC                0x10 // Port Power Control
+#define HCCP_64BIT              0x01 // 64-bit addressing capability
+
+//
+// Operational register offset
+//
+#define EHC_USBCMD_OFFSET       0x0  // USB command register offset
+#define EHC_USBSTS_OFFSET       0x04 // Statue register offset
+#define EHC_USBINTR_OFFSET      0x08 // USB interrutp offset
+#define EHC_FRINDEX_OFFSET      0x0C // Frame index offset
+#define EHC_CTRLDSSEG_OFFSET    0x10 // Control data structure segment offset
+#define EHC_FRAME_BASE_OFFSET   0x14 // Frame list base address offset
+#define EHC_ASYNC_HEAD_OFFSET   0x18 // Next asynchronous list address offset
+#define EHC_CONFIG_FLAG_OFFSET  0x40 // Configure flag register offset
+#define EHC_PORT_STAT_OFFSET    0x44 // Port status/control offset
+
+#define EHC_FRAME_LEN           1024
+
+//
+// Register bit definition
+//
+#define CONFIGFLAG_ROUTE_EHC    0x01 // Route port to EHC
+
+#define USBCMD_RUN              0x01   // Run/stop
+#define USBCMD_RESET            0x02   // Start the host controller reset
+#define USBCMD_ENABLE_PERIOD    0x10   // Enable periodic schedule
+#define USBCMD_ENABLE_ASYNC     0x20   // Enable asynchronous schedule
+#define USBCMD_IAAD             0x40   // Interrupt on async advance doorbell
+
+#define USBSTS_IAA              0x20   // Interrupt on async advance
+#define USBSTS_PERIOD_ENABLED   0x4000 // Periodic schedule status
+#define USBSTS_ASYNC_ENABLED    0x8000 // Asynchronous schedule status
+#define USBSTS_HALT             0x1000 // Host controller halted
+#define USBSTS_SYS_ERROR        0x10   // Host system error
+#define USBSTS_INTACK_MASK      0x003F // Mask for the interrupt ACK, the WC
+                                       // (write clean) bits in USBSTS register
+
+#define PORTSC_CONN             0x01   // Current Connect Status
+#define PORTSC_CONN_CHANGE      0x02   // Connect Status Change
+#define PORTSC_ENABLED          0x04   // Port Enable / Disable
+#define PORTSC_ENABLE_CHANGE    0x08   // Port Enable / Disable Change
+#define PORTSC_OVERCUR          0x10   // Over current Active
+#define PORTSC_OVERCUR_CHANGE   0x20   // Over current Change
+#define PORSTSC_RESUME          0x40   // Force Port Resume
+#define PORTSC_SUSPEND          0x80   // Port Suspend State
+#define PORTSC_RESET            0x100  // Port Reset
+#define PORTSC_LINESTATE_K      0x400  // Line Status K-state
+#define PORTSC_LINESTATE_J      0x800  // Line Status J-state
+#define PORTSC_POWER            0x1000 // Port Power
+#define PORTSC_OWNER            0x2000 // Port Owner
+#define PORTSC_CHANGE_MASK      0x2A   // Mask of the port change bits,
+                                       // they are WC (write clean)
+//
+// PCI Configuration Registers
+//
+#define EHC_BAR_INDEX           0      // how many bytes away from USB_BASE to 0x10
+
+//
+// Debug port capability id
+//
+#define EHC_DEBUG_PORT_CAP_ID   0x0A
+
+#define EHC_LINK_TERMINATED(Link) (((Link) & 0x01) != 0)
+
+#define EHC_ADDR(High, QhHw32)   \
+        ((VOID *) (UINTN) (LShiftU64 ((High), 32) | ((QhHw32) & 0xFFFFFFF0)))
+
+#define EHCI_IS_DATAIN(EndpointAddr) EHC_BIT_IS_SET((EndpointAddr), 0x80)
+
+//
+// Structure to map the hardware port states to the
+// UEFI's port states.
+//
+typedef struct {
+  UINT16                  HwState;
+  UINT16                  UefiState;
+} USB_PORT_STATE_MAP;
+
+//
+// Ehci Data and Ctrl Structures
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   ProgInterface;
+  UINT8                   SubClassCode;
+  UINT8                   BaseCode;
+} USB_CLASSC;
+#pragma pack()
+
+/**
+  Read EHCI capability register.
+
+  @param  Ehc     The EHCI device.
+  @param  Offset  Capability register address.
+
+  @return The register content.
+
+**/
+UINT32
+EhcReadCapRegister (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Read EHCI debug port register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       Debug port register address.
+
+  @return The register content read.
+  @retval If err, return 0xffff.
+
+**/
+UINT32
+EhcReadDbgRegister (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Read EHCI Operation register.
+
+  @param  Ehc      The EHCI device.
+  @param  Offset   The operation register offset.
+
+  @return The register content.
+
+**/
+UINT32
+EhcReadOpReg (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  );
+
+
+/**
+  Write  the data to the EHCI operation register.
+
+  @param  Ehc      The EHCI device.
+  @param  Offset   EHCI operation register offset.
+  @param  Data     The data to write.
+
+**/
+VOID
+EhcWriteOpReg (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+EhcSetOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to clear.
+
+**/
+VOID
+EhcClearOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Add support for UEFI Over Legacy (UoL) feature, stop
+  the legacy USB SMI support.
+
+  @param  Ehc      The EHCI device.
+
+**/
+VOID
+EhcClearLegacySupport (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+
+/**
+  Set door bell and wait it to be ACKed by host controller.
+  This function is used to synchronize with the hardware.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  Synchronized with the hardware.
+  @retval EFI_TIMEOUT  Time out happened while waiting door bell to set.
+
+**/
+EFI_STATUS
+EhcSetAndWaitDoorBell (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32               Timeout
+  );
+
+
+/**
+  Clear all the interrutp status bits, these bits are Write-Clean.
+
+  @param  Ehc      The EHCI device.
+
+**/
+VOID
+EhcAckAllInterrupt (
+  IN  USB2_HC_DEV         *Ehc
+  );
+
+
+
+/**
+  Whether Ehc is halted.
+
+  @param  Ehc     The EHCI device.
+
+  @retval TRUE    The controller is halted.
+  @retval FALSE   It isn't halted.
+
+**/
+BOOLEAN
+EhcIsHalt (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Whether system error occurred.
+
+  @param  Ehc      The EHCI device.
+
+  @retval TRUE     System error happened.
+  @retval FALSE    No system error.
+
+**/
+BOOLEAN
+EhcIsSysError (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Reset the host controller.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The host controller is reset.
+  @return Others       Failed to reset the host.
+
+**/
+EFI_STATUS
+EhcResetHC (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  );
+
+
+/**
+  Halt the host controller.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @return EFI_SUCCESS  The EHCI is halt.
+  @return EFI_TIMEOUT  Failed to halt the controller before Timeout.
+
+**/
+EFI_STATUS
+EhcHaltHC (
+  IN USB2_HC_DEV         *Ehc,
+  IN UINT32              Timeout
+  );
+
+
+/**
+  Set the EHCI to run.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @return EFI_SUCCESS  The EHCI is running.
+  @return Others       Failed to set the EHCI to run.
+
+**/
+EFI_STATUS
+EhcRunHC (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  );
+
+
+
+/**
+  Initialize the HC hardware.
+  EHCI spec lists the five things to do to initialize the hardware:
+  1. Program CTRLDSSEGMENT
+  2. Set USBINTR to enable interrupts
+  3. Set periodic list base
+  4. Set USBCMD, interrupt threshold, frame list size etc
+  5. Write 1 to CONFIGFLAG to route all ports to EHCI
+
+  @param  Ehc          The EHCI device.
+
+  @return EFI_SUCCESS  The EHCI has come out of halt state.
+  @return EFI_TIMEOUT  Time out happened.
+
+**/
+EFI_STATUS
+EhcInitHC (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.c
new file mode 100644
index 0000000000..5594e6699e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.c
@@ -0,0 +1,1052 @@
+/** @file
+
+  EHCI transfer scheduling routines.
+
+Copyright (c) 2007 - 2013, 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 "Ehci.h"
+
+
+/**
+  Create helper QTD/QH for the EHCI device.
+
+  @param  Ehc                   The EHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for helper QTD/QH.
+  @retval EFI_SUCCESS           Helper QH/QTD are created.
+
+**/
+EFI_STATUS
+EhcCreateHelpQ (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  USB_ENDPOINT            Ep;
+  EHC_QH                  *Qh;
+  QH_HW                   *QhHw;
+  EHC_QTD                 *Qtd;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  //
+  // Create an inactive Qtd to terminate the short packet read.
+  //
+  Qtd = EhcCreateQtd (Ehc, NULL, NULL, 0, QTD_PID_INPUT, 0, 64);
+
+  if (Qtd == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Qtd->QtdHw.Status   = QTD_STAT_HALTED;
+  Ehc->ShortReadStop  = Qtd;
+
+  //
+  // Create a QH to act as the EHC reclamation header.
+  // Set the header to loopback to itself.
+  //
+  Ep.DevAddr    = 0;
+  Ep.EpAddr     = 1;
+  Ep.Direction  = EfiUsbDataIn;
+  Ep.DevSpeed   = EFI_USB_SPEED_HIGH;
+  Ep.MaxPacket  = 64;
+  Ep.HubAddr    = 0;
+  Ep.HubPort    = 0;
+  Ep.Toggle     = 0;
+  Ep.Type       = EHC_BULK_TRANSFER;
+  Ep.PollRate   = 1;
+
+  Qh            = EhcCreateQh (Ehc, &Ep);
+
+  if (Qh == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  PciAddr           = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Qh, sizeof (EHC_QH));
+  QhHw              = &Qh->QhHw;
+  QhHw->HorizonLink = QH_LINK (PciAddr + OFFSET_OF(EHC_QH, QhHw), EHC_TYPE_QH, FALSE);
+  QhHw->Status      = QTD_STAT_HALTED;
+  QhHw->ReclaimHead = 1;
+  Qh->NextQh        = Qh;
+  Ehc->ReclaimHead  = Qh;
+
+  //
+  // Create a dummy QH to act as the terminator for periodical schedule
+  //
+  Ep.EpAddr   = 2;
+  Ep.Type     = EHC_INT_TRANSFER_SYNC;
+
+  Qh          = EhcCreateQh (Ehc, &Ep);
+
+  if (Qh == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Qh->QhHw.Status = QTD_STAT_HALTED;
+  Ehc->PeriodOne  = Qh;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialize the schedule data structure such as frame list.
+
+  @param  Ehc                   The EHCI device to init schedule data.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to init schedule data.
+  @retval EFI_SUCCESS           The schedule data is initialized.
+
+**/
+EFI_STATUS
+EhcInitSched (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  VOID                  *Buf;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+  VOID                  *Map;
+  UINTN                 Pages;
+  UINTN                 Bytes;
+  UINTN                 Index;
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  PciAddr;
+
+  //
+  // First initialize the periodical schedule data:
+  // 1. Allocate and map the memory for the frame list
+  // 2. Create the help QTD/QH
+  // 3. Initialize the frame entries
+  // 4. Set the frame list register
+  //
+  PciIo = Ehc->PciIo;
+
+  Bytes = 4096;
+  Pages = EFI_SIZE_TO_PAGES (Bytes);
+
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    Pages,
+                    &Buf,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    Buf,
+                    &Bytes,
+                    &PhyAddr,
+                    &Map
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != 4096)) {
+    PciIo->FreeBuffer (PciIo, Pages, Buf);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Ehc->PeriodFrame      = Buf;
+  Ehc->PeriodFrameMap   = Map;
+
+  //
+  // Program the FRAMELISTBASE register with the low 32 bit addr
+  //
+  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, EHC_LOW_32BIT (PhyAddr));
+  //
+  // Program the CTRLDSSEGMENT register with the high 32 bit addr
+  //
+  EhcWriteOpReg (Ehc, EHC_CTRLDSSEG_OFFSET, EHC_HIGH_32BIT (PhyAddr));
+
+  //
+  // Init memory pool management then create the helper
+  // QTD/QH. If failed, previously allocated resources
+  // will be freed by EhcFreeSched
+  //
+  Ehc->MemPool = UsbHcInitMemPool (
+                   PciIo,
+                   EHC_BIT_IS_SET (Ehc->HcCapParams, HCCP_64BIT),
+                   EHC_HIGH_32BIT (PhyAddr)
+                   );
+
+  if (Ehc->MemPool == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit1;
+  }
+
+  Status = EhcCreateHelpQ (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Initialize the frame list entries then set the registers
+  //
+  Ehc->PeriodFrameHost      = AllocateZeroPool (EHC_FRAME_LEN * sizeof (UINTN));
+  if (Ehc->PeriodFrameHost == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  PciAddr  = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (EHC_QH));
+
+  for (Index = 0; Index < EHC_FRAME_LEN; Index++) {
+    //
+    // Store the pci bus address of the QH in period frame list which will be accessed by pci bus master.
+    //
+    ((UINT32 *)(Ehc->PeriodFrame))[Index] = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+    //
+    // Store the host address of the QH in period frame list which will be accessed by host.
+    //
+    ((UINTN *)(Ehc->PeriodFrameHost))[Index] = (UINTN)Ehc->PeriodOne;
+  }
+
+  //
+  // Second initialize the asynchronous schedule:
+  // Only need to set the AsynListAddr register to
+  // the reclamation header
+  //
+  PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (EHC_QH));
+  EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, EHC_LOW_32BIT (PciAddr));
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (Ehc->PeriodOne != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (EHC_QH));
+    Ehc->PeriodOne = NULL;
+  }
+
+  if (Ehc->ReclaimHead != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (EHC_QH));
+    Ehc->ReclaimHead = NULL;
+  }
+
+  if (Ehc->ShortReadStop != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (EHC_QTD));
+    Ehc->ShortReadStop = NULL;
+  }
+
+ErrorExit1:
+  PciIo->FreeBuffer (PciIo, Pages, Buf);
+  PciIo->Unmap (PciIo, Map);
+
+  return Status;
+}
+
+
+/**
+  Free the schedule data. It may be partially initialized.
+
+  @param  Ehc                   The EHCI device.
+
+**/
+VOID
+EhcFreeSched (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+
+  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, 0);
+  EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, 0);
+
+  if (Ehc->PeriodOne != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (EHC_QH));
+    Ehc->PeriodOne = NULL;
+  }
+
+  if (Ehc->ReclaimHead != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (EHC_QH));
+    Ehc->ReclaimHead = NULL;
+  }
+
+  if (Ehc->ShortReadStop != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (EHC_QTD));
+    Ehc->ShortReadStop = NULL;
+  }
+
+  if (Ehc->MemPool != NULL) {
+    UsbHcFreeMemPool (Ehc->MemPool);
+    Ehc->MemPool = NULL;
+  }
+
+  if (Ehc->PeriodFrame != NULL) {
+    PciIo = Ehc->PciIo;
+    ASSERT (PciIo != NULL);
+
+    PciIo->Unmap (PciIo, Ehc->PeriodFrameMap);
+
+    PciIo->FreeBuffer (
+             PciIo,
+             EFI_SIZE_TO_PAGES (EFI_PAGE_SIZE),
+             Ehc->PeriodFrame
+             );
+
+    Ehc->PeriodFrame = NULL;
+  }
+
+  if (Ehc->PeriodFrameHost != NULL) {
+    FreePool (Ehc->PeriodFrameHost);
+    Ehc->PeriodFrameHost = NULL;
+  }
+}
+
+
+/**
+  Link the queue head to the asynchronous schedule list.
+  UEFI only supports one CTRL/BULK transfer at a time
+  due to its interfaces. This simplifies the AsynList
+  management: A reclamation header is always linked to
+  the AsyncListAddr, the only active QH is appended to it.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qh                    The queue head to link.
+
+**/
+VOID
+EhcLinkQhToAsync (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  )
+{
+  EHC_QH                  *Head;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  //
+  // Append the queue head after the reclaim header, then
+  // fix the hardware visiable parts (EHCI R1.0 page 72).
+  // ReclaimHead is always linked to the EHCI's AsynListAddr.
+  //
+  Head                    = Ehc->ReclaimHead;
+
+  Qh->NextQh              = Head->NextQh;
+  Head->NextQh            = Qh;
+
+  PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Qh->NextQh, sizeof (EHC_QH));
+  Qh->QhHw.HorizonLink    = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+  PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Head->NextQh, sizeof (EHC_QH));
+  Head->QhHw.HorizonLink  = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+}
+
+
+/**
+  Unlink a queue head from the asynchronous schedule list.
+  Need to synchronize with hardware.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qh                    The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromAsync (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  )
+{
+  EHC_QH                  *Head;
+  EFI_STATUS              Status;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  ASSERT (Ehc->ReclaimHead->NextQh == Qh);
+
+  //
+  // Remove the QH from reclamation head, then update the hardware
+  // visiable part: Only need to loopback the ReclaimHead. The Qh
+  // is pointing to ReclaimHead (which is staill in the list).
+  //
+  Head                    = Ehc->ReclaimHead;
+
+  Head->NextQh            = Qh->NextQh;
+  Qh->NextQh              = NULL;
+
+  PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Head->NextQh, sizeof (EHC_QH));
+  Head->QhHw.HorizonLink  = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+
+  //
+  // Set and wait the door bell to synchronize with the hardware
+  //
+  Status = EhcSetAndWaitDoorBell (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcUnlinkQhFromAsync: Failed to synchronize with doorbell\n"));
+  }
+}
+
+
+/**
+  Link a queue head for interrupt transfer to the periodic
+  schedule frame list. This code is very much the same as
+  that in UHCI.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qh                    The queue head to link.
+
+**/
+VOID
+EhcLinkQhToPeriod (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  )
+{
+  UINTN                   Index;
+  EHC_QH                  *Prev;
+  EHC_QH                  *Next;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  for (Index = 0; Index < EHC_FRAME_LEN; Index += Qh->Interval) {
+    //
+    // First QH can't be NULL because we always keep PeriodOne
+    // heads on the frame list
+    //
+    ASSERT (!EHC_LINK_TERMINATED (((UINT32*)Ehc->PeriodFrame)[Index]));
+    Next  = (EHC_QH*)((UINTN*)Ehc->PeriodFrameHost)[Index];
+    Prev  = NULL;
+
+    //
+    // Now, insert the queue head (Qh) into this frame:
+    // 1. Find a queue head with the same poll interval, just insert
+    //    Qh after this queue head, then we are done.
+    //
+    // 2. Find the position to insert the queue head into:
+    //      Previous head's interval is bigger than Qh's
+    //      Next head's interval is less than Qh's
+    //    Then, insert the Qh between then
+    //
+    while (Next->Interval > Qh->Interval) {
+      Prev  = Next;
+      Next  = Next->NextQh;
+    }
+
+    ASSERT (Next != NULL);
+
+    //
+    // The entry may have been linked into the frame by early insertation.
+    // For example: if insert a Qh with Qh.Interval == 4, and there is a Qh
+    // with Qh.Interval == 8 on the frame. If so, we are done with this frame.
+    // It isn't necessary to compare all the QH with the same interval to
+    // Qh. This is because if there is other QH with the same interval, Qh
+    // should has been inserted after that at Frames[0] and at Frames[0] it is
+    // impossible for (Next == Qh)
+    //
+    if (Next == Qh) {
+      continue;
+    }
+
+    if (Next->Interval == Qh->Interval) {
+      //
+      // If there is a QH with the same interval, it locates at
+      // Frames[0], and we can simply insert it after this QH. We
+      // are all done.
+      //
+      ASSERT ((Index == 0) && (Qh->NextQh == NULL));
+
+      Prev                    = Next;
+      Next                    = Next->NextQh;
+
+      Qh->NextQh              = Next;
+      Prev->NextQh            = Qh;
+
+      Qh->QhHw.HorizonLink    = Prev->QhHw.HorizonLink;
+      PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Qh, sizeof (EHC_QH));
+      Prev->QhHw.HorizonLink  = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+      break;
+    }
+
+    //
+    // OK, find the right position, insert it in. If Qh's next
+    // link has already been set, it is in position. This is
+    // guarranted by 2^n polling interval.
+    //
+    if (Qh->NextQh == NULL) {
+      Qh->NextQh              = Next;
+      PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Next, sizeof (EHC_QH));
+      Qh->QhHw.HorizonLink    = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+    }
+
+    PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Qh, sizeof (EHC_QH));
+
+    if (Prev == NULL) {
+      ((UINT32*)Ehc->PeriodFrame)[Index]     = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+      ((UINTN*)Ehc->PeriodFrameHost)[Index]  = (UINTN)Qh;
+    } else {
+      Prev->NextQh            = Qh;
+      Prev->QhHw.HorizonLink  = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+    }
+  }
+}
+
+
+/**
+  Unlink an interrupt queue head from the periodic
+  schedule frame list.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qh                    The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromPeriod (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  )
+{
+  UINTN                   Index;
+  EHC_QH                  *Prev;
+  EHC_QH                  *This;
+
+  for (Index = 0; Index < EHC_FRAME_LEN; Index += Qh->Interval) {
+    //
+    // Frame link can't be NULL because we always keep PeroidOne
+    // on the frame list
+    //
+    ASSERT (!EHC_LINK_TERMINATED (((UINT32*)Ehc->PeriodFrame)[Index]));
+    This  = (EHC_QH*)((UINTN*)Ehc->PeriodFrameHost)[Index];
+    Prev  = NULL;
+
+    //
+    // Walk through the frame's QH list to find the
+    // queue head to remove
+    //
+    while ((This != NULL) && (This != Qh)) {
+      Prev  = This;
+      This  = This->NextQh;
+    }
+
+    //
+    // Qh may have already been unlinked from this frame
+    // by early action. See the comments in EhcLinkQhToPeriod.
+    //
+    if (This == NULL) {
+      continue;
+    }
+
+    if (Prev == NULL) {
+      //
+      // Qh is the first entry in the frame
+      //
+      ((UINT32*)Ehc->PeriodFrame)[Index] = Qh->QhHw.HorizonLink;
+      ((UINTN*)Ehc->PeriodFrameHost)[Index] = (UINTN)Qh->NextQh;
+    } else {
+      Prev->NextQh            = Qh->NextQh;
+      Prev->QhHw.HorizonLink  = Qh->QhHw.HorizonLink;
+    }
+  }
+}
+
+
+/**
+  Check the URB's execution result and update the URB's
+  result accordingly.
+
+  @param  Ehc                   The EHCI device.
+  @param  Urb                   The URB to check result.
+
+  @return Whether the result of URB transfer is finialized.
+
+**/
+BOOLEAN
+EhcCheckUrbResult (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  URB                 *Urb
+  )
+{
+  LIST_ENTRY              *Entry;
+  EHC_QTD                 *Qtd;
+  QTD_HW                  *QtdHw;
+  UINT8                   State;
+  BOOLEAN                 Finished;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  ASSERT ((Ehc != NULL) && (Urb != NULL) && (Urb->Qh != NULL));
+
+  Finished        = TRUE;
+  Urb->Completed  = 0;
+
+  Urb->Result     = EFI_USB_NOERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    Urb->Result |= EFI_USB_ERR_SYSTEM;
+    goto ON_EXIT;
+  }
+
+  EFI_LIST_FOR_EACH (Entry, &Urb->Qh->Qtds) {
+    Qtd   = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+    QtdHw = &Qtd->QtdHw;
+    State = (UINT8) QtdHw->Status;
+
+    if (EHC_BIT_IS_SET (State, QTD_STAT_HALTED)) {
+      //
+      // EHCI will halt the queue head when met some error.
+      // If it is halted, the result of URB is finialized.
+      //
+      if ((State & QTD_STAT_ERR_MASK) == 0) {
+        Urb->Result |= EFI_USB_ERR_STALL;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_BABBLE_ERR)) {
+        Urb->Result |= EFI_USB_ERR_BABBLE;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_BUFF_ERR)) {
+        Urb->Result |= EFI_USB_ERR_BUFFER;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_TRANS_ERR) && (QtdHw->ErrCnt == 0)) {
+        Urb->Result |= EFI_USB_ERR_TIMEOUT;
+      }
+
+      Finished = TRUE;
+      goto ON_EXIT;
+
+    } else if (EHC_BIT_IS_SET (State, QTD_STAT_ACTIVE)) {
+      //
+      // The QTD is still active, no need to check furthur.
+      //
+      Urb->Result |= EFI_USB_ERR_NOTEXECUTE;
+
+      Finished = FALSE;
+      goto ON_EXIT;
+
+    } else {
+      //
+      // This QTD is finished OK or met short packet read. Update the
+      // transfer length if it isn't a setup.
+      //
+      if (QtdHw->Pid != QTD_PID_SETUP) {
+        Urb->Completed += Qtd->DataLen - QtdHw->TotalBytes;
+      }
+
+      if ((QtdHw->TotalBytes != 0) && (QtdHw->Pid == QTD_PID_INPUT)) {
+        EhcDumpQh (Urb->Qh, "Short packet read", FALSE);
+
+        //
+        // Short packet read condition. If it isn't a setup transfer,
+        // no need to check furthur: the queue head will halt at the
+        // ShortReadStop. If it is a setup transfer, need to check the
+        // Status Stage of the setup transfer to get the finial result
+        //
+        PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (EHC_QTD));
+        if (QtdHw->AltNext == QTD_LINK (PciAddr, FALSE)) {
+          DEBUG ((EFI_D_VERBOSE, "EhcCheckUrbResult: Short packet read, break\n"));
+
+          Finished = TRUE;
+          goto ON_EXIT;
+        }
+
+        DEBUG ((EFI_D_VERBOSE, "EhcCheckUrbResult: Short packet read, continue\n"));
+      }
+    }
+  }
+
+ON_EXIT:
+  //
+  // Return the data toggle set by EHCI hardware, bulk and interrupt
+  // transfer will use this to initialize the next transaction. For
+  // Control transfer, it always start a new data toggle sequence for
+  // new transfer.
+  //
+  // NOTICE: don't move DT update before the loop, otherwise there is
+  // a race condition that DT is wrong.
+  //
+  Urb->DataToggle = (UINT8) Urb->Qh->QhHw.DataToggle;
+
+  return Finished;
+}
+
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Ehc               The EHCI device.
+  @param  Urb               The URB to execute.
+  @param  TimeOut           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+EhcExecTransfer (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  URB                 *Urb,
+  IN  UINTN               TimeOut
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   Index;
+  UINTN                   Loop;
+  BOOLEAN                 Finished;
+  BOOLEAN                 InfiniteLoop;
+
+  Status       = EFI_SUCCESS;
+  Loop         = TimeOut * EHC_1_MILLISECOND;
+  Finished     = FALSE;
+  InfiniteLoop = FALSE;
+
+  //
+  // According to UEFI spec section 16.2.4, If Timeout is 0, then the caller
+  // must wait for the function to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR
+  // is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  for (Index = 0; InfiniteLoop || (Index < Loop); Index++) {
+    Finished = EhcCheckUrbResult (Ehc, Urb);
+
+    if (Finished) {
+      break;
+    }
+
+    gBS->Stall (EHC_1_MICROSECOND);
+  }
+
+  if (!Finished) {
+    DEBUG ((EFI_D_ERROR, "EhcExecTransfer: transfer not finished in %dms\n", (UINT32)TimeOut));
+    EhcDumpQh (Urb->Qh, NULL, FALSE);
+
+    Status = EFI_TIMEOUT;
+
+  } else if (Urb->Result != EFI_USB_NOERROR) {
+    DEBUG ((EFI_D_ERROR, "EhcExecTransfer: transfer failed with %x\n", Urb->Result));
+    EhcDumpQh (Urb->Qh, NULL, FALSE);
+
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+
+  @param  Ehc                   The EHCI device.
+  @param  DevAddr               The address of the target device.
+  @param  EpNum                 The endpoint of the target.
+  @param  DataToggle            Return the next data toggle to use.
+
+  @retval EFI_SUCCESS           An asynchronous transfer is removed.
+  @retval EFI_NOT_FOUND         No transfer for the device is found.
+
+**/
+EFI_STATUS
+EhciDelAsyncIntTransfer (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               EpNum,
+  OUT UINT8               *DataToggle
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  URB                     *Urb;
+  EFI_USB_DATA_DIRECTION  Direction;
+
+  Direction = (((EpNum & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut);
+  EpNum    &= 0x0F;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Ehc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+
+    if ((Urb->Ep.DevAddr == DevAddr) && (Urb->Ep.EpAddr == EpNum) &&
+        (Urb->Ep.Direction == Direction)) {
+      //
+      // Check the URB status to retrieve the next data toggle
+      // from the associated queue head.
+      //
+      EhcCheckUrbResult (Ehc, Urb);
+      *DataToggle = Urb->DataToggle;
+
+      EhcUnlinkQhFromPeriod (Ehc, Urb->Qh);
+      RemoveEntryList (&Urb->UrbList);
+
+      gBS->FreePool (Urb->Data);
+      EhcFreeUrb (Ehc, Urb);
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Remove all the asynchronous interrutp transfers.
+
+  @param  Ehc                   The EHCI device.
+
+**/
+VOID
+EhciDelAllAsyncIntTransfers (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  URB                     *Urb;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Ehc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+
+    EhcUnlinkQhFromPeriod (Ehc, Urb->Qh);
+    RemoveEntryList (&Urb->UrbList);
+
+    gBS->FreePool (Urb->Data);
+    EhcFreeUrb (Ehc, Urb);
+  }
+}
+
+
+/**
+  Flush data from PCI controller specific address to mapped system
+  memory address.
+
+  @param  Ehc                The EHCI device.
+  @param  Urb                The URB to unmap.
+
+  @retval EFI_SUCCESS        Success to flush data to mapped system memory.
+  @retval EFI_DEVICE_ERROR   Fail to flush data to mapped system memory.
+
+**/
+EFI_STATUS
+EhcFlushAsyncIntMap (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  URB                 *Urb
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  EFI_PCI_IO_PROTOCOL_OPERATION MapOp;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  UINTN                         Len;
+  VOID                          *Map;
+
+  PciIo = Ehc->PciIo;
+  Len   = Urb->DataLen;
+
+  if (Urb->Ep.Direction == EfiUsbDataIn) {
+    MapOp = EfiPciIoOperationBusMasterWrite;
+  } else {
+    MapOp = EfiPciIoOperationBusMasterRead;
+  }
+
+  Status = PciIo->Unmap (PciIo, Urb->DataMap);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Urb->DataMap = NULL;
+
+  Status = PciIo->Map (PciIo, MapOp, Urb->Data, &Len, &PhyAddr, &Map);
+  if (EFI_ERROR (Status) || (Len != Urb->DataLen)) {
+    goto ON_ERROR;
+  }
+
+  Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+  Urb->DataMap  = Map;
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  return EFI_DEVICE_ERROR;
+}
+
+
+/**
+  Update the queue head for next round of asynchronous transfer.
+
+  @param  Ehc                   The EHCI device.
+  @param  Urb                   The URB to update.
+
+**/
+VOID
+EhcUpdateAsyncRequest (
+  IN  USB2_HC_DEV         *Ehc,
+  IN URB                  *Urb
+  )
+{
+  LIST_ENTRY              *Entry;
+  EHC_QTD                 *FirstQtd;
+  QH_HW                   *QhHw;
+  EHC_QTD                 *Qtd;
+  QTD_HW                  *QtdHw;
+  UINTN                   Index;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  Qtd = NULL;
+
+  if (Urb->Result == EFI_USB_NOERROR) {
+    FirstQtd = NULL;
+
+    EFI_LIST_FOR_EACH (Entry, &Urb->Qh->Qtds) {
+      Qtd = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+
+      if (FirstQtd == NULL) {
+        FirstQtd = Qtd;
+      }
+
+      //
+      // Update the QTD for next round of transfer. Host control
+      // may change dt/Total Bytes to Transfer/C_Page/Cerr/Status/
+      // Current Offset. These fields need to be updated. DT isn't
+      // used by interrupt transfer. It uses DT in queue head.
+      // Current Offset is in Page[0], only need to reset Page[0]
+      // to initial data buffer.
+      //
+      QtdHw             = &Qtd->QtdHw;
+      QtdHw->Status     = QTD_STAT_ACTIVE;
+      QtdHw->ErrCnt     = QTD_MAX_ERR;
+      QtdHw->CurPage    = 0;
+      QtdHw->TotalBytes = (UINT32) Qtd->DataLen;
+      //
+      // calculate physical address by offset.
+      //
+      PciAddr = (UINTN)Urb->DataPhy + ((UINTN)Qtd->Data - (UINTN)Urb->Data); 
+      QtdHw->Page[0]    = EHC_LOW_32BIT (PciAddr);
+      QtdHw->PageHigh[0]= EHC_HIGH_32BIT (PciAddr);
+    }
+
+    //
+    // Update QH for next round of transfer. Host control only
+    // touch the fields in transfer overlay area. Only need to
+    // zero out the overlay area and set NextQtd to the first
+    // QTD. DateToggle bit is left untouched.
+    //
+    QhHw              = &Urb->Qh->QhHw;
+    QhHw->CurQtd      = QTD_LINK (0, TRUE);
+    QhHw->AltQtd      = 0;
+
+    QhHw->Status      = 0;
+    QhHw->Pid         = 0;
+    QhHw->ErrCnt      = 0;
+    QhHw->CurPage     = 0;
+    QhHw->Ioc         = 0;
+    QhHw->TotalBytes  = 0;
+
+    for (Index = 0; Index < 5; Index++) {
+      QhHw->Page[Index]     = 0;
+      QhHw->PageHigh[Index] = 0;
+    }
+
+    PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, FirstQtd, sizeof (EHC_QTD));
+    QhHw->NextQtd = QTD_LINK (PciAddr, FALSE);
+  }
+
+  return ;
+}
+
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                 Interrupt event.
+  @param  Context               Pointer to USB2_HC_DEV.
+
+**/
+VOID
+EFIAPI
+EhcMonitorAsyncRequests (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  BOOLEAN                 Finished;
+  UINT8                   *ProcBuf;
+  URB                     *Urb;
+  EFI_STATUS              Status;
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+  Ehc     = (USB2_HC_DEV *) Context;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Ehc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+
+    //
+    // Check the result of URB execution. If it is still
+    // active, check the next one.
+    //
+    Finished = EhcCheckUrbResult (Ehc, Urb);
+
+    if (!Finished) {
+      continue;
+    }
+
+    //
+    // Flush any PCI posted write transactions from a PCI host
+    // bridge to system memory.
+    //
+    Status = EhcFlushAsyncIntMap (Ehc, Urb);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "EhcMonitorAsyncRequests: Fail to Flush AsyncInt Mapped Memeory\n"));
+    }
+
+    //
+    // Allocate a buffer then copy the transferred data for user.
+    // If failed to allocate the buffer, update the URB for next
+    // round of transfer. Ignore the data of this round.
+    //
+    ProcBuf = NULL;
+
+    if (Urb->Result == EFI_USB_NOERROR) {
+      ASSERT (Urb->Completed <= Urb->DataLen);
+
+      ProcBuf = AllocatePool (Urb->Completed);
+
+      if (ProcBuf == NULL) {
+        EhcUpdateAsyncRequest (Ehc, Urb);
+        continue;
+      }
+
+      CopyMem (ProcBuf, Urb->Data, Urb->Completed);
+    }
+
+    EhcUpdateAsyncRequest (Ehc, Urb);
+
+    //
+    // Leave error recovery to its related device driver. A
+    // common case of the error recovery is to re-submit the
+    // interrupt transfer which is linked to the head of the
+    // list. This function scans from head to tail. So the
+    // re-submitted interrupt transfer's callback function
+    // will not be called again in this round. Don't touch this
+    // URB after the callback, it may have been removed by the
+    // callback.
+    //
+    if (Urb->Callback != NULL) {
+      //
+      // Restore the old TPL, USB bus maybe connect device in
+      // his callback. Some drivers may has a lower TPL restriction.
+      //
+      gBS->RestoreTPL (OldTpl);
+      (Urb->Callback) (ProcBuf, Urb->Completed, Urb->Context, Urb->Result);
+      OldTpl = gBS->RaiseTPL (EHC_TPL);
+    }
+
+    if (ProcBuf != NULL) {
+      FreePool (ProcBuf);
+    }
+  }
+
+  gBS->RestoreTPL (OldTpl);
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.h
new file mode 100644
index 0000000000..c03bd619d7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.h
@@ -0,0 +1,180 @@
+/** @file
+
+  This file contains the definination for host controller schedule routines.
+
+Copyright (c) 2007 - 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_SCHED_H_
+#define _EFI_EHCI_SCHED_H_
+
+
+/**
+  Initialize the schedule data structure such as frame list.
+
+  @param Ehc                    The EHCI device to init schedule data for.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to init schedule data.
+  @retval EFI_SUCCESS           The schedule data is initialized.
+
+**/
+EFI_STATUS
+EhcInitSched (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Free the schedule data. It may be partially initialized.
+
+  @param  Ehc            The EHCI device.
+
+**/
+VOID
+EhcFreeSched (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Link the queue head to the asynchronous schedule list.
+  UEFI only supports one CTRL/BULK transfer at a time
+  due to its interfaces. This simplifies the AsynList
+  management: A reclamation header is always linked to
+  the AsyncListAddr, the only active QH is appended to it.
+
+  @param  Ehc            The EHCI device.
+  @param  Qh             The queue head to link.
+
+**/
+VOID
+EhcLinkQhToAsync (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  );
+
+
+/**
+  Unlink a queue head from the asynchronous schedule list.
+  Need to synchronize with hardware.
+
+  @param  Ehc            The EHCI device.
+  @param  Qh             The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromAsync (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  );
+
+
+/**
+  Link a queue head for interrupt transfer to the periodic
+  schedule frame list. This code is very much the same as
+  that in UHCI.
+
+  @param  Ehc            The EHCI device.
+  @param  Qh             The queue head to link.
+
+**/
+VOID
+EhcLinkQhToPeriod (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  );
+
+
+/**
+  Unlink an interrupt queue head from the periodic
+  schedule frame list.
+
+  @param  Ehc            The EHCI device.
+  @param  Qh             The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromPeriod (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  );
+
+
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Ehc               The EHCI device.
+  @param  Urb               The URB to execute.
+  @param  TimeOut           The time to wait before abort, in millisecond.
+
+  @retval EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @retval EFI_TIMEOUT       The transfer failed due to time out.
+  @retval EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+EhcExecTransfer (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  URB                 *Urb,
+  IN  UINTN               TimeOut
+  );
+
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+
+  @param  Ehc            The EHCI device.
+  @param  DevAddr        The address of the target device.
+  @param  EpNum          The endpoint of the target.
+  @param  DataToggle     Return the next data toggle to use.
+
+  @retval EFI_SUCCESS    An asynchronous transfer is removed.
+  @retval EFI_NOT_FOUND  No transfer for the device is found.
+
+**/
+EFI_STATUS
+EhciDelAsyncIntTransfer (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               EpNum,
+  OUT UINT8               *DataToggle
+  );
+
+
+/**
+  Remove all the asynchronous interrutp transfers.
+
+  @param  Ehc            The EHCI device.
+
+**/
+VOID
+EhciDelAllAsyncIntTransfers (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event          Interrupt event.
+  @param  Context        Pointer to USB2_HC_DEV.
+
+**/
+VOID
+EFIAPI
+EhcMonitorAsyncRequests (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.c
new file mode 100644
index 0000000000..6afb327df1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.c
@@ -0,0 +1,657 @@
+/** @file
+
+    This file contains URB request, each request is warpped in a
+    URB (Usb Request Block).
+
+Copyright (c) 2007 - 2010, 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 "Ehci.h"
+
+
+/**
+  Create a single QTD to hold the data.
+
+  @param  Ehc                   The EHCI device.
+  @param  Data                  The cpu memory address of current data not associated with a QTD.
+  @param  DataPhy               The pci bus address of current data not associated with a QTD.
+  @param  DataLen               The length of the data.
+  @param  PktId                 Packet ID to use in the QTD.
+  @param  Toggle                Data toggle to use in the QTD.
+  @param  MaxPacket             Maximu packet length of the endpoint.
+
+  @return Created QTD or NULL if failed to create one.
+
+**/
+EHC_QTD *
+EhcCreateQtd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN UINT8                PktId,
+  IN UINT8                Toggle,
+  IN UINTN                MaxPacket
+  )
+{
+  EHC_QTD                 *Qtd;
+  QTD_HW                  *QtdHw;
+  UINTN                   Index;
+  UINTN                   Len;
+  UINTN                   ThisBufLen;
+
+  ASSERT (Ehc != NULL);
+
+  Qtd = UsbHcAllocateMem (Ehc->MemPool, sizeof (EHC_QTD));
+
+  if (Qtd == NULL) {
+    return NULL;
+  }
+
+  Qtd->Signature    = EHC_QTD_SIG;
+  Qtd->Data         = Data;
+  Qtd->DataLen      = 0;
+
+  InitializeListHead (&Qtd->QtdList);
+
+  QtdHw             = &Qtd->QtdHw;
+  QtdHw->NextQtd    = QTD_LINK (NULL, TRUE);
+  QtdHw->AltNext    = QTD_LINK (NULL, TRUE);
+  QtdHw->Status     = QTD_STAT_ACTIVE;
+  QtdHw->Pid        = PktId;
+  QtdHw->ErrCnt     = QTD_MAX_ERR;
+  QtdHw->Ioc        = 0;
+  QtdHw->TotalBytes = 0;
+  QtdHw->DataToggle = Toggle;
+
+  //
+  // Fill in the buffer points
+  //
+  if (Data != NULL) {
+    Len = 0;
+
+    for (Index = 0; Index <= QTD_MAX_BUFFER; Index++) {
+      //
+      // Set the buffer point (Check page 41 EHCI Spec 1.0). No need to
+      // compute the offset and clear Reserved fields. This is already
+      // done in the data point.
+      //
+      QtdHw->Page[Index]      = EHC_LOW_32BIT (DataPhy);
+      QtdHw->PageHigh[Index]  = EHC_HIGH_32BIT (DataPhy);
+
+      ThisBufLen              = QTD_BUF_LEN - (EHC_LOW_32BIT (DataPhy) & QTD_BUF_MASK);
+
+      if (Len + ThisBufLen >= DataLen) {
+        Len = DataLen;
+        break;
+      }
+
+      Len += ThisBufLen;
+      Data += ThisBufLen;
+      DataPhy += ThisBufLen;
+    }
+
+    //
+    // Need to fix the last pointer if the Qtd can't hold all the
+    // user's data to make sure that the length is in the unit of
+    // max packets. If it can hold all the data, there is no such
+    // need.
+    //
+    if (Len < DataLen) {
+      Len = Len - Len % MaxPacket;
+    }
+
+    QtdHw->TotalBytes = (UINT32) Len;
+    Qtd->DataLen      = Len;
+  }
+
+  return Qtd;
+}
+
+
+
+/**
+  Initialize the queue head for interrupt transfer,
+  that is, initialize the following three fields:
+  1. SplitXState in the Status field
+  2. Microframe S-mask
+  3. Microframe C-mask
+
+  @param  Ep                    The queue head's related endpoint.
+  @param  QhHw                  The queue head to initialize.
+
+**/
+VOID
+EhcInitIntQh (
+  IN USB_ENDPOINT         *Ep,
+  IN QH_HW                *QhHw
+  )
+{
+  //
+  // Because UEFI interface can't utilitize an endpoint with
+  // poll rate faster than 1ms, only need to set one bit in
+  // the queue head. simple. But it may be changed later. If
+  // sub-1ms interrupt is supported, need to update the S-Mask
+  // here
+  //
+  if (Ep->DevSpeed == EFI_USB_SPEED_HIGH) {
+    QhHw->SMask = QH_MICROFRAME_0;
+    return ;
+  }
+
+  //
+  // For low/full speed device, the transfer must go through
+  // the split transaction. Need to update three fields
+  // 1. SplitXState in the status
+  // 2. Microframe S-Mask
+  // 3. Microframe C-Mask
+  // UEFI USB doesn't exercise admission control. It simplely
+  // schedule the high speed transactions in microframe 0, and
+  // full/low speed transactions at microframe 1. This also
+  // avoid the use of FSTN.
+  //
+  QhHw->SMask = QH_MICROFRAME_1;
+  QhHw->CMask = QH_MICROFRAME_3 | QH_MICROFRAME_4 | QH_MICROFRAME_5;
+}
+
+
+
+/**
+  Allocate and initialize a EHCI queue head.
+
+  @param  Ehci                  The EHCI device.
+  @param  Ep                    The endpoint to create queue head for.
+
+  @return Created queue head or NULL if failed to create one.
+
+**/
+EHC_QH *
+EhcCreateQh (
+  IN USB2_HC_DEV          *Ehci,
+  IN USB_ENDPOINT         *Ep
+  )
+{
+  EHC_QH                  *Qh;
+  QH_HW                   *QhHw;
+
+  Qh = UsbHcAllocateMem (Ehci->MemPool, sizeof (EHC_QH));
+
+  if (Qh == NULL) {
+    return NULL;
+  }
+
+  Qh->Signature       = EHC_QH_SIG;
+  Qh->NextQh          = NULL;
+  Qh->Interval        = Ep->PollRate;
+
+  InitializeListHead (&Qh->Qtds);
+
+  QhHw                = &Qh->QhHw;
+  QhHw->HorizonLink   = QH_LINK (NULL, 0, TRUE);
+  QhHw->DeviceAddr    = Ep->DevAddr;
+  QhHw->Inactive      = 0;
+  QhHw->EpNum         = Ep->EpAddr;
+  QhHw->EpSpeed       = Ep->DevSpeed;
+  QhHw->DtCtrl        = 0;
+  QhHw->ReclaimHead   = 0;
+  QhHw->MaxPacketLen  = (UINT32) Ep->MaxPacket;
+  QhHw->CtrlEp        = 0;
+  QhHw->NakReload     = QH_NAK_RELOAD;
+  QhHw->HubAddr       = Ep->HubAddr;
+  QhHw->PortNum       = Ep->HubPort;
+  QhHw->Multiplier    = 1;
+  QhHw->DataToggle    = Ep->Toggle;
+
+  if (Ep->DevSpeed != EFI_USB_SPEED_HIGH) {
+    QhHw->Status |= QTD_STAT_DO_SS;
+  }
+
+  switch (Ep->Type) {
+  case EHC_CTRL_TRANSFER:
+    //
+    // Special initialization for the control transfer:
+    // 1. Control transfer initialize data toggle from each QTD
+    // 2. Set the Control Endpoint Flag (C) for low/full speed endpoint.
+    //
+    QhHw->DtCtrl = 1;
+
+    if (Ep->DevSpeed != EFI_USB_SPEED_HIGH) {
+      QhHw->CtrlEp = 1;
+    }
+    break;
+
+  case EHC_INT_TRANSFER_ASYNC:
+  case EHC_INT_TRANSFER_SYNC:
+    //
+    // Special initialization for the interrupt transfer
+    // to set the S-Mask and C-Mask
+    //
+    QhHw->NakReload = 0;
+    EhcInitIntQh (Ep, QhHw);
+    break;
+
+  case EHC_BULK_TRANSFER:
+    if ((Ep->DevSpeed == EFI_USB_SPEED_HIGH) && (Ep->Direction == EfiUsbDataOut)) {
+      QhHw->Status |= QTD_STAT_DO_PING;
+    }
+
+    break;
+  }
+
+  return Qh;
+}
+
+
+/**
+  Convert the poll interval from application to that
+  be used by EHCI interface data structure. Only need
+  to get the max 2^n that is less than interval. UEFI
+  can't support high speed endpoint with a interval less
+  than 8 microframe because interval is specified in
+  the unit of ms (millisecond).
+
+  @param  Interval              The interval to convert.
+
+  @return The converted interval.
+
+**/
+UINTN
+EhcConvertPollRate (
+  IN  UINTN               Interval
+  )
+{
+  UINTN                   BitCount;
+
+  if (Interval == 0) {
+    return 1;
+  }
+
+  //
+  // Find the index (1 based) of the highest non-zero bit
+  //
+  BitCount = 0;
+
+  while (Interval != 0) {
+    Interval >>= 1;
+    BitCount++;
+  }
+
+  return (UINTN)1 << (BitCount - 1);
+}
+
+
+/**
+  Free a list of QTDs.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qtds                  The list head of the QTD.
+
+**/
+VOID
+EhcFreeQtds (
+  IN USB2_HC_DEV          *Ehc,
+  IN LIST_ENTRY           *Qtds
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  EHC_QTD                 *Qtd;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+
+    RemoveEntryList (&Qtd->QtdList);
+    UsbHcFreeMem (Ehc->MemPool, Qtd, sizeof (EHC_QTD));
+  }
+}
+
+
+/**
+  Free an allocated URB. It is possible for it to be partially inited.
+
+  @param  Ehc                   The EHCI device.
+  @param  Urb                   The URB to free.
+
+**/
+VOID
+EhcFreeUrb (
+  IN USB2_HC_DEV          *Ehc,
+  IN URB                  *Urb
+  )
+{
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+
+  PciIo = Ehc->PciIo;
+
+  if (Urb->RequestPhy != NULL) {
+    PciIo->Unmap (PciIo, Urb->RequestMap);
+  }
+
+  if (Urb->DataMap != NULL) {
+    PciIo->Unmap (PciIo, 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 (EHC_QH));
+  }
+
+  gBS->FreePool (Urb);
+}
+
+
+/**
+  Create a list of QTDs for the URB.
+
+  @param  Ehc                   The EHCI device.
+  @param  Urb                   The URB to create QTDs for.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for QTD.
+  @retval EFI_SUCCESS           The QTDs are allocated for the URB.
+
+**/
+EFI_STATUS
+EhcCreateQtds (
+  IN USB2_HC_DEV          *Ehc,
+  IN URB                  *Urb
+  )
+{
+  USB_ENDPOINT            *Ep;
+  EHC_QH                  *Qh;
+  EHC_QTD                 *Qtd;
+  EHC_QTD                 *StatusQtd;
+  EHC_QTD                 *NextQtd;
+  LIST_ENTRY              *Entry;
+  UINT32                  AlterNext;
+  UINT8                   Toggle;
+  UINTN                   Len;
+  UINT8                   Pid;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+
+  ASSERT ((Urb != NULL) && (Urb->Qh != NULL));
+
+  //
+  // EHCI follows the alternet next QTD pointer if it meets
+  // a short read and the AlterNext pointer is valid. UEFI
+  // EHCI driver should terminate the transfer except the
+  // control transfer.
+  //
+  Toggle    = 0;
+  Qh        = Urb->Qh;
+  Ep        = &Urb->Ep;
+  StatusQtd = NULL;
+  AlterNext = QTD_LINK (NULL, TRUE);
+
+  PhyAddr   = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (EHC_QTD));
+  if (Ep->Direction == EfiUsbDataIn) {
+    AlterNext = QTD_LINK (PhyAddr, FALSE);
+  }
+
+  //
+  // Build the Setup and status packets for control transfer
+  //
+  if (Urb->Ep.Type == EHC_CTRL_TRANSFER) {
+    Len = sizeof (EFI_USB_DEVICE_REQUEST);
+    Qtd = EhcCreateQtd (Ehc, (UINT8 *)Urb->Request, (UINT8 *)Urb->RequestPhy, Len, QTD_PID_SETUP, 0, Ep->MaxPacket);
+
+    if (Qtd == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    InsertTailList (&Qh->Qtds, &Qtd->QtdList);
+
+    //
+    // Create the status packet now. Set the AlterNext to it. So, when
+    // EHCI meets a short control read, it can resume at the status stage.
+    // Use the opposite direction of the data stage, or IN if there is
+    // no data stage.
+    //
+    if (Ep->Direction == EfiUsbDataIn) {
+      Pid = QTD_PID_OUTPUT;
+    } else {
+      Pid = QTD_PID_INPUT;
+    }
+
+    StatusQtd = EhcCreateQtd (Ehc, NULL, NULL, 0, Pid, 1, Ep->MaxPacket);
+
+    if (StatusQtd == NULL) {
+      goto ON_ERROR;
+    }
+
+    if (Ep->Direction == EfiUsbDataIn) {
+      PhyAddr   = UsbHcGetPciAddressForHostMem (Ehc->MemPool, StatusQtd, sizeof (EHC_QTD));
+      AlterNext = QTD_LINK (PhyAddr, FALSE);
+    }
+
+    Toggle = 1;
+  }
+
+  //
+  // Build the data packets for all the transfers
+  //
+  if (Ep->Direction == EfiUsbDataIn) {
+    Pid = QTD_PID_INPUT;
+  } else {
+    Pid = QTD_PID_OUTPUT;
+  }
+
+  Qtd = NULL;
+  Len = 0;
+
+  while (Len < Urb->DataLen) {
+    Qtd = EhcCreateQtd (
+            Ehc,
+            (UINT8 *) Urb->Data + Len,
+            (UINT8 *) Urb->DataPhy + Len,
+            Urb->DataLen - Len,
+            Pid,
+            Toggle,
+            Ep->MaxPacket
+            );
+
+    if (Qtd == NULL) {
+      goto ON_ERROR;
+    }
+
+    Qtd->QtdHw.AltNext = AlterNext;
+    InsertTailList (&Qh->Qtds, &Qtd->QtdList);
+
+    //
+    // Switch the Toggle bit if odd number of packets are included in the QTD.
+    //
+    if (((Qtd->DataLen + Ep->MaxPacket - 1) / Ep->MaxPacket) % 2) {
+      Toggle = (UINT8) (1 - Toggle);
+    }
+
+    Len += Qtd->DataLen;
+  }
+
+  //
+  // Insert the status packet for control transfer
+  //
+  if (Ep->Type == EHC_CTRL_TRANSFER) {
+    InsertTailList (&Qh->Qtds, &StatusQtd->QtdList);
+  }
+
+  //
+  // OK, all the QTDs needed are created. Now, fix the NextQtd point
+  //
+  EFI_LIST_FOR_EACH (Entry, &Qh->Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+
+    //
+    // break if it is the last entry on the list
+    //
+    if (Entry->ForwardLink == &Qh->Qtds) {
+      break;
+    }
+
+    NextQtd             = EFI_LIST_CONTAINER (Entry->ForwardLink, EHC_QTD, QtdList);
+    PhyAddr             = UsbHcGetPciAddressForHostMem (Ehc->MemPool, NextQtd, sizeof (EHC_QTD));
+    Qtd->QtdHw.NextQtd  = QTD_LINK (PhyAddr, FALSE);
+  }
+
+  //
+  // Link the QTDs to the queue head
+  //
+  NextQtd           = EFI_LIST_CONTAINER (Qh->Qtds.ForwardLink, EHC_QTD, QtdList);
+  PhyAddr           = UsbHcGetPciAddressForHostMem (Ehc->MemPool, NextQtd, sizeof (EHC_QTD));
+  Qh->QhHw.NextQtd  = QTD_LINK (PhyAddr, FALSE);
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  EhcFreeQtds (Ehc, &Qh->Qtds);
+  return EFI_OUT_OF_RESOURCES;
+}
+
+
+/**
+  Create a new URB and its associated QTD.
+
+  @param  Ehc                   The EHCI device.
+  @param  DevAddr               The device address.
+  @param  EpAddr                Endpoint addrress & its direction.
+  @param  DevSpeed              The device speed.
+  @param  Toggle                Initial data toggle to use.
+  @param  MaxPacket             The max packet length of the endpoint.
+  @param  Hub                   The transaction translator to use.
+  @param  Type                  The transaction type.
+  @param  Request               The standard USB request for control transfer.
+  @param  Data                  The user data to transfer.
+  @param  DataLen               The length of data buffer.
+  @param  Callback              The function to call when data is transferred.
+  @param  Context               The context to the callback.
+  @param  Interval              The interval for interrupt transfer.
+
+  @return Created URB or NULL.
+
+**/
+URB *
+EhcCreateUrb (
+  IN USB2_HC_DEV                        *Ehc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINT8                              Toggle,
+  IN UINTN                              MaxPacket,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR *Hub,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context,
+  IN UINTN                              Interval
+  )
+{
+  USB_ENDPOINT                  *Ep;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  EFI_PCI_IO_PROTOCOL_OPERATION MapOp;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  EFI_STATUS                    Status;
+  UINTN                         Len;
+  URB                           *Urb;
+  VOID                          *Map;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature  = EHC_URB_SIG;
+  InitializeListHead (&Urb->UrbList);
+
+  Ep              = &Urb->Ep;
+  Ep->DevAddr     = DevAddr;
+  Ep->EpAddr      = (UINT8) (EpAddr & 0x0F);
+  Ep->Direction   = (((EpAddr & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut);
+  Ep->DevSpeed    = DevSpeed;
+  Ep->MaxPacket   = MaxPacket;
+
+  Ep->HubAddr     = 0;
+  Ep->HubPort     = 0;
+
+  if (DevSpeed != EFI_USB_SPEED_HIGH) {
+    ASSERT (Hub != NULL);
+
+    Ep->HubAddr   = Hub->TranslatorHubAddress;
+    Ep->HubPort   = Hub->TranslatorPortNumber;
+  }
+
+  Ep->Toggle      = Toggle;
+  Ep->Type        = Type;
+  Ep->PollRate    = EhcConvertPollRate (Interval);
+
+  Urb->Request    = Request;
+  Urb->Data       = Data;
+  Urb->DataLen    = DataLen;
+  Urb->Callback   = Callback;
+  Urb->Context    = Context;
+
+  PciIo           = Ehc->PciIo;
+  Urb->Qh         = EhcCreateQh (Ehc, &Urb->Ep);
+
+  if (Urb->Qh == NULL) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Map the request and user data
+  //
+  if (Request != NULL) {
+    Len     = sizeof (EFI_USB_DEVICE_REQUEST);
+    MapOp   = EfiPciIoOperationBusMasterRead;
+    Status  = PciIo->Map (PciIo, 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;
+  }
+
+  if (Data != NULL) {
+    Len     = DataLen;
+
+    if (Ep->Direction == EfiUsbDataIn) {
+      MapOp = EfiPciIoOperationBusMasterWrite;
+    } else {
+      MapOp = EfiPciIoOperationBusMasterRead;
+    }
+
+    Status  = PciIo->Map (PciIo, MapOp, Data, &Len, &PhyAddr, &Map);
+
+    if (EFI_ERROR (Status) || (Len != DataLen)) {
+      goto ON_ERROR;
+    }
+
+    Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+    Urb->DataMap  = Map;
+  }
+
+  Status = EhcCreateQtds (Ehc, Urb);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return Urb;
+
+ON_ERROR:
+  EhcFreeUrb (Ehc, Urb);
+  return NULL;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.h
new file mode 100644
index 0000000000..02e9af81be
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.h
@@ -0,0 +1,336 @@
+/** @file
+
+    This file contains URB request, each request is warpped in a
+    URB (Usb Request Block).
+
+Copyright (c) 2007 - 2010, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_URB_H_
+#define _EFI_EHCI_URB_H_
+
+
+typedef struct _EHC_QTD  EHC_QTD;
+typedef struct _EHC_QH   EHC_QH;
+typedef struct _URB      URB;
+
+//
+// Transfer types, used in URB to identify the transfer type
+//
+#define EHC_CTRL_TRANSFER       0x01
+#define EHC_BULK_TRANSFER       0x02
+#define EHC_INT_TRANSFER_SYNC   0x04
+#define EHC_INT_TRANSFER_ASYNC  0x08
+
+#define EHC_QTD_SIG             SIGNATURE_32 ('U', 'S', 'B', 'T')
+#define EHC_QH_SIG              SIGNATURE_32 ('U', 'S', 'B', 'H')
+#define EHC_URB_SIG             SIGNATURE_32 ('U', 'S', 'B', 'R')
+
+//
+// Hardware related bit definitions
+//
+#define EHC_TYPE_ITD            0x00
+#define EHC_TYPE_QH             0x02
+#define EHC_TYPE_SITD           0x04
+#define EHC_TYPE_FSTN           0x06
+
+#define QH_NAK_RELOAD           3
+#define QH_HSHBW_MULTI          1
+
+#define QTD_MAX_ERR             3
+#define QTD_PID_OUTPUT          0x00
+#define QTD_PID_INPUT           0x01
+#define QTD_PID_SETUP           0x02
+
+#define QTD_STAT_DO_OUT         0
+#define QTD_STAT_DO_SS          0
+#define QTD_STAT_DO_PING        0x01
+#define QTD_STAT_DO_CS          0x02
+#define QTD_STAT_TRANS_ERR      0x08
+#define QTD_STAT_BABBLE_ERR     0x10
+#define QTD_STAT_BUFF_ERR       0x20
+#define QTD_STAT_HALTED         0x40
+#define QTD_STAT_ACTIVE         0x80
+#define QTD_STAT_ERR_MASK       (QTD_STAT_TRANS_ERR | QTD_STAT_BABBLE_ERR | QTD_STAT_BUFF_ERR)
+
+#define QTD_MAX_BUFFER          4
+#define QTD_BUF_LEN             4096
+#define QTD_BUF_MASK            0x0FFF
+
+#define QH_MICROFRAME_0         0x01
+#define QH_MICROFRAME_1         0x02
+#define QH_MICROFRAME_2         0x04
+#define QH_MICROFRAME_3         0x08
+#define QH_MICROFRAME_4         0x10
+#define QH_MICROFRAME_5         0x20
+#define QH_MICROFRAME_6         0x40
+#define QH_MICROFRAME_7         0x80
+
+#define USB_ERR_SHORT_PACKET    0x200
+
+//
+// Fill in the hardware link point: pass in a EHC_QH/QH_HW
+// pointer to QH_LINK; A EHC_QTD/QTD_HW pointer to QTD_LINK
+//
+#define QH_LINK(Addr, Type, Term) \
+          ((UINT32) ((EHC_LOW_32BIT (Addr) & 0xFFFFFFE0) | (Type) | ((Term) ? 1 : 0)))
+
+#define QTD_LINK(Addr, Term)      QH_LINK((Addr), 0, (Term))
+
+//
+// The defination of EHCI hardware used data structure for
+// little endian architecture. The QTD and QH structures
+// are required to be 32 bytes aligned. Don't add members
+// to the head of the associated software strucuture.
+//
+#pragma pack(1)
+typedef struct {
+  UINT32                  NextQtd;
+  UINT32                  AltNext;
+
+  UINT32                  Status       : 8;
+  UINT32                  Pid          : 2;
+  UINT32                  ErrCnt       : 2;
+  UINT32                  CurPage      : 3;
+  UINT32                  Ioc          : 1;
+  UINT32                  TotalBytes   : 15;
+  UINT32                  DataToggle   : 1;
+
+  UINT32                  Page[5];
+  UINT32                  PageHigh[5];
+} QTD_HW;
+
+typedef struct {
+  UINT32                  HorizonLink;
+  //
+  // Endpoint capabilities/Characteristics DWord 1 and DWord 2
+  //
+  UINT32                  DeviceAddr   : 7;
+  UINT32                  Inactive     : 1;
+  UINT32                  EpNum        : 4;
+  UINT32                  EpSpeed      : 2;
+  UINT32                  DtCtrl       : 1;
+  UINT32                  ReclaimHead  : 1;
+  UINT32                  MaxPacketLen : 11;
+  UINT32                  CtrlEp       : 1;
+  UINT32                  NakReload    : 4;
+
+  UINT32                  SMask        : 8;
+  UINT32                  CMask        : 8;
+  UINT32                  HubAddr      : 7;
+  UINT32                  PortNum      : 7;
+  UINT32                  Multiplier   : 2;
+
+  //
+  // Transaction execution overlay area
+  //
+  UINT32                  CurQtd;
+  UINT32                  NextQtd;
+  UINT32                  AltQtd;
+
+  UINT32                  Status       : 8;
+  UINT32                  Pid          : 2;
+  UINT32                  ErrCnt       : 2;
+  UINT32                  CurPage      : 3;
+  UINT32                  Ioc          : 1;
+  UINT32                  TotalBytes   : 15;
+  UINT32                  DataToggle   : 1;
+
+  UINT32                  Page[5];
+  UINT32                  PageHigh[5];
+} QH_HW;
+#pragma pack()
+
+
+//
+// Endpoint address and its capabilities
+//
+typedef struct _USB_ENDPOINT {
+  UINT8                   DevAddr;
+  UINT8                   EpAddr;     // Endpoint address, no direction encoded in
+  EFI_USB_DATA_DIRECTION  Direction;
+  UINT8                   DevSpeed;
+  UINTN                   MaxPacket;
+  UINT8                   HubAddr;
+  UINT8                   HubPort;
+  UINT8                   Toggle;     // Data toggle, not used for control transfer
+  UINTN                   Type;
+  UINTN                   PollRate;   // Polling interval used by EHCI
+} USB_ENDPOINT;
+
+//
+// Software QTD strcture, this is used to manage all the
+// QTD generated from a URB. Don't add fields before QtdHw.
+//
+struct _EHC_QTD {
+  QTD_HW                  QtdHw;
+  UINT32                  Signature;
+  LIST_ENTRY              QtdList;   // The list of QTDs to one end point
+  UINT8                   *Data;     // Buffer of the original data
+  UINTN                   DataLen;   // Original amount of data in this QTD
+};
+
+//
+// Software QH structure. All three different transaction types
+// supported by UEFI USB, that is the control/bulk/interrupt
+// transfers use the queue head and queue token strcuture.
+//
+// Interrupt QHs are linked to periodic frame list in the reversed
+// 2^N tree. Each interrupt QH is linked to the list starting at
+// frame 0. There is a dummy interrupt QH linked to each frame as
+// a sentinental whose polling interval is 1. Synchronous interrupt
+// transfer is linked after this dummy QH.
+//
+// For control/bulk transfer, only synchronous (in the sense of UEFI)
+// transfer is supported. A dummy QH is linked to EHCI AsyncListAddr
+// as the reclamation header. New transfer is inserted after this QH.
+//
+struct _EHC_QH {
+  QH_HW                   QhHw;
+  UINT32                  Signature;
+  EHC_QH                  *NextQh;    // The queue head pointed to by horizontal link
+  LIST_ENTRY              Qtds;       // The list of QTDs to this queue head
+  UINTN                   Interval;
+};
+
+//
+// URB (Usb Request Block) contains information for all kinds of
+// usb requests.
+//
+struct _URB {
+  UINT32                          Signature;
+  LIST_ENTRY                      UrbList;
+
+  //
+  // Transaction information
+  //
+  USB_ENDPOINT                    Ep;
+  EFI_USB_DEVICE_REQUEST          *Request;     // Control transfer only
+  VOID                            *RequestPhy;  // Address of the mapped request
+  VOID                            *RequestMap;
+  VOID                            *Data;
+  UINTN                           DataLen;
+  VOID                            *DataPhy;     // Address of the mapped user data
+  VOID                            *DataMap;
+  EFI_ASYNC_USB_TRANSFER_CALLBACK Callback;
+  VOID                            *Context;
+
+  //
+  // Schedule data
+  //
+  EHC_QH                          *Qh;
+
+  //
+  // Transaction result
+  //
+  UINT32                          Result;
+  UINTN                           Completed;    // completed data length
+  UINT8                           DataToggle;
+};
+
+
+
+/**
+  Create a single QTD to hold the data.
+
+  @param  Ehc        The EHCI device.
+  @param  Data       The cpu memory address of current data not associated with a QTD.
+  @param  DataPhy    The pci bus address of current data not associated with a QTD.
+  @param  DataLen    The length of the data.
+  @param  PktId      Packet ID to use in the QTD.
+  @param  Toggle     Data toggle to use in the QTD.
+  @param  MaxPacket  Maximu packet length of the endpoint.
+
+  @return Created QTD or NULL if failed to create one.
+
+**/
+EHC_QTD *
+EhcCreateQtd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN UINT8                PktId,
+  IN UINT8                Toggle,
+  IN UINTN                MaxPacket
+  );
+
+
+
+/**
+  Allocate and initialize a EHCI queue head.
+
+  @param  Ehci       The EHCI device.
+  @param  Ep         The endpoint to create queue head for.
+
+  @return Created queue head or NULL if failed to create one.
+
+**/
+EHC_QH *
+EhcCreateQh (
+  IN USB2_HC_DEV          *Ehci,
+  IN USB_ENDPOINT         *Ep
+  );
+
+
+/**
+  Free an allocated URB. It is possible for it to be partially inited.
+
+  @param  Ehc        The EHCI device.
+  @param  Urb        The URB to free.
+
+**/
+VOID
+EhcFreeUrb (
+  IN USB2_HC_DEV          *Ehc,
+  IN URB                  *Urb
+  );
+
+
+/**
+  Create a new URB and its associated QTD.
+
+  @param  Ehc        The EHCI device.
+  @param  DevAddr    The device address.
+  @param  EpAddr     Endpoint addrress & its direction.
+  @param  DevSpeed   The device speed.
+  @param  Toggle     Initial data toggle to use.
+  @param  MaxPacket  The max packet length of the endpoint.
+  @param  Hub        The transaction translator to use.
+  @param  Type       The transaction type.
+  @param  Request    The standard USB request for control transfer.
+  @param  Data       The user data to transfer.
+  @param  DataLen    The length of data buffer.
+  @param  Callback   The function to call when data is transferred.
+  @param  Context    The context to the callback.
+  @param  Interval   The interval for interrupt transfer.
+
+  @return Created URB or NULL.
+
+**/
+URB *
+EhcCreateUrb (
+  IN USB2_HC_DEV                        *Ehc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINT8                              Toggle,
+  IN UINTN                              MaxPacket,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR *Hub,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context,
+  IN UINTN                              Interval
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.c
new file mode 100644
index 0000000000..28be3803f1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.c
@@ -0,0 +1,566 @@
+/** @file
+
+  Routine procedures for memory allocate/free.
+
+Copyright (c) 2007 - 2013, 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 "Ehci.h"
+
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pool           The buffer pool to allocate memory for.
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Pages
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  VOID                    *BufHost;
+  VOID                    *Mapping;
+  EFI_PHYSICAL_ADDRESS    MappedAddr;
+  UINTN                   Bytes;
+  EFI_STATUS              Status;
+
+  PciIo = Pool->PciIo;
+
+  Block = AllocateZeroPool (sizeof (USBHC_MEM_BLOCK));
+  if (Block == NULL) {
+    return NULL;
+  }
+
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (USBHC_MEM_UNIT * 8);
+  Block->Bits     = AllocateZeroPool (Block->BitsLen);
+
+  if (Block->Bits == NULL) {
+    gBS->FreePool (Block);
+    return NULL;
+  }
+
+  //
+  // Allocate the number of Pages of memory, then map it for
+  // bus master read and write.
+  //
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    Pages,
+                    &BufHost,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_BITARRAY;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (Pages);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    BufHost,
+                    &Bytes,
+                    &MappedAddr,
+                    &Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (Pages))) {
+    goto FREE_BUFFER;
+  }
+
+  //
+  // Check whether the data structure used by the host controller
+  // should be restricted into the same 4G
+  //
+  if (Pool->Check4G && (Pool->Which4G != USB_HC_HIGH_32BIT (MappedAddr))) {
+    PciIo->Unmap (PciIo, Mapping);
+    goto FREE_BUFFER;
+  }
+
+  Block->BufHost  = BufHost;
+  Block->Buf      = (UINT8 *) ((UINTN) MappedAddr);
+  Block->Mapping  = Mapping;
+
+  return Block;
+
+FREE_BUFFER:
+  PciIo->FreeBuffer (PciIo, Pages, BufHost);
+
+FREE_BITARRAY:
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+  return NULL;
+}
+
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL       *Pool,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+
+  ASSERT ((Pool != NULL) && (Block != NULL));
+
+  PciIo = Pool->PciIo;
+
+  //
+  // Unmap the common buffer then free the structures
+  //
+  PciIo->Unmap (PciIo, Block->Mapping);
+  PciIo->FreeBuffer (PciIo, EFI_SIZE_TO_PAGES (Block->BufLen), Block->BufHost);
+
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+}
+
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block          The memory block to allocate memory from.
+  @param  Units          Number of memory units to allocate.
+
+  @return The pointer to the allocated memory. If couldn't allocate the needed memory,
+          the return value is NULL.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN  USBHC_MEM_BLOCK     *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->BufHost + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  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.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+
+/**
+  Is the memory block empty?
+
+  @param  Block   The memory block to check.
+
+  @retval TRUE    The memory block is empty.
+  @retval FALSE   The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo                The PciIo that can be used to access the host controller.
+  @param  Check4G              Whether the host controller requires allocated memory
+                               from one 4G address space.
+  @param  Which4G              The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS          The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE  Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  )
+{
+  USBHC_MEM_POOL          *Pool;
+
+  Pool = AllocatePool (sizeof (USBHC_MEM_POOL));
+
+  if (Pool == NULL) {
+    return Pool;
+  }
+
+  Pool->PciIo   = PciIo;
+  Pool->Check4G = Check4G;
+  Pool->Which4G = Which4G;
+  Pool->Head    = UsbHcAllocMemBlock (Pool, USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    gBS->FreePool (Pool);
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+
+/**
+  Release the memory management pool.
+
+  @param  Pool              The USB memory pool to free.
+
+  @retval EFI_SUCCESS       The memory pool is freed.
+  @retval EFI_DEVICE_ERROR  Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+{
+  USBHC_MEM_BLOCK *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UsbHcUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    UsbHcUnlinkMemBlock (Pool->Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+  gBS->FreePool (Pool);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool           The host controller's memory pool.
+  @param  Size           Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  USBHC_MEM_BLOCK         *NewBlock;
+  VOID                    *Mem;
+  UINTN                   AllocSize;
+  UINTN                   Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+
+  NewBlock = UsbHcAllocMemBlock (Pool, Pages);
+
+  if (NewBlock == NULL) {
+    DEBUG ((EFI_D_ERROR, "UsbHcAllocateMem: failed to allocate block\n"));
+    return NULL;
+  }
+
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->BufHost <= ToFree) && ((ToFree + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    UsbHcUnlinkMemBlock (Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.h
new file mode 100644
index 0000000000..84ced8d580
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.h
@@ -0,0 +1,157 @@
+/** @file
+
+  This file contains the definination for host controller memory management routines.
+
+Copyright (c) 2007 - 2010, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_MEM_H_
+#define _EFI_EHCI_MEM_H_
+
+#define USB_HC_BIT(a)                  ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+#define USB_HC_HIGH_32BIT(Addr64)    \
+          ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+struct _USBHC_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen;
+  UINT8                   *Buf;
+  UINT8                   *BufHost;
+  UINTN                   BufLen;   // Memory size in bytes
+  VOID                    *Mapping;
+  USBHC_MEM_BLOCK         *Next;
+};
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB
+// host controller. EHCI requires the control memory and transfer
+// data to be on the same 4G memory.
+//
+typedef struct _USBHC_MEM_POOL {
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  BOOLEAN                 Check4G;
+  UINT32                  Which4G;
+  USBHC_MEM_BLOCK         *Head;
+} USBHC_MEM_POOL;
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT           64
+
+#define USBHC_MEM_UNIT_MASK      (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_DEFAULT_PAGES  16
+
+#define USBHC_MEM_ROUND(Len)  (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo               The PciIo that can be used to access the host controller.
+  @param  Check4G             Whether the host controller requires allocated memory
+                              from one 4G address space.
+  @param  Which4G             The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS         The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  );
+
+
+/**
+  Release the memory management pool.
+
+  @param   Pool               The USB memory pool to free.
+
+  @retval EFI_SUCCESS       The memory pool is freed.
+  @retval EFI_DEVICE_ERROR  Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  );
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool  The host controller's memory pool.
+  @param  Size  Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  );
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool  The memory pool of the host controller.
+  @param  Mem   The memory to free.
+  @param  Size  The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**
+  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
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c
new file mode 100644
index 0000000000..09769eaf24
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c
@@ -0,0 +1,1275 @@
+/** @file
+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>
+  
+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"
+
+//
+// Two arrays used to translate the EHCI port state (change)
+// to the UEFI protocol's port state (change).
+//
+USB_PORT_STATE_MAP  mUsbPortStateMap[] = {
+  {PORTSC_CONN,     USB_PORT_STAT_CONNECTION},
+  {PORTSC_ENABLED,  USB_PORT_STAT_ENABLE},
+  {PORTSC_SUSPEND,  USB_PORT_STAT_SUSPEND},
+  {PORTSC_OVERCUR,  USB_PORT_STAT_OVERCURRENT},
+  {PORTSC_RESET,    USB_PORT_STAT_RESET},
+  {PORTSC_POWER,    USB_PORT_STAT_POWER},
+  {PORTSC_OWNER,    USB_PORT_STAT_OWNER}
+};
+
+USB_PORT_STATE_MAP  mUsbPortChangeMap[] = {
+  {PORTSC_CONN_CHANGE,    USB_PORT_STAT_C_CONNECTION},
+  {PORTSC_ENABLE_CHANGE,  USB_PORT_STAT_C_ENABLE},
+  {PORTSC_OVERCUR_CHANGE, USB_PORT_STAT_C_OVERCURRENT}
+};
+
+/**
+  Read Ehc Operation register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    The operation register offset.
+
+  @retval the register content read.
+
+**/
+UINT32
+EhcReadOpReg (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+ 
+  ASSERT (Ehc->CapLen != 0);
+
+  Data = MmioRead32 (Ehc->UsbHostControllerBaseAddress + Ehc->CapLen + Offset);
+  
+  return Data;
+}
+
+/**
+  Write the data to the EHCI operation register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    EHCI operation register offset.
+  @param  Data      The data to write.
+
+**/
+VOID
+EhcWriteOpReg (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+
+  ASSERT (Ehc->CapLen != 0);
+
+  MmioWrite32(Ehc->UsbHostControllerBaseAddress + Ehc->CapLen + Offset, Data);
+
+}
+
+/**
+  Set one bit of the operational register while keeping other bits.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    The offset of the operational register.
+  @param  Bit       The bit mask of the register to set.
+
+**/
+VOID
+EhcSetOpRegBit (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = EhcReadOpReg (Ehc, Offset);
+  Data |= Bit;
+  EhcWriteOpReg (Ehc, Offset, Data);
+}
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    The offset of the operational register.
+  @param  Bit       The bit mask of the register to clear.
+
+**/
+VOID
+EhcClearOpRegBit (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = EhcReadOpReg (Ehc, Offset);
+  Data &= ~Bit;
+  EhcWriteOpReg (Ehc, Offset, Data);
+}
+
+/**
+  Wait the operation register's bit as specified by Bit 
+  to become set (or clear).
+  
+  @param  Ehc           The EHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to wait for.
+  @param  WaitToSet     Wait the bit to set or clear.
+  @param  Timeout       The time to wait before abort (in millisecond).
+
+  @retval EFI_SUCCESS   The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT   The time out occurred.
+
+**/
+EFI_STATUS
+EhcWaitOpRegBit (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit,
+  IN BOOLEAN              WaitToSet,
+  IN UINT32               Timeout
+  )
+{
+  UINT32                  Index;
+
+  for (Index = 0; Index < Timeout / EHC_SYNC_POLL_INTERVAL + 1; Index++) {
+    if (EHC_REG_BIT_IS_SET (Ehc, Offset, Bit) == WaitToSet) {
+      return EFI_SUCCESS;
+    }
+
+    MicroSecondDelay (EHC_SYNC_POLL_INTERVAL);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Read EHCI capability register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    Capability register address.
+
+  @retval the register content read.
+
+**/
+UINT32
+EhcReadCapRegister (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  
+  Data = MmioRead32(Ehc->UsbHostControllerBaseAddress + Offset);
+  
+  return Data;
+}
+
+/**
+  Set door bell and wait it to be ACKed by host controller.
+  This function is used to synchronize with the hardware.
+  
+  @param  Ehc       The EHCI device.
+  @param  Timeout   The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT       Time out happened while waiting door bell to set.
+  @retval EFI_SUCCESS       Synchronized with the hardware.
+
+**/
+EFI_STATUS
+EhcSetAndWaitDoorBell (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+  UINT32                  Data;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_IAAD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_IAA, TRUE, Timeout);
+
+  //
+  // ACK the IAA bit in USBSTS register. Make sure other
+  // interrupt bits are not ACKed. These bits are WC (Write Clean).
+  //
+  Data  = EhcReadOpReg (Ehc, EHC_USBSTS_OFFSET);
+  Data &= ~USBSTS_INTACK_MASK;
+  Data |= USBSTS_IAA;
+
+  EhcWriteOpReg (Ehc, EHC_USBSTS_OFFSET, Data);
+
+  return Status;
+}
+
+/**
+  Clear all the interrutp status bits, these bits 
+  are Write-Clean.
+  
+  @param  Ehc       The EHCI device.
+
+**/
+VOID
+EhcAckAllInterrupt (
+  IN  PEI_USB2_HC_DEV         *Ehc
+  )
+{
+  EhcWriteOpReg (Ehc, EHC_USBSTS_OFFSET, USBSTS_INTACK_MASK);
+}
+
+/**
+  Enable the periodic schedule then wait EHC to 
+  actually enable it.
+  
+  @param  Ehc       The EHCI device.
+  @param  Timeout   The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT       Time out happened while enabling periodic schedule.
+  @retval EFI_SUCCESS       The periodical schedule is enabled.
+
+**/
+EFI_STATUS
+EhcEnablePeriodSchd (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_PERIOD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_PERIOD_ENABLED, TRUE, Timeout);
+  return Status;
+}
+
+/**
+  Enable asynchrounous schedule.
+  
+  @param  Ehc       The EHCI device.
+  @param  Timeout   Time to wait before abort.
+
+  @retval EFI_SUCCESS       The EHCI asynchronous schedule is enabled.
+  @retval Others            Failed to enable the asynchronous scheudle.
+
+**/
+EFI_STATUS
+EhcEnableAsyncSchd (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_ASYNC);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_ASYNC_ENABLED, TRUE, Timeout);
+  return Status;
+}
+
+/**
+  Check whether Ehc is halted.
+  
+  @param  Ehc       The EHCI device.
+
+  @retval TRUE      The controller is halted.
+  @retval FALSE     The controller isn't halted.
+
+**/
+BOOLEAN
+EhcIsHalt (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  return EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT);
+}
+
+/**
+  Check whether system error occurred.
+  
+  @param  Ehc       The EHCI device.
+
+  @retval TRUE      System error happened.
+  @retval FALSE     No system error.
+
+**/
+BOOLEAN
+EhcIsSysError (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  return EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_SYS_ERROR);
+}
+
+/**
+  Reset the host controller.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT     The transfer failed due to time out.
+  @retval Others          Failed to reset the host.
+
+**/
+EFI_STATUS
+EhcResetHC (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Host can only be reset when it is halt. If not so, halt it
+  //
+  if (!EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT)) {
+    Status = EhcHaltHC (Ehc, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RESET);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RESET, FALSE, Timeout);
+  return Status;
+}
+
+/**
+  Halt the host controller.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort.
+
+  @retval EFI_TIMEOUT     Failed to halt the controller before Timeout.
+  @retval EFI_SUCCESS     The EHCI is halt.
+
+**/
+EFI_STATUS
+EhcHaltHC (
+  IN PEI_USB2_HC_DEV     *Ehc,
+  IN UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT, TRUE, Timeout);
+  return Status;
+}
+
+/**
+  Set the EHCI to run.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort.
+
+  @retval EFI_SUCCESS     The EHCI is running.
+  @retval Others          Failed to set the EHCI to run.
+
+**/
+EFI_STATUS
+EhcRunHC (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT, FALSE, Timeout);
+  return Status;
+}
+
+/**
+  Power On All EHCI Ports.
+  
+  @param  Ehc             The EHCI device.
+
+**/
+VOID
+EhcPowerOnAllPorts (
+  IN PEI_USB2_HC_DEV          *Ehc
+  )
+{
+  UINT8 PortNumber;
+  UINT8 Index;
+
+  PortNumber = (UINT8)(Ehc->HcStructParams & HCSP_NPORTS);
+  for (Index = 0; Index < PortNumber; Index++) {
+    EhcSetOpRegBit (Ehc, EHC_PORT_STAT_OFFSET + 4 * Index, PORTSC_POWER);
+  }
+}
+
+/**
+  Initialize the HC hardware. 
+  EHCI spec lists the five things to do to initialize the hardware.
+  1. Program CTRLDSSEGMENT.
+  2. Set USBINTR to enable interrupts.
+  3. Set periodic list base.
+  4. Set USBCMD, interrupt threshold, frame list size etc.
+  5. Write 1 to CONFIGFLAG to route all ports to EHCI.
+  
+  @param  Ehc             The EHCI device.
+
+  @retval EFI_SUCCESS     The EHCI has come out of halt state.
+  @retval EFI_TIMEOUT     Time out happened.
+
+**/
+EFI_STATUS
+EhcInitHC (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  EFI_STATUS              Status;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+  UINTN               PageNumber;
+  
+  ASSERT (EhcIsHalt (Ehc));
+
+  //
+  // Allocate the periodic frame and associated memeory
+  // management facilities if not already done.
+  //
+  if (Ehc->PeriodFrame != NULL) {
+    EhcFreeSched (Ehc);
+  }
+  PageNumber =  sizeof(PEI_URB)/PAGESIZE +1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             PageNumber,
+             &TempPtr
+             );
+  Ehc->Urb = (PEI_URB *) ((UINTN) TempPtr);
+  if (Ehc->Urb  == NULL) {
+    return Status;
+  }
+
+  EhcPowerOnAllPorts (Ehc);  
+  MicroSecondDelay (EHC_ROOT_PORT_RECOVERY_STALL);
+  
+  Status = EhcInitSched (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // 1. Program the CTRLDSSEGMENT register with the high 32 bit addr
+  //
+  EhcWriteOpReg (Ehc, EHC_CTRLDSSEG_OFFSET, Ehc->High32bitAddr);
+
+  //
+  // 2. Clear USBINTR to disable all the interrupt. UEFI works by polling
+  //
+  EhcWriteOpReg (Ehc, EHC_USBINTR_OFFSET, 0);
+
+  //
+  // 3. Program periodic frame list, already done in EhcInitSched
+  // 4. Start the Host Controller
+  //
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+
+  //
+  // 5. Set all ports routing to EHC
+  //
+  EhcSetOpRegBit (Ehc, EHC_CONFIG_FLAG_OFFSET, CONFIGFLAG_ROUTE_EHC);
+
+  //
+  // Wait roothub port power stable
+  //
+  MicroSecondDelay (EHC_ROOT_PORT_RECOVERY_STALL);
+
+  Status = EhcEnablePeriodSchd (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = EhcEnableAsyncSchd (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support 
+                                bulk transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of 
+                                sending or receiving.
+  @param  Data                  Array of pointers to the buffers of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to use of 
+                                the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+                                If Timeout is 0, then the caller must wait for the function
+                                to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Translator            A pointr to the transaction translator data.                                
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcBulkTransfer (
+  IN EFI_PEI_SERVICES                     **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI         *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  PEI_URB                 *Urb;
+  EFI_STATUS              Status;
+
+  //
+  // Validate the parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) || 
+      (Data == NULL) || (Data[0] == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 0) && (*DataToggle != 1)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((EFI_USB_SPEED_HIGH == DeviceSpeed) && (MaximumPacketLength > 512))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ehc =PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS(This);
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          *DataToggle,
+          MaximumPacketLength,
+          Translator,
+          EHC_BULK_TRANSFER,
+          NULL,
+          Data[0],
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToAsync (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromAsync (Ehc, Urb->Qh);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+  *DataToggle     = Urb->DataToggle;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+  EhcFreeUrb (Ehc, Urb);
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices   The pointer to the PEI Services Table.
+  @param[in]  This          The pointer to this instance of the 
+                            PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber    The pointer to the number of the root hub ports.                                
+                                
+  @retval EFI_SUCCESS           The port number was retrieved successfully.
+  @retval EFI_INVALID_PARAMETER PortNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetRootHubPortNumber (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  OUT UINT8                                 *PortNumber
+  )
+{
+
+  PEI_USB2_HC_DEV             *EhcDev;
+  EhcDev = PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS (This);
+  
+  if (PortNumber == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }  
+  
+  *PortNumber = (UINT8)(EhcDev->HcStructParams & HCSP_NPORTS);
+  return EFI_SUCCESS;
+  
+}
+
+/**
+  Clears a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  PortNumber            Specifies the root hub port whose feature
+                                is requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was cleared 
+                                 for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcClearRootHubPortFeature (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN  UINT8                 PortNumber,
+  IN  EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+
+  Ehc       = PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset  = EHC_PORT_STAT_OFFSET + (4 * PortNumber);
+  State   = EhcReadOpReg (Ehc, Offset);
+  State &= ~PORTSC_CHANGE_MASK;
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Clear PORT_ENABLE feature means disable port.
+    //
+    State &= ~PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    //
+    // A write of zero to this bit is ignored by the host
+    // controller. The host controller will unconditionally
+    // set this bit to a zero when:
+    //   1. software sets the Forct Port Resume bit to a zero from a one.
+    //   2. software sets the Port Reset bit to a one frome a zero.
+    //
+    State &= ~PORSTSC_RESUME;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // Clear PORT_RESET means clear the reset signal.
+    //
+    State &= ~PORTSC_RESET;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortOwner:
+    //
+    // Clear port owner means this port owned by EHC
+    //
+    State &= ~PORTSC_OWNER;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortConnectChange:
+    //
+    // Clear connect status change
+    //
+    State |= PORTSC_CONN_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortEnableChange:
+    //
+    // Clear enable status change
+    //
+    State |= PORTSC_ENABLE_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortOverCurrentChange:
+    //
+    // Clear PortOverCurrent change
+    //
+    State |= PORTSC_OVERCUR_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+  case EfiUsbPortSuspendChange:
+  case EfiUsbPortResetChange:
+    //
+    // Not supported or not related operation
+    //
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Sets a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES
+  @param  This                  The pointer of PEI_USB2_HOST_CONTROLLER_PPI
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_TIMEOUT            The time out occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcSetRootHubPortFeature (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN UINT8                                  PortNumber,
+  IN EFI_USB_PORT_FEATURE                   PortFeature
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+
+  Ehc       = PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset  = (UINT32) (EHC_PORT_STAT_OFFSET + (4 * PortNumber));
+  State   = EhcReadOpReg (Ehc, Offset);
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bit
+  //
+  State &= ~PORTSC_CHANGE_MASK;
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Sofeware can't set this bit, Port can only be enable by
+    // EHCI as a part of the reset and enable
+    //
+    State |= PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    State |= PORTSC_SUSPEND;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // Make sure Host Controller not halt before reset it
+    //
+    if (EhcIsHalt (Ehc)) {
+      Status = EhcRunHC (Ehc, EHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        break;
+      }
+    }
+    
+    //
+    // Set one to PortReset bit must also set zero to PortEnable bit
+    //
+    State |= PORTSC_RESET;
+    State &= ~PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // Not supported, ignore the operation
+    //
+    Status = EFI_SUCCESS;
+    break;
+
+  case EfiUsbPortOwner:
+    State |= PORTSC_OWNER;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Retrieves the current status of a USB root hub port.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  PortNumber             The root hub port to retrieve the state from.  
+  @param  PortStatus             Variable to receive the port state.
+
+  @retval EFI_SUCCESS            The status of the USB root hub port specified.
+                                 by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetRootHubPortStatus (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN  UINT8                                 PortNumber,
+  OUT EFI_USB_PORT_STATUS                   *PortStatus
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  UINTN                   Index;
+  UINTN                   MapSize;
+  EFI_STATUS              Status;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ehc       =  PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS(This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset                        = (UINT32) (EHC_PORT_STAT_OFFSET + (4 * PortNumber));
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  State                         = EhcReadOpReg (Ehc, Offset);
+
+  //
+  // Identify device speed. If in K state, it is low speed.
+  // If the port is enabled after reset, the device is of 
+  // high speed. The USB bus driver should retrieve the actual
+  // port speed after reset. 
+  //
+  if (EHC_BIT_IS_SET (State, PORTSC_LINESTATE_K)) {
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+
+  } else if (EHC_BIT_IS_SET (State, PORTSC_ENABLED)) {
+    PortStatus->PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+  }
+  
+  //
+  // Convert the EHCI port/port change state to UEFI status
+  //
+  MapSize = sizeof (mUsbPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (EHC_BIT_IS_SET (State, mUsbPortStateMap[Index].HwState)) {
+      PortStatus->PortStatus = (UINT16) (PortStatus->PortStatus | mUsbPortStateMap[Index].UefiState);
+    }
+  }
+
+  MapSize = sizeof (mUsbPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (EHC_BIT_IS_SET (State, mUsbPortChangeMap[Index].HwState)) {
+      PortStatus->PortChangeStatus = (UINT16) (PortStatus->PortChangeStatus | mUsbPortChangeMap[Index].UefiState);
+    }
+  }
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress          The target device address.
+  @param  DeviceSpeed            Target device speed.
+  @param  MaximumPacketLength    Maximum packet size the default control transfer 
+                                 endpoint is capable of sending or receiving.
+  @param  Request                USB device request to send.
+  @param  TransferDirection      Specifies the data direction for the data stage.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+  @param  TimeOut                Indicates the maximum timeout, in millisecond.
+                                 If Timeout is 0, then the caller must wait for the function
+                                 to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Translator             Transaction translator to be used by this device.
+  @param  TransferResult         Return the result of this control transfer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcControlTransfer (
+  IN  EFI_PEI_SERVICES                    **PeiServices,
+  IN  PEI_USB2_HOST_CONTROLLER_PPI        *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  EFI_USB_DEVICE_REQUEST              *Request,
+  IN  EFI_USB_DATA_DIRECTION              TransferDirection,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  PEI_URB                 *Urb;
+  UINT8                   Endpoint;
+  EFI_STATUS              Status;
+
+  //
+  // Validate parameters
+  //
+  if ((Request == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbDataIn) &&
+      (TransferDirection != EfiUsbDataOut) &&
+      (TransferDirection != EfiUsbNoData)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection == EfiUsbNoData) && 
+      ((Data != NULL) || (*DataLength != 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) && 
+     ((Data == NULL) || (*DataLength == 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8)  && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((EFI_USB_SPEED_HIGH == DeviceSpeed) && (MaximumPacketLength > 512))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ehc             = PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS (This);
+
+  Status          = EFI_DEVICE_ERROR;
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  //
+  // Encode the direction in address, although default control
+  // endpoint is bidirectional. EhcCreateUrb expects this
+  // combination of Ep addr and its direction.
+  //
+  Endpoint = (UINT8) (0 | ((TransferDirection == EfiUsbDataIn) ? 0x80 : 0));
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          Endpoint,
+          DeviceSpeed,
+          0,
+          MaximumPacketLength,
+          Translator,
+          EHC_CTRL_TRANSFER,
+          Request,
+          Data,
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToAsync (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromAsync (Ehc, Urb->Qh);
+
+  //
+  // Get the status from URB. The result is updated in EhcCheckUrbResult
+  // which is called by EhcExecTransfer
+  //
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+  EhcFreeUrb (Ehc, Urb);
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            PPI successfully installed.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcPeimEntry (
+  IN EFI_PEI_FILE_HANDLE     FileHandle,
+  IN CONST EFI_PEI_SERVICES  **PeiServices
+  )
+{
+  PEI_USB_CONTROLLER_PPI      *ChipSetUsbControllerPpi;
+  EFI_STATUS                  Status;
+  UINT8                       Index;
+  UINTN                       ControllerType;
+  UINTN                       BaseAddress;
+  UINTN                       MemPages;
+  PEI_USB2_HC_DEV             *EhcDev;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  Status = PeiServicesLocatePpi (
+             &gPeiUsbControllerPpiGuid,
+             0,
+             NULL,
+             (VOID **) &ChipSetUsbControllerPpi
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Index = 0;
+  while (TRUE) {
+    Status = ChipSetUsbControllerPpi->GetUsbController (
+                                        (EFI_PEI_SERVICES **) PeiServices,
+                                        ChipSetUsbControllerPpi,
+                                        Index,
+                                        &ControllerType,
+                                        &BaseAddress
+                                        );
+    //
+    // When status is error, meant no controller is found
+    //
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+    
+    //
+    // This PEIM is for UHC type controller.
+    //
+    if (ControllerType != PEI_EHCI_CONTROLLER) {
+      Index++;
+      continue;
+    }
+
+    MemPages = sizeof (PEI_USB2_HC_DEV) / PAGESIZE + 1;
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesCode,
+               MemPages,
+               &TempPtr
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ZeroMem((VOID *)(UINTN)TempPtr, MemPages*PAGESIZE);
+    EhcDev = (PEI_USB2_HC_DEV *) ((UINTN) TempPtr);
+
+    EhcDev->Signature = USB2_HC_DEV_SIGNATURE;
+
+    EhcDev->UsbHostControllerBaseAddress = (UINT32) BaseAddress;
+
+
+    EhcDev->HcStructParams = EhcReadCapRegister (EhcDev, EHC_HCSPARAMS_OFFSET);
+    EhcDev->HcCapParams    = EhcReadCapRegister (EhcDev, EHC_HCCPARAMS_OFFSET);
+    EhcDev->CapLen         = EhcReadCapRegister (EhcDev, EHC_CAPLENGTH_OFFSET) & 0x0FF;
+    //
+    // Initialize Uhc's hardware
+    //
+    Status = InitializeUsbHC (EhcDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    EhcDev->Usb2HostControllerPpi.ControlTransfer          = EhcControlTransfer;
+    EhcDev->Usb2HostControllerPpi.BulkTransfer             = EhcBulkTransfer;
+    EhcDev->Usb2HostControllerPpi.GetRootHubPortNumber     = EhcGetRootHubPortNumber;
+    EhcDev->Usb2HostControllerPpi.GetRootHubPortStatus     = EhcGetRootHubPortStatus;
+    EhcDev->Usb2HostControllerPpi.SetRootHubPortFeature    = EhcSetRootHubPortFeature;
+    EhcDev->Usb2HostControllerPpi.ClearRootHubPortFeature  = EhcClearRootHubPortFeature;
+
+    EhcDev->PpiDescriptor.Flags = (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+    EhcDev->PpiDescriptor.Guid = &gPeiUsb2HostControllerPpiGuid;
+    EhcDev->PpiDescriptor.Ppi = &EhcDev->Usb2HostControllerPpi;
+
+    Status = PeiServicesInstallPpi (&EhcDev->PpiDescriptor);
+    if (EFI_ERROR (Status)) {
+      Index++;
+      continue;
+    }
+
+    Index++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  @param  EhcDev                 EHCI Device.
+
+  @retval EFI_SUCCESS            EHCI successfully initialized.
+  @retval EFI_ABORTED            EHCI was failed to be initialized.
+
+**/
+EFI_STATUS
+InitializeUsbHC (
+  IN PEI_USB2_HC_DEV      *EhcDev  
+  )
+{
+  EFI_STATUS  Status;
+
+   	
+  EhcResetHC (EhcDev, EHC_RESET_TIMEOUT);
+
+  Status = EhcInitHC (EhcDev);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;      
+  }
+  
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h
new file mode 100644
index 0000000000..74037769a4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h
@@ -0,0 +1,224 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010 - 2015, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _RECOVERY_EHC_H_
+#define _RECOVERY_EHC_H_
+
+#include <PiPei.h>
+
+#include <Ppi/UsbController.h>
+#include <Ppi/Usb2HostController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/IoLib.h>
+
+typedef struct _PEI_USB2_HC_DEV PEI_USB2_HC_DEV;
+
+#define EFI_LIST_ENTRY LIST_ENTRY
+
+#include "UsbHcMem.h"
+#include "EhciReg.h"
+#include "EhciUrb.h"
+#include "EhciSched.h"
+
+#define EFI_USB_SPEED_FULL 0x0000
+#define EFI_USB_SPEED_LOW  0x0001
+#define EFI_USB_SPEED_HIGH 0x0002
+
+#define PAGESIZE           4096
+
+#define EHC_1_MICROSECOND            1
+#define EHC_1_MILLISECOND            (1000 * EHC_1_MICROSECOND)
+#define EHC_1_SECOND                 (1000 * EHC_1_MILLISECOND)
+
+//
+// EHCI register operation timeout, set by experience
+//
+#define EHC_RESET_TIMEOUT            (1 * EHC_1_SECOND)
+#define EHC_GENERIC_TIMEOUT          (10 * EHC_1_MILLISECOND)
+
+
+//
+// Wait for roothub port power stable, refers to Spec[EHCI1.0-2.3.9]
+//
+#define EHC_ROOT_PORT_RECOVERY_STALL (20 * EHC_1_MILLISECOND)
+
+//
+// Sync transfer polling interval, set by experience.
+//
+#define EHC_SYNC_POLL_INTERVAL       (6 * EHC_1_MILLISECOND)
+
+//
+//Iterate through the doule linked list. NOT delete safe
+//
+#define EFI_LIST_FOR_EACH(Entry, ListHead)    \
+  for(Entry = (ListHead)->ForwardLink; Entry != (ListHead); Entry = Entry->ForwardLink)
+
+//
+//Iterate through the doule linked list. This is delete-safe.
+//Don't touch NextEntry
+//
+#define EFI_LIST_FOR_EACH_SAFE(Entry, NextEntry, ListHead)            \
+  for(Entry = (ListHead)->ForwardLink, NextEntry = Entry->ForwardLink;\
+      Entry != (ListHead); Entry = NextEntry, NextEntry = Entry->ForwardLink)
+
+#define EFI_LIST_CONTAINER(Entry, Type, Field) BASE_CR(Entry, Type, Field)
+
+
+#define EHC_LOW_32BIT(Addr64)     ((UINT32)(((UINTN)(Addr64)) & 0XFFFFFFFF))
+#define EHC_HIGH_32BIT(Addr64)    ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+#define EHC_BIT_IS_SET(Data, Bit) ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define EHC_REG_BIT_IS_SET(Ehc, Offset, Bit) \
+          (EHC_BIT_IS_SET(EhcReadOpReg ((Ehc), (Offset)), (Bit)))
+
+#define USB2_HC_DEV_SIGNATURE  SIGNATURE_32 ('e', 'h', 'c', 'i')
+
+struct _PEI_USB2_HC_DEV {
+  UINTN                               Signature;
+  PEI_USB2_HOST_CONTROLLER_PPI        Usb2HostControllerPpi;
+  EFI_PEI_PPI_DESCRIPTOR              PpiDescriptor;                    
+  UINT32                              UsbHostControllerBaseAddress;
+  PEI_URB                             *Urb;
+  USBHC_MEM_POOL                      *MemPool;
+
+  //
+  // Schedule data shared between asynchronous and periodic
+  // transfers:
+  // ShortReadStop, as its name indicates, is used to terminate
+  // the short read except the control transfer. EHCI follows
+  // the alternative next QTD point when a short read happens.
+  // For control transfer, even the short read happens, try the
+  // status stage.
+  //
+  PEI_EHC_QTD                         *ShortReadStop;
+  EFI_EVENT                           PollTimer;
+  
+  //
+  // Asynchronous(bulk and control) transfer schedule data: 
+  // ReclaimHead is used as the head of the asynchronous transfer
+  // list. It acts as the reclamation header. 
+  //
+  PEI_EHC_QH                          *ReclaimHead;
+  
+  //
+  // Peroidic (interrupt) transfer schedule data:
+  //
+  VOID                                *PeriodFrame;     // Mapped as common buffer 
+  VOID                                *PeriodFrameHost;
+  VOID                                *PeriodFrameMap;
+                                      
+  PEI_EHC_QH                          *PeriodOne;
+  EFI_LIST_ENTRY                      AsyncIntTransfers;
+
+  //
+  // EHCI configuration data
+  //
+  UINT32                              HcStructParams; // Cache of HC structure parameter, EHC_HCSPARAMS_OFFSET
+  UINT32                              HcCapParams;    // Cache of HC capability parameter, HCCPARAMS
+  UINT32                              CapLen;         // Capability length
+  UINT32                              High32bitAddr;
+};
+
+#define PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS(a)  CR (a, PEI_USB2_HC_DEV, Usb2HostControllerPpi, USB2_HC_DEV_SIGNATURE)
+
+/**
+  @param  EhcDev                 EHCI Device.
+
+  @retval EFI_SUCCESS            EHCI successfully initialized.
+  @retval EFI_ABORTED            EHCI was failed to be initialized.
+
+**/
+EFI_STATUS
+InitializeUsbHC (
+  IN PEI_USB2_HC_DEV      *EhcDev  
+  );
+
+/**
+  Initialize the memory management pool for the host controller.
+  
+  @param  Ehc                   The EHCI device.
+  @param  Check4G               Whether the host controller requires allocated memory 
+                                from one 4G address space.
+  @param  Which4G               The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS           The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE   Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  )
+;
+            
+/**
+  Release the memory management pool.
+  
+  @param  Pool                  The USB memory pool to free.
+
+  @retval EFI_DEVICE_ERROR      Fail to free the memory pool.
+  @retval EFI_SUCCESS           The memory pool is freed.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+;
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+  
+  @param  Ehc       The EHCI device.
+  @param  Pool      The host controller's memory pool.
+  @param  Size      Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+;
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf
new file mode 100644
index 0000000000..7083f86681
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf
@@ -0,0 +1,70 @@
+## @file
+# The EhcPeim driver is responsible for managing EHCI host controller at PEI phase.
+#
+# 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>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions
+# of the BSD License which accompanies this distribution.  The
+# full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php
+# 
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = EhciPei
+  MODULE_UNI_FILE                = EhciPei.uni
+  FILE_GUID                      = BAB4F20F-0981-4b5f-A047-6EF83BEEAB3C
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = EhcPeimEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  EhcPeim.c
+  EhcPeim.h
+  EhciUrb.c
+  EhciSched.c
+  UsbHcMem.c
+  EhciReg.h
+  EhciSched.h
+  EhciUrb.h
+  UsbHcMem.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  IoLib
+  TimerLib
+  BaseMemoryLib
+  PeimEntryPoint
+  PeiServicesLib
+
+
+[Ppis]
+  gPeiUsb2HostControllerPpiGuid                 ## PRODUCES
+  gPeiUsbControllerPpiGuid                      ## CONSUMES
+
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gPeiUsbControllerPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  EhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.uni b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.uni
new file mode 100644
index 0000000000..c0f6890275
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPeiExtra.uni
new file mode 100644
index 0000000000..3f5163896d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciReg.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciReg.h
new file mode 100644
index 0000000000..34c61d8a94
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciReg.h
@@ -0,0 +1,310 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_REG_H_
+#define _EFI_EHCI_REG_H_
+
+
+
+//
+// Capability register offset
+//
+#define EHC_CAPLENGTH_OFFSET    0    // Capability register length offset
+#define EHC_HCSPARAMS_OFFSET    0x04 // Structural Parameters 04-07h
+#define EHC_HCCPARAMS_OFFSET    0x08 // Capability parameters offset
+
+//
+// Capability register bit definition
+//
+#define HCSP_NPORTS             0x0F // Number of root hub port
+#define HCCP_64BIT              0x01 // 64-bit addressing capability
+
+//
+// Operational register offset
+//
+#define EHC_USBCMD_OFFSET       0x0  // USB command register offset
+#define EHC_USBSTS_OFFSET       0x04 // Statue register offset
+#define EHC_USBINTR_OFFSET      0x08 // USB interrutp offset
+#define EHC_FRINDEX_OFFSET      0x0C // Frame index offset
+#define EHC_CTRLDSSEG_OFFSET    0x10 // Control data structure segment offset
+#define EHC_FRAME_BASE_OFFSET   0x14 // Frame list base address offset
+#define EHC_ASYNC_HEAD_OFFSET   0x18 // Next asynchronous list address offset
+#define EHC_CONFIG_FLAG_OFFSET  0x40 // Configure flag register offset
+#define EHC_PORT_STAT_OFFSET    0x44 // Port status/control offset
+
+#define EHC_FRAME_LEN           1024
+
+//
+// Register bit definition
+//
+#define CONFIGFLAG_ROUTE_EHC    0x01 // Route port to EHC
+
+#define USBCMD_RUN              0x01   // Run/stop
+#define USBCMD_RESET            0x02   // Start the host controller reset
+#define USBCMD_ENABLE_PERIOD    0x10   // Enable periodic schedule
+#define USBCMD_ENABLE_ASYNC     0x20   // Enable asynchronous schedule
+#define USBCMD_IAAD             0x40   // Interrupt on async advance doorbell
+
+#define USBSTS_IAA              0x20   // Interrupt on async advance
+#define USBSTS_PERIOD_ENABLED   0x4000 // Periodic schedule status
+#define USBSTS_ASYNC_ENABLED    0x8000 // Asynchronous schedule status
+#define USBSTS_HALT             0x1000 // Host controller halted
+#define USBSTS_SYS_ERROR        0x10   // Host system error
+#define USBSTS_INTACK_MASK      0x003F // Mask for the interrupt ACK, the WC
+                                       // (write clean) bits in USBSTS register
+
+#define PORTSC_CONN             0x01   // Current Connect Status
+#define PORTSC_CONN_CHANGE      0x02   // Connect Status Change
+#define PORTSC_ENABLED          0x04   // Port Enable / Disable
+#define PORTSC_ENABLE_CHANGE    0x08   // Port Enable / Disable Change
+#define PORTSC_OVERCUR          0x10   // Over current Active
+#define PORTSC_OVERCUR_CHANGE   0x20   // Over current Change
+#define PORSTSC_RESUME          0x40   // Force Port Resume
+#define PORTSC_SUSPEND          0x80   // Port Suspend State
+#define PORTSC_RESET            0x100  // Port Reset
+#define PORTSC_LINESTATE_K      0x400  // Line Status K-state
+#define PORTSC_LINESTATE_J      0x800  // Line Status J-state
+#define PORTSC_POWER            0x1000 // Port Power
+#define PORTSC_OWNER            0x2000 // Port Owner
+#define PORTSC_CHANGE_MASK      0x2A   // Mask of the port change bits,
+                                       // they are WC (write clean)
+//
+// PCI Configuration Registers
+//
+#define EHC_BAR_INDEX           0      // how many bytes away from USB_BASE to 0x10
+
+#define EHC_LINK_TERMINATED(Link) (((Link) & 0x01) != 0)
+
+#define EHC_ADDR(High, QhHw32)   \
+        ((VOID *) (UINTN) (LShiftU64 ((High), 32) | ((QhHw32) & 0xFFFFFFF0)))
+
+#define EHCI_IS_DATAIN(EndpointAddr) EHC_BIT_IS_SET((EndpointAddr), 0x80)
+
+//
+// Structure to map the hardware port states to the 
+// UEFI's port states. 
+//
+typedef struct {
+  UINT16                  HwState;
+  UINT16                  UefiState;
+} USB_PORT_STATE_MAP;
+
+//
+// Ehci Data and Ctrl Structures
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Pi;
+  UINT8                   SubClassCode;
+  UINT8                   BaseCode;
+} USB_CLASSC;
+#pragma pack()
+
+
+/**
+  Read EHCI capability register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    Capability register address.
+
+  @retval the register content read.
+
+**/
+UINT32
+EhcReadCapRegister (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Offset
+  )
+;
+
+/**
+  Read Ehc Operation register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    The operation register offset.
+
+  @retval the register content read.
+
+**/
+UINT32
+EhcReadOpReg (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Offset
+  )
+;
+
+/**
+  Write the data to the EHCI operation register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    EHCI operation register offset.
+  @param  Data      The data to write.
+
+**/
+VOID
+EhcWriteOpReg (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+;
+
+/**
+  Stop the legacy USB SMI support.
+  
+  @param  Ehc       The EHCI device.
+
+**/
+VOID
+EhcClearLegacySupport (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+;
+
+/**
+  Set door bell and wait it to be ACKed by host controller.
+  This function is used to synchronize with the hardware.
+  
+  @param  Ehc       The EHCI device.
+  @param  Timeout   The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT       Time out happened while waiting door bell to set.
+  @retval EFI_SUCCESS       Synchronized with the hardware.
+
+**/
+EFI_STATUS
+EhcSetAndWaitDoorBell (
+  IN  PEI_USB2_HC_DEV      *Ehc,
+  IN  UINT32               Timeout
+  )
+;
+
+/**
+  Clear all the interrutp status bits, these bits 
+  are Write-Clean.
+  
+  @param  Ehc       The EHCI device.
+
+**/
+VOID
+EhcAckAllInterrupt (
+  IN  PEI_USB2_HC_DEV     *Ehc
+  )
+;
+
+/**
+  Check whether Ehc is halted.
+  
+  @param  Ehc       The EHCI device.
+
+  @retval TRUE      The controller is halted.
+  @retval FALSE     The controller isn't halted.
+
+**/
+BOOLEAN
+EhcIsHalt (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+;
+
+/**
+  Check whether system error occurred.
+  
+  @param  Ehc       The EHCI device.
+
+  @retval TRUE      System error happened.
+  @retval FALSE     No system error.
+
+**/
+BOOLEAN
+EhcIsSysError (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+;
+
+/**
+  Reset the host controller.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT     The transfer failed due to time out.
+  @retval Others          Failed to reset the host.
+
+**/
+EFI_STATUS
+EhcResetHC (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+;
+
+/**
+  Halt the host controller.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort.
+
+  @retval EFI_TIMEOUT     Failed to halt the controller before Timeout.
+  @retval EFI_SUCCESS     The EHCI is halt.
+
+**/
+EFI_STATUS
+EhcHaltHC (
+  IN PEI_USB2_HC_DEV     *Ehc,
+  IN UINT32              Timeout
+  )
+;
+
+/**
+  Set the EHCI to run
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort.
+
+  @retval EFI_SUCCESS     The EHCI is running.
+  @retval Others          Failed to set the EHCI to run.
+
+**/
+EFI_STATUS
+EhcRunHC (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+;
+
+/**
+  Initialize the HC hardware. 
+  EHCI spec lists the five things to do to initialize the hardware.
+  1. Program CTRLDSSEGMENT.
+  2. Set USBINTR to enable interrupts.
+  3. Set periodic list base.
+  4. Set USBCMD, interrupt threshold, frame list size etc.
+  5. Write 1 to CONFIGFLAG to route all ports to EHCI.
+  
+  @param  Ehc             The EHCI device.
+
+  @retval EFI_SUCCESS     The EHCI has come out of halt state.
+  @retval EFI_TIMEOUT     Time out happened.
+
+**/
+EFI_STATUS
+EhcInitHC (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c
new file mode 100644
index 0000000000..e992d4f287
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c
@@ -0,0 +1,461 @@
+/** @file
+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>
+  
+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"
+
+/**
+  Create helper QTD/QH for the EHCI device.
+  
+  @param  Ehc         The EHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for helper QTD/QH.
+  @retval EFI_SUCCESS           Helper QH/QTD are created.
+
+**/
+EFI_STATUS
+EhcCreateHelpQ (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  USB_ENDPOINT            Ep;
+  PEI_EHC_QH              *Qh;
+  QH_HW                   *QhHw;
+  PEI_EHC_QTD             *Qtd;
+
+  //
+  // Create an inactive Qtd to terminate the short packet read.
+  //
+  Qtd = EhcCreateQtd (Ehc, NULL, 0, QTD_PID_INPUT, 0, 64);
+
+  if (Qtd == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Qtd->QtdHw.Status   = QTD_STAT_HALTED;
+  Ehc->ShortReadStop  = Qtd;
+
+  //
+  // Create a QH to act as the EHC reclamation header.
+  // Set the header to loopback to itself.
+  //
+  Ep.DevAddr    = 0;
+  Ep.EpAddr     = 1;
+  Ep.Direction  = EfiUsbDataIn;
+  Ep.DevSpeed   = EFI_USB_SPEED_HIGH;
+  Ep.MaxPacket  = 64;
+  Ep.HubAddr    = 0;
+  Ep.HubPort    = 0;
+  Ep.Toggle     = 0;
+  Ep.Type       = EHC_BULK_TRANSFER;
+  Ep.PollRate   = 1;
+
+  Qh            = EhcCreateQh (Ehc, &Ep);
+
+  if (Qh == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  QhHw              = &Qh->QhHw;
+  QhHw->HorizonLink = QH_LINK (QhHw, EHC_TYPE_QH, FALSE);
+  QhHw->Status      = QTD_STAT_HALTED;
+  QhHw->ReclaimHead = 1;
+  Ehc->ReclaimHead  = Qh;
+
+  //
+  // Create a dummy QH to act as the terminator for periodical schedule
+  //
+  Ep.EpAddr   = 2;
+  Ep.Type     = EHC_INT_TRANSFER_SYNC;
+
+  Qh          = EhcCreateQh (Ehc, &Ep);
+
+  if (Qh == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Qh->QhHw.Status = QTD_STAT_HALTED;
+  Ehc->PeriodOne  = Qh;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the schedule data structure such as frame list.
+  
+  @param  Ehc                   The EHCI device to init schedule data for.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to init schedule data.
+  @retval EFI_SUCCESS           The schedule data is initialized.
+
+**/
+EFI_STATUS
+EhcInitSched (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+  VOID                  *Map;
+  UINTN                 Index;
+  UINT32                *Desc;
+  EFI_STATUS            Status;
+
+  //
+  // First initialize the periodical schedule data:
+  // 1. Allocate and map the memory for the frame list
+  // 2. Create the help QTD/QH
+  // 3. Initialize the frame entries
+  // 4. Set the frame list register
+  //
+  //
+  // The Frame List ocupies 4K bytes,
+  // and must be aligned on 4-Kbyte boundaries.
+  //
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             1,
+             &PhyAddr
+             );
+
+  Map = NULL;
+  Ehc->PeriodFrameHost  = (VOID *)(UINTN)PhyAddr;
+  Ehc->PeriodFrame      = (VOID *)(UINTN)PhyAddr;
+  Ehc->PeriodFrameMap   = Map;
+  Ehc->High32bitAddr    = EHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // Init memory pool management then create the helper
+  // QTD/QH. If failed, previously allocated resources
+  // will be freed by EhcFreeSched
+  //
+  Ehc->MemPool = UsbHcInitMemPool (
+                   Ehc,
+                   EHC_BIT_IS_SET (Ehc->HcCapParams, HCCP_64BIT),
+                   Ehc->High32bitAddr
+                   );
+
+  if (Ehc->MemPool == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = EhcCreateHelpQ (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // Initialize the frame list entries then set the registers
+  //
+  Desc = (UINT32 *) Ehc->PeriodFrame;
+
+  for (Index = 0; Index < EHC_FRAME_LEN; Index++) {
+    Desc[Index] = QH_LINK (Ehc->PeriodOne, EHC_TYPE_QH, FALSE);
+  }
+
+  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, EHC_LOW_32BIT (Ehc->PeriodFrame));
+
+  //
+  // 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));
+  return EFI_SUCCESS;
+}
+
+/**
+  Free the schedule data. It may be partially initialized.
+  
+  @param  Ehc   The EHCI device. 
+
+**/
+VOID
+EhcFreeSched (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, 0);
+  EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, 0);
+
+  if (Ehc->PeriodOne != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (PEI_EHC_QH));
+    Ehc->PeriodOne = NULL;
+  }
+
+  if (Ehc->ReclaimHead != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (PEI_EHC_QH));
+    Ehc->ReclaimHead = NULL;
+  }
+
+  if (Ehc->ShortReadStop != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (PEI_EHC_QTD));
+    Ehc->ShortReadStop = NULL;
+  }
+
+  if (Ehc->MemPool != NULL) {
+    UsbHcFreeMemPool (Ehc->MemPool);
+    Ehc->MemPool = NULL;
+  }
+
+  if (Ehc->PeriodFrame != NULL) {
+    Ehc->PeriodFrame = NULL;
+  }
+}
+
+/**
+  Link the queue head to the asynchronous schedule list.
+  UEFI only supports one CTRL/BULK transfer at a time
+  due to its interfaces. This simplifies the AsynList
+  management: A reclamation header is always linked to
+  the AsyncListAddr, the only active QH is appended to it.
+  
+  @param  Ehc   The EHCI device.
+  @param  Qh    The queue head to link.
+
+**/
+VOID
+EhcLinkQhToAsync (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_EHC_QH           *Qh
+  )
+{
+  PEI_EHC_QH               *Head;
+
+  //
+  // Append the queue head after the reclaim header, then
+  // fix the hardware visiable parts (EHCI R1.0 page 72).
+  // ReclaimHead is always linked to the EHCI's AsynListAddr.
+  //
+  Head                    = Ehc->ReclaimHead;
+
+  Qh->NextQh              = Head->NextQh;
+  Head->NextQh            = Qh;
+
+  Qh->QhHw.HorizonLink    = QH_LINK (Head, EHC_TYPE_QH, FALSE);;
+  Head->QhHw.HorizonLink  = QH_LINK (Qh, EHC_TYPE_QH, FALSE);
+}
+
+/**
+  Unlink a queue head from the asynchronous schedule list.
+  Need to synchronize with hardware.
+  
+  @param  Ehc   The EHCI device.
+  @param  Qh    The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromAsync (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_EHC_QH           *Qh
+  )
+{
+  PEI_EHC_QH              *Head;
+
+  ASSERT (Ehc->ReclaimHead->NextQh == Qh);
+
+  //
+  // Remove the QH from reclamation head, then update the hardware
+  // visiable part: Only need to loopback the ReclaimHead. The Qh
+  // is pointing to ReclaimHead (which is staill in the list).
+  //
+  Head                    = Ehc->ReclaimHead;
+
+  Head->NextQh            = Qh->NextQh;
+  Qh->NextQh              = NULL;
+
+  Head->QhHw.HorizonLink  = QH_LINK (Head, EHC_TYPE_QH, FALSE);
+
+  //
+  // Set and wait the door bell to synchronize with the hardware
+  //
+  EhcSetAndWaitDoorBell (Ehc, EHC_GENERIC_TIMEOUT);
+  
+  return;
+}
+
+/**
+  Check the URB's execution result and update the URB's
+  result accordingly. 
+
+  @param Ehc   The EHCI device.
+  @param Urb   The URB to check result.
+
+  @retval TRUE    URB transfer is finialized.
+  @retval FALSE   URB transfer is not finialized.
+
+**/
+BOOLEAN
+EhcCheckUrbResult (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  PEI_URB             *Urb
+  )
+{
+  EFI_LIST_ENTRY          *Entry;
+  PEI_EHC_QTD             *Qtd;
+  QTD_HW                  *QtdHw;
+  UINT8                   State;
+  BOOLEAN                 Finished;
+
+  ASSERT ((Ehc != NULL) && (Urb != NULL) && (Urb->Qh != NULL));
+
+  Finished        = TRUE;
+  Urb->Completed  = 0;
+
+  Urb->Result     = EFI_USB_NOERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    Urb->Result |= EFI_USB_ERR_SYSTEM;
+    goto ON_EXIT;
+  }
+
+  EFI_LIST_FOR_EACH (Entry, &Urb->Qh->Qtds) {
+    Qtd   = EFI_LIST_CONTAINER (Entry, PEI_EHC_QTD, QtdList);
+    QtdHw = &Qtd->QtdHw;
+    State = (UINT8) QtdHw->Status;
+
+    if (EHC_BIT_IS_SET (State, QTD_STAT_HALTED)) {
+      //
+      // EHCI will halt the queue head when met some error.
+      // If it is halted, the result of URB is finialized.
+      //
+      if ((State & QTD_STAT_ERR_MASK) == 0) {
+        Urb->Result |= EFI_USB_ERR_STALL;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_BABBLE_ERR)) {
+        Urb->Result |= EFI_USB_ERR_BABBLE;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_BUFF_ERR)) {
+        Urb->Result |= EFI_USB_ERR_BUFFER;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_TRANS_ERR) && (QtdHw->ErrCnt == 0)) {
+        Urb->Result |= EFI_USB_ERR_TIMEOUT;
+      }
+
+      Finished = TRUE;
+      goto ON_EXIT;
+      
+    } else if (EHC_BIT_IS_SET (State, QTD_STAT_ACTIVE)) {
+      //
+      // The QTD is still active, no need to check furthur.
+      //
+      Urb->Result |= EFI_USB_ERR_NOTEXECUTE;
+      
+      Finished = FALSE;
+      goto ON_EXIT;
+
+    } else {
+      //
+      // This QTD is finished OK or met short packet read. Update the
+      // transfer length if it isn't a setup.
+      //
+      if (QtdHw->Pid != QTD_PID_SETUP) {
+        Urb->Completed += Qtd->DataLen - QtdHw->TotalBytes;
+      }
+
+      if ((QtdHw->TotalBytes != 0) && (QtdHw->Pid == QTD_PID_INPUT)) {
+        //EHC_DUMP_QH ((Urb->Qh, "Short packet read", FALSE));
+
+        //
+        // Short packet read condition. If it isn't a setup transfer,
+        // no need to check furthur: the queue head will halt at the
+        // ShortReadStop. If it is a setup transfer, need to check the
+        // Status Stage of the setup transfer to get the finial result
+        //
+        if (QtdHw->AltNext == QTD_LINK (Ehc->ShortReadStop, FALSE)) {
+          
+          Finished = TRUE;
+          goto ON_EXIT;
+        }
+      }
+    }
+  }
+
+ON_EXIT:
+  //
+  // Return the data toggle set by EHCI hardware, bulk and interrupt
+  // transfer will use this to initialize the next transaction. For
+  // Control transfer, it always start a new data toggle sequence for
+  // new transfer.
+  //
+  // NOTICE: don't move DT update before the loop, otherwise there is
+  // a race condition that DT is wrong.
+  //
+  Urb->DataToggle = (UINT8) Urb->Qh->QhHw.DataToggle;
+
+  return Finished;
+}
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+  
+  @param  Ehc               The EHCI device.
+  @param  Urb               The URB to execute.
+  @param  TimeOut           The time to wait before abort, in millisecond.
+
+  @retval EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @retval EFI_TIMEOUT       The transfer failed due to time out.
+  @retval EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+EhcExecTransfer (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  PEI_URB             *Urb,
+  IN  UINTN               TimeOut
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   Index;
+  UINTN                   Loop;
+  BOOLEAN                 Finished;
+  BOOLEAN                 InfiniteLoop;
+
+  Status    = EFI_SUCCESS;
+  Loop      = TimeOut * EHC_1_MILLISECOND;
+  Finished     = FALSE;
+  InfiniteLoop = FALSE;
+
+  //
+  // If Timeout is 0, then the caller must wait for the function to be completed
+  // until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  for (Index = 0; InfiniteLoop || (Index < Loop); Index++) {
+    Finished = EhcCheckUrbResult (Ehc, Urb);
+
+    if (Finished) {
+      break;
+    }
+
+    MicroSecondDelay (EHC_1_MICROSECOND);
+  }
+
+  if (!Finished) {
+    Status = EFI_TIMEOUT;
+  } else if (Urb->Result != EFI_USB_NOERROR) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.h
new file mode 100644
index 0000000000..6cc52f8d0a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.h
@@ -0,0 +1,100 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010 - 2011, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_SCHED_H_
+#define _EFI_EHCI_SCHED_H_
+
+/**
+  Initialize the schedule data structure such as frame list.
+  
+  @param  Ehc   The EHCI device to init schedule data for.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to init schedule data.
+  @retval EFI_SUCCESS           The schedule data is initialized.
+
+**/
+EFI_STATUS
+EhcInitSched (
+  IN PEI_USB2_HC_DEV          *Ehc
+  )
+;
+
+/**
+  Free the schedule data. It may be partially initialized.
+  
+  @param  Ehc   The EHCI device.
+
+**/
+VOID
+EhcFreeSched (
+  IN PEI_USB2_HC_DEV          *Ehc
+  )
+;
+
+/**
+  Link the queue head to the asynchronous schedule list.
+  UEFI only supports one CTRL/BULK transfer at a time
+  due to its interfaces. This simplifies the AsynList
+  management: A reclamation header is always linked to
+  the AsyncListAddr, the only active QH is appended to it.
+  
+  @param  Ehc   The EHCI device.
+  @param  Qh    The queue head to link.
+
+**/
+VOID
+EhcLinkQhToAsync (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_EHC_QH           *Qh
+  )
+;
+
+/**
+  Unlink a queue head from the asynchronous schedule list.
+  Need to synchronize with hardware.
+  
+  @param  Ehc   The EHCI device.
+  @param  Qh    The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromAsync (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_EHC_QH           *Qh
+  )
+;
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+  
+  @param  Ehc               The EHCI device.
+  @param  Urb               The URB to execute.
+  @param  TimeOut           The time to wait before abort, in millisecond.
+
+  @retval EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @retval EFI_TIMEOUT       The transfer failed due to time out.
+  @retval EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+EhcExecTransfer (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  PEI_URB             *Urb,
+  IN  UINTN               TimeOut
+  )
+;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c
new file mode 100644
index 0000000000..597a4947f5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c
@@ -0,0 +1,610 @@
+/** @file
+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>
+  
+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"
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+  
+  @param  Ehc         The EHCI device.
+  @param  Data        Current data not associated with a QTD.
+  @param  DataLen     The length of the data.
+  @param  PktId       Packet ID to use in the QTD.
+  @param  Toggle      Data toggle to use in the QTD.
+  @param  MaxPacket   Maximu packet length of the endpoint.
+
+  @retval the pointer to the created QTD or NULL if failed to create one.
+
+**/
+PEI_EHC_QTD *
+EhcCreateQtd (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT8                *Data,
+  IN UINTN                DataLen,
+  IN UINT8                PktId,
+  IN UINT8                Toggle,
+  IN UINTN                MaxPacket
+  )
+{
+  PEI_EHC_QTD             *Qtd;
+  QTD_HW                  *QtdHw;
+  UINTN                   Index;
+  UINTN                   Len;
+  UINTN                   ThisBufLen;
+
+  ASSERT (Ehc != NULL);
+
+  Qtd = UsbHcAllocateMem (Ehc, Ehc->MemPool, sizeof (PEI_EHC_QTD));
+
+  if (Qtd == NULL) {
+    return NULL;
+  }
+
+  Qtd->Signature    = EHC_QTD_SIG;
+  Qtd->Data         = Data;
+  Qtd->DataLen      = 0;
+
+  InitializeListHead (&Qtd->QtdList);
+
+  QtdHw             = &Qtd->QtdHw;
+  QtdHw->NextQtd    = QTD_LINK (NULL, TRUE);
+  QtdHw->AltNext    = QTD_LINK (NULL, TRUE);
+  QtdHw->Status     = QTD_STAT_ACTIVE;
+  QtdHw->Pid        = PktId;
+  QtdHw->ErrCnt     = QTD_MAX_ERR;
+  QtdHw->Ioc        = 0;
+  QtdHw->TotalBytes = 0;
+  QtdHw->DataToggle = Toggle;
+
+  //
+  // Fill in the buffer points
+  //
+  if (Data != NULL) {
+    Len = 0;
+
+    for (Index = 0; Index <= QTD_MAX_BUFFER; Index++) {
+      //
+      // Set the buffer point (Check page 41 EHCI Spec 1.0). No need to
+      // compute the offset and clear Reserved fields. This is already
+      // done in the data point.
+      //
+      QtdHw->Page[Index]      = EHC_LOW_32BIT (Data);
+      QtdHw->PageHigh[Index]  = EHC_HIGH_32BIT (Data);
+
+      ThisBufLen              = QTD_BUF_LEN - (EHC_LOW_32BIT (Data) & QTD_BUF_MASK);
+
+      if (Len + ThisBufLen >= DataLen) {
+        Len = DataLen;
+        break;
+      }
+
+      Len += ThisBufLen;
+      Data += ThisBufLen;
+    }
+    
+    //
+    // Need to fix the last pointer if the Qtd can't hold all the
+    // user's data to make sure that the length is in the unit of
+    // max packets. If it can hold all the data, there is no such
+    // need.
+    //
+    if (Len < DataLen) {
+      Len = Len - Len % MaxPacket;
+    }
+
+    QtdHw->TotalBytes = (UINT32) Len;
+    Qtd->DataLen      = Len;
+  }
+
+  return Qtd;
+}
+
+/**
+  Initialize the queue head for interrupt transfer,
+  that is, initialize the following three fields:
+    1. SplitXState in the Status field.
+    2. Microframe S-mask.
+    3. Microframe C-mask.
+  
+  @param  Ep    The queue head's related endpoint.
+  @param  QhHw  The queue head to initialize.
+
+**/
+VOID
+EhcInitIntQh (
+  IN USB_ENDPOINT         *Ep,
+  IN QH_HW                *QhHw
+  )
+{
+  //
+  // Because UEFI interface can't utilitize an endpoint with
+  // poll rate faster than 1ms, only need to set one bit in
+  // the queue head. simple. But it may be changed later. If
+  // sub-1ms interrupt is supported, need to update the S-Mask
+  // here
+  //
+  if (Ep->DevSpeed == EFI_USB_SPEED_HIGH) {
+    QhHw->SMask = QH_MICROFRAME_0;
+    return ;
+  }
+  
+  //
+  // For low/full speed device, the transfer must go through
+  // the split transaction. Need to update three fields
+  // 1. SplitXState in the status
+  // 2. Microframe S-Mask
+  // 3. Microframe C-Mask
+  // UEFI USB doesn't exercise admission control. It simplely
+  // schedule the high speed transactions in microframe 0, and
+  // full/low speed transactions at microframe 1. This also
+  // avoid the use of FSTN.
+  //
+  QhHw->SMask = QH_MICROFRAME_1;
+  QhHw->CMask = QH_MICROFRAME_3 | QH_MICROFRAME_4 | QH_MICROFRAME_5;
+}
+
+/**
+  Allocate and initialize a EHCI queue head.
+  
+  @param  Ehci      The EHCI device.
+  @param  Ep        The endpoint to create queue head for.
+
+  @retval the pointer to the created queue head or NULL if failed to create one.
+
+**/
+PEI_EHC_QH *
+EhcCreateQh (
+  IN PEI_USB2_HC_DEV      *Ehci,
+  IN USB_ENDPOINT         *Ep
+  )
+{
+  PEI_EHC_QH              *Qh;
+  QH_HW                   *QhHw;
+
+  Qh = UsbHcAllocateMem (Ehci, Ehci->MemPool, sizeof (PEI_EHC_QH));
+
+  if (Qh == NULL) {
+    return NULL;
+  }
+
+  Qh->Signature       = EHC_QH_SIG;
+  Qh->NextQh          = NULL;
+  Qh->Interval        = Ep->PollRate;
+  
+  InitializeListHead (&Qh->Qtds);
+
+  QhHw                = &Qh->QhHw;
+  QhHw->HorizonLink   = QH_LINK (NULL, 0, TRUE);
+  QhHw->DeviceAddr    = Ep->DevAddr;
+  QhHw->Inactive      = 0;
+  QhHw->EpNum         = Ep->EpAddr;
+  QhHw->EpSpeed       = Ep->DevSpeed;
+  QhHw->DtCtrl        = 0;
+  QhHw->ReclaimHead   = 0;
+  QhHw->MaxPacketLen  = (UINT32) Ep->MaxPacket;
+  QhHw->CtrlEp        = 0;
+  QhHw->NakReload     = QH_NAK_RELOAD;
+  QhHw->HubAddr       = Ep->HubAddr;
+  QhHw->PortNum       = Ep->HubPort;
+  QhHw->Multiplier    = 1;
+  QhHw->DataToggle    = Ep->Toggle;
+
+  if (Ep->DevSpeed != EFI_USB_SPEED_HIGH) {
+    QhHw->Status |= QTD_STAT_DO_SS;
+  }
+
+  switch (Ep->Type) {
+  case EHC_CTRL_TRANSFER:
+    //
+    // Special initialization for the control transfer:
+    // 1. Control transfer initialize data toggle from each QTD
+    // 2. Set the Control Endpoint Flag (C) for low/full speed endpoint.
+    //
+    QhHw->DtCtrl = 1;
+
+    if (Ep->DevSpeed != EFI_USB_SPEED_HIGH) {
+      QhHw->CtrlEp = 1;
+    }
+    break;
+
+  case EHC_INT_TRANSFER_ASYNC:
+  case EHC_INT_TRANSFER_SYNC:
+    //
+    // Special initialization for the interrupt transfer
+    // to set the S-Mask and C-Mask
+    //
+    QhHw->NakReload = 0;
+    EhcInitIntQh (Ep, QhHw);
+    break;
+
+  case EHC_BULK_TRANSFER:
+    if ((Ep->DevSpeed == EFI_USB_SPEED_HIGH) && (Ep->Direction == EfiUsbDataOut)) {
+      QhHw->Status |= QTD_STAT_DO_PING;
+    }
+
+    break;
+  }
+
+  return Qh;
+}
+
+/**
+  Convert the poll interval from application to that
+  be used by EHCI interface data structure. Only need
+  to get the max 2^n that is less than interval. UEFI
+  can't support high speed endpoint with a interval less
+  than 8 microframe because interval is specified in
+  the unit of ms (millisecond).
+  
+  @param Interval       The interval to convert.
+
+  @retval The converted interval.
+
+**/
+UINTN
+EhcConvertPollRate (
+  IN  UINTN               Interval
+  )
+{
+  UINTN                   BitCount;
+
+  if (Interval == 0) {
+    return 1;
+  }
+  
+  //
+  // Find the index (1 based) of the highest non-zero bit
+  //
+  BitCount = 0;
+
+  while (Interval != 0) {
+    Interval >>= 1;
+    BitCount++;
+  }
+
+  return (UINTN)1 << (BitCount - 1);
+}
+
+/**
+  Free a list of QTDs.
+  
+  @param  Ehc         The EHCI device.
+  @param  Qtds        The list head of the QTD.
+
+**/
+VOID
+EhcFreeQtds (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN EFI_LIST_ENTRY       *Qtds
+  )
+{
+  EFI_LIST_ENTRY          *Entry;
+  EFI_LIST_ENTRY          *Next;
+  PEI_EHC_QTD             *Qtd;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, PEI_EHC_QTD, QtdList);
+
+    RemoveEntryList (&Qtd->QtdList);
+    UsbHcFreeMem (Ehc->MemPool, Qtd, sizeof (PEI_EHC_QTD));
+  }
+}
+
+/**
+  Free an allocated URB. It is possible for it to be partially inited.
+  
+  @param  Ehc         The EHCI device.
+  @param  Urb         The URB to free.
+
+**/
+VOID
+EhcFreeUrb (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_URB              *Urb
+  )
+{
+  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));
+  }
+}
+
+/**
+  Create a list of QTDs for the URB.
+  
+  @param  Ehc         The EHCI device.
+  @param  Urb         The URB to create QTDs for.
+
+  @retval EFI_OUT_OF_RESOURCES    Failed to allocate resource for QTD.
+  @retval EFI_SUCCESS             The QTDs are allocated for the URB.
+
+**/
+EFI_STATUS
+EhcCreateQtds (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_URB              *Urb
+  )
+{
+  USB_ENDPOINT            *Ep;
+  PEI_EHC_QH              *Qh;
+  PEI_EHC_QTD             *Qtd;
+  PEI_EHC_QTD             *StatusQtd;
+  PEI_EHC_QTD             *NextQtd;
+  EFI_LIST_ENTRY          *Entry;
+  UINT32                  AlterNext;
+  UINT8                   Toggle;
+  UINTN                   Len;
+  UINT8                   Pid;
+
+  ASSERT ((Urb != NULL) && (Urb->Qh != NULL));
+
+  //
+  // EHCI follows the alternet next QTD pointer if it meets
+  // a short read and the AlterNext pointer is valid. UEFI
+  // EHCI driver should terminate the transfer except the
+  // control transfer.
+  //
+  Toggle    = 0;
+  Qh        = Urb->Qh;
+  Ep        = &Urb->Ep;
+  StatusQtd = NULL;
+  AlterNext = QTD_LINK (NULL, TRUE);
+
+  if (Ep->Direction == EfiUsbDataIn) {
+    AlterNext = QTD_LINK (Ehc->ShortReadStop, FALSE);
+  }
+  
+  //
+  // Build the Setup and status packets for control transfer
+  //
+  if (Urb->Ep.Type == EHC_CTRL_TRANSFER) {
+    Len = sizeof (EFI_USB_DEVICE_REQUEST);
+    Qtd = EhcCreateQtd (Ehc, Urb->RequestPhy, Len, QTD_PID_SETUP, 0, Ep->MaxPacket);
+
+    if (Qtd == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    InsertTailList (&Qh->Qtds, &Qtd->QtdList);
+
+    //
+    // Create the status packet now. Set the AlterNext to it. So, when
+    // EHCI meets a short control read, it can resume at the status stage.
+    // Use the opposite direction of the data stage, or IN if there is
+    // no data stage.
+    //
+    if (Ep->Direction == EfiUsbDataIn) {
+      Pid = QTD_PID_OUTPUT;
+    } else {
+      Pid = QTD_PID_INPUT;
+    }
+
+    StatusQtd = EhcCreateQtd (Ehc, NULL, 0, Pid, 1, Ep->MaxPacket);
+
+    if (StatusQtd == NULL) {
+      goto ON_ERROR;
+    }
+
+    if (Ep->Direction == EfiUsbDataIn) {
+      AlterNext = QTD_LINK (StatusQtd, FALSE);
+    }
+
+    Toggle = 1;
+  }
+
+  //
+  // Build the data packets for all the transfers
+  //
+  if (Ep->Direction == EfiUsbDataIn) {
+    Pid = QTD_PID_INPUT;
+  } else {
+    Pid = QTD_PID_OUTPUT;
+  }
+
+  Qtd = NULL;
+  Len = 0;
+
+  while (Len < Urb->DataLen) {
+    Qtd = EhcCreateQtd (
+            Ehc,
+            (UINT8 *) Urb->DataPhy + Len,
+            Urb->DataLen - Len,
+            Pid,
+            Toggle,
+            Ep->MaxPacket
+            );
+
+    if (Qtd == NULL) {
+      goto ON_ERROR;
+    }
+
+    Qtd->QtdHw.AltNext = AlterNext;
+    InsertTailList (&Qh->Qtds, &Qtd->QtdList);
+
+    //
+    // Switch the Toggle bit if odd number of packets are included in the QTD.
+    //
+    if (((Qtd->DataLen + Ep->MaxPacket - 1) / Ep->MaxPacket) % 2) {
+      Toggle = (UINT8) (1 - Toggle);
+    }
+
+    Len += Qtd->DataLen;
+  }
+  
+  //
+  // Insert the status packet for control transfer
+  //
+  if (Ep->Type == EHC_CTRL_TRANSFER) {
+    InsertTailList (&Qh->Qtds, &StatusQtd->QtdList);
+  }
+
+  //
+  // OK, all the QTDs needed are created. Now, fix the NextQtd point
+  //
+  EFI_LIST_FOR_EACH (Entry, &Qh->Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, PEI_EHC_QTD, QtdList);
+
+    //
+    // break if it is the last entry on the list
+    //
+    if (Entry->ForwardLink == &Qh->Qtds) {
+      break;
+    }
+
+    NextQtd             = EFI_LIST_CONTAINER (Entry->ForwardLink, PEI_EHC_QTD, QtdList);
+    Qtd->QtdHw.NextQtd  = QTD_LINK (NextQtd, FALSE);
+  }
+
+  //
+  // Link the QTDs to the queue head
+  //
+  NextQtd           = EFI_LIST_CONTAINER (Qh->Qtds.ForwardLink, PEI_EHC_QTD, QtdList);
+  Qh->QhHw.NextQtd  = QTD_LINK (NextQtd, FALSE);
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  EhcFreeQtds (Ehc, &Qh->Qtds);
+  return EFI_OUT_OF_RESOURCES;
+}
+
+/**
+  Create a new URB and its associated QTD.
+  
+  @param  Ehc               The EHCI device.
+  @param  DevAddr           The device address.
+  @param  EpAddr            Endpoint addrress & its direction.
+  @param  DevSpeed          The device speed.
+  @param  Toggle            Initial data toggle to use.
+  @param  MaxPacket         The max packet length of the endpoint.
+  @param  Hub               The transaction translator to use.
+  @param  Type              The transaction type.
+  @param  Request           The standard USB request for control transfer.
+  @param  Data              The user data to transfer.
+  @param  DataLen           The length of data buffer.
+  @param  Callback          The function to call when data is transferred.
+  @param  Context           The context to the callback.
+  @param  Interval          The interval for interrupt transfer.
+
+  @retval the pointer to the created URB or NULL.
+
+**/
+PEI_URB *
+EhcCreateUrb (
+  IN PEI_USB2_HC_DEV                    *Ehc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,  
+  IN UINT8                              DevSpeed,
+  IN UINT8                              Toggle,
+  IN UINTN                              MaxPacket,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR *Hub,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context,
+  IN UINTN                              Interval
+  )
+{
+  USB_ENDPOINT                  *Ep;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  EFI_STATUS                    Status;
+  UINTN                         Len;
+  PEI_URB                       *Urb;
+  VOID                          *Map;
+
+    
+  Map = NULL;
+  
+  Urb = Ehc->Urb;
+  Urb->Signature  = EHC_URB_SIG;
+  InitializeListHead (&Urb->UrbList);
+
+  Ep              = &Urb->Ep;
+  Ep->DevAddr     = DevAddr;
+  Ep->EpAddr      = (UINT8) (EpAddr & 0x0F);
+  Ep->Direction   = (((EpAddr & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut);
+  Ep->DevSpeed    = DevSpeed;
+  Ep->MaxPacket   = MaxPacket;
+
+  Ep->HubAddr     = 0;
+  Ep->HubPort     = 0;
+
+  if (DevSpeed != EFI_USB_SPEED_HIGH) {
+    ASSERT (Hub != NULL);
+
+    Ep->HubAddr   = Hub->TranslatorHubAddress;
+    Ep->HubPort   = Hub->TranslatorPortNumber;
+  }
+
+  Ep->Toggle      = Toggle;
+  Ep->Type        = Type;
+  Ep->PollRate    = EhcConvertPollRate (Interval);
+
+  Urb->Request    = Request;
+  Urb->Data       = Data;
+  Urb->DataLen    = DataLen;
+  Urb->Callback   = Callback;
+  Urb->Context    = Context;
+  Urb->Qh         = EhcCreateQh (Ehc, &Urb->Ep);
+
+  if (Urb->Qh == NULL) {
+    goto ON_ERROR;
+  }
+  
+  //
+  // Map the request and user data
+  //
+  if (Request != NULL) {
+    Len     = sizeof (EFI_USB_DEVICE_REQUEST);
+    PhyAddr =  (EFI_PHYSICAL_ADDRESS) (UINTN) Request ;
+    if ( (Len != sizeof (EFI_USB_DEVICE_REQUEST))) {
+      goto ON_ERROR;
+    }
+
+    Urb->RequestPhy = (VOID *) ((UINTN) PhyAddr);
+    Urb->RequestMap = Map;
+  }
+
+  if (Data != NULL) {
+    Len      = DataLen;
+    PhyAddr  =  (EFI_PHYSICAL_ADDRESS) (UINTN) Data ;
+    if ( (Len != DataLen)) {
+      goto ON_ERROR;
+    }
+
+    Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+    Urb->DataMap  = Map;
+  }
+
+  Status = EhcCreateQtds (Ehc, Urb);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return Urb;
+
+ON_ERROR:
+  EhcFreeUrb (Ehc, Urb);
+  return NULL;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.h
new file mode 100644
index 0000000000..3fe93fb294
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.h
@@ -0,0 +1,331 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_URB_H_
+#define _EFI_EHCI_URB_H_
+
+typedef struct _PEI_EHC_QTD PEI_EHC_QTD;
+typedef struct _PEI_EHC_QH  PEI_EHC_QH;
+typedef struct _PEI_URB     PEI_URB;
+
+#define EHC_CTRL_TRANSFER       0x01
+#define EHC_BULK_TRANSFER       0x02
+#define EHC_INT_TRANSFER_SYNC   0x04
+#define EHC_INT_TRANSFER_ASYNC  0x08
+
+#define EHC_QTD_SIG             SIGNATURE_32 ('U', 'S', 'B', 'T')
+#define EHC_QH_SIG              SIGNATURE_32 ('U', 'S', 'B', 'H')
+#define EHC_URB_SIG             SIGNATURE_32 ('U', 'S', 'B', 'R')
+
+//
+// Hardware related bit definitions
+//
+#define EHC_TYPE_ITD            0x00
+#define EHC_TYPE_QH             0x02
+#define EHC_TYPE_SITD           0x04
+#define EHC_TYPE_FSTN           0x06
+
+#define QH_NAK_RELOAD           3
+#define QH_HSHBW_MULTI          1
+
+#define QTD_MAX_ERR             3
+#define QTD_PID_OUTPUT          0x00
+#define QTD_PID_INPUT           0x01
+#define QTD_PID_SETUP           0x02
+
+#define QTD_STAT_DO_OUT         0
+#define QTD_STAT_DO_SS          0
+#define QTD_STAT_DO_PING        0x01
+#define QTD_STAT_DO_CS          0x02
+#define QTD_STAT_TRANS_ERR      0x08
+#define QTD_STAT_BABBLE_ERR     0x10
+#define QTD_STAT_BUFF_ERR       0x20
+#define QTD_STAT_HALTED         0x40
+#define QTD_STAT_ACTIVE         0x80
+#define QTD_STAT_ERR_MASK       (QTD_STAT_TRANS_ERR | QTD_STAT_BABBLE_ERR | QTD_STAT_BUFF_ERR)
+
+#define QTD_MAX_BUFFER          4
+#define QTD_BUF_LEN             4096
+#define QTD_BUF_MASK            0x0FFF
+
+#define QH_MICROFRAME_0         0x01
+#define QH_MICROFRAME_1         0x02
+#define QH_MICROFRAME_2         0x04
+#define QH_MICROFRAME_3         0x08
+#define QH_MICROFRAME_4         0x10
+#define QH_MICROFRAME_5         0x20
+#define QH_MICROFRAME_6         0x40
+#define QH_MICROFRAME_7         0x80
+
+#define USB_ERR_SHORT_PACKET    0x200
+
+//
+// Fill in the hardware link point: pass in a EHC_QH/QH_HW 
+// pointer to QH_LINK; A EHC_QTD/QTD_HW pointer to QTD_LINK
+//
+#define QH_LINK(Addr, Type, Term) \
+          ((UINT32) ((EHC_LOW_32BIT (Addr) & 0xFFFFFFE0) | (Type) | ((Term) ? 1 : 0)))
+
+#define QTD_LINK(Addr, Term)      QH_LINK((Addr), 0, (Term))
+
+//
+// The defination of EHCI hardware used data structure for 
+// little endian architecture. The QTD and QH structures 
+// are required to be 32 bytes aligned. Don't add members 
+// to the head of the associated software strucuture.
+//
+#pragma pack(1)
+typedef struct {
+  UINT32                  NextQtd;
+  UINT32                  AltNext;
+  
+  UINT32                  Status       : 8;
+  UINT32                  Pid          : 2;
+  UINT32                  ErrCnt       : 2;
+  UINT32                  CurPage      : 3;
+  UINT32                  Ioc          : 1;
+  UINT32                  TotalBytes   : 15;
+  UINT32                  DataToggle   : 1;
+
+  UINT32                  Page[5];
+  UINT32                  PageHigh[5];
+} QTD_HW;
+
+typedef struct {
+  UINT32                  HorizonLink;  
+  //
+  // Endpoint capabilities/Characteristics DWord 1 and DWord 2
+  //
+  UINT32                  DeviceAddr   : 7;
+  UINT32                  Inactive     : 1;
+  UINT32                  EpNum        : 4;
+  UINT32                  EpSpeed      : 2;
+  UINT32                  DtCtrl       : 1;
+  UINT32                  ReclaimHead  : 1;
+  UINT32                  MaxPacketLen : 11;
+  UINT32                  CtrlEp       : 1;
+  UINT32                  NakReload    : 4;
+
+  UINT32                  SMask        : 8;
+  UINT32                  CMask        : 8;
+  UINT32                  HubAddr      : 7;
+  UINT32                  PortNum      : 7;
+  UINT32                  Multiplier   : 2;
+
+  //
+  // Transaction execution overlay area
+  //
+  UINT32                  CurQtd;
+  UINT32                  NextQtd;
+  UINT32                  AltQtd;
+  
+  UINT32                  Status       : 8;
+  UINT32                  Pid          : 2;
+  UINT32                  ErrCnt       : 2;
+  UINT32                  CurPage      : 3;
+  UINT32                  Ioc          : 1;
+  UINT32                  TotalBytes   : 15;
+  UINT32                  DataToggle   : 1;
+
+  UINT32                  Page[5];
+  UINT32                  PageHigh[5];
+} QH_HW;
+#pragma pack()
+
+
+//
+// Endpoint address and its capabilities
+//
+typedef struct _USB_ENDPOINT {
+  UINT8                   DevAddr;
+  UINT8                   EpAddr;     // Endpoint address, no direction encoded in
+  EFI_USB_DATA_DIRECTION  Direction;
+  UINT8                   DevSpeed;
+  UINTN                   MaxPacket;
+  UINT8                   HubAddr;
+  UINT8                   HubPort;
+  UINT8                   Toggle;     // Data toggle, not used for control transfer
+  UINTN                   Type;
+  UINTN                   PollRate;   // Polling interval used by EHCI
+} USB_ENDPOINT;
+
+//
+// Software QTD strcture, this is used to manage all the 
+// QTD generated from a URB. Don't add fields before QtdHw.
+//
+struct _PEI_EHC_QTD {
+  QTD_HW                  QtdHw;
+  UINT32                  Signature;
+  EFI_LIST_ENTRY          QtdList;   // The list of QTDs to one end point
+  UINT8                   *Data;     // Buffer of the original data
+  UINTN                   DataLen;   // Original amount of data in this QTD
+};
+
+
+
+//
+// Software QH structure. All three different transaction types 
+// supported by UEFI USB, that is the control/bulk/interrupt 
+// transfers use the queue head and queue token strcuture. 
+//
+// Interrupt QHs are linked to periodic frame list in the reversed
+// 2^N tree. Each interrupt QH is linked to the list starting at 
+// frame 0. There is a dummy interrupt QH linked to each frame as
+// a sentinental whose polling interval is 1. Synchronous interrupt
+// transfer is linked after this dummy QH. 
+// 
+// For control/bulk transfer, only synchronous (in the sense of UEFI) 
+// transfer is supported. A dummy QH is linked to EHCI AsyncListAddr
+// as the reclamation header. New transfer is inserted after this QH.
+//
+struct _PEI_EHC_QH {
+  QH_HW                   QhHw;
+  UINT32                  Signature;
+  PEI_EHC_QH              *NextQh;    // The queue head pointed to by horizontal link
+  EFI_LIST_ENTRY          Qtds;       // The list of QTDs to this queue head
+  UINTN                   Interval; 
+};
+
+//
+// URB (Usb Request Block) contains information for all kinds of 
+// usb requests.
+//
+struct _PEI_URB {
+  UINT32                          Signature;
+  EFI_LIST_ENTRY                  UrbList;
+  
+  //
+  // Transaction information
+  //
+  USB_ENDPOINT                    Ep;
+  EFI_USB_DEVICE_REQUEST          *Request;     // Control transfer only
+  VOID                            *RequestPhy;  // Address of the mapped request
+  VOID                            *RequestMap;
+  VOID                            *Data;
+  UINTN                           DataLen;
+  VOID                            *DataPhy;     // Address of the mapped user data
+  VOID                            *DataMap;
+  EFI_ASYNC_USB_TRANSFER_CALLBACK Callback; 
+  VOID                            *Context;
+
+  //
+  // Schedule data
+  //
+  PEI_EHC_QH                      *Qh;
+  
+  //
+  // Transaction result
+  //
+  UINT32                          Result;
+  UINTN                           Completed;    // completed data length
+  UINT8                           DataToggle;
+};
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+  
+  @param  Ehc         The EHCI device.
+  @param  Data        Current data not associated with a QTD.
+  @param  DataLen     The length of the data.
+  @param  PktId       Packet ID to use in the QTD.
+  @param  Toggle      Data toggle to use in the QTD.
+  @param  MaxPacket   Maximu packet length of the endpoint.
+
+  @retval the pointer to the created QTD or NULL if failed to create one.
+
+**/
+PEI_EHC_QTD *
+EhcCreateQtd (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT8                *Data,
+  IN UINTN                DataLen,
+  IN UINT8                PktId,
+  IN UINT8                Toggle,
+  IN UINTN                MaxPacket
+  )
+;
+
+/**
+  Allocate and initialize a EHCI queue head.
+  
+  @param  Ehci      The EHCI device.
+  @param  Ep        The endpoint to create queue head for.
+
+  @retval the pointer to the created queue head or NULL if failed to create one.
+
+**/
+PEI_EHC_QH *
+EhcCreateQh (
+  IN PEI_USB2_HC_DEV      *Ehci,
+  IN USB_ENDPOINT         *Ep
+  )
+;
+
+/**
+  Free an allocated URB. It is possible for it to be partially inited.
+  
+  @param  Ehc         The EHCI device.
+  @param  Urb         The URB to free.
+
+**/
+VOID
+EhcFreeUrb (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_URB              *Urb
+  )
+;
+
+/**
+  Create a new URB and its associated QTD.
+  
+  @param  Ehc               The EHCI device.
+  @param  DevAddr           The device address.
+  @param  EpAddr            Endpoint addrress & its direction.
+  @param  DevSpeed          The device speed.
+  @param  Toggle            Initial data toggle to use.
+  @param  MaxPacket         The max packet length of the endpoint.
+  @param  Hub               The transaction translator to use.
+  @param  Type              The transaction type.
+  @param  Request           The standard USB request for control transfer.
+  @param  Data              The user data to transfer.
+  @param  DataLen           The length of data buffer.
+  @param  Callback          The function to call when data is transferred.
+  @param  Context           The context to the callback.
+  @param  Interval          The interval for interrupt transfer.
+
+  @retval the pointer to the created URB or NULL.
+
+**/
+PEI_URB *
+EhcCreateUrb (
+  IN PEI_USB2_HC_DEV                    *Ehc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,  
+  IN UINT8                              DevSpeed,
+  IN UINT8                              Toggle,
+  IN UINTN                              MaxPacket,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR *Hub,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context,
+  IN UINTN                              Interval
+  )
+;
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c
new file mode 100644
index 0000000000..6b3755852b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c
@@ -0,0 +1,493 @@
+/** @file
+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>
+  
+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"
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Ehc            The EHCI device.
+  @param  Pool           The buffer pool to allocate memory for.
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Pages
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+  VOID                    *BufHost;
+  VOID                    *Mapping;
+  EFI_PHYSICAL_ADDRESS    MappedAddr;
+  EFI_STATUS              Status;
+  UINTN               PageNumber;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+
+  Mapping = NULL;
+  PageNumber =  sizeof(USBHC_MEM_BLOCK)/PAGESIZE +1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             PageNumber,
+             &TempPtr
+             );
+
+  if (EFI_ERROR (Status)) {
+      return NULL;
+  }
+  ZeroMem ((VOID   *)(UINTN)TempPtr, PageNumber*EFI_PAGE_SIZE);
+  
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+  
+  Block = (USBHC_MEM_BLOCK*)(UINTN)TempPtr;
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (USBHC_MEM_UNIT * 8);
+  
+  PageNumber =  (Block->BitsLen)/PAGESIZE +1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             PageNumber,
+             &TempPtr
+             );
+  
+    if (EFI_ERROR (Status)) {
+      return NULL;
+    }
+  ZeroMem ((VOID   *)(UINTN)TempPtr, PageNumber*EFI_PAGE_SIZE);	
+
+  Block->Bits  = (UINT8 *)(UINTN)TempPtr;
+
+  
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             Pages,
+             &TempPtr
+             );
+  ZeroMem ((VOID   *)(UINTN)TempPtr, 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
+  //
+  if (Pool->Check4G && (Pool->Which4G != USB_HC_HIGH_32BIT (MappedAddr))) {
+      return NULL;
+  }
+
+  Block->BufHost  = BufHost;
+  Block->Buf      = (UINT8 *) ((UINTN) MappedAddr);
+  Block->Mapping  = Mapping;
+  Block->Next      = NULL;
+
+  return Block;
+
+}
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL       *Pool,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Pool != NULL) && (Block != NULL));
+}
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block          The memory block to allocate memory from.
+  @param  Units          Number of memory units to allocate.
+
+  @return The pointer to the allocated memory. If couldn't allocate the needed memory,
+          the return value is NULL.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN  USBHC_MEM_BLOCK     *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+  
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | (UINT8) USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->Buf + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head           The head of the memory pool's block list.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+/**
+  Is the memory block empty?
+
+  @param  Block   The memory block to check.
+
+  @retval TRUE    The memory block is empty.
+  @retval FALSE   The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+/**
+  Initialize the memory management pool for the host controller.
+  
+  @param  Ehc                   The EHCI device.
+  @param  Check4G               Whether the host controller requires allocated memory.
+                                from one 4G address space.
+  @param  Which4G               The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS           The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE   Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  )
+{
+  USBHC_MEM_POOL          *Pool;
+  UINTN               PageNumber;
+  EFI_STATUS              Status;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+  
+  PageNumber =  sizeof(USBHC_MEM_POOL)/PAGESIZE +1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             PageNumber,
+              &TempPtr
+             );
+
+    if (EFI_ERROR (Status)) {
+      return NULL;
+    }
+  ZeroMem ((VOID   *)(UINTN)TempPtr, PageNumber*EFI_PAGE_SIZE);	
+
+  Pool = (USBHC_MEM_POOL *) ((UINTN) TempPtr);	
+
+  Pool->Check4G = Check4G;
+  Pool->Which4G = Which4G;
+  Pool->Head    = UsbHcAllocMemBlock (Ehc, Pool, USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+/**
+  Release the memory management pool.
+  
+  @param  Pool                  The USB memory pool to free.
+
+  @retval EFI_DEVICE_ERROR      Fail to free the memory pool.
+  @retval EFI_SUCCESS           The memory pool is freed.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+{
+  USBHC_MEM_BLOCK *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // 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);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+  
+  @param  Ehc       The EHCI device.
+  @param  Pool      The host controller's memory pool.
+  @param  Size      Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  USBHC_MEM_BLOCK         *NewBlock;
+  VOID                    *Mem;
+  UINTN                   AllocSize;
+  UINTN                   Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+  NewBlock = UsbHcAllocMemBlock (Ehc,Pool, Pages);
+
+  if (NewBlock == NULL) {
+    return NULL;
+  }
+  
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->Buf <= ToFree) && ((ToFree + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->Buf) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->Buf) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h
new file mode 100644
index 0000000000..586d12af96
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h
@@ -0,0 +1,77 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_MEM_H_
+#define _EFI_EHCI_MEM_H_
+
+#include <Uefi.h>
+#include <IndustryStandard/Pci22.h>
+
+#define USB_HC_BIT(a)                  ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+#define USB_HC_HIGH_32BIT(Addr64)    \
+          ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+
+struct _USBHC_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen; 
+  UINT8                   *Buf;
+  UINT8                   *BufHost;
+  UINTN                   BufLen;   // Memory size in bytes
+  VOID                    *Mapping;     
+  USBHC_MEM_BLOCK         *Next;
+};
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB 
+// host controller. EHCI requires the control memory and transfer
+// data to be on the same 4G memory. 
+//
+typedef struct _USBHC_MEM_POOL {
+  BOOLEAN                 Check4G;      
+  UINT32                  Which4G;      
+  USBHC_MEM_BLOCK         *Head;
+} USBHC_MEM_POOL;
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT           64
+
+#define USBHC_MEM_UNIT_MASK      (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_DEFAULT_PAGES  16
+
+#define USBHC_MEM_ROUND(Len)  (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)       
+
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.c b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.c
new file mode 100644
index 0000000000..be1b829200
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.c
@@ -0,0 +1,2501 @@
+/** @file
+PEIM to produce gEfiPeiVirtualBlockIoPpiGuid & gEfiPeiVirtualBlockIo2PpiGuid PPI for
+ATA controllers in the platform.
+
+This PPI can be consumed by PEIM which produce gEfiPeiDeviceRecoveryModulePpiGuid
+for Atapi CD ROM device.
+
+Copyright (c) 2006 - 2015, 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 "AtapiPeim.h"
+
+/**
+  Initializes the Atapi Block Io PPI.  
+  
+  @param[in]  FileHandle           Handle of the file being invoked.
+  @param[in]  PeiServices          Describes the list of possible PEI Services.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+  @retval     EFI_OUT_OF_RESOURCES Not enough memory to allocate.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiPeimEntry (
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  PEI_ATA_CONTROLLER_PPI  *AtaControllerPpi;
+  EFI_STATUS              Status;
+  ATAPI_BLK_IO_DEV        *AtapiBlkIoDev;
+
+  Status = PeiServicesRegisterForShadow (FileHandle);
+  if (!EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PeiServicesLocatePpi (
+              &gPeiAtaControllerPpiGuid,
+              0,
+              NULL,
+              (VOID **) &AtaControllerPpi
+              );
+  ASSERT_EFI_ERROR (Status);
+
+  AtapiBlkIoDev = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (*AtapiBlkIoDev)));
+  if (AtapiBlkIoDev == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  AtapiBlkIoDev->Signature        = ATAPI_BLK_IO_DEV_SIGNATURE;
+  AtapiBlkIoDev->AtaControllerPpi = AtaControllerPpi;
+
+  //
+  // atapi device enumeration and build private data
+  //
+  AtapiEnumerateDevices (AtapiBlkIoDev);
+
+  AtapiBlkIoDev->AtapiBlkIo.GetNumberOfBlockDevices = AtapiGetNumberOfBlockDevices;
+  AtapiBlkIoDev->AtapiBlkIo.GetBlockDeviceMediaInfo = AtapiGetBlockDeviceMediaInfo;
+  AtapiBlkIoDev->AtapiBlkIo.ReadBlocks              = AtapiReadBlocks;
+  AtapiBlkIoDev->AtapiBlkIo2.Revision                = EFI_PEI_RECOVERY_BLOCK_IO2_PPI_REVISION;
+  AtapiBlkIoDev->AtapiBlkIo2.GetNumberOfBlockDevices = AtapiGetNumberOfBlockDevices2;
+  AtapiBlkIoDev->AtapiBlkIo2.GetBlockDeviceMediaInfo = AtapiGetBlockDeviceMediaInfo2;
+  AtapiBlkIoDev->AtapiBlkIo2.ReadBlocks              = AtapiReadBlocks2;
+
+  AtapiBlkIoDev->PpiDescriptor.Flags                = EFI_PEI_PPI_DESCRIPTOR_PPI;
+  AtapiBlkIoDev->PpiDescriptor.Guid                 = &gEfiPeiVirtualBlockIoPpiGuid;
+  AtapiBlkIoDev->PpiDescriptor.Ppi                  = &AtapiBlkIoDev->AtapiBlkIo;
+
+  AtapiBlkIoDev->PpiDescriptor2.Flags                = (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+  AtapiBlkIoDev->PpiDescriptor2.Guid                 = &gEfiPeiVirtualBlockIo2PpiGuid;
+  AtapiBlkIoDev->PpiDescriptor2.Ppi                  = &AtapiBlkIoDev->AtapiBlkIo2;
+
+  DEBUG ((EFI_D_INFO, "Atatpi Device Count is %d\n", AtapiBlkIoDev->DeviceCount));
+  if (AtapiBlkIoDev->DeviceCount != 0) {
+    Status = PeiServicesInstallPpi (&AtapiBlkIoDev->PpiDescriptor);
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetNumberOfBlockDevices (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI   *This,
+  OUT  UINTN                             *NumberBlockDevices
+  )
+{
+  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev;
+
+  AtapiBlkIoDev = NULL;
+
+  AtapiBlkIoDev       = PEI_RECOVERY_ATAPI_FROM_BLKIO_THIS (This);
+
+  *NumberBlockDevices = AtapiBlkIoDev->DeviceCount;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+  
+  @retval EFI_SUCCESS           Media information about the specified block device 
+                                was obtained successfully.
+  @retval EFI_DEVICE_ERROR      Cannot get the media information due to a hardware 
+                                error.
+  @retval Others                Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetBlockDeviceMediaInfo (
+  IN   EFI_PEI_SERVICES                     **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI        *This,
+  IN   UINTN                                DeviceIndex,
+  OUT  EFI_PEI_BLOCK_IO_MEDIA               *MediaInfo
+  )
+{
+  UINTN             DeviceCount;
+  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev;
+  EFI_STATUS        Status;
+  UINTN             Index;
+
+  AtapiBlkIoDev = NULL;
+
+  if (This == NULL || MediaInfo == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO_THIS (This);
+
+  DeviceCount   = AtapiBlkIoDev->DeviceCount;
+
+  //
+  // DeviceIndex is a value from 1 to NumberBlockDevices.
+  //
+  if ((DeviceIndex < 1) || (DeviceIndex > DeviceCount) || (DeviceIndex > MAX_IDE_DEVICES)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Index = DeviceIndex - 1;
+
+  //
+  // probe media and retrieve latest media information
+  //
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo DevicePosition is %d\n", AtapiBlkIoDev->DeviceInfo[Index].DevicePosition));  
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo DeviceType is   %d\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.DeviceType));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo MediaPresent is %d\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.MediaPresent));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo BlockSize is  0x%x\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.BlockSize));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo LastBlock is  0x%x\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.LastBlock));
+
+  Status = DetectMedia (
+             AtapiBlkIoDev,
+             AtapiBlkIoDev->DeviceInfo[Index].DevicePosition,
+             &AtapiBlkIoDev->DeviceInfo[Index].MediaInfo,
+             &AtapiBlkIoDev->DeviceInfo[Index].MediaInfo2
+             );
+  if (Status != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo DevicePosition is %d\n", AtapiBlkIoDev->DeviceInfo[Index].DevicePosition));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo DeviceType is   %d\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.DeviceType));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo MediaPresent is %d\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.MediaPresent));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo BlockSize is  0x%x\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.BlockSize));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo LastBlock is  0x%x\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.LastBlock));
+  
+  //
+  // Get media info from AtapiBlkIoDev
+  //
+  CopyMem (MediaInfo, &AtapiBlkIoDev->DeviceInfo[Index].MediaInfo, sizeof(EFI_PEI_BLOCK_IO_MEDIA));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiReadBlocks (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI     *This,
+  IN   UINTN                             DeviceIndex,
+  IN   EFI_PEI_LBA                       StartLBA,
+  IN   UINTN                             BufferSize,
+  OUT  VOID                              *Buffer
+  )
+{
+
+  EFI_PEI_BLOCK_IO_MEDIA  MediaInfo;
+  EFI_STATUS              Status;
+  UINTN                   NumberOfBlocks;
+  UINTN                   BlockSize;
+  ATAPI_BLK_IO_DEV        *AtapiBlkIoDev;
+
+  AtapiBlkIoDev = NULL;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO_THIS (This);
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  Status = AtapiGetBlockDeviceMediaInfo (
+            PeiServices,
+            This,
+            DeviceIndex,
+            &MediaInfo
+            );
+  if (Status != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (!MediaInfo.MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  BlockSize = MediaInfo.BlockSize;
+
+  if (BufferSize % BlockSize != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NumberOfBlocks = BufferSize / BlockSize;
+
+  if ((StartLBA + NumberOfBlocks - 1) > AtapiBlkIoDev->DeviceInfo[DeviceIndex - 1].MediaInfo2.LastBlock) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = ReadSectors (
+            AtapiBlkIoDev,
+            AtapiBlkIoDev->DeviceInfo[DeviceIndex - 1].DevicePosition,
+            Buffer,
+            StartLBA,
+            NumberOfBlocks,
+            BlockSize
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process.
+  To the PEI legacy floppy driver, it returns the number of all the legacy
+  devices it finds during its enumeration process. If no device is detected,
+  then the function will return zero.
+
+  @param[in]  PeiServices          General-purpose services that are available
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetNumberOfBlockDevices2 (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI    *This,
+  OUT  UINTN                             *NumberBlockDevices
+  )
+{
+  EFI_STATUS        Status;
+  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev;
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO2_THIS (This);
+
+  Status = AtapiGetNumberOfBlockDevices (
+             PeiServices,
+             &AtapiBlkIoDev->AtapiBlkIo,
+             NumberBlockDevices
+             );
+
+  return Status;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media
+  information. If the media changes, calling this function will update the media
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.
+                            The caller is responsible for the ownership of this
+                            data structure.
+
+  @retval EFI_SUCCESS           Media information about the specified block device
+                                was obtained successfully.
+  @retval EFI_DEVICE_ERROR      Cannot get the media information due to a hardware
+                                error.
+  @retval Others                Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetBlockDeviceMediaInfo2 (
+  IN   EFI_PEI_SERVICES                     **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI       *This,
+  IN   UINTN                                DeviceIndex,
+  OUT  EFI_PEI_BLOCK_IO2_MEDIA              *MediaInfo
+  )
+{
+  ATAPI_BLK_IO_DEV           *AtapiBlkIoDev;
+  EFI_STATUS                 Status;
+  EFI_PEI_BLOCK_IO_MEDIA     Media;
+
+  AtapiBlkIoDev = NULL;
+
+  if (This == NULL || MediaInfo == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO2_THIS (This);
+
+  Status = AtapiGetBlockDeviceMediaInfo (
+             PeiServices,
+             &AtapiBlkIoDev->AtapiBlkIo,
+             DeviceIndex,
+             &Media
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Get media info from AtapiBlkIoDev
+  //
+  CopyMem (MediaInfo, &AtapiBlkIoDev->DeviceInfo[DeviceIndex - 1].MediaInfo2, sizeof(EFI_PEI_BLOCK_IO2_MEDIA));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the device
+                            index that was assigned during the enumeration process.
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiReadBlocks2 (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI    *This,
+  IN   UINTN                             DeviceIndex,
+  IN   EFI_PEI_LBA                       StartLBA,
+  IN   UINTN                             BufferSize,
+  OUT  VOID                              *Buffer
+  )
+{
+  EFI_STATUS          Status;
+  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev;
+
+  AtapiBlkIoDev = NULL;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO2_THIS (This);
+
+  Status = AtapiReadBlocks (
+             PeiServices,
+             &AtapiBlkIoDev->AtapiBlkIo,
+             DeviceIndex,
+             StartLBA,
+             BufferSize,
+             Buffer
+             );
+
+  return Status;
+}
+
+
+/**
+  Enumerate Atapi devices.
+
+  This function is used to enumerate Atatpi device in Ide channel.
+
+  @param[in]  AtapiBlkIoDev  A pointer to atapi block IO device
+
+**/
+VOID
+AtapiEnumerateDevices (
+  IN  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev
+  )
+{
+  UINT8                   Index1;
+  UINT8                   Index2;
+  UINTN                   DevicePosition;
+  EFI_PEI_BLOCK_IO_MEDIA  MediaInfo;
+  EFI_PEI_BLOCK_IO2_MEDIA MediaInfo2;
+  EFI_STATUS              Status;
+  UINTN                   DeviceCount;
+  UINT16                  CommandBlockBaseAddr;
+  UINT16                  ControlBlockBaseAddr;
+  UINT32                  IdeEnabledNumber;
+  IDE_REGS_BASE_ADDR      IdeRegsBaseAddr[MAX_IDE_CHANNELS];
+
+  DeviceCount = 0;
+  DevicePosition = 0;
+
+  //
+  // Scan IDE bus for ATAPI devices
+  //
+
+  //
+  // Enable Sata and IDE controller.
+  //
+  AtapiBlkIoDev->AtaControllerPpi->EnableAtaChannel (
+                                  (EFI_PEI_SERVICES **) GetPeiServicesTablePointer(),
+                                  AtapiBlkIoDev->AtaControllerPpi,
+                                  PEI_ICH_IDE_PRIMARY | PEI_ICH_IDE_SECONDARY
+                                  );
+
+  //
+  // Allow SATA Devices to spin-up. This is needed if 
+  // SEC and PEI phase is too short, for example Release Build.
+  //
+  DEBUG ((EFI_D_INFO, "Delay for %d seconds for SATA devices to spin-up\n", PcdGet16 (PcdSataSpinUpDelayInSecForRecoveryPath)));
+  MicroSecondDelay (PcdGet16 (PcdSataSpinUpDelayInSecForRecoveryPath) * 1000 * 1000); //
+
+  //
+  // Get four channels (primary or secondary Pata, Sata Channel) Command and Control Regs Base address.
+  //
+  IdeEnabledNumber = AtapiBlkIoDev->AtaControllerPpi->GetIdeRegsBaseAddr (
+                                                      (EFI_PEI_SERVICES **) GetPeiServicesTablePointer(),
+                                                      AtapiBlkIoDev->AtaControllerPpi,
+                                                      IdeRegsBaseAddr
+                                                      );
+
+  //
+  // Using Command and Control Regs Base Address to fill other registers.
+  //
+  for (Index1 = 0; Index1 < IdeEnabledNumber; Index1 ++) { 
+    CommandBlockBaseAddr               = IdeRegsBaseAddr[Index1].CommandBlockBaseAddr;
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Data         = CommandBlockBaseAddr;
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Reg1.Feature = (UINT16) (CommandBlockBaseAddr + 0x1);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].SectorCount  = (UINT16) (CommandBlockBaseAddr + 0x2);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].SectorNumber = (UINT16) (CommandBlockBaseAddr + 0x3);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].CylinderLsb  = (UINT16) (CommandBlockBaseAddr + 0x4);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].CylinderMsb  = (UINT16) (CommandBlockBaseAddr + 0x5);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Head         = (UINT16) (CommandBlockBaseAddr + 0x6);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Reg.Command  = (UINT16) (CommandBlockBaseAddr + 0x7);
+
+    ControlBlockBaseAddr                = IdeRegsBaseAddr[Index1].ControlBlockBaseAddr;
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Alt.DeviceControl = ControlBlockBaseAddr;
+    AtapiBlkIoDev->IdeIoPortReg[Index1].DriveAddress      = (UINT16) (ControlBlockBaseAddr + 0x1);
+    
+    //
+    // Scan IDE bus for ATAPI devices IDE or Sata device
+    //
+    for (Index2 = IdeMaster; Index2 < IdeMaxDevice; Index2++) {
+      //
+      // Pata & Sata, Primary & Secondary channel, Master & Slave device
+      //
+      DevicePosition = (UINTN) (Index1 * 2 + Index2);
+
+      if (DiscoverAtapiDevice (AtapiBlkIoDev, DevicePosition, &MediaInfo, &MediaInfo2)) {
+        //
+        // ATAPI Device at DevicePosition is found.
+        //
+        AtapiBlkIoDev->DeviceInfo[DeviceCount].DevicePosition = DevicePosition;
+        //
+        // Retrieve Media Info
+        //
+        Status  = DetectMedia (AtapiBlkIoDev, DevicePosition, &MediaInfo, &MediaInfo2);
+        CopyMem (&(AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo), &MediaInfo, sizeof (MediaInfo));
+        CopyMem (&(AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo2), &MediaInfo2, sizeof (MediaInfo2));
+
+        DEBUG ((EFI_D_INFO, "Atatpi Device Position is %d\n", DevicePosition));
+        DEBUG ((EFI_D_INFO, "Atatpi DeviceType is   %d\n", MediaInfo.DeviceType));
+        DEBUG ((EFI_D_INFO, "Atatpi MediaPresent is %d\n", MediaInfo.MediaPresent));
+        DEBUG ((EFI_D_INFO, "Atatpi BlockSize is  0x%x\n", MediaInfo.BlockSize));
+
+        if (EFI_ERROR (Status)) {
+          AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo.MediaPresent = FALSE;
+          AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo.LastBlock    = 0;
+          AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo2.MediaPresent = FALSE;
+          AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo2.LastBlock    = 0;
+        }
+        DeviceCount += 1;
+      }
+    }
+  }
+
+  AtapiBlkIoDev->DeviceCount = DeviceCount;
+}
+
+/**
+  Detect Atapi devices.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[out] MediaInfo       The media information of the specified block media.
+  @param[out] MediaInfo2      The media information 2 of the specified block media.
+
+  @retval TRUE                Atapi device exists in specified position.
+  @retval FALSE               Atapi device does not exist in specified position.
+
+**/
+BOOLEAN
+DiscoverAtapiDevice (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  OUT EFI_PEI_BLOCK_IO_MEDIA        *MediaInfo,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA       *MediaInfo2
+  )
+{
+  EFI_STATUS  Status;
+
+  if (!DetectIDEController (AtapiBlkIoDev, DevicePosition)) {
+    return FALSE;
+  }
+  //
+  // test if it is an ATAPI device (only supported device)
+  //
+  if (ATAPIIdentify (AtapiBlkIoDev, DevicePosition) == EFI_SUCCESS) {
+
+    Status = Inquiry (AtapiBlkIoDev, DevicePosition, MediaInfo, MediaInfo2);
+    if (!EFI_ERROR (Status)) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Check power mode of Atapi devices.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[in]  AtaCommand      The Ata Command passed in.
+
+  @retval EFI_SUCCESS         The Atapi device support power mode.
+  @retval EFI_NOT_FOUND       The Atapi device not found.
+  @retval EFI_TIMEOUT         Atapi command transaction is time out.
+  @retval EFI_ABORTED         Atapi command abort.
+
+**/
+EFI_STATUS
+CheckPowerMode (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition,
+  IN  UINT8               AtaCommand
+  )
+{
+  UINT8       Channel;
+  UINT8       Device;
+  UINT16      StatusRegister;
+  UINT16      HeadRegister;
+  UINT16      CommandRegister;
+  UINT16      ErrorRegister;
+  UINT16      SectorCountRegister;
+  EFI_STATUS  Status;
+  UINT8       StatusValue;
+  UINT8       ErrorValue;
+  UINT8       SectorCountValue;
+
+  Channel             = (UINT8) (DevicePosition / 2);
+  Device              = (UINT8) (DevicePosition % 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+
+  StatusRegister      = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Status;
+  HeadRegister        = AtapiBlkIoDev->IdeIoPortReg[Channel].Head;
+  CommandRegister     = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Command;
+  ErrorRegister       = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg1.Error;
+  SectorCountRegister = AtapiBlkIoDev->IdeIoPortReg[Channel].SectorCount;
+
+  //
+  // select device
+  //
+  IoWrite8 (HeadRegister, (UINT8) ((Device << 4) | 0xe0));
+
+  //
+  // refresh the SectorCount register
+  //
+  SectorCountValue = 0x55;
+  IoWrite8 (SectorCountRegister, SectorCountValue);
+
+  //
+  // select device
+  //
+  IoWrite8 (HeadRegister, (UINT8) ((Device << 4) | 0xe0));
+
+  Status = DRDYReady (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 100);
+
+  //
+  // select device
+  //
+  IoWrite8 (HeadRegister, (UINT8) ((Device << 4) | 0xe0));
+  //
+  // send 'check power' commandd via Command Register
+  //
+  IoWrite8 (CommandRegister, AtaCommand);
+
+  Status = WaitForBSYClear (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 3000);
+  if (EFI_ERROR (Status)) {
+    return EFI_TIMEOUT;
+  }
+
+  StatusValue = IoRead8 (StatusRegister);
+
+  //
+  // command returned status is DRDY, indicating device supports the command,
+  // so device is present.
+  //
+  if ((StatusValue & ATA_STSREG_DRDY) == ATA_STSREG_DRDY) {
+    return EFI_SUCCESS;
+  }
+
+  SectorCountValue = IoRead8 (SectorCountRegister);
+
+  //
+  // command returned status is ERR & ABRT_ERR, indicating device does not support
+  // the command, so device is present.
+  //
+  if ((StatusValue & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+    ErrorValue = IoRead8 (ErrorRegister);
+    if ((ErrorValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+      return EFI_ABORTED;
+    } else {
+      //
+      // According to spec, no other error code is valid
+      //
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  if ((SectorCountValue == 0x00) || (SectorCountValue == 0x80) || (SectorCountValue == 0xff)) {
+    //
+    // Write SectorCount 0x55 but return valid state value. Maybe no device
+    // exists or some slow kind of ATAPI device exists.
+    //
+    IoWrite8 (HeadRegister, (UINT8) ((Device << 4) | 0xe0));
+
+    //
+    // write 0x55 and 0xaa to SectorCounter register,
+    // if the data could be written into the register,
+    // indicating the device is present, otherwise the device is not present.
+    //
+    SectorCountValue = 0x55;
+    IoWrite8 (SectorCountRegister, SectorCountValue);
+    MicroSecondDelay (10000);
+
+    SectorCountValue = IoRead8 (SectorCountRegister);
+    if (SectorCountValue != 0x55) {
+      return EFI_NOT_FOUND;
+    }
+    //
+    // Send a "ATAPI TEST UNIT READY" command ... slow but accurate
+    //
+    Status = TestUnitReady (AtapiBlkIoDev, DevicePosition);
+    return Status;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Detect if an IDE controller exists in specified position.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+
+  @retval TRUE         The Atapi device exists.
+  @retval FALSE        The Atapi device does not present.
+
+**/
+BOOLEAN
+DetectIDEController (
+  IN  ATAPI_BLK_IO_DEV   *AtapiBlkIoDev,
+  IN  UINTN              DevicePosition
+  )
+{
+  UINT8       Channel;
+  EFI_STATUS  Status;
+  UINT8       AtaCommand;
+
+  Channel           = (UINT8) (DevicePosition / 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+  //
+  //  Wait 31 seconds for BSY clear
+  //
+  Status = WaitForBSYClear (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 31000);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  //
+  // Send 'check power' command for IDE device
+  //
+  AtaCommand  = 0xE5;
+  Status      = CheckPowerMode (AtapiBlkIoDev, DevicePosition, AtaCommand);
+  if ((Status == EFI_ABORTED) || (Status == EFI_SUCCESS)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Wait specified time interval to poll for BSY bit clear in the Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        BSY bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        BSY bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+WaitForBSYClear (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  StatusRegister;
+  UINT8   StatusValue;
+
+  StatusValue     = 0;
+
+  StatusRegister  = IdeIoRegisters->Reg.Status;
+
+  Delay           = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+    StatusValue = IoRead8 (StatusRegister);
+    if ((StatusValue & ATA_STSREG_BSY) == 0x00) {
+      break;
+    }
+    MicroSecondDelay (250);
+
+    Delay--;
+
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRDY bit set in the Status register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRDY bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRDY bit is not set in the specified time interval.
+
+**/
+EFI_STATUS
+DRDYReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  StatusRegister;
+  UINT8   StatusValue;
+  UINT8   ErrValue;
+
+  StatusValue     = 0;
+
+  StatusRegister  = IdeIoRegisters->Reg.Status;
+
+  Delay           = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+    StatusValue = IoRead8 (StatusRegister);
+    //
+    //  BSY == 0 , DRDY == 1
+    //
+    if ((StatusValue & (ATA_STSREG_DRDY | ATA_STSREG_BSY)) == ATA_STSREG_DRDY) {
+      break;
+    }
+
+  if ((StatusValue & (ATA_STSREG_ERR | ATA_STSREG_BSY)) == ATA_STSREG_ERR) {
+    ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+    if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+    return EFI_ABORTED;
+    }
+  }
+  
+    MicroSecondDelay (250);
+
+    Delay--;
+
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRQ bit clear in the Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+DRQClear (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  StatusRegister;
+  UINT8   StatusValue;
+  UINT8   ErrValue;
+
+  StatusValue     = 0;
+
+  StatusRegister  = IdeIoRegisters->Reg.Status;
+
+  Delay           = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+
+    StatusValue = IoRead8 (StatusRegister);
+
+    //
+    // wait for BSY == 0 and DRQ == 0
+    //
+    if ((StatusValue & (ATA_STSREG_DRQ | ATA_STSREG_BSY)) == 0) {
+      break;
+    }
+
+  if ((StatusValue & (ATA_STSREG_BSY | ATA_STSREG_ERR)) == ATA_STSREG_ERR) {
+    ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+    if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+    return EFI_ABORTED;
+    }
+  }
+  
+    MicroSecondDelay (250);
+
+    Delay--;
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRQ bit clear in the Alternate Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+DRQClear2 (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  AltStatusRegister;
+  UINT8   AltStatusValue;
+  UINT8   ErrValue;
+
+  AltStatusValue    = 0;
+
+  AltStatusRegister = IdeIoRegisters->Alt.AltStatus;
+
+  Delay             = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+
+    AltStatusValue = IoRead8 (AltStatusRegister);
+
+    //
+    // wait for BSY == 0 and DRQ == 0
+    //
+    if ((AltStatusValue & (ATA_STSREG_DRQ | ATA_STSREG_BSY)) == 0) {
+      break;
+    }
+
+  if ((AltStatusValue & (ATA_STSREG_BSY | ATA_STSREG_ERR)) == ATA_STSREG_ERR) {
+    ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+    if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+    return EFI_ABORTED;
+    }
+  }
+  
+    MicroSecondDelay (250);
+
+    Delay--;
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRQ bit set in the Status Register.
+
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not set in the specified time interval.
+  @retval EFI_ABORTED        Operation Aborted.
+
+**/
+EFI_STATUS
+DRQReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  StatusRegister;
+  UINT8   StatusValue;
+  UINT8   ErrValue;
+
+  StatusValue     = 0;
+  ErrValue        = 0;
+
+  StatusRegister  = IdeIoRegisters->Reg.Status;
+
+  Delay           = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+    //
+    //  read Status Register will clear interrupt
+    //
+    StatusValue = IoRead8 (StatusRegister);
+
+    //
+    //  BSY==0,DRQ==1
+    //
+    if ((StatusValue & (ATA_STSREG_BSY | ATA_STSREG_DRQ)) == ATA_STSREG_DRQ) {
+      break;
+    }
+
+    if ((StatusValue & (ATA_STSREG_BSY | ATA_STSREG_ERR)) == ATA_STSREG_ERR) {
+
+      ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+      if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+        return EFI_ABORTED;
+      }
+    }
+    MicroSecondDelay (250);
+
+    Delay--;
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRQ bit set in the Alternate Status Register.
+
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not set in the specified time interval.
+  @retval EFI_ABORTED        Operation Aborted.
+
+**/
+EFI_STATUS
+DRQReady2 (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  AltStatusRegister;
+  UINT8   AltStatusValue;
+  UINT8   ErrValue;
+
+  AltStatusValue    = 0;
+
+  AltStatusRegister = IdeIoRegisters->Alt.AltStatus;
+
+  Delay             = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+
+    AltStatusValue = IoRead8 (AltStatusRegister);
+
+    //
+    //  BSY==0,DRQ==1
+    //
+    if ((AltStatusValue & (ATA_STSREG_BSY | ATA_STSREG_DRQ)) == ATA_STSREG_DRQ) {
+      break;
+    }
+
+    if ((AltStatusValue & (ATA_STSREG_BSY | ATA_STSREG_ERR)) == ATA_STSREG_ERR) {
+
+      ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+      if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+        return EFI_ABORTED;
+      }
+    }
+    MicroSecondDelay (250);
+
+    Delay--;
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if there is an error in Status Register.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  StatusReg         The address to IDE IO registers.
+
+  @retval EFI_SUCCESS        Operation success.
+  @retval EFI_DEVICE_ERROR   Device error.
+
+**/
+EFI_STATUS
+CheckErrorStatus (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINT16              StatusReg
+  )
+{
+  UINT8 StatusValue;
+
+  StatusValue = IoRead8 (StatusReg);
+
+  if ((StatusValue & (ATA_STSREG_ERR | ATA_STSREG_DWF | ATA_STSREG_CORR)) == 0) {
+
+    return EFI_SUCCESS;
+  }
+
+  return EFI_DEVICE_ERROR;
+
+}
+
+/**
+  Idendify Atapi devices.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+
+  @retval EFI_SUCCESS        Identify successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be identified successfully.
+
+**/
+EFI_STATUS
+ATAPIIdentify (
+  IN  ATAPI_BLK_IO_DEV        *AtapiBlkIoDev,
+  IN  UINTN                   DevicePosition
+  )
+{
+  ATAPI_IDENTIFY_DATA  AtapiIdentifyData;
+  UINT8                Channel;
+  UINT8                Device;
+  UINT16               StatusReg;
+  UINT16               HeadReg;
+  UINT16               CommandReg;
+  UINT16               DataReg;
+  UINT16               SectorCountReg;
+  UINT16               SectorNumberReg;
+  UINT16               CylinderLsbReg;
+  UINT16               CylinderMsbReg;
+
+  UINT32               WordCount;
+  UINT32               Increment;
+  UINT32               Index;
+  UINT32               ByteCount;
+  UINT16               *Buffer16;
+
+  EFI_STATUS           Status;
+
+  ByteCount       = sizeof (AtapiIdentifyData);
+  Buffer16        = (UINT16 *) &AtapiIdentifyData;
+
+  Channel         = (UINT8) (DevicePosition / 2);
+  Device          = (UINT8) (DevicePosition % 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+
+  StatusReg       = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Status;
+  HeadReg         = AtapiBlkIoDev->IdeIoPortReg[Channel].Head;
+  CommandReg      = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Command;
+  DataReg         = AtapiBlkIoDev->IdeIoPortReg[Channel].Data;
+  SectorCountReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].SectorCount;
+  SectorNumberReg = AtapiBlkIoDev->IdeIoPortReg[Channel].SectorNumber;
+  CylinderLsbReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].CylinderLsb;
+  CylinderMsbReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].CylinderMsb;
+
+  //
+  // Send ATAPI Identify Command to get IDENTIFY data.
+  //
+  if (WaitForBSYClear (
+        AtapiBlkIoDev,
+        &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+        ATATIMEOUT
+        ) != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // select device via Head/Device register.
+  // Before write Head/Device register, BSY and DRQ must be 0.
+  //
+  if (DRQClear2 (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), ATATIMEOUT) != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  //  e0:1110,0000-- bit7 and bit5 are reserved bits.
+  //           bit6 set means LBA mode
+  //
+  IoWrite8 (HeadReg, (UINT8) ((Device << 4) | 0xe0));
+
+  //
+  // set all the command parameters
+  // Before write to all the following registers, BSY and DRQ must be 0.
+  //
+  if (DRQClear2 (
+        AtapiBlkIoDev,
+        &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+        ATATIMEOUT
+        ) != EFI_SUCCESS) {
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  IoWrite8 (SectorCountReg, 0);
+  IoWrite8 (SectorNumberReg, 0);
+  IoWrite8 (CylinderLsbReg, 0);
+  IoWrite8 (CylinderMsbReg, 0);
+
+  //
+  // send command via Command Register
+  //
+  IoWrite8 (CommandReg, ATA_CMD_IDENTIFY_DEVICE);
+
+  //
+  // According to PIO data in protocol, host can perform a series of reads to the
+  // data register after each time device set DRQ ready;
+  // The data size of "a series of read" is command specific.
+  // For most ATA command, data size received from device will not exceed 1 sector,
+  // hense the data size for "a series of read" can be the whole data size of one command request.
+  // For ATA command such as Read Sector command, whole data size of one ATA command request is often larger
+  // than 1 sector, according to the Read Sector command, the data size of "a series of read" is exactly
+  // 1 sector.
+  // Here for simplification reason, we specify the data size for "a series of read" to
+  // 1 sector (256 words) if whole data size of one ATA commmand request is larger than 256 words.
+  //
+  Increment = 256;
+  //
+  // 256 words
+  //
+  WordCount = 0;
+  //
+  // WordCount is used to record bytes of currently transfered data
+  //
+  while (WordCount < ByteCount / 2) {
+    //
+    // Poll DRQ bit set, data transfer can be performed only when DRQ is ready.
+    //
+    Status = DRQReady2 (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), ATATIMEOUT);
+    if (Status != EFI_SUCCESS) {
+      return Status;
+    }
+
+    if (CheckErrorStatus (AtapiBlkIoDev, StatusReg) != EFI_SUCCESS) {
+
+      return EFI_DEVICE_ERROR;
+    }
+    //
+    // Get the byte count for one series of read
+    //
+    if ((WordCount + Increment) > ByteCount / 2) {
+      Increment = ByteCount / 2 - WordCount;
+    }
+    //
+    // perform a series of read without check DRQ ready
+    //
+    for (Index = 0; Index < Increment; Index++) {
+      *Buffer16++ = IoRead16 (DataReg);
+    }
+
+    WordCount += Increment;
+
+  }
+  //
+  // while
+  //
+  if (DRQClear (
+        AtapiBlkIoDev,
+        &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+        ATATIMEOUT
+        ) != EFI_SUCCESS) {
+    return CheckErrorStatus (AtapiBlkIoDev, StatusReg);
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Sends out ATAPI Test Unit Ready Packet Command to the specified device
+  to find out whether device is accessible.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+
+  @retval EFI_SUCCESS        TestUnit command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed TestUnit command successfully.
+
+**/
+EFI_STATUS
+TestUnitReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition
+  )
+{
+  ATAPI_PACKET_COMMAND  Packet;
+  EFI_STATUS            Status;
+
+  //
+  // fill command packet
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Packet.TestUnitReady.opcode = ATA_CMD_TEST_UNIT_READY;
+
+  //
+  // send command packet
+  //
+  Status = AtapiPacketCommandIn (AtapiBlkIoDev, DevicePosition, &Packet, NULL, 0, ATAPITIMEOUT);
+  return Status;
+}
+
+/**
+  Send out ATAPI commands conforms to the Packet Command with PIO Data In Protocol.
+  
+  @param[in]  AtapiBlkIoDev         A pointer to atapi block IO device.
+  @param[in]  DevicePosition        An integer to signify device position.
+  @param[in]  Packet                A pointer to ATAPI command packet.
+  @param[in]  Buffer                Buffer to contain requested transfer data from device.
+  @param[in]  ByteCount             Requested transfer data length.
+  @param[in]  TimeoutInMilliSeconds Time out value, in unit of milliseconds.
+
+  @retval EFI_SUCCESS        Command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed command successfully.
+
+**/
+EFI_STATUS
+AtapiPacketCommandIn (
+  IN  ATAPI_BLK_IO_DEV      *AtapiBlkIoDev,
+  IN  UINTN                 DevicePosition,
+  IN  ATAPI_PACKET_COMMAND  *Packet,
+  IN  UINT16                *Buffer,
+  IN  UINT32                ByteCount,
+  IN  UINTN                 TimeoutInMilliSeconds
+  )
+{
+  UINT8       Channel;
+  UINT8       Device;
+  UINT16      StatusReg;
+  UINT16      HeadReg;
+  UINT16      CommandReg;
+  UINT16      FeatureReg;
+  UINT16      CylinderLsbReg;
+  UINT16      CylinderMsbReg;
+  UINT16      DeviceControlReg;
+  UINT16      DataReg;
+  EFI_STATUS  Status;
+  UINT32      Count;
+  UINT16      *CommandIndex;
+  UINT16      *PtrBuffer;
+  UINT32      Index;
+  UINT8       StatusValue;
+  UINT32      WordCount;
+
+  //
+  // required transfer data in word unit.
+  //
+  UINT32      RequiredWordCount;
+
+  //
+  // actual transfer data in word unit.
+  //
+  UINT32      ActualWordCount;
+
+  Channel           = (UINT8) (DevicePosition / 2);
+  Device            = (UINT8) (DevicePosition % 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+
+  StatusReg         = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Status;
+  HeadReg           = AtapiBlkIoDev->IdeIoPortReg[Channel].Head;
+  CommandReg        = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Command;
+  FeatureReg        = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg1.Feature;
+  CylinderLsbReg    = AtapiBlkIoDev->IdeIoPortReg[Channel].CylinderLsb;
+  CylinderMsbReg    = AtapiBlkIoDev->IdeIoPortReg[Channel].CylinderMsb;
+  DeviceControlReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].Alt.DeviceControl;
+  DataReg           = AtapiBlkIoDev->IdeIoPortReg[Channel].Data;
+
+  //
+  // Set all the command parameters by fill related registers.
+  // Before write to all the following registers, BSY and DRQ must be 0.
+  //
+  if (DRQClear2 (
+        AtapiBlkIoDev,
+        &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+        ATATIMEOUT
+        ) != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Select device via Device/Head Register.
+  // DEFAULT_CMD: 0xa0 (1010,0000)
+  //
+  IoWrite8 (HeadReg, (UINT8) ((Device << 4) | ATA_DEFAULT_CMD));
+
+  //
+  // No OVL; No DMA
+  //
+  IoWrite8 (FeatureReg, 0x00);
+
+  //
+  // set the transfersize to MAX_ATAPI_BYTE_COUNT to let the device
+  // determine how many data should be transfered.
+  //
+  IoWrite8 (CylinderLsbReg, (UINT8) (ATAPI_MAX_BYTE_COUNT & 0x00ff));
+  IoWrite8 (CylinderMsbReg, (UINT8) (ATAPI_MAX_BYTE_COUNT >> 8));
+
+  //
+  //  DEFAULT_CTL:0x0a (0000,1010)
+  //  Disable interrupt
+  //
+  IoWrite8 (DeviceControlReg, ATA_DEFAULT_CTL);
+
+  //
+  // Send Packet command to inform device
+  // that the following data bytes are command packet.
+  //
+  IoWrite8 (CommandReg, ATA_CMD_PACKET);
+
+  Status = DRQReady (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), TimeoutInMilliSeconds);
+  if (Status != EFI_SUCCESS) {
+    return Status;
+  }
+  //
+  // Send out command packet
+  //
+  CommandIndex = Packet->Data16;
+  for (Count = 0; Count < 6; Count++, CommandIndex++) {
+    IoWrite16 (DataReg, *CommandIndex);
+    MicroSecondDelay (10);
+  }
+
+  StatusValue = IoRead8 (StatusReg);
+  if ((StatusValue & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+    //
+    // Trouble! Something's wrong here... Wait some time and return. 3 second is
+    // supposed to be long enough for a device reset latency or error recovery
+    //
+    MicroSecondDelay (3000000);
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (Buffer == NULL || ByteCount == 0) {
+    return EFI_SUCCESS;
+  }
+  //
+  // call PioReadWriteData() function to get
+  // requested transfer data form device.
+  //
+  PtrBuffer         = Buffer;
+  RequiredWordCount = ByteCount / 2;
+  //
+  // ActuralWordCount means the word count of data really transfered.
+  //
+  ActualWordCount = 0;
+
+  Status          = EFI_SUCCESS;
+  while ((Status == EFI_SUCCESS) && (ActualWordCount < RequiredWordCount)) {
+    //
+    // before each data transfer stream, the host should poll DRQ bit ready,
+    // which informs device is ready to transfer data.
+    //
+    if (DRQReady2 (
+          AtapiBlkIoDev,
+          &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+          TimeoutInMilliSeconds
+          ) != EFI_SUCCESS) {
+      return CheckErrorStatus (AtapiBlkIoDev, StatusReg);
+    }
+    //
+    // read Status Register will clear interrupt
+    //
+    StatusValue = IoRead8 (StatusReg);
+
+    //
+    // get current data transfer size from Cylinder Registers.
+    //
+    WordCount = IoRead8 (CylinderMsbReg) << 8;
+    WordCount = WordCount | IoRead8 (CylinderLsbReg);
+    WordCount = WordCount & 0xffff;
+    WordCount /= 2;
+
+    //
+    // perform a series data In/Out.
+    //
+    for (Index = 0; (Index < WordCount) && (ActualWordCount < RequiredWordCount); Index++, ActualWordCount++) {
+
+      *PtrBuffer = IoRead16 (DataReg);
+
+      PtrBuffer++;
+
+    }
+
+    if (((ATAPI_REQUEST_SENSE_CMD *) Packet)->opcode == ATA_CMD_REQUEST_SENSE && ActualWordCount >= 4) {
+      RequiredWordCount = MIN (
+                            RequiredWordCount,
+                            (UINT32) (4 + (((ATAPI_REQUEST_SENSE_DATA *) Buffer)->addnl_sense_length / 2))
+                            );
+    }
+
+  }
+  //
+  // After data transfer is completed, normally, DRQ bit should clear.
+  //
+  Status = DRQClear2 (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), TimeoutInMilliSeconds);
+  if (Status != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // read status register to check whether error happens.
+  //
+  Status = CheckErrorStatus (AtapiBlkIoDev, StatusReg);
+  return Status;
+}
+
+/**
+  Sends out ATAPI Inquiry Packet Command to the specified device.
+  This command will return INQUIRY data of the device.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[out] MediaInfo       The media information of the specified block media.
+  @param[out] MediaInfo2      The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS        Command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed command successfully.
+  @retval EFI_UNSUPPORTED    Unsupported device type.
+
+**/
+EFI_STATUS
+Inquiry (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  OUT EFI_PEI_BLOCK_IO_MEDIA        *MediaInfo,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA       *MediaInfo2
+  )
+{
+  ATAPI_PACKET_COMMAND        Packet;
+  EFI_STATUS                  Status;
+  ATAPI_INQUIRY_DATA          Idata;
+
+  //
+  // prepare command packet for the ATAPI Inquiry Packet Command.
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  ZeroMem (&Idata, sizeof (ATAPI_INQUIRY_DATA));
+
+  Packet.Inquiry.opcode             = ATA_CMD_INQUIRY;
+  Packet.Inquiry.page_code          = 0;
+  Packet.Inquiry.allocation_length  = (UINT8) sizeof (ATAPI_INQUIRY_DATA);
+
+  //
+  // Send command packet and get requested Inquiry data.
+  //
+  Status = AtapiPacketCommandIn (
+            AtapiBlkIoDev,
+            DevicePosition,
+            &Packet,
+            (UINT16 *) (&Idata),
+            sizeof (ATAPI_INQUIRY_DATA),
+            ATAPITIMEOUT
+            //50
+            );
+
+  if (Status != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Identify device type via INQUIRY data.
+  //
+  switch (Idata.peripheral_type & 0x1f) {
+  case 0x00:
+    //
+    // Magnetic Disk
+    //
+    MediaInfo->DeviceType   = IdeLS120;
+    MediaInfo->MediaPresent = FALSE;
+    MediaInfo->LastBlock    = 0;
+    MediaInfo->BlockSize    = 0x200;
+    MediaInfo2->InterfaceType  = MSG_ATAPI_DP;
+    MediaInfo2->RemovableMedia = TRUE;
+    MediaInfo2->MediaPresent   = FALSE;
+    MediaInfo2->ReadOnly       = FALSE;
+    MediaInfo2->BlockSize      = 0x200;
+    MediaInfo2->LastBlock      = 0;
+    break;
+
+  case 0x05:
+    //
+    // CD-ROM
+    //
+    MediaInfo->DeviceType   = IdeCDROM;
+    MediaInfo->MediaPresent = FALSE;
+    MediaInfo->LastBlock    = 0;
+    MediaInfo->BlockSize    = 0x800;
+    MediaInfo2->InterfaceType  = MSG_ATAPI_DP;
+    MediaInfo2->RemovableMedia = TRUE;
+    MediaInfo2->MediaPresent   = FALSE;
+    MediaInfo2->ReadOnly       = TRUE;
+    MediaInfo2->BlockSize      = 0x200;
+    MediaInfo2->LastBlock      = 0;
+    break;
+
+  default:
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**  
+  Used before read/write blocks from/to ATAPI device media. 
+  Since ATAPI device media is removable, it is necessary to detect
+  whether media is present and get current present media's information.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in, out] MediaInfo     The media information of the specified block media.
+  @param[in, out] MediaInfo2    The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+  @retval EFI_OUT_OF_RESOURCES  Can not allocate required resources.
+
+**/
+EFI_STATUS
+DetectMedia (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  IN OUT EFI_PEI_BLOCK_IO_MEDIA     *MediaInfo,
+  IN OUT EFI_PEI_BLOCK_IO2_MEDIA    *MediaInfo2
+  )
+{
+
+  UINTN                     Index;
+  UINTN                     RetryNum;
+  UINTN                     MaxRetryNum;
+  ATAPI_REQUEST_SENSE_DATA  *SenseBuffers;
+  BOOLEAN                   NeedReadCapacity;
+  BOOLEAN                   NeedRetry;
+  EFI_STATUS                Status;
+  UINT8                     SenseCounts;
+
+  SenseBuffers = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (*SenseBuffers)));
+  if (SenseBuffers == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  //
+  // Test Unit Ready command is used to detect whether device is accessible,
+  // the device will produce corresponding Sense data.
+  //
+  for (Index = 0; Index < 2; Index++) {
+
+    Status = TestUnitReady (AtapiBlkIoDev, DevicePosition);
+    if (Status != EFI_SUCCESS) {
+      Status = ResetDevice (AtapiBlkIoDev, DevicePosition, FALSE);
+
+      if (Status != EFI_SUCCESS) {
+        ResetDevice (AtapiBlkIoDev, DevicePosition, TRUE);
+      }
+
+    } else {
+      break;
+    }
+  }
+
+  SenseCounts       = MAX_SENSE_KEY_COUNT;
+  Status            = EFI_SUCCESS;
+  NeedReadCapacity  = TRUE;
+
+  for (Index = 0; Index < 5; Index++) {
+    SenseCounts = MAX_SENSE_KEY_COUNT;
+    Status = RequestSense (
+              AtapiBlkIoDev,
+              DevicePosition,
+              SenseBuffers,
+              &SenseCounts
+              );
+    DEBUG ((EFI_D_INFO, "Atapi Request Sense Count is %d\n", SenseCounts));
+    if (IsDeviceStateUnclear (SenseBuffers, SenseCounts) || IsNoMedia (SenseBuffers, SenseCounts)) {
+      //
+      // We are not sure whether the media is present or not, try again
+      //
+      TestUnitReady (AtapiBlkIoDev, DevicePosition);
+    } else {
+      break;
+    }
+  }
+
+  if (Status == EFI_SUCCESS) {
+
+    if (IsNoMedia (SenseBuffers, SenseCounts)) {
+
+      NeedReadCapacity        = FALSE;
+      MediaInfo->MediaPresent = FALSE;
+      MediaInfo->LastBlock    = 0;
+      MediaInfo2->MediaPresent = FALSE;
+      MediaInfo2->LastBlock    = 0;
+    }
+
+    if (IsMediaError (SenseBuffers, SenseCounts)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  if (NeedReadCapacity) {
+    //
+    // at most retry 5 times
+    //
+    MaxRetryNum = 5;
+    RetryNum    = 1;
+    //
+    // initial retry once
+    //
+    for (Index = 0; (Index < RetryNum) && (Index < MaxRetryNum); Index++) {
+
+      Status = ReadCapacity (AtapiBlkIoDev, DevicePosition, MediaInfo, MediaInfo2);
+      MicroSecondDelay (200000);
+      SenseCounts = MAX_SENSE_KEY_COUNT;
+
+      if (Status != EFI_SUCCESS) {
+
+        Status = RequestSense (AtapiBlkIoDev, DevicePosition, SenseBuffers, &SenseCounts);
+        //
+        // If Request Sense data failed, reset the device and retry.
+        //
+        if (Status != EFI_SUCCESS) {
+
+          Status = ResetDevice (AtapiBlkIoDev, DevicePosition, FALSE);
+          //
+          // if ATAPI soft reset fail,
+          // use stronger reset mechanism -- ATA soft reset.
+          //
+          if (Status != EFI_SUCCESS) {
+            ResetDevice (AtapiBlkIoDev, DevicePosition, TRUE);
+          }
+
+          RetryNum++;
+          //
+          // retry once more
+          //
+          continue;
+        }
+        //
+        // No Media
+        //
+        if (IsNoMedia (SenseBuffers, SenseCounts)) {
+
+          MediaInfo->MediaPresent = FALSE;
+          MediaInfo->LastBlock    = 0;
+          MediaInfo2->MediaPresent = FALSE;
+          MediaInfo2->LastBlock    = 0;
+          break;
+        }
+
+        if (IsMediaError (SenseBuffers, SenseCounts)) {
+          return EFI_DEVICE_ERROR;
+        }
+
+        if (!IsDriveReady (SenseBuffers, SenseCounts, &NeedRetry)) {
+          //
+          // Drive not ready: if NeedRetry, then retry once more;
+          // else return error
+          //
+          if (NeedRetry) {
+            RetryNum++;
+            continue;
+          } else {
+            return EFI_DEVICE_ERROR;
+          }
+        }
+        //
+        // if read capacity fail not for above reasons, retry once more
+        //
+        RetryNum++;
+
+      }
+
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**  
+  Reset specified Atapi device.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in]  Extensive         If TRUE, use ATA soft reset, otherwise use Atapi soft reset.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ResetDevice (
+  IN  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev,
+  IN  UINTN             DevicePosition,
+  IN  BOOLEAN           Extensive
+  )
+{
+  UINT8   DevControl;
+  UINT8   Command;
+  UINT8   DeviceSelect;
+  UINT16  DeviceControlReg;
+  UINT16  CommandReg;
+  UINT16  HeadReg;
+  UINT8   Channel;
+  UINT8   Device;
+
+  Channel           = (UINT8) (DevicePosition / 2);
+  Device            = (UINT8) (DevicePosition % 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+
+  DeviceControlReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].Alt.DeviceControl;
+  CommandReg        = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Command;
+  HeadReg           = AtapiBlkIoDev->IdeIoPortReg[Channel].Head;
+
+  if (Extensive) {
+
+    DevControl = 0;
+    DevControl |= ATA_CTLREG_SRST;
+    //
+    // set SRST bit to initiate soft reset
+    //
+    DevControl |= BIT1;
+    //
+    // disable Interrupt
+    //
+    IoWrite8 (DeviceControlReg, DevControl);
+
+    //
+    // Wait 10us
+    //
+    MicroSecondDelay (10);
+
+    //
+    // Clear SRST bit
+    //
+    DevControl &= 0xfb;
+    //
+    // 0xfb:1111,1011
+    //
+    IoWrite8 (DeviceControlReg, DevControl);
+
+    //
+    // slave device needs at most 31s to clear BSY
+    //
+    if (WaitForBSYClear (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 31000) == EFI_TIMEOUT) {
+      return EFI_DEVICE_ERROR;
+    }
+
+  } else {
+    //
+    // for ATAPI device, no need to wait DRDY ready after device selecting.
+    // bit7 and bit5 are both set to 1 for backward compatibility
+    //
+    DeviceSelect = (UINT8) (((BIT7 | BIT5) | (Device << 4)));
+    IoWrite8 (HeadReg, DeviceSelect);
+
+    Command = ATA_CMD_SOFT_RESET;
+    IoWrite8 (CommandReg, Command);
+
+    //
+    // BSY cleared is the only status return to the host by the device when reset is completed
+    // slave device needs at most 31s to clear BSY
+    //
+    if (WaitForBSYClear (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 31000) != EFI_SUCCESS) {
+      return EFI_DEVICE_ERROR;
+    }
+    //
+    // stall 5 seconds to make the device status stable
+    //
+    MicroSecondDelay (STALL_1_SECONDS * 5);
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+/**  
+  Sends out ATAPI Request Sense Packet Command to the specified device.
+
+  @param[in]      AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]      DevicePosition  An integer to signify device position.
+  @param[in]      SenseBuffers    Pointer to sense buffer.
+  @param[in, out] SenseCounts     Length of sense buffer.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+RequestSense (
+  IN  ATAPI_BLK_IO_DEV           *AtapiBlkIoDev,
+  IN  UINTN                      DevicePosition,
+  IN  ATAPI_REQUEST_SENSE_DATA   *SenseBuffers,
+  IN  OUT  UINT8                 *SenseCounts
+  )
+{
+  EFI_STATUS            Status;
+  ATAPI_REQUEST_SENSE_DATA    *Sense;
+  UINT16                *Ptr;
+  BOOLEAN               SenseReq;
+  ATAPI_PACKET_COMMAND  Packet;
+
+  ZeroMem (SenseBuffers, sizeof (ATAPI_REQUEST_SENSE_DATA) * (*SenseCounts));
+  //
+  // fill command packet for Request Sense Packet Command
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Packet.RequestSence.opcode            = ATA_CMD_REQUEST_SENSE;
+  Packet.RequestSence.allocation_length = (UINT8) sizeof (ATAPI_REQUEST_SENSE_DATA);
+
+  Ptr = (UINT16 *) SenseBuffers;
+  //
+  // initialize pointer
+  //
+  *SenseCounts = 0;
+  //
+  //  request sense data from device continiously until no sense data exists in the device.
+  //
+  for (SenseReq = TRUE; SenseReq;) {
+
+    Sense = (ATAPI_REQUEST_SENSE_DATA *) Ptr;
+
+    //
+    // send out Request Sense Packet Command and get one Sense data form device
+    //
+    Status = AtapiPacketCommandIn (
+              AtapiBlkIoDev,
+              DevicePosition,
+              &Packet,
+              Ptr,
+              sizeof (ATAPI_REQUEST_SENSE_DATA),
+              ATAPITIMEOUT
+              );
+    //
+    // failed to get Sense data
+    //
+    if (Status != EFI_SUCCESS) {
+      if (*SenseCounts == 0) {
+        return EFI_DEVICE_ERROR;
+      } else {
+        return EFI_SUCCESS;
+      }
+    }
+
+    (*SenseCounts)++;
+
+    if (*SenseCounts > MAX_SENSE_KEY_COUNT) {
+      return EFI_SUCCESS;
+    }
+    //
+    // We limit MAX sense data count to 20 in order to avoid dead loop. Some
+    // incompatible ATAPI devices don't retrive NO_SENSE when there is no media.
+    // In this case, dead loop occurs if we don't have a gatekeeper. 20 is
+    // supposed to be large enough for any ATAPI device.
+    //
+    if ((Sense->sense_key != ATA_SK_NO_SENSE) && ((*SenseCounts) < 20)) {
+
+      Ptr += sizeof (ATAPI_REQUEST_SENSE_DATA) / 2;
+      //
+      // Ptr is word based pointer
+      //
+    } else {
+      //
+      // when no sense key, skip out the loop
+      //
+      SenseReq = FALSE;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**  
+  Sends out ATAPI Read Capacity Packet Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in, out] MediaInfo     The media information of the specified block media.
+  @param[in, out] MediaInfo2    The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ReadCapacity (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  IN OUT EFI_PEI_BLOCK_IO_MEDIA     *MediaInfo,
+  IN OUT EFI_PEI_BLOCK_IO2_MEDIA    *MediaInfo2
+  )
+{
+  EFI_STATUS                Status;
+  ATAPI_PACKET_COMMAND      Packet;
+
+  //
+  // used for capacity data returned from ATAPI device
+  //
+  ATAPI_READ_CAPACITY_DATA        Data;
+  ATAPI_READ_FORMAT_CAPACITY_DATA FormatData;
+
+  ZeroMem (&Data, sizeof (Data));
+  ZeroMem (&FormatData, sizeof (FormatData));
+
+  if (MediaInfo->DeviceType == IdeCDROM) {
+
+    ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+    Packet.Inquiry.opcode = ATA_CMD_READ_CAPACITY;
+    Status = AtapiPacketCommandIn (
+              AtapiBlkIoDev,
+              DevicePosition,
+              &Packet,
+              (UINT16 *) (&Data),
+              sizeof (ATAPI_READ_CAPACITY_DATA),
+              ATAPITIMEOUT
+              );
+
+  } else {
+    //
+    // DeviceType == IdeLS120
+    //
+    ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+    Packet.ReadFormatCapacity.opcode                = ATA_CMD_READ_FORMAT_CAPACITY;
+    Packet.ReadFormatCapacity.allocation_length_lo  = 12;
+    Status = AtapiPacketCommandIn (
+              AtapiBlkIoDev,
+              DevicePosition,
+              &Packet,
+              (UINT16 *) (&FormatData),
+              sizeof (ATAPI_READ_FORMAT_CAPACITY_DATA),
+              ATAPITIMEOUT*10
+              );
+  }
+
+  if (Status == EFI_SUCCESS) {
+
+    if (MediaInfo->DeviceType == IdeCDROM) {
+
+      MediaInfo->LastBlock    = (Data.LastLba3 << 24) | (Data.LastLba2 << 16) | (Data.LastLba1 << 8) | Data.LastLba0;
+      MediaInfo->MediaPresent = TRUE;
+      //
+      // Because the user data portion in the sector of the Data CD supported
+      // is always 800h
+      //
+      MediaInfo->BlockSize     = 0x800;
+
+      MediaInfo2->LastBlock    = MediaInfo->LastBlock;
+      MediaInfo2->MediaPresent = MediaInfo->MediaPresent;
+      MediaInfo2->BlockSize    = (UINT32)MediaInfo->BlockSize;
+    }
+
+    if (MediaInfo->DeviceType == IdeLS120) {
+
+      if (FormatData.DesCode == 3) {
+        MediaInfo->MediaPresent = FALSE;
+        MediaInfo->LastBlock    = 0;
+        MediaInfo2->MediaPresent = FALSE;
+        MediaInfo2->LastBlock    = 0;
+      } else {
+        MediaInfo->LastBlock = (FormatData.LastLba3 << 24) |
+          (FormatData.LastLba2 << 16) |
+          (FormatData.LastLba1 << 8) |
+          FormatData.LastLba0;
+        MediaInfo->LastBlock--;
+
+        MediaInfo->MediaPresent = TRUE;
+
+        MediaInfo->BlockSize    = 0x200;
+
+        MediaInfo2->LastBlock    = MediaInfo->LastBlock;
+        MediaInfo2->MediaPresent = MediaInfo->MediaPresent;
+        MediaInfo2->BlockSize    = (UINT32)MediaInfo->BlockSize;
+
+      }
+    }
+
+    return EFI_SUCCESS;
+
+  } else {
+    return EFI_DEVICE_ERROR;
+  }
+}
+
+/**  
+  Perform read from disk in block unit.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[in]  Buffer          Buffer to contain read data.
+  @param[in]  StartLba        Starting LBA address.
+  @param[in]  NumberOfBlocks  Number of blocks to read.
+  @param[in]  BlockSize       Size of each block.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ReadSectors (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition,
+  IN  VOID                *Buffer,
+  IN  EFI_PEI_LBA         StartLba,
+  IN  UINTN               NumberOfBlocks,
+  IN  UINTN               BlockSize
+  )
+{
+
+  ATAPI_PACKET_COMMAND  Packet;
+  ATAPI_READ10_CMD      *Read10Packet;
+  EFI_STATUS            Status;
+  UINTN                 BlocksRemaining;
+  UINT32                Lba32;
+  UINT32                ByteCount;
+  UINT16                SectorCount;
+  VOID                  *PtrBuffer;
+  UINT16                MaxBlock;
+
+  //
+  // fill command packet for Read(10) command
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Read10Packet  = &Packet.Read10;
+  Lba32         = (UINT32) StartLba;
+  PtrBuffer     = Buffer;
+
+  //
+  // limit the data bytes that can be transfered by one Read(10) Command
+  //
+  MaxBlock = (UINT16) (0x10000 / BlockSize);
+  //
+  // (64k bytes)
+  //
+  BlocksRemaining = NumberOfBlocks;
+
+  Status          = EFI_SUCCESS;
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      SectorCount = (UINT16) BlocksRemaining;
+    } else {
+      SectorCount = MaxBlock;
+    }
+    //
+    // fill the Packet data sturcture
+    //
+    Read10Packet->opcode = ATA_CMD_READ_10;
+
+    //
+    // Lba0 ~ Lba3 specify the start logical block address of the data transfer.
+    // Lba0 is MSB, Lba3 is LSB
+    //
+    Read10Packet->Lba3  = (UINT8) (Lba32 & 0xff);
+    Read10Packet->Lba2  = (UINT8) (Lba32 >> 8);
+    Read10Packet->Lba1  = (UINT8) (Lba32 >> 16);
+    Read10Packet->Lba0  = (UINT8) (Lba32 >> 24);
+
+    //
+    // TranLen0 ~ TranLen1 specify the transfer length in block unit.
+    // TranLen0 is MSB, TranLen is LSB
+    //
+    Read10Packet->TranLen1  = (UINT8) (SectorCount & 0xff);
+    Read10Packet->TranLen0  = (UINT8) (SectorCount >> 8);
+
+    ByteCount               = (UINT32) (SectorCount * BlockSize);
+
+    Status = AtapiPacketCommandIn (
+              AtapiBlkIoDev,
+              DevicePosition,
+              &Packet,
+              (UINT16 *) PtrBuffer,
+              ByteCount,
+              ATAPILONGTIMEOUT
+              );
+    if (Status != EFI_SUCCESS) {
+      return Status;
+    }
+
+    Lba32 += SectorCount;
+    PtrBuffer = (UINT8 *) PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  return Status;
+}
+
+/**  
+  Check if there is media according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    No media
+  @retval FALSE   Media exists
+
+**/
+BOOLEAN
+IsNoMedia (
+  IN  ATAPI_REQUEST_SENSE_DATA  *SenseData,
+  IN  UINTN                     SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   IsNoMedia;
+
+  IsNoMedia = FALSE;
+
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    if ((SensePtr->sense_key == ATA_SK_NOT_READY) && (SensePtr->addnl_sense_code == ATA_ASC_NO_MEDIA)) {
+      IsNoMedia = TRUE;
+    }
+
+    SensePtr++;
+  }
+
+  return IsNoMedia;
+}
+
+/**  
+  Check if device state is unclear according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    Device state is unclear
+  @retval FALSE   Device state is clear  
+
+**/
+BOOLEAN
+IsDeviceStateUnclear (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                       SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   Unclear;
+
+  Unclear  = FALSE;
+
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    if (SensePtr->sense_key == 0x06) {
+      //
+      // Sense key is 0x06 means the device is just be reset or media just
+      // changed. The current state of the device is unclear.
+      //
+      Unclear = TRUE;
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return Unclear;
+}
+
+/**  
+  Check if there is media error according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    Media error
+  @retval FALSE   No media error
+
+**/
+BOOLEAN
+IsMediaError (
+  IN  ATAPI_REQUEST_SENSE_DATA  *SenseData,
+  IN  UINTN                     SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   IsError;
+
+  IsError   = FALSE;
+
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->sense_key) {
+
+    case ATA_SK_MEDIUM_ERROR:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_ERR1:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR2:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR3:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR4:
+        IsError = TRUE;
+        break;
+
+      default:
+        break;
+      }
+
+      break;
+
+    case ATA_SK_NOT_READY:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_UPSIDE_DOWN:
+        IsError = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return IsError;
+}
+
+/**  
+  Check if drive is ready according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+  @param[out] NeedRetry   Indicate if retry is needed.
+
+  @retval TRUE    Drive ready
+  @retval FALSE   Drive not ready
+
+**/
+BOOLEAN
+IsDriveReady (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                       SenseCounts,
+  OUT BOOLEAN                     *NeedRetry
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   IsReady;
+
+  IsReady     = TRUE;
+  *NeedRetry  = FALSE;
+
+  SensePtr    = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->sense_key) {
+
+    case ATA_SK_NOT_READY:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_NOT_READY:
+        switch (SensePtr->addnl_sense_code_qualifier) {
+        case ATA_ASCQ_IN_PROGRESS:
+          IsReady     = FALSE;
+          *NeedRetry  = TRUE;
+          break;
+
+        default:
+          IsReady     = FALSE;
+          *NeedRetry  = FALSE;
+          break;
+        }
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return IsReady;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.h b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.h
new file mode 100644
index 0000000000..20b35c4afd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.h
@@ -0,0 +1,789 @@
+/** @file
+Private Include file for IdeBus PEIM.
+
+Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _RECOVERY_ATAPI_H_
+#define _RECOVERY_ATAPI_H_
+
+#include <PiPei.h>
+
+#include <Ppi/BlockIo.h>
+#include <Ppi/BlockIo2.h>
+#include <Ppi/AtaController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/IoLib.h>
+#include <Library/PeiServicesTablePointerLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+
+
+#include <IndustryStandard/Atapi.h>
+
+#define MAX_SENSE_KEY_COUNT 6
+#define MAX_IDE_CHANNELS    4  // Ide and Sata Primary, Secondary Channel.
+#define MAX_IDE_DEVICES     8  // Ide, Sata Primary, Secondary and Master, Slave device.
+
+typedef enum {
+  IdePrimary    = 0,
+  IdeSecondary  = 1,
+  IdeMaxChannel = 2
+} EFI_IDE_CHANNEL;
+
+typedef enum {
+  IdeMaster     = 0,
+  IdeSlave      = 1,
+  IdeMaxDevice  = 2
+} EFI_IDE_DEVICE;
+
+//
+// IDE Registers
+//
+typedef union {
+  UINT16  Command;        /* when write */
+  UINT16  Status;         /* when read */
+} IDE_CMD_OR_STATUS;
+
+typedef union {
+  UINT16  Error;          /* when read */
+  UINT16  Feature;        /* when write */
+} IDE_ERROR_OR_FEATURE;
+
+typedef union {
+  UINT16  AltStatus;      /* when read */
+  UINT16  DeviceControl;  /* when write */
+} IDE_ALTSTATUS_OR_DEVICECONTROL;
+
+//
+// IDE registers set
+//
+typedef struct {
+  UINT16                          Data;
+  IDE_ERROR_OR_FEATURE            Reg1;
+  UINT16                          SectorCount;
+  UINT16                          SectorNumber;
+  UINT16                          CylinderLsb;
+  UINT16                          CylinderMsb;
+  UINT16                          Head;
+  IDE_CMD_OR_STATUS               Reg;
+
+  IDE_ALTSTATUS_OR_DEVICECONTROL  Alt;
+  UINT16                          DriveAddress;
+} IDE_BASE_REGISTERS;
+
+typedef struct {
+
+  UINTN                   DevicePosition;
+  EFI_PEI_BLOCK_IO_MEDIA  MediaInfo;
+  EFI_PEI_BLOCK_IO2_MEDIA MediaInfo2;
+
+} PEI_ATAPI_DEVICE_INFO;
+
+#define ATAPI_BLK_IO_DEV_SIGNATURE  SIGNATURE_32 ('a', 'b', 'i', 'o')
+typedef struct {
+  UINTN                           Signature;
+
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI   AtapiBlkIo;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI  AtapiBlkIo2;
+  EFI_PEI_PPI_DESCRIPTOR          PpiDescriptor;
+  EFI_PEI_PPI_DESCRIPTOR          PpiDescriptor2;
+  PEI_ATA_CONTROLLER_PPI          *AtaControllerPpi;
+
+  UINTN                           DeviceCount;
+  PEI_ATAPI_DEVICE_INFO           DeviceInfo[MAX_IDE_DEVICES];   //for max 8 device
+  IDE_BASE_REGISTERS              IdeIoPortReg[MAX_IDE_CHANNELS]; //for max 4 channel.
+} ATAPI_BLK_IO_DEV;
+
+#define PEI_RECOVERY_ATAPI_FROM_BLKIO_THIS(a) CR (a, ATAPI_BLK_IO_DEV, AtapiBlkIo, ATAPI_BLK_IO_DEV_SIGNATURE)
+#define PEI_RECOVERY_ATAPI_FROM_BLKIO2_THIS(a) CR (a, ATAPI_BLK_IO_DEV, AtapiBlkIo2, ATAPI_BLK_IO_DEV_SIGNATURE)
+
+
+#define STALL_1_MILLI_SECOND  1000  // stall 1 ms
+#define STALL_1_SECONDS       1000 * STALL_1_MILLI_SECOND
+
+//
+// Time Out Value For IDE Device Polling
+//
+// ATATIMEOUT is used for waiting time out for ATA device
+//
+#define ATATIMEOUT  1000  // 1 second
+// ATAPITIMEOUT is used for waiting operation
+// except read and write time out for ATAPI device
+//
+#define ATAPITIMEOUT  1000  // 1 second
+// ATAPILONGTIMEOUT is used for waiting read and
+// write operation timeout for ATAPI device
+//
+#define CDROMLONGTIMEOUT  2000  // 2 seconds
+#define ATAPILONGTIMEOUT  5000  // 5 seconds
+
+//
+// PEI Recovery Block I/O PPI
+//
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetNumberOfBlockDevices (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI     *This,
+  OUT  UINTN                             *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+  
+  @retval EFI_SUCCESS           Media information about the specified block device 
+                                was obtained successfully.
+  @retval EFI_DEVICE_ERROR      Cannot get the media information due to a hardware 
+                                error.
+  @retval Others                Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetBlockDeviceMediaInfo (
+  IN   EFI_PEI_SERVICES                     **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI        *This,
+  IN   UINTN                                DeviceIndex,
+  OUT  EFI_PEI_BLOCK_IO_MEDIA               *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiReadBlocks (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI     *This,
+  IN   UINTN                             DeviceIndex,
+  IN   EFI_PEI_LBA                       StartLBA,
+  IN   UINTN                             BufferSize,
+  OUT  VOID                              *Buffer
+  );
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process.
+  To the PEI legacy floppy driver, it returns the number of all the legacy
+  devices it finds during its enumeration process. If no device is detected,
+  then the function will return zero.
+
+  @param[in]  PeiServices          General-purpose services that are available
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetNumberOfBlockDevices2 (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI    *This,
+  OUT  UINTN                             *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media
+  information. If the media changes, calling this function will update the media
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.
+                            The caller is responsible for the ownership of this
+                            data structure.
+
+  @retval EFI_SUCCESS           Media information about the specified block device
+                                was obtained successfully.
+  @retval EFI_DEVICE_ERROR      Cannot get the media information due to a hardware
+                                error.
+  @retval Others                Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetBlockDeviceMediaInfo2 (
+  IN   EFI_PEI_SERVICES                     **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI       *This,
+  IN   UINTN                                DeviceIndex,
+  OUT  EFI_PEI_BLOCK_IO2_MEDIA              *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the device
+                            index that was assigned during the enumeration process.
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiReadBlocks2 (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI    *This,
+  IN   UINTN                             DeviceIndex,
+  IN   EFI_PEI_LBA                       StartLBA,
+  IN   UINTN                             BufferSize,
+  OUT  VOID                              *Buffer
+  );
+
+//
+// Internal functions
+//
+
+/**
+  Enumerate Atapi devices.
+
+  This function is used to enumerate Atatpi device in Ide channel.
+
+  @param[in]  AtapiBlkIoDev  A pointer to atapi block IO device
+
+**/
+VOID
+AtapiEnumerateDevices (
+  IN  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev
+  );
+
+/**
+  Detect Atapi devices.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[out] MediaInfo       The media information of the specified block media.
+  @param[out] MediaInfo2      The media information 2 of the specified block media.
+
+  @retval TRUE                Atapi device exists in specified position.
+  @retval FALSE               Atapi device does not exist in specified position.
+
+**/
+BOOLEAN
+DiscoverAtapiDevice (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  OUT EFI_PEI_BLOCK_IO_MEDIA        *MediaInfo,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA       *MediaInfo2
+  );
+
+/**
+  Detect if an IDE controller exists in specified position.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+
+  @retval TRUE         The Atapi device exists.
+  @retval FALSE        The Atapi device does not present.
+
+**/
+BOOLEAN
+DetectIDEController (
+  IN  ATAPI_BLK_IO_DEV   *AtapiBlkIoDev,
+  IN  UINTN              DevicePosition
+  );
+
+/**
+  Wait specified time interval to poll for BSY bit clear in the Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        BSY bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        BSY bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+WaitForBSYClear (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRDY bit set in the Status register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRDY bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRDY bit is not set in the specified time interval.
+
+**/
+EFI_STATUS
+DRDYReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRQ bit clear in the Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+DRQClear (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRQ bit clear in the Alternate Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+DRQClear2 (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRQ bit set in the Status Register.
+
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not set in the specified time interval.
+  @retval EFI_ABORTED        Operation Aborted.
+
+**/
+EFI_STATUS
+DRQReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRQ bit set in the Alternate Status Register.
+
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not set in the specified time interval.
+  @retval EFI_ABORTED        Operation Aborted.
+
+**/
+EFI_STATUS
+DRQReady2 (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Check if there is an error in Status Register.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  StatusReg         The address to IDE IO registers.
+
+  @retval EFI_SUCCESS        Operation success.
+  @retval EFI_DEVICE_ERROR   Device error.
+
+**/
+EFI_STATUS
+CheckErrorStatus (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINT16              StatusReg
+  );
+
+/**
+  Idendify Atapi devices.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+
+  @retval EFI_SUCCESS        Identify successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be identified successfully.
+
+**/
+EFI_STATUS
+ATAPIIdentify (
+  IN  ATAPI_BLK_IO_DEV        *AtapiBlkIoDev,
+  IN  UINTN                   DevicePosition
+  );
+
+/**
+  Sends out ATAPI Test Unit Ready Packet Command to the specified device
+  to find out whether device is accessible.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+
+  @retval EFI_SUCCESS        TestUnit command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed TestUnit command successfully.
+
+**/
+EFI_STATUS
+TestUnitReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition
+  ) ;
+
+/**
+  Send out ATAPI commands conforms to the Packet Command with PIO Data In Protocol.
+  
+  @param[in]  AtapiBlkIoDev         A pointer to atapi block IO device.
+  @param[in]  DevicePosition        An integer to signify device position.
+  @param[in]  Packet                A pointer to ATAPI command packet.
+  @param[in]  Buffer                Buffer to contain requested transfer data from device.
+  @param[in]  ByteCount             Requested transfer data length.
+  @param[in]  TimeoutInMilliSeconds Time out value, in unit of milliseconds.
+
+  @retval EFI_SUCCESS        Command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed command successfully.
+
+**/
+EFI_STATUS
+AtapiPacketCommandIn (
+  IN  ATAPI_BLK_IO_DEV      *AtapiBlkIoDev,
+  IN  UINTN                 DevicePosition,
+  IN  ATAPI_PACKET_COMMAND  *Packet,
+  IN  UINT16                *Buffer,
+  IN  UINT32                ByteCount,
+  IN  UINTN                 TimeoutInMilliSeconds
+  );
+
+/**
+  Sends out ATAPI Inquiry Packet Command to the specified device.
+  This command will return INQUIRY data of the device.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[out] MediaInfo       The media information of the specified block media.
+  @param[out] MediaInfo2      The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS        Command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed command successfully.
+  @retval EFI_UNSUPPORTED    Unsupported device type.
+
+**/
+EFI_STATUS
+Inquiry (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  OUT EFI_PEI_BLOCK_IO_MEDIA        *MediaInfo,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA       *MediaInfo2
+  );
+
+/**  
+  Used before read/write blocks from/to ATAPI device media. 
+  Since ATAPI device media is removable, it is necessary to detect
+  whether media is present and get current present media's information.  
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in, out] MediaInfo     The media information of the specified block media.
+  @param[in, out] MediaInfo2    The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+  @retval EFI_OUT_OF_RESOURCES  Can not allocate required resources.
+
+**/
+EFI_STATUS
+DetectMedia (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  IN OUT EFI_PEI_BLOCK_IO_MEDIA     *MediaInfo,
+  IN OUT EFI_PEI_BLOCK_IO2_MEDIA    *MediaInfo2
+  );
+
+/**  
+  Reset specified Atapi device.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in]  Extensive         If TRUE, use ATA soft reset, otherwise use Atapi soft reset.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ResetDevice (
+  IN  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev,
+  IN  UINTN             DevicePosition,
+  IN  BOOLEAN           Extensive
+  );
+
+/**  
+  Sends out ATAPI Request Sense Packet Command to the specified device.
+
+  @param[in]      AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]      DevicePosition  An integer to signify device position.
+  @param[in]      SenseBuffers    Pointer to sense buffer.
+  @param[in, out] SenseCounts     Length of sense buffer.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+RequestSense (
+  IN  ATAPI_BLK_IO_DEV          *AtapiBlkIoDev,
+  IN  UINTN                     DevicePosition,
+  IN  ATAPI_REQUEST_SENSE_DATA  *SenseBuffers,
+  IN OUT  UINT8                 *SenseCounts
+  );
+
+/**  
+  Sends out ATAPI Read Capacity Packet Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[in, out] MediaInfo   The media information of the specified block media.
+  @param[in, out] MediaInfo2  The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ReadCapacity (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  IN OUT EFI_PEI_BLOCK_IO_MEDIA     *MediaInfo,
+  IN OUT EFI_PEI_BLOCK_IO2_MEDIA    *MediaInfo2
+  );
+
+/**  
+  Perform read from disk in block unit.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[in]  Buffer          Buffer to contain read data.
+  @param[in]  StartLba        Starting LBA address.
+  @param[in]  NumberOfBlocks  Number of blocks to read.
+  @param[in]  BlockSize       Size of each block.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ReadSectors (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition,
+  IN  VOID                *Buffer,
+  IN  EFI_PEI_LBA         StartLba,
+  IN  UINTN               NumberOfBlocks,
+  IN  UINTN               BlockSize
+  );
+
+/**  
+  Check if there is media according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    No media
+  @retval FALSE   Media exists
+
+**/
+BOOLEAN
+IsNoMedia (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if device state is unclear according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    Device state is unclear
+  @retval FALSE   Device state is clear  
+
+**/
+BOOLEAN
+IsDeviceStateUnclear (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if there is media error according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    Media error
+  @retval FALSE   No media error
+
+**/
+BOOLEAN
+IsMediaError (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if drive is ready according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+  @param[out] NeedRetry   Indicate if retry is needed.
+
+  @retval TRUE    Drive ready
+  @retval FALSE   Drive not ready
+
+**/
+BOOLEAN
+IsDriveReady (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts,
+  OUT BOOLEAN               *NeedRetry
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.inf b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.inf
new file mode 100644
index 0000000000..8bcef7bc6a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.inf
@@ -0,0 +1,69 @@
+## @file
+# PEIM to produce gEfiPeiVirtualBlockIoPpiGuid PPI for ATA controllers in the platform.
+# This PPI can be consumed by PEIM which produce gEfiPeiDeviceRecoveryModulePpiGuid
+# for Atapi CD ROM device.
+#
+# This module discovers CDROM devices in Legacy and native mode and installs block IO ppis for them.
+# Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions
+# of the BSD License which accompanies this distribution.  The
+# full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = IdeBusPei
+  MODULE_UNI_FILE                = IdeBusPei.uni
+  FILE_GUID                      = B7A5041A-78BA-49e3-B73B-54C757811FB6
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = AtapiPeimEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  AtapiPeim.h
+  AtapiPeim.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  IoLib
+  BaseMemoryLib
+  PeiServicesLib
+  PeimEntryPoint
+  DebugLib
+  TimerLib
+  PeiServicesTablePointerLib
+  MemoryAllocationLib
+  PcdLib
+
+[Ppis]
+  gPeiAtaControllerPpiGuid                      ## CONSUMES
+  gEfiPeiVirtualBlockIoPpiGuid                  ## PRODUCES
+  gEfiPeiVirtualBlockIo2PpiGuid                 ## PRODUCES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSataSpinUpDelayInSecForRecoveryPath   ## CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid AND gPeiAtaControllerPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  IdeBusPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.uni b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.uni
new file mode 100644
index 0000000000..75ab58e15d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPeiExtra.uni
new file mode 100644
index 0000000000..ec4663cf91
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.c b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.c
new file mode 100644
index 0000000000..3d581b69fd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.c
@@ -0,0 +1,387 @@
+/** @file
+  This module is one template module for Incompatible PCI Device Support protocol.
+  It includes one incompatile pci devices list template.
+  
+  Incompatible PCI Device Support protocol allows the PCI bus driver to support
+  resource allocation for some PCI devices that do not comply with the PCI Specification.
+
+Copyright (c) 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <PiDxe.h>
+#include <Protocol/IncompatiblePciDeviceSupport.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DebugLib.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/Acpi.h>
+
+typedef struct {
+  UINT64  VendorId;
+  UINT64  DeviceId;
+  UINT64  RevisionId;
+  UINT64  SubsystemVendorId;
+  UINT64  SubsystemDeviceId;
+} EFI_PCI_DEVICE_HEADER_INFO;
+
+typedef struct {
+  UINT64  ResType;
+  UINT64  GenFlag;
+  UINT64  SpecificFlag;
+  UINT64  AddrSpaceGranularity;
+  UINT64  AddrRangeMin;
+  UINT64  AddrRangeMax;
+  UINT64  AddrTranslationOffset;
+  UINT64  AddrLen;
+} EFI_PCI_RESOUCE_DESCRIPTOR;
+
+#define PCI_DEVICE_ID(VendorId, DeviceId, Revision, SubVendorId, SubDeviceId) \
+    VendorId, DeviceId, Revision, SubVendorId, SubDeviceId
+
+#define PCI_BAR_TYPE_IO   ACPI_ADDRESS_SPACE_TYPE_IO
+#define PCI_BAR_TYPE_MEM  ACPI_ADDRESS_SPACE_TYPE_MEM
+
+#define DEVICE_INF_TAG    0xFFF2
+#define DEVICE_RES_TAG    0xFFF1
+#define LIST_END_TAG      0x0000
+
+
+/**
+  Returns a list of ACPI resource descriptors that detail the special
+  resource configuration requirements for an incompatible PCI device.
+
+  @param  This                  Pointer to the EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL instance.
+  @param  VendorId              A unique ID to identify the manufacturer of the PCI device.
+  @param  DeviceId              A unique ID to identify the particular PCI device.
+  @param  RevisionId            A PCI device-specific revision identifier.
+  @param  SubsystemVendorId     Specifies the subsystem vendor ID.
+  @param  SubsystemDeviceId     Specifies the subsystem device ID.
+  @param  Configuration         A list of ACPI resource descriptors returned that detail
+                                the configuration requirement.
+
+  @retval EFI_SUCCESS           Successfully got ACPI resource for specified PCI device.
+  @retval EFI_INVALID_PARAMETER Configuration is NULL. 
+  @retval EFI_OUT_OF_RESOURCES  No memory available.
+  @retval EFI_UNSUPPORTED       The specified PCI device wasn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+PCheckDevice (
+  IN  EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL  *This,
+  IN  UINTN                                         VendorId,
+  IN  UINTN                                         DeviceId,
+  IN  UINTN                                         RevisionId,
+  IN  UINTN                                         SubsystemVendorId,
+  IN  UINTN                                         SubsystemDeviceId,
+  OUT VOID                                          **Configuration
+  );
+
+//
+// Handle onto which the Incompatible PCI Device List is installed
+//
+EFI_HANDLE                                    mIncompatiblePciDeviceSupportHandle = NULL;
+
+//
+// The Incompatible PCI Device Support Protocol instance produced by this driver
+//
+EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL  mIncompatiblePciDeviceSupport = {
+  PCheckDevice
+};
+
+//
+// The incompatible PCI devices list template
+//
+GLOBAL_REMOVE_IF_UNREFERENCED UINT64 mIncompatiblePciDeviceList[] = {
+  //
+  // DEVICE_INF_TAG,
+  // PCI_DEVICE_ID (VendorID, DeviceID, Revision, SubVendorId, SubDeviceId),
+  // DEVICE_RES_TAG,
+  // ResType,  GFlag , SFlag,   Granularity,  RangeMin,
+  // RangeMax, Offset, AddrLen
+  //
+  //
+  // Device Adaptec 9004
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x9004, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // Device Adaptec 9005
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x9005, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // Device QLogic  1007
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x1077, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // Device Agilent 103C
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x103C, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // Device Agilent 15BC
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x15BC, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // The end of the list
+  //
+  LIST_END_TAG
+};
+
+
+/**
+  Entry point of the incompatible pci device support code. Setup an incompatible device list template
+  and install EFI Incompatible PCI Device Support protocol.
+
+  @param ImageHandle             A handle for the image that is initializing this driver.
+  @param SystemTable             A pointer to the EFI system table.
+
+  @retval EFI_SUCCESS            Installed EFI Incompatible PCI Device Support Protocol successfully.
+  @retval others                 Failed to install protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+IncompatiblePciDeviceSupportEntryPoint (
+  IN EFI_HANDLE                   ImageHandle,
+  IN EFI_SYSTEM_TABLE             *SystemTable
+  )
+{
+  EFI_STATUS                         Status;
+
+  //
+  // Install EFI Incompatible PCI Device Support Protocol on a new handle
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mIncompatiblePciDeviceSupportHandle,
+                  &gEfiIncompatiblePciDeviceSupportProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mIncompatiblePciDeviceSupport
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Returns a list of ACPI resource descriptors that detail the special
+  resource configuration requirements for an incompatible PCI device.
+
+  @param  This                  Pointer to the EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL instance.
+  @param  VendorId              A unique ID to identify the manufacturer of the PCI device.
+  @param  DeviceId              A unique ID to identify the particular PCI device.
+  @param  RevisionId            A PCI device-specific revision identifier.
+  @param  SubsystemVendorId     Specifies the subsystem vendor ID.
+  @param  SubsystemDeviceId     Specifies the subsystem device ID.
+  @param  Configuration         A list of ACPI resource descriptors returned that detail
+                                the configuration requirement.
+
+  @retval EFI_SUCCESS           Successfully got ACPI resource for specified PCI device.
+  @retval EFI_INVALID_PARAMETER Configuration is NULL. 
+  @retval EFI_OUT_OF_RESOURCES  No memory available.
+  @retval EFI_UNSUPPORTED       The specified PCI device wasn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+PCheckDevice (
+  IN  EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL  *This,
+  IN  UINTN                                         VendorId,
+  IN  UINTN                                         DeviceId,
+  IN  UINTN                                         RevisionId,
+  IN  UINTN                                         SubsystemVendorId,
+  IN  UINTN                                         SubsystemDeviceId,
+  OUT VOID                                          **Configuration
+  )
+{
+  UINT64                            Tag;
+  UINT64                            *ListPtr;
+  UINT64                            *TempListPtr;
+  EFI_PCI_DEVICE_HEADER_INFO        *Header;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *AcpiPtr;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *OldAcpiPtr;
+  EFI_PCI_RESOUCE_DESCRIPTOR        *Dsc;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *PtrEnd;
+  UINTN                             Index;
+
+  //
+  // Validate the parameters
+  //
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Initialize the return value to NULL
+  //
+  * (VOID **) Configuration = NULL;
+
+  ListPtr                   = mIncompatiblePciDeviceList;
+  while (*ListPtr != LIST_END_TAG) {
+
+    Tag = *ListPtr;
+
+    switch (Tag) {
+    case DEVICE_INF_TAG:
+      Header  = (EFI_PCI_DEVICE_HEADER_INFO *) (ListPtr + 1);
+      ListPtr = ListPtr + 1 + sizeof (EFI_PCI_DEVICE_HEADER_INFO) / sizeof (UINT64);
+      //
+      // See if the Header matches the parameters passed in
+      //
+      if (Header->VendorId != DEVICE_ID_NOCARE) {
+        if (Header->VendorId != VendorId) {
+          continue;
+        }
+      }
+
+      if (Header->DeviceId != DEVICE_ID_NOCARE) {
+        if (DeviceId != Header->DeviceId) {
+          continue;
+        }
+      }
+
+      if (Header->RevisionId != DEVICE_ID_NOCARE) {
+        if (RevisionId != Header->RevisionId) {
+          continue;
+        }
+      }
+
+      if (Header->SubsystemVendorId != DEVICE_ID_NOCARE) {
+        if (SubsystemVendorId != Header->SubsystemVendorId) {
+          continue;
+        }
+      }
+
+      if (Header->SubsystemDeviceId != DEVICE_ID_NOCARE) {
+        if (SubsystemDeviceId != Header->SubsystemDeviceId) {
+          continue;
+        }
+      }
+      //
+      // Matched an item, so construct the ACPI descriptor for the resource.
+      //
+      //
+      // Count the resource items so that to allocate space
+      //
+      for (Index = 0, TempListPtr = ListPtr; *TempListPtr == DEVICE_RES_TAG; Index++) {
+        TempListPtr = TempListPtr + 1 + ((sizeof (EFI_PCI_RESOUCE_DESCRIPTOR)) / sizeof (UINT64));
+      }
+      //
+      // If there is at least one type of resource request,
+      // allocate an acpi resource node
+      //
+      if (Index == 0) {
+        return EFI_UNSUPPORTED;
+      }
+
+      AcpiPtr = AllocateZeroPool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) * Index + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+      if (AcpiPtr == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      OldAcpiPtr = AcpiPtr;
+      //
+      // Fill the EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR structure
+      // according to the EFI_PCI_RESOUCE_DESCRIPTOR structure
+      //
+      for (; *ListPtr == DEVICE_RES_TAG;) {
+
+        Dsc = (EFI_PCI_RESOUCE_DESCRIPTOR *) (ListPtr + 1);
+
+        AcpiPtr->Desc = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+        AcpiPtr->Len = (UINT16) sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR);
+        AcpiPtr->ResType = (UINT8) Dsc->ResType;
+        AcpiPtr->GenFlag = (UINT8) Dsc->GenFlag;
+        AcpiPtr->SpecificFlag = (UINT8) Dsc->SpecificFlag;
+        AcpiPtr->AddrSpaceGranularity = Dsc->AddrSpaceGranularity;;
+        AcpiPtr->AddrRangeMin = Dsc->AddrRangeMin;
+        AcpiPtr->AddrRangeMax = Dsc->AddrRangeMax;
+        AcpiPtr->AddrTranslationOffset = Dsc->AddrTranslationOffset;
+        AcpiPtr->AddrLen = Dsc->AddrLen;
+
+        ListPtr = ListPtr + 1 + ((sizeof (EFI_PCI_RESOUCE_DESCRIPTOR)) / sizeof (UINT64));
+        AcpiPtr++;
+      }
+      //
+      // Put the checksum
+      //
+      PtrEnd                    = (EFI_ACPI_END_TAG_DESCRIPTOR *) (AcpiPtr);
+      PtrEnd->Desc              = ACPI_END_TAG_DESCRIPTOR;
+      PtrEnd->Checksum          = 0;
+
+      *(VOID **) Configuration  = OldAcpiPtr;
+
+      return EFI_SUCCESS;
+
+    case DEVICE_RES_TAG:
+      //
+      // Adjust the pointer to the next PCI resource descriptor item
+      //
+      ListPtr = ListPtr + 1 + ((sizeof (EFI_PCI_RESOUCE_DESCRIPTOR)) / sizeof (UINT64));
+      break;
+
+    default:
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.uni b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.uni
new file mode 100644
index 0000000000..46732f797a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportDxe.inf b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportDxe.inf
new file mode 100644
index 0000000000..a83edaec7b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportDxe.inf
@@ -0,0 +1,53 @@
+## @file
+#  PCI Incompatible device support module template.
+#
+#  Installs EFI PCI Incompatible Device Support protocol and includes one incompatile 
+#  pci devices list template.
+#
+#  Copyright (c) 2009 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = IncompatiblePciDeviceSupport
+  MODULE_UNI_FILE                = IncompatiblePciDeviceSupport.uni
+  FILE_GUID                      = AD70855E-0CC5-4abf-8979-BE762A949EA3
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = IncompatiblePciDeviceSupportEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  IncompatiblePciDeviceSupport.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  DebugLib
+
+[Protocols]
+  gEfiIncompatiblePciDeviceSupportProtocolGuid    ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  IncompatiblePciDeviceSupportExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportExtra.uni b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportExtra.uni
new file mode 100644
index 0000000000..0a3aaac3da
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/ComponentName.c
new file mode 100644
index 0000000000..60849aea03
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/ComponentName.c
@@ -0,0 +1,233 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013, 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 "NvmExpress.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL gNvmExpressComponentName = {
+  NvmExpressComponentNameGetDriverName,
+  NvmExpressComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gNvmExpressComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) NvmExpressComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) NvmExpressComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mNvmExpressDriverNameTable[] = {
+  { "eng;en", L"NVM Express Driver" },
+  { NULL, NULL }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mNvmExpressControllerNameTable[] = {
+  { "eng;en", L"NVM Express Controller" },
+  { NULL, NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL       *This,
+  IN  CHAR8                             *Language,
+  OUT CHAR16                            **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mNvmExpressDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gNvmExpressComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_BLOCK_IO_PROTOCOL               *BlockIo;
+  NVME_DEVICE_PRIVATE_DATA            *Device;
+  EFI_UNICODE_STRING_TABLE            *ControllerNameTable;
+
+  //
+  // Make sure this driver is currently managing ControllHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gNvmExpressDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ControllerNameTable = mNvmExpressControllerNameTable;
+  if (ChildHandle != NULL) {
+    Status = EfiTestChildHandle (
+               ControllerHandle,
+               ChildHandle,
+               &gEfiPciIoProtocolGuid
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Get the child context
+    //
+    Status = gBS->OpenProtocol (
+                    ChildHandle,
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlockIo,
+                    gNvmExpressDriverBinding.DriverBindingHandle,
+                    ChildHandle,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+    Device = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (BlockIo);
+    ControllerNameTable = Device->ControllerNameTable;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gNvmExpressComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.c
new file mode 100644
index 0000000000..9938bf4541
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.c
@@ -0,0 +1,1138 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, 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 "NvmExpress.h"
+
+//
+// NVM Express Driver Binding Protocol Instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gNvmExpressDriverBinding = {
+  NvmExpressDriverBindingSupported,
+  NvmExpressDriverBindingStart,
+  NvmExpressDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// NVM Express EFI Driver Supported EFI Version Protocol Instance
+//
+EFI_DRIVER_SUPPORTED_EFI_VERSION_PROTOCOL gNvmExpressDriverSupportedEfiVersion = {
+  sizeof (EFI_DRIVER_SUPPORTED_EFI_VERSION_PROTOCOL), // Size of Protocol structure.
+  0                                                   // Version number to be filled at start up.
+};
+
+//
+// Template for NVM Express Pass Thru Mode data structure.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_NVM_EXPRESS_PASS_THRU_MODE gEfiNvmExpressPassThruMode = {
+  EFI_NVM_EXPRESS_PASS_THRU_ATTRIBUTES_PHYSICAL | EFI_NVM_EXPRESS_PASS_THRU_ATTRIBUTES_LOGICAL | EFI_NVM_EXPRESS_PASS_THRU_ATTRIBUTES_CMD_SET_NVM,
+  sizeof (UINTN),
+  0x10100
+};
+
+/**
+  Check if the specified Nvm Express device namespace is active, and create child handles
+  for them with BlockIo and DiskInfo protocol instances.
+
+  @param[in] Private         The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param[in] NamespaceId     The NVM Express namespace ID  for which a device path node is to be
+                             allocated and built. Caller must set the NamespaceId to zero if the
+                             device path node will contain a valid UUID.
+
+  @retval EFI_SUCCESS        All the namespaces in the device are successfully enumerated.
+  @return Others             Some error occurs when enumerating the namespaces.
+
+**/
+EFI_STATUS
+EnumerateNvmeDevNamespace (
+  IN NVME_CONTROLLER_PRIVATE_DATA       *Private,
+  UINT32                                NamespaceId
+  )
+{
+  NVME_ADMIN_NAMESPACE_DATA             *NamespaceData;
+  EFI_DEVICE_PATH_PROTOCOL              *NewDevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+  EFI_HANDLE                            DeviceHandle;
+  EFI_DEVICE_PATH_PROTOCOL              *ParentDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL              *RemainingDevicePath;
+  NVME_DEVICE_PRIVATE_DATA              *Device;
+  EFI_STATUS                            Status;
+  UINT32                                Lbads;
+  UINT32                                Flbas;
+  UINT32                                LbaFmtIdx;
+
+  NewDevicePathNode = NULL;
+  DevicePath        = NULL;
+  Device            = NULL;
+
+  //
+  // Allocate a buffer for Identify Namespace data
+  //
+  NamespaceData = AllocateZeroPool(sizeof (NVME_ADMIN_NAMESPACE_DATA));
+  if(NamespaceData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ParentDevicePath = Private->ParentDevicePath;
+  //
+  // Identify Namespace
+  //
+  Status = NvmeIdentifyNamespace (
+             Private,
+             NamespaceId,
+             (VOID *)NamespaceData
+             );
+  if (EFI_ERROR(Status)) {
+    goto Exit;
+  }
+  //
+  // Validate Namespace
+  //
+  if (NamespaceData->Ncap == 0) {
+    Status = EFI_DEVICE_ERROR;
+  } else {
+    //
+    // allocate device private data for each discovered namespace
+    //
+    Device = AllocateZeroPool(sizeof(NVME_DEVICE_PRIVATE_DATA));
+    if (Device == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    //
+    // Initialize SSD namespace instance data
+    //
+    Device->Signature           = NVME_DEVICE_PRIVATE_DATA_SIGNATURE;
+    Device->NamespaceId         = NamespaceId;
+    Device->NamespaceUuid       = NamespaceData->Eui64;
+
+    Device->ControllerHandle    = Private->ControllerHandle;
+    Device->DriverBindingHandle = Private->DriverBindingHandle;
+    Device->Controller          = Private;
+
+    //
+    // Build BlockIo media structure
+    //
+    Device->Media.MediaId        = 0;
+    Device->Media.RemovableMedia = FALSE;
+    Device->Media.MediaPresent   = TRUE;
+    Device->Media.LogicalPartition = FALSE;
+    Device->Media.ReadOnly       = FALSE;
+    Device->Media.WriteCaching   = FALSE;
+
+    Flbas     = NamespaceData->Flbas;
+    LbaFmtIdx = Flbas & 0xF;
+    Lbads     = NamespaceData->LbaFormat[LbaFmtIdx].Lbads;
+    Device->Media.BlockSize = (UINT32)1 << Lbads;
+
+    Device->Media.LastBlock                     = NamespaceData->Nsze - 1;
+    Device->Media.LogicalBlocksPerPhysicalBlock = 1;
+    Device->Media.LowestAlignedLba              = 1;
+
+    //
+    // Create BlockIo Protocol instance
+    //
+    Device->BlockIo.Revision     = EFI_BLOCK_IO_PROTOCOL_REVISION2;
+    Device->BlockIo.Media        = &Device->Media;
+    Device->BlockIo.Reset        = NvmeBlockIoReset;
+    Device->BlockIo.ReadBlocks   = NvmeBlockIoReadBlocks;
+    Device->BlockIo.WriteBlocks  = NvmeBlockIoWriteBlocks;
+    Device->BlockIo.FlushBlocks  = NvmeBlockIoFlushBlocks;
+
+    //
+    // Create StorageSecurityProtocol Instance
+    //
+    Device->StorageSecurity.ReceiveData = NvmeStorageSecurityReceiveData;
+    Device->StorageSecurity.SendData    = NvmeStorageSecuritySendData;
+
+    //
+    // Create DiskInfo Protocol instance
+    //
+    InitializeDiskInfo (Device);
+
+    //
+    // Create a Nvm Express Namespace Device Path Node
+    //
+    Status = Private->Passthru.BuildDevicePath (
+                                 &Private->Passthru,
+                                 Device->NamespaceId,
+                                 &NewDevicePathNode
+                                 );
+
+    if (EFI_ERROR(Status)) {
+      goto Exit;
+    }
+
+    //
+    // Append the SSD node to the controller's device path
+    //
+    DevicePath = AppendDevicePathNode (ParentDevicePath, NewDevicePathNode);
+    if (DevicePath == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    DeviceHandle = NULL;
+    RemainingDevicePath = DevicePath;
+    Status = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &RemainingDevicePath, &DeviceHandle);
+    if (!EFI_ERROR (Status) && (DeviceHandle != NULL) && IsDevicePathEnd(RemainingDevicePath)) {
+      Status = EFI_ALREADY_STARTED;
+      FreePool (DevicePath);
+      goto Exit;
+    }
+
+    Device->DevicePath = DevicePath;
+
+    //
+    // Make sure the handle is NULL so we create a new handle
+    //
+    Device->DeviceHandle = NULL;
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Device->DeviceHandle,
+                    &gEfiDevicePathProtocolGuid,
+                    Device->DevicePath,
+                    &gEfiBlockIoProtocolGuid,
+                    &Device->BlockIo,
+                    &gEfiDiskInfoProtocolGuid,
+                    &Device->DiskInfo,
+                    NULL
+                    );
+
+    if(EFI_ERROR(Status)) {
+      goto Exit;
+    }
+
+    //
+    // Check if the NVMe controller supports the Security Send and Security Receive commands
+    //
+    if ((Private->ControllerData->Oacs & SECURITY_SEND_RECEIVE_SUPPORTED) != 0) {
+      Status = gBS->InstallProtocolInterface (
+                      &Device->DeviceHandle,
+                      &gEfiStorageSecurityCommandProtocolGuid,
+                      EFI_NATIVE_INTERFACE,
+                      &Device->StorageSecurity
+                      );
+      if(EFI_ERROR(Status)) {
+        gBS->UninstallMultipleProtocolInterfaces (
+               &Device->DeviceHandle,
+               &gEfiDevicePathProtocolGuid,
+               Device->DevicePath,
+               &gEfiBlockIoProtocolGuid,
+               &Device->BlockIo,
+               &gEfiDiskInfoProtocolGuid,
+               &Device->DiskInfo,
+               NULL
+               );
+        goto Exit;
+      }
+    }
+
+    gBS->OpenProtocol (
+           Private->ControllerHandle,
+           &gEfiNvmExpressPassThruProtocolGuid,
+           (VOID **) &Private->Passthru,
+           Private->DriverBindingHandle,
+           Device->DeviceHandle,
+           EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+           );
+
+    //
+    // Dump NvmExpress Identify Namespace Data
+    //
+    DEBUG ((EFI_D_INFO, " == NVME IDENTIFY NAMESPACE [%d] DATA ==\n", NamespaceId));
+    DEBUG ((EFI_D_INFO, "    NSZE        : 0x%x\n", NamespaceData->Nsze));
+    DEBUG ((EFI_D_INFO, "    NCAP        : 0x%x\n", NamespaceData->Ncap));
+    DEBUG ((EFI_D_INFO, "    NUSE        : 0x%x\n", NamespaceData->Nuse));
+    DEBUG ((EFI_D_INFO, "    LBAF0.LBADS : 0x%x\n", (NamespaceData->LbaFormat[0].Lbads)));
+
+    //
+    // Build controller name for Component Name (2) protocol.
+    //
+    UnicodeSPrintAsciiFormat (Device->ModelName, sizeof (Device->ModelName), "%a-%a-%x", Private->ControllerData->Sn, Private->ControllerData->Mn, NamespaceData->Eui64);
+
+    AddUnicodeString2 (
+      "eng",
+      gNvmExpressComponentName.SupportedLanguages,
+      &Device->ControllerNameTable,
+      Device->ModelName,
+      TRUE
+      );
+
+    AddUnicodeString2 (
+      "en",
+      gNvmExpressComponentName2.SupportedLanguages,
+      &Device->ControllerNameTable,
+      Device->ModelName,
+      FALSE
+      );
+  }
+
+Exit:
+  if(NamespaceData != NULL) {
+    FreePool (NamespaceData);
+  }
+
+  if (NewDevicePathNode != NULL) {
+    FreePool (NewDevicePathNode);
+  }
+
+  if(EFI_ERROR(Status) && (Device != NULL) && (Device->DevicePath != NULL)) {
+    FreePool (Device->DevicePath);
+  }
+  if(EFI_ERROR(Status) && (Device != NULL)) {
+    FreePool (Device);
+  }
+  return Status;
+}
+
+/**
+  Discover all Nvm Express device namespaces, and create child handles for them with BlockIo
+  and DiskInfo protocol instances.
+
+  @param[in] Private         The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS        All the namespaces in the device are successfully enumerated.
+  @return Others             Some error occurs when enumerating the namespaces.
+
+**/
+EFI_STATUS
+DiscoverAllNamespaces (
+  IN NVME_CONTROLLER_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS                            Status;
+  UINT32                                NamespaceId;
+  EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL    *Passthru;
+
+  NamespaceId   = 0xFFFFFFFF;
+  Passthru      = &Private->Passthru;
+
+  while (TRUE) {
+    Status = Passthru->GetNextNamespace (
+                         Passthru,
+                         (UINT32 *)&NamespaceId
+                         );
+
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    Status = EnumerateNvmeDevNamespace (
+               Private,
+               NamespaceId
+               );
+
+    if (EFI_ERROR(Status)) {
+      continue;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Unregisters a Nvm Express device namespace.
+
+  This function removes the protocols installed on the controller handle and
+  frees the resources allocated for the namespace.
+
+  @param  This                  The pointer to EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The controller handle of the namespace.
+  @param  Handle                The child handle.
+
+  @retval EFI_SUCCESS           The namespace is successfully unregistered.
+  @return Others                Some error occurs when unregistering the namespace.
+
+**/
+EFI_STATUS
+UnregisterNvmeNamespace (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  )
+{
+  EFI_STATUS                               Status;
+  EFI_BLOCK_IO_PROTOCOL                    *BlockIo;
+  NVME_DEVICE_PRIVATE_DATA                 *Device;
+  NVME_CONTROLLER_PRIVATE_DATA             *Private;
+  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *StorageSecurity;
+
+  BlockIo = NULL;
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlockIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Device  = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (BlockIo);
+  Private = Device->Controller;
+
+  //
+  // Close the child handle
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiNvmExpressPassThruProtocolGuid,
+         This->DriverBindingHandle,
+         Handle
+         );
+
+  //
+  // The Nvm Express driver installs the BlockIo and DiskInfo in the DriverBindingStart().
+  // Here should uninstall both of them.
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  Device->DevicePath,
+                  &gEfiBlockIoProtocolGuid,
+                  &Device->BlockIo,
+                  &gEfiDiskInfoProtocolGuid,
+                  &Device->DiskInfo,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->OpenProtocol (
+           Controller,
+           &gEfiNvmExpressPassThruProtocolGuid,
+           (VOID **) &Private->Passthru,
+           This->DriverBindingHandle,
+           Handle,
+           EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+           );
+    return Status;
+  }
+
+  //
+  // If Storage Security Command Protocol is installed, then uninstall this protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiStorageSecurityCommandProtocolGuid,
+                  (VOID **) &StorageSecurity,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->UninstallProtocolInterface (
+                    Handle,
+                    &gEfiStorageSecurityCommandProtocolGuid,
+                    &Device->StorageSecurity
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+        Controller,
+        &gEfiNvmExpressPassThruProtocolGuid,
+        (VOID **) &Private->Passthru,
+        This->DriverBindingHandle,
+        Handle,
+        EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+        );
+      return Status;
+    }
+  }
+
+  if(Device->DevicePath != NULL) {
+    FreePool (Device->DevicePath);
+  }
+
+  if (Device->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (Device->ControllerNameTable);
+  }
+
+  FreePool (Device);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEV_PATH_PTR          DevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  UINT8                     ClassCode[3];
+
+  //
+  // Check whether device path is valid
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node,
+    // if yes, go on checking other conditions
+    //
+    if (!IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node,
+      // check its validation
+      //
+      DevicePathNode.DevPath = RemainingDevicePath;
+
+      if ((DevicePathNode.DevPath->Type    != MESSAGING_DEVICE_PATH) ||
+          (DevicePathNode.DevPath->SubType != MSG_NVME_NAMESPACE_DP) ||
+          (DevicePathNodeLength(DevicePathNode.DevPath) != sizeof(NVME_NAMESPACE_DEVICE_PATH))) {
+         return EFI_UNSUPPORTED;
+      }
+    }
+  }
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close protocol, don't use device path protocol in the Support() function
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiDevicePathProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  //
+  // Attempt to Open PCI I/O Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Now further check the PCI header: Base class (offset 0x0B) and Sub Class (offset 0x0A).
+  // This controller should be a Nvm Express controller.
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (ClassCode),
+                        ClassCode
+                        );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Examine Nvm Express controller PCI Configuration table fields
+  //
+  if ((ClassCode[0] != PCI_IF_NVMHCI) || (ClassCode[1] != PCI_CLASS_MASS_STORAGE_NVM) || (ClassCode[2] != PCI_CLASS_MASS_STORAGE)) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+Done:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_PCI_IO_PROTOCOL                 *PciIo;
+  NVME_CONTROLLER_PRIVATE_DATA        *Private;
+  EFI_DEVICE_PATH_PROTOCOL            *ParentDevicePath;
+  UINT32                              NamespaceId;
+  EFI_PHYSICAL_ADDRESS                MappedAddr;
+  UINTN                               Bytes;
+  EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL  *Passthru;
+
+  DEBUG ((EFI_D_INFO, "NvmExpressDriverBindingStart: start\n"));
+
+  Private          = NULL;
+  Passthru         = NULL;
+  ParentDevicePath = NULL;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if ((EFI_ERROR (Status)) && (Status != EFI_ALREADY_STARTED)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    return Status;
+  }
+
+  //
+  // Check EFI_ALREADY_STARTED to reuse the original NVME_CONTROLLER_PRIVATE_DATA.
+  //
+  if (Status != EFI_ALREADY_STARTED) {
+    Private = AllocateZeroPool (sizeof (NVME_CONTROLLER_PRIVATE_DATA));
+
+    if (Private == NULL) {
+      DEBUG ((EFI_D_ERROR, "NvmExpressDriverBindingStart: allocating pool for Nvme Private Data failed!\n"));
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    //
+    // 4 x 4kB aligned buffers will be carved out of this buffer.
+    // 1st 4kB boundary is the start of the admin submission queue.
+    // 2nd 4kB boundary is the start of the admin completion queue.
+    // 3rd 4kB boundary is the start of I/O submission queue #1.
+    // 4th 4kB boundary is the start of I/O completion queue #1.
+    //
+    // Allocate 4 pages of memory, then map it for bus master read and write.
+    //
+    Status = PciIo->AllocateBuffer (
+                      PciIo,
+                      AllocateAnyPages,
+                      EfiBootServicesData,
+                      4,
+                      (VOID**)&Private->Buffer,
+                      0
+                      );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    Bytes = EFI_PAGES_TO_SIZE (4);
+    Status = PciIo->Map (
+                      PciIo,
+                      EfiPciIoOperationBusMasterCommonBuffer,
+                      Private->Buffer,
+                      &Bytes,
+                      &MappedAddr,
+                      &Private->Mapping
+                      );
+
+    if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (4))) {
+      goto Exit;
+    }
+
+    Private->BufferPciAddr = (UINT8 *)(UINTN)MappedAddr;
+    ZeroMem (Private->Buffer, EFI_PAGES_TO_SIZE (4));
+
+    Private->Signature = NVME_CONTROLLER_PRIVATE_DATA_SIGNATURE;
+    Private->ControllerHandle          = Controller;
+    Private->ImageHandle               = This->DriverBindingHandle;
+    Private->DriverBindingHandle       = This->DriverBindingHandle;
+    Private->PciIo                     = PciIo;
+    Private->ParentDevicePath          = ParentDevicePath;
+    Private->Passthru.Mode             = &Private->PassThruMode;
+    Private->Passthru.PassThru         = NvmExpressPassThru;
+    Private->Passthru.GetNextNamespace = NvmExpressGetNextNamespace;
+    Private->Passthru.BuildDevicePath  = NvmExpressBuildDevicePath;
+    Private->Passthru.GetNamespace     = NvmExpressGetNamespace;
+    CopyMem (&Private->PassThruMode, &gEfiNvmExpressPassThruMode, sizeof (EFI_NVM_EXPRESS_PASS_THRU_MODE));
+
+    Status = NvmeControllerInit (Private);
+    if (EFI_ERROR(Status)) {
+      goto Exit;
+    }
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Controller,
+                    &gEfiNvmExpressPassThruProtocolGuid,
+                    &Private->Passthru,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+  } else {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiNvmExpressPassThruProtocolGuid,
+                    (VOID **) &Passthru,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    Private = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (Passthru);
+  }
+
+  if (RemainingDevicePath == NULL) {
+    //
+    // Enumerate all NVME namespaces in the controller
+    //
+    Status = DiscoverAllNamespaces (
+               Private
+               );
+
+  } else if (!IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // Enumerate the specified NVME namespace
+    //
+    Status = Private->Passthru.GetNamespace (
+                                 &Private->Passthru,
+                                 RemainingDevicePath,
+                                 &NamespaceId
+                                 );
+
+    if (!EFI_ERROR (Status)) {
+      Status = EnumerateNvmeDevNamespace (
+                 Private,
+                 NamespaceId
+                 );
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "NvmExpressDriverBindingStart: end successfully\n"));
+  return EFI_SUCCESS;
+
+Exit:
+  if ((Private != NULL) && (Private->Mapping != NULL)) {
+    PciIo->Unmap (PciIo, Private->Mapping);
+  }
+
+  if ((Private != NULL) && (Private->Buffer != NULL)) {
+    PciIo->FreeBuffer (PciIo, 4, Private->Buffer);
+  }
+
+  if (Private != NULL) {
+    FreePool (Private);
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiDevicePathProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  DEBUG ((EFI_D_INFO, "NvmExpressDriverBindingStart: end with %r\n", Status));
+
+  return Status;
+}
+
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                          Status;
+  BOOLEAN                             AllChildrenStopped;
+  UINTN                               Index;
+  NVME_CONTROLLER_PRIVATE_DATA        *Private;
+  EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL  *PassThru;
+
+  if (NumberOfChildren == 0) {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiNvmExpressPassThruProtocolGuid,
+                    (VOID **) &PassThru,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+
+    if (!EFI_ERROR (Status)) {
+      Private = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (PassThru);
+      gBS->UninstallMultipleProtocolInterfaces (
+            Controller,
+            &gEfiNvmExpressPassThruProtocolGuid,
+            PassThru,
+            NULL
+            );
+
+      if (Private->Mapping != NULL) {
+        Private->PciIo->Unmap (Private->PciIo, Private->Mapping);
+      }
+
+      if (Private->Buffer != NULL) {
+        Private->PciIo->FreeBuffer (Private->PciIo, 4, Private->Buffer);
+      }
+
+      FreePool (Private->ControllerData);
+      FreePool (Private);
+    }
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiPciIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+    Status = UnregisterNvmeNamespace (This, Controller, ChildHandleBuffer[Index]);
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This is the unload handle for the NVM Express driver.
+
+  Disconnect the driver specified by ImageHandle from the NVMe device in the handle database.
+  Uninstall all the protocols installed in the driver.
+
+  @param[in]  ImageHandle       The drivers' driver image.
+
+  @retval EFI_SUCCESS           The image is unloaded.
+  @retval Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressUnload (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HANDLE                        *DeviceHandleBuffer;
+  UINTN                             DeviceHandleCount;
+  UINTN                             Index;
+  EFI_COMPONENT_NAME_PROTOCOL       *ComponentName;
+  EFI_COMPONENT_NAME2_PROTOCOL      *ComponentName2;
+
+  //
+  // Get the list of the device handles managed by this driver.
+  // If there is an error getting the list, then means the driver
+  // doesn't manage any device. At this way, we would only close
+  // those protocols installed at image handle.
+  //
+  DeviceHandleBuffer = NULL;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiNvmExpressPassThruProtocolGuid,
+                  NULL,
+                  &DeviceHandleCount,
+                  &DeviceHandleBuffer
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Disconnect the driver specified by ImageHandle from all
+    // the devices in the handle database.
+    //
+    for (Index = 0; Index < DeviceHandleCount; Index++) {
+      Status = gBS->DisconnectController (
+                      DeviceHandleBuffer[Index],
+                      ImageHandle,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        goto EXIT;
+      }
+    }
+  }
+
+  //
+  // Uninstall all the protocols installed in the driver entry point
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  ImageHandle,
+                  &gEfiDriverBindingProtocolGuid,
+                  &gNvmExpressDriverBinding,
+                  &gEfiDriverSupportedEfiVersionProtocolGuid,
+                  &gNvmExpressDriverSupportedEfiVersion,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  //
+  // Note we have to one by one uninstall the following protocols.
+  // It's because some of them are optionally installed based on
+  // the following PCD settings.
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnosticsDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentNameDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnostics2Disable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentName2Disable
+  //
+  Status = gBS->HandleProtocol (
+                  ImageHandle,
+                  &gEfiComponentNameProtocolGuid,
+                  (VOID **) &ComponentName
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           ImageHandle,
+           &gEfiComponentNameProtocolGuid,
+           ComponentName
+           );
+  }
+
+  Status = gBS->HandleProtocol (
+                  ImageHandle,
+                  &gEfiComponentName2ProtocolGuid,
+                  (VOID **) &ComponentName2
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           ImageHandle,
+           &gEfiComponentName2ProtocolGuid,
+           ComponentName2
+           );
+  }
+
+  Status = EFI_SUCCESS;
+
+EXIT:
+  //
+  // Free the buffer containing the list of handles from the handle database
+  //
+  if (DeviceHandleBuffer != NULL) {
+    gBS->FreePool (DeviceHandleBuffer);
+  }
+  return Status;
+}
+
+/**
+  The entry point for Nvm Express driver, used to install Nvm Express driver on the ImageHandle.
+
+  @param  ImageHandle   The firmware allocated handle for this driver image.
+  @param  SystemTable   Pointer to the EFI system table.
+
+  @retval EFI_SUCCESS   Driver loaded.
+  @retval other         Driver not loaded.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverEntry (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gNvmExpressDriverBinding,
+             ImageHandle,
+             &gNvmExpressComponentName,
+             &gNvmExpressComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install EFI Driver Supported EFI Version Protocol required for
+  // EFI drivers that are on PCI and other plug in cards.
+  //
+  gNvmExpressDriverSupportedEfiVersion.FirmwareVersion = 0x00020028;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ImageHandle,
+                  &gEfiDriverSupportedEfiVersionProtocolGuid,
+                  &gNvmExpressDriverSupportedEfiVersion,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.h b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.h
new file mode 100644
index 0000000000..21c6255caa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.h
@@ -0,0 +1,610 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_NVM_EXPRESS_H_
+#define _EFI_NVM_EXPRESS_H_
+
+#include <Uefi.h>
+
+#include <IndustryStandard/Pci.h>
+
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/NvmExpressPassthru.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/DiskInfo.h>
+#include <Protocol/DriverSupportedEfiVersion.h>
+#include <Protocol/StorageSecurityCommand.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+
+typedef struct _NVME_CONTROLLER_PRIVATE_DATA NVME_CONTROLLER_PRIVATE_DATA;
+typedef struct _NVME_DEVICE_PRIVATE_DATA     NVME_DEVICE_PRIVATE_DATA;
+
+#include "NvmExpressBlockIo.h"
+#include "NvmExpressDiskInfo.h"
+#include "NvmExpressHci.h"
+
+extern EFI_DRIVER_BINDING_PROTOCOL                gNvmExpressDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL                gNvmExpressComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL               gNvmExpressComponentName2;
+extern EFI_DRIVER_SUPPORTED_EFI_VERSION_PROTOCOL  gNvmExpressDriverSupportedEfiVersion;
+
+#define PCI_CLASS_MASS_STORAGE_NVM                0x08  // mass storage sub-class non-volatile memory.
+#define PCI_IF_NVMHCI                             0x02  // mass storage programming interface NVMHCI.
+
+#define NVME_ASQ_SIZE                             1     // Number of admin submission queue entries, which is 0-based
+#define NVME_ACQ_SIZE                             1     // Number of admin completion queue entries, which is 0-based
+
+#define NVME_CSQ_SIZE                             1     // Number of I/O submission queue entries, which is 0-based
+#define NVME_CCQ_SIZE                             1     // Number of I/O completion queue entries, which is 0-based
+
+#define NVME_MAX_QUEUES                           2     // Number of queues supported by the driver
+
+#define NVME_CONTROLLER_ID                        0
+
+//
+// Time out value for Nvme transaction execution
+//
+#define NVME_GENERIC_TIMEOUT                      EFI_TIMER_PERIOD_SECONDS (5)
+
+//
+// Unique signature for private data structure.
+//
+#define NVME_CONTROLLER_PRIVATE_DATA_SIGNATURE    SIGNATURE_32 ('N','V','M','E')
+
+//
+// Nvme private data structure.
+//
+struct _NVME_CONTROLLER_PRIVATE_DATA {
+  UINT32                              Signature;
+
+  EFI_HANDLE                          ControllerHandle;
+  EFI_HANDLE                          ImageHandle;
+  EFI_HANDLE                          DriverBindingHandle;
+
+  EFI_PCI_IO_PROTOCOL                 *PciIo;
+  UINT64                              PciAttributes;
+
+  EFI_DEVICE_PATH_PROTOCOL            *ParentDevicePath;
+
+  EFI_NVM_EXPRESS_PASS_THRU_MODE      PassThruMode;
+  EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL  Passthru;
+
+  //
+  // pointer to identify controller data
+  //
+  NVME_ADMIN_CONTROLLER_DATA          *ControllerData;
+
+  //
+  // 6 x 4kB aligned buffers will be carved out of this buffer.
+  // 1st 4kB boundary is the start of the admin submission queue.
+  // 2nd 4kB boundary is the start of the I/O submission queue #1.
+  // 3rd 4kB boundary is the start of the admin completion queue.
+  // 4th 4kB boundary is the start of the I/O completion queue #1.
+  // 5th 4kB boundary is the start of the first PRP list page.
+  // 6th 4kB boundary is the start of the second PRP list page.
+  //
+  UINT8                               *Buffer;
+  UINT8                               *BufferPciAddr;
+
+  //
+  // Pointers to 4kB aligned submission & completion queues.
+  //
+  NVME_SQ                             *SqBuffer[NVME_MAX_QUEUES];
+  NVME_CQ                             *CqBuffer[NVME_MAX_QUEUES];
+  NVME_SQ                             *SqBufferPciAddr[NVME_MAX_QUEUES];
+  NVME_CQ                             *CqBufferPciAddr[NVME_MAX_QUEUES];
+
+  //
+  // Submission and completion queue indices.
+  //
+  NVME_SQTDBL                         SqTdbl[NVME_MAX_QUEUES];
+  NVME_CQHDBL                         CqHdbl[NVME_MAX_QUEUES];
+
+  UINT8                               Pt[NVME_MAX_QUEUES];
+  UINT16                              Cid[NVME_MAX_QUEUES];
+
+  //
+  // Nvme controller capabilities
+  //
+  NVME_CAP                            Cap;
+
+  VOID                                *Mapping;
+};
+
+#define NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU(a) \
+  CR (a, \
+      NVME_CONTROLLER_PRIVATE_DATA, \
+      Passthru, \
+      NVME_CONTROLLER_PRIVATE_DATA_SIGNATURE \
+      )
+
+//
+// Unique signature for private data structure.
+//
+#define NVME_DEVICE_PRIVATE_DATA_SIGNATURE     SIGNATURE_32 ('X','S','S','D')
+
+//
+// Nvme device private data structure
+//
+struct _NVME_DEVICE_PRIVATE_DATA {
+  UINT32                                   Signature;
+
+  EFI_HANDLE                               DeviceHandle;
+  EFI_HANDLE                               ControllerHandle;
+  EFI_HANDLE                               DriverBindingHandle;
+
+  EFI_DEVICE_PATH_PROTOCOL                 *DevicePath;
+
+  EFI_UNICODE_STRING_TABLE                 *ControllerNameTable;
+
+  UINT32                                   NamespaceId;
+  UINT64                                   NamespaceUuid;
+
+  EFI_BLOCK_IO_MEDIA                       Media;
+  EFI_BLOCK_IO_PROTOCOL                    BlockIo;
+  EFI_DISK_INFO_PROTOCOL                   DiskInfo;
+  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    StorageSecurity;
+
+  EFI_LBA                                  NumBlocks;
+
+  CHAR16                                   ModelName[80];
+  NVME_ADMIN_NAMESPACE_DATA                NamespaceData;
+
+  NVME_CONTROLLER_PRIVATE_DATA             *Controller;
+
+};
+
+//
+// Statments to retrieve the private data from produced protocols.
+//
+#define NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO(a) \
+  CR (a, \
+      NVME_DEVICE_PRIVATE_DATA, \
+      BlockIo, \
+      NVME_DEVICE_PRIVATE_DATA_SIGNATURE \
+      )
+
+#define NVME_DEVICE_PRIVATE_DATA_FROM_DISK_INFO(a) \
+  CR (a, \
+      NVME_DEVICE_PRIVATE_DATA, \
+      DiskInfo, \
+      NVME_DEVICE_PRIVATE_DATA_SIGNATURE \
+      )
+
+#define NVME_DEVICE_PRIVATE_DATA_FROM_STORAGE_SECURITY(a)\
+  CR (a,                                                 \
+      NVME_DEVICE_PRIVATE_DATA,                          \
+      StorageSecurity,                                   \
+      NVME_DEVICE_PRIVATE_DATA_SIGNATURE                 \
+      )
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Sends an NVM Express Command Packet to an NVM Express controller or namespace. This function supports
+  both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the nonblocking
+  I/O functionality is optional.
+
+  @param[in]     This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]     NamespaceId         Is a 32 bit Namespace ID to which the Express HCI command packet will be sent.
+                                     A value of 0 denotes the NVM Express controller, a value of all 0FFh in the namespace
+                                     ID specifies that the command packet should be sent to all valid namespaces.
+  @param[in,out] Packet              A pointer to the NVM Express HCI Command Packet to send to the NVMe namespace specified
+                                     by NamespaceId.
+  @param[in]     Event               If nonblocking I/O is not supported then Event is ignored, and blocking I/O is performed.
+                                     If Event is NULL, then blocking I/O is performed. If Event is not NULL and non blocking I/O
+                                     is supported, then nonblocking I/O is performed, and Event will be signaled when the NVM
+                                     Express Command Packet completes.
+
+  @retval EFI_SUCCESS                The NVM Express Command Packet was sent by the host. TransferLength bytes were transferred
+                                     to, or from DataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The NVM Express Command Packet was not executed. The number of bytes that could be transferred
+                                     is returned in TransferLength.
+  @retval EFI_NOT_READY              The NVM Express Command Packet could not be sent because the controller is not ready. The caller
+                                     may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting to send the NVM Express Command Packet.
+  @retval EFI_INVALID_PARAMETER      Namespace, or the contents of EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET are invalid. The NVM
+                                     Express Command Packet was not sent, so no additional status information is available.
+  @retval EFI_UNSUPPORTED            The command described by the NVM Express Command Packet is not supported by the host adapter.
+                                     The NVM Express Command Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT                A timeout occurred while waiting for the NVM Express Command Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressPassThru (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     UINT32                                      NamespaceId,
+  IN OUT EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET    *Packet,
+  IN     EFI_EVENT                                   Event OPTIONAL
+  );
+
+/**
+  Used to retrieve the next namespace ID for this NVM Express controller.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.GetNextNamespace() function retrieves the next valid
+  namespace ID on this NVM Express controller.
+
+  If on input the value pointed to by NamespaceId is 0xFFFFFFFF, then the first valid namespace
+  ID defined on the NVM Express controller is returned in the location pointed to by NamespaceId
+  and a status of EFI_SUCCESS is returned.
+
+  If on input the value pointed to by NamespaceId is an invalid namespace ID other than 0xFFFFFFFF,
+  then EFI_INVALID_PARAMETER is returned.
+
+  If on input the value pointed to by NamespaceId is a valid namespace ID, then the next valid
+  namespace ID on the NVM Express controller is returned in the location pointed to by NamespaceId,
+  and EFI_SUCCESS is returned.
+
+  If the value pointed to by NamespaceId is the namespace ID of the last namespace on the NVM
+  Express controller, then EFI_NOT_FOUND is returned.
+
+  @param[in]     This           A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in,out] NamespaceId    On input, a pointer to a legal NamespaceId for an NVM Express
+                                namespace present on the NVM Express controller. On output, a
+                                pointer to the next NamespaceId of an NVM Express namespace on
+                                an NVM Express controller. An input value of 0xFFFFFFFF retrieves
+                                the first NamespaceId for an NVM Express namespace present on an
+                                NVM Express controller.
+
+  @retval EFI_SUCCESS           The Namespace ID of the next Namespace was returned.
+  @retval EFI_NOT_FOUND         There are no more namespaces defined on this controller.
+  @retval EFI_INVALID_PARAMETER NamespaceId is an invalid value other than 0xFFFFFFFF.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressGetNextNamespace (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN OUT UINT32                                      *NamespaceId
+  );
+
+/**
+  Used to translate a device path node to a namespace ID.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.GetNamespace() function determines the namespace ID associated with the
+  namespace described by DevicePath.
+
+  If DevicePath is a device path node type that the NVM Express Pass Thru driver supports, then the NVM Express
+  Pass Thru driver will attempt to translate the contents DevicePath into a namespace ID.
+
+  If this translation is successful, then that namespace ID is returned in NamespaceId, and EFI_SUCCESS is returned
+
+  @param[in]  This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]  DevicePath          A pointer to the device path node that describes an NVM Express namespace on
+                                  the NVM Express controller.
+  @param[out] NamespaceId         The NVM Express namespace ID contained in the device path node.
+
+  @retval EFI_SUCCESS             DevicePath was successfully translated to NamespaceId.
+  @retval EFI_INVALID_PARAMETER   If DevicePath or NamespaceId are NULL, then EFI_INVALID_PARAMETER is returned.
+  @retval EFI_UNSUPPORTED         If DevicePath is not a device path node type that the NVM Express Pass Thru driver
+                                  supports, then EFI_UNSUPPORTED is returned.
+  @retval EFI_NOT_FOUND           If DevicePath is a device path node type that the NVM Express Pass Thru driver
+                                  supports, but there is not a valid translation from DevicePath to a namespace ID,
+                                  then EFI_NOT_FOUND is returned.
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressGetNamespace (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     EFI_DEVICE_PATH_PROTOCOL                    *DevicePath,
+     OUT UINT32                                      *NamespaceId
+  );
+
+/**
+  Used to allocate and build a device path node for an NVM Express namespace on an NVM Express controller.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.BuildDevicePath() function allocates and builds a single device
+  path node for the NVM Express namespace specified by NamespaceId.
+
+  If the NamespaceId is not valid, then EFI_NOT_FOUND is returned.
+
+  If DevicePath is NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If there are not enough resources to allocate the device path node, then EFI_OUT_OF_RESOURCES is returned.
+
+  Otherwise, DevicePath is allocated with the boot service AllocatePool(), the contents of DevicePath are
+  initialized to describe the NVM Express namespace specified by NamespaceId, and EFI_SUCCESS is returned.
+
+  @param[in]     This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]     NamespaceId         The NVM Express namespace ID  for which a device path node is to be
+                                     allocated and built. Caller must set the NamespaceId to zero if the
+                                     device path node will contain a valid UUID.
+  @param[in,out] DevicePath          A pointer to a single device path node that describes the NVM Express
+                                     namespace specified by NamespaceId. This function is responsible for
+                                     allocating the buffer DevicePath with the boot service AllocatePool().
+                                     It is the caller's responsibility to free DevicePath when the caller
+                                     is finished with DevicePath.
+  @retval EFI_SUCCESS                The device path node that describes the NVM Express namespace specified
+                                     by NamespaceId was allocated and returned in DevicePath.
+  @retval EFI_NOT_FOUND              The NamespaceId is not valid.
+  @retval EFI_INVALID_PARAMETER      DevicePath is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources to allocate the DevicePath node.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressBuildDevicePath (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     UINT32                                      NamespaceId,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL                    **DevicePath
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.c
new file mode 100644
index 0000000000..6cb2f92e8d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.c
@@ -0,0 +1,883 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, 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 "NvmExpress.h"
+
+/**
+  Read some sectors from the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+  @param  Buffer                 The buffer used to store the data read from the device.
+  @param  Lba                    The start block number.
+  @param  Blocks                 Total block number to be read.
+
+  @retval EFI_SUCCESS            Datum are read from the device.
+  @retval Others                 Fail to read all the datum.
+
+**/
+EFI_STATUS
+ReadSectors (
+  IN NVME_DEVICE_PRIVATE_DATA           *Device,
+  IN UINT64                             Buffer,
+  IN UINT64                             Lba,
+  IN UINT32                             Blocks
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA             *Private;
+  UINT32                                   Bytes;
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  UINT32                                   BlockSize;
+
+  Private    = Device->Controller;
+  BlockSize  = Device->Media.BlockSize;
+  Bytes      = Blocks * BlockSize;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  CommandPacket.NvmeCmd->Cdw0.Opcode = NVME_IO_READ_OPC;
+  CommandPacket.NvmeCmd->Nsid        = Device->NamespaceId;
+  CommandPacket.TransferBuffer       = (VOID *)(UINTN)Buffer;
+
+  CommandPacket.TransferLength = Bytes;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_IO_QUEUE;
+
+  CommandPacket.NvmeCmd->Cdw10 = (UINT32)Lba;
+  CommandPacket.NvmeCmd->Cdw11 = (UINT32)(Lba >> 32);
+  CommandPacket.NvmeCmd->Cdw12 = (Blocks - 1) & 0xFFFF;
+
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID | CDW12_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               Device->NamespaceId,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Write some sectors to the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+  @param  Buffer                 The buffer to be written into the device.
+  @param  Lba                    The start block number.
+  @param  Blocks                 Total block number to be written.
+
+  @retval EFI_SUCCESS            Datum are written into the buffer.
+  @retval Others                 Fail to write all the datum.
+
+**/
+EFI_STATUS
+WriteSectors (
+  IN NVME_DEVICE_PRIVATE_DATA      *Device,
+  IN UINT64                        Buffer,
+  IN UINT64                        Lba,
+  IN UINT32                        Blocks
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA             *Private;
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  UINT32                                   Bytes;
+  UINT32                                   BlockSize;
+
+  Private    = Device->Controller;
+  BlockSize  = Device->Media.BlockSize;
+  Bytes      = Blocks * BlockSize;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  CommandPacket.NvmeCmd->Cdw0.Opcode = NVME_IO_WRITE_OPC;
+  CommandPacket.NvmeCmd->Nsid  = Device->NamespaceId;
+  CommandPacket.TransferBuffer = (VOID *)(UINTN)Buffer;
+
+  CommandPacket.TransferLength = Bytes;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_IO_QUEUE;
+
+  CommandPacket.NvmeCmd->Cdw10 = (UINT32)Lba;
+  CommandPacket.NvmeCmd->Cdw11 = (UINT32)(Lba >> 32);
+  CommandPacket.NvmeCmd->Cdw12 = (Blocks - 1) & 0xFFFF;
+
+  CommandPacket.MetadataBuffer = NULL;
+  CommandPacket.MetadataLength = 0;
+
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID | CDW12_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               Device->NamespaceId,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Read some blocks from the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+  @param  Buffer                 The buffer used to store the data read from the device.
+  @param  Lba                    The start block number.
+  @param  Blocks                 Total block number to be read.
+
+  @retval EFI_SUCCESS            Datum are read from the device.
+  @retval Others                 Fail to read all the datum.
+
+**/
+EFI_STATUS
+NvmeRead (
+  IN     NVME_DEVICE_PRIVATE_DATA       *Device,
+     OUT VOID                           *Buffer,
+  IN     UINT64                         Lba,
+  IN     UINTN                          Blocks
+  )
+{
+  EFI_STATUS                       Status;
+  UINT32                           BlockSize;
+  NVME_CONTROLLER_PRIVATE_DATA     *Private;
+  UINT32                           MaxTransferBlocks;
+  UINTN                            OrginalBlocks;
+
+  Status        = EFI_SUCCESS;
+  Private       = Device->Controller;
+  BlockSize     = Device->Media.BlockSize;
+  OrginalBlocks = Blocks;
+
+  if (Private->ControllerData->Mdts != 0) {
+    MaxTransferBlocks = (1 << (Private->ControllerData->Mdts)) * (1 << (Private->Cap.Mpsmin + 12)) / BlockSize;
+  } else {
+    MaxTransferBlocks = 1024;
+  }
+
+  while (Blocks > 0) {
+    if (Blocks > MaxTransferBlocks) {
+      Status = ReadSectors (Device, (UINT64)(UINTN)Buffer, Lba, MaxTransferBlocks);
+
+      Blocks -= MaxTransferBlocks;
+      Buffer  = (VOID *)(UINTN)((UINT64)(UINTN)Buffer + MaxTransferBlocks * BlockSize);
+      Lba    += MaxTransferBlocks;
+    } else {
+      Status = ReadSectors (Device, (UINT64)(UINTN)Buffer, Lba, (UINT32)Blocks);
+      Blocks = 0;
+    }
+
+    if (EFI_ERROR(Status)) {
+      break;
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "NvmeRead()  Lba = 0x%08x, Original = 0x%08x, Remaining = 0x%08x, BlockSize = 0x%x Status = %r\n", Lba, OrginalBlocks, Blocks, BlockSize, Status));
+
+  return Status;
+}
+
+/**
+  Write some blocks to the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+  @param  Buffer                 The buffer to be written into the device.
+  @param  Lba                    The start block number.
+  @param  Blocks                 Total block number to be written.
+
+  @retval EFI_SUCCESS            Datum are written into the buffer.
+  @retval Others                 Fail to write all the datum.
+
+**/
+EFI_STATUS
+NvmeWrite (
+  IN NVME_DEVICE_PRIVATE_DATA           *Device,
+  IN VOID                               *Buffer,
+  IN UINT64                             Lba,
+  IN UINTN                              Blocks
+  )
+{
+  EFI_STATUS                       Status;
+  UINT32                           BlockSize;
+  NVME_CONTROLLER_PRIVATE_DATA     *Private;
+  UINT32                           MaxTransferBlocks;
+  UINTN                            OrginalBlocks;
+
+  Status        = EFI_SUCCESS;
+  Private       = Device->Controller;
+  BlockSize     = Device->Media.BlockSize;
+  OrginalBlocks = Blocks;
+
+  if (Private->ControllerData->Mdts != 0) {
+    MaxTransferBlocks = (1 << (Private->ControllerData->Mdts)) * (1 << (Private->Cap.Mpsmin + 12)) / BlockSize;
+  } else {
+    MaxTransferBlocks = 1024;
+  }
+
+  while (Blocks > 0) {
+    if (Blocks > MaxTransferBlocks) {
+      Status = WriteSectors (Device, (UINT64)(UINTN)Buffer, Lba, MaxTransferBlocks);
+
+      Blocks -= MaxTransferBlocks;
+      Buffer  = (VOID *)(UINTN)((UINT64)(UINTN)Buffer + MaxTransferBlocks * BlockSize);
+      Lba    += MaxTransferBlocks;
+    } else {
+      Status = WriteSectors (Device, (UINT64)(UINTN)Buffer, Lba, (UINT32)Blocks);
+      Blocks = 0;
+    }
+
+    if (EFI_ERROR(Status)) {
+      break;
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "NvmeWrite() Lba = 0x%08x, Original = 0x%08x, Remaining = 0x%08x, BlockSize = 0x%x Status = %r\n", Lba, OrginalBlocks, Blocks, BlockSize, Status));
+
+  return Status;
+}
+
+/**
+  Flushes all modified data to the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS            Datum are written into the buffer.
+  @retval Others                 Fail to write all the datum.
+
+**/
+EFI_STATUS
+NvmeFlush (
+  IN NVME_DEVICE_PRIVATE_DATA      *Device
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA             *Private;
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+
+  Private = Device->Controller;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  CommandPacket.NvmeCmd->Cdw0.Opcode = NVME_IO_FLUSH_OPC;
+  CommandPacket.NvmeCmd->Nsid  = Device->NamespaceId;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_IO_QUEUE;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               Device->NamespaceId,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+
+/**
+  Reset the Block Device.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_TPL                         OldTpl;
+  NVME_CONTROLLER_PRIVATE_DATA    *Private;
+  NVME_DEVICE_PRIVATE_DATA        *Device;
+  EFI_STATUS                      Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // For Nvm Express subsystem, reset block device means reset controller.
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Device  = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (This);
+
+  Private = Device->Controller;
+
+  Status  = NvmeControllerInit (Private);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Lba        The starting Logical Block Address to read from.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the destination buffer for the data. The caller is
+                     responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  NVME_DEVICE_PRIVATE_DATA          *Device;
+  EFI_STATUS                        Status;
+  EFI_BLOCK_IO_MEDIA                *Media;
+  UINTN                             BlockSize;
+  UINTN                             NumberOfBlocks;
+  UINTN                             IoAlign;
+  EFI_TPL                           OldTpl;
+
+  //
+  // Check parameters.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Media = This->Media;
+
+  if (MediaId != Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  BlockSize = Media->BlockSize;
+  if ((BufferSize % BlockSize) != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IoAlign = Media->IoAlign;
+  if (IoAlign > 0 && (((UINTN) Buffer & (IoAlign - 1)) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (This);
+
+  Status = NvmeRead (Device, Buffer, Lba, NumberOfBlocks);
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    The media ID that the write request is for.
+  @param  Lba        The starting logical block address to be written. The caller is
+                     responsible for writing to only legitimate locations.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  )
+{
+  NVME_DEVICE_PRIVATE_DATA          *Device;
+  EFI_STATUS                        Status;
+  EFI_BLOCK_IO_MEDIA                *Media;
+  UINTN                             BlockSize;
+  UINTN                             NumberOfBlocks;
+  UINTN                             IoAlign;
+  EFI_TPL                           OldTpl;
+
+  //
+  // Check parameters.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Media = This->Media;
+
+  if (MediaId != Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  BlockSize = Media->BlockSize;
+  if ((BufferSize % BlockSize) != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IoAlign = Media->IoAlign;
+  if (IoAlign > 0 && (((UINTN) Buffer & (IoAlign - 1)) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (This);
+
+  Status = NvmeWrite (Device, Buffer, Lba, NumberOfBlocks);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Flush the Block Device.
+
+  @param  This              Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device.
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data.
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  )
+{
+  NVME_DEVICE_PRIVATE_DATA          *Device;
+  EFI_STATUS                        Status;
+  EFI_TPL                           OldTpl;
+
+  //
+  // Check parameters.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (This);
+
+  Status = NvmeFlush (Device);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Trust transfer data from/to NVMe device.
+
+  This function performs one NVMe transaction to do a trust transfer from/to NVMe device.
+
+  @param  Private                      The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Buffer                       The pointer to the current transaction buffer.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  TransferLength               The block number or sector count of the transfer.
+  @param  IsTrustSend                  Indicates whether it is a trust send operation or not.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  TransferLengthOut            A pointer to a buffer to store the size in bytes of the data
+                                       written to the buffer. Ignore it when IsTrustSend is TRUE.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+TrustTransferNvmeDevice (
+  IN OUT NVME_CONTROLLER_PRIVATE_DATA      *Private,
+  IN OUT VOID                              *Buffer,
+  IN UINT8                                 SecurityProtocolId,
+  IN UINT16                                SecurityProtocolSpecificData,
+  IN UINTN                                 TransferLength,
+  IN BOOLEAN                               IsTrustSend,
+  IN UINT64                                Timeout,
+  OUT UINTN                                *TransferLengthOut
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  UINT16                                   SpecificData;
+
+  ZeroMem (&CommandPacket, sizeof (EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof (EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof (EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  // 
+  // Change Endianness of SecurityProtocolSpecificData
+  //
+  SpecificData = (((SecurityProtocolSpecificData << 8) & 0xFF00) | (SecurityProtocolSpecificData >> 8));
+
+  if (IsTrustSend) {
+    Command.Cdw0.Opcode          = NVME_ADMIN_SECURITY_SEND_CMD;
+    CommandPacket.TransferBuffer = Buffer;
+    CommandPacket.TransferLength = (UINT32)TransferLength;
+    CommandPacket.NvmeCmd->Cdw10 = (UINT32)((SecurityProtocolId << 24) | (SpecificData << 8));
+    CommandPacket.NvmeCmd->Cdw11 = (UINT32)TransferLength;
+  } else {
+    Command.Cdw0.Opcode          = NVME_ADMIN_SECURITY_RECEIVE_CMD;
+    CommandPacket.TransferBuffer = Buffer;
+    CommandPacket.TransferLength = (UINT32)TransferLength;
+    CommandPacket.NvmeCmd->Cdw10 = (UINT32)((SecurityProtocolId << 24) | (SpecificData << 8));
+    CommandPacket.NvmeCmd->Cdw11 = (UINT32)TransferLength;
+  }
+
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID;
+  CommandPacket.NvmeCmd->Nsid  = NVME_CONTROLLER_ID;
+  CommandPacket.CommandTimeout = Timeout;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               NVME_CONTROLLER_ID,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  if (!IsTrustSend) {
+    if (EFI_ERROR (Status))  {
+      *TransferLengthOut = 0;
+    } else {
+      *TransferLengthOut = (UINTN) TransferLength;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Send a security protocol command to a device that receives data and/or the result
+  of one or more commands sent by SendData.
+
+  The ReceiveData function sends a security protocol command to the given MediaId.
+  The security protocol command sent is defined by SecurityProtocolId and contains
+  the security protocol specific data SecurityProtocolSpecificData. The function
+  returns the data from the security protocol command in PayloadBuffer.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL IN command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED RECEIVE commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero.
+
+  If the PayloadBufferSize is zero, the security protocol command is sent using the
+  Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBufferSize is too small to store the available data from the security
+  protocol command, the function shall copy PayloadBufferSize bytes into the
+  PayloadBuffer and return EFI_WARN_BUFFER_TOO_SMALL.
+
+  If PayloadBuffer or PayloadTransferSize is NULL and PayloadBufferSize is non-zero,
+  the function shall return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function shall
+  return EFI_UNSUPPORTED. If there is no media in the device, the function returns
+  EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the device,
+  the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall
+  return EFI_SUCCESS. If the security protocol command completes with an error, the
+  function shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command. The caller is responsible for having
+                                       either implicit or explicit ownership of the buffer.
+  @param  PayloadTransferSize          A pointer to a buffer to store the size in bytes of the
+                                       data written to the payload data buffer.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL    The PayloadBufferSize was too small to store the available
+                                       data from the device. The PayloadBuffer contains the truncated data.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer or PayloadTransferSize is NULL and
+                                       PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeStorageSecurityReceiveData (
+  IN  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN  UINT32                                   MediaId,
+  IN  UINT64                                   Timeout,
+  IN  UINT8                                    SecurityProtocolId,
+  IN  UINT16                                   SecurityProtocolSpecificData,
+  IN  UINTN                                    PayloadBufferSize,
+  OUT VOID                                     *PayloadBuffer,
+  OUT UINTN                                    *PayloadTransferSize
+  )
+{
+  EFI_STATUS                       Status;
+  NVME_DEVICE_PRIVATE_DATA         *Device;
+  
+  Status  = EFI_SUCCESS;
+
+  if ((PayloadBuffer == NULL) || (PayloadTransferSize == NULL) || (PayloadBufferSize == 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_STORAGE_SECURITY (This);
+
+  if (MediaId != Device->BlockIo.Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (!Device->BlockIo.Media->MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  Status = TrustTransferNvmeDevice (
+             Device->Controller,
+             PayloadBuffer,
+             SecurityProtocolId,
+             SecurityProtocolSpecificData,
+             PayloadBufferSize,
+             FALSE,
+             Timeout,
+             PayloadTransferSize
+             );
+
+  return Status;
+}
+
+/**
+  Send a security protocol command to a device.
+
+  The SendData function sends a security protocol command containing the payload
+  PayloadBuffer to the given MediaId. The security protocol command sent is
+  defined by SecurityProtocolId and contains the security protocol specific data
+  SecurityProtocolSpecificData. If the underlying protocol command requires a
+  specific padding for the command payload, the SendData function shall add padding
+  bytes to the command payload to satisfy the padding requirements.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL OUT command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED SEND commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero. If the PayloadBufferSize is zero, the security protocol command is
+  sent using the Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBuffer is NULL and PayloadBufferSize is non-zero, the function shall
+  return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function
+  shall return EFI_UNSUPPORTED. If there is no media in the device, the function
+  returns EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the
+  device, the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall return
+  EFI_SUCCESS. If the security protocol command completes with an error, the function
+  shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the send data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer is NULL and PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeStorageSecuritySendData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  IN VOID                                     *PayloadBuffer
+  )
+{
+  EFI_STATUS                       Status; 
+  NVME_DEVICE_PRIVATE_DATA         *Device;
+
+  Status  = EFI_SUCCESS;
+
+  if ((PayloadBuffer == NULL) && (PayloadBufferSize != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_STORAGE_SECURITY (This);
+
+  if (MediaId != Device->BlockIo.Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (!Device->BlockIo.Media->MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  Status = TrustTransferNvmeDevice (
+             Device->Controller,
+             PayloadBuffer,
+             SecurityProtocolId,
+             SecurityProtocolSpecificData,
+             PayloadBufferSize,
+             TRUE,
+             Timeout,
+             NULL
+             );
+
+  return Status;
+}
+
+
+
+
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.h b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.h
new file mode 100644
index 0000000000..1c71a81ec5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.h
@@ -0,0 +1,269 @@
+/** @file
+  Header file for EFI_BLOCK_IO_PROTOCOL interface.
+
+Copyright (c) 2013 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_NVME_BLOCKIO_H_
+#define _EFI_NVME_BLOCKIO_H_
+
+/**
+  Reset the Block Device.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Lba        The starting Logical Block Address to read from
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the destination buffer for the data. The caller is
+                     responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    The media ID that the write request is for.
+  @param  Lba        The starting logical block address to be written. The caller is
+                     responsible for writing to only legitimate locations.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  );
+
+/**
+  Flush the Block Device.
+
+  @param  This              Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  );
+
+/**
+  Send a security protocol command to a device that receives data and/or the result
+  of one or more commands sent by SendData.
+
+  The ReceiveData function sends a security protocol command to the given MediaId.
+  The security protocol command sent is defined by SecurityProtocolId and contains
+  the security protocol specific data SecurityProtocolSpecificData. The function
+  returns the data from the security protocol command in PayloadBuffer.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL IN command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED RECEIVE commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero.
+
+  If the PayloadBufferSize is zero, the security protocol command is sent using the
+  Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBufferSize is too small to store the available data from the security
+  protocol command, the function shall copy PayloadBufferSize bytes into the
+  PayloadBuffer and return EFI_WARN_BUFFER_TOO_SMALL.
+
+  If PayloadBuffer or PayloadTransferSize is NULL and PayloadBufferSize is non-zero,
+  the function shall return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function shall
+  return EFI_UNSUPPORTED. If there is no media in the device, the function returns
+  EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the device,
+  the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall
+  return EFI_SUCCESS. If the security protocol command completes with an error, the
+  function shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command. The caller is responsible for having
+                                       either implicit or explicit ownership of the buffer.
+  @param  PayloadTransferSize          A pointer to a buffer to store the size in bytes of the
+                                       data written to the payload data buffer.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL    The PayloadBufferSize was too small to store the available
+                                       data from the device. The PayloadBuffer contains the truncated data.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer or PayloadTransferSize is NULL and
+                                       PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeStorageSecurityReceiveData (
+  IN  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL   *This,
+  IN  UINT32                                  MediaId,
+  IN  UINT64                                  Timeout,
+  IN  UINT8                                   SecurityProtocolId,
+  IN  UINT16                                  SecurityProtocolSpecificData,
+  IN  UINTN                                   PayloadBufferSize,
+  OUT VOID                                    *PayloadBuffer,
+  OUT UINTN                                   *PayloadTransferSize
+  );
+
+/**
+  Send a security protocol command to a device.
+
+  The SendData function sends a security protocol command containing the payload
+  PayloadBuffer to the given MediaId. The security protocol command sent is
+  defined by SecurityProtocolId and contains the security protocol specific data
+  SecurityProtocolSpecificData. If the underlying protocol command requires a
+  specific padding for the command payload, the SendData function shall add padding
+  bytes to the command payload to satisfy the padding requirements.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL OUT command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED SEND commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero. If the PayloadBufferSize is zero, the security protocol command is
+  sent using the Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBuffer is NULL and PayloadBufferSize is non-zero, the function shall
+  return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function
+  shall return EFI_UNSUPPORTED. If there is no media in the device, the function
+  returns EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the
+  device, the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall return
+  EFI_SUCCESS. If the security protocol command completes with an error, the function
+  shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer is NULL and PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeStorageSecuritySendData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  IN VOID                                     *PayloadBuffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.c
new file mode 100644
index 0000000000..ea108a851d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.c
@@ -0,0 +1,162 @@
+/** @file
+  This file is used to implement the EFI_DISK_INFO_PROTOCOL interface..
+
+  Copyright (c) 2013, 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 "NvmExpress.h"
+
+EFI_DISK_INFO_PROTOCOL gNvmExpressDiskInfoProtocolTemplate = {
+  EFI_DISK_INFO_NVME_INTERFACE_GUID,
+  NvmExpressDiskInfoInquiry,
+  NvmExpressDiskInfoIdentify,
+  NvmExpressDiskInfoSenseData,
+  NvmExpressDiskInfoWhichIde
+};
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with NVME interface GUID.
+
+  @param[in]  Device  The pointer of NVME_DEVICE_PRIVATE_DATA.
+
+**/
+VOID
+InitializeDiskInfo (
+  IN  NVME_DEVICE_PRIVATE_DATA    *Device
+  )
+{
+  CopyMem (&Device->DiskInfo, &gNvmExpressDiskInfoProtocolTemplate, sizeof (EFI_DISK_INFO_PROTOCOL));
+}
+
+
+/**
+  Provides inquiry information for the controller type.
+
+  This function is used to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  )
+{
+  EFI_STATUS                      Status;
+  NVME_DEVICE_PRIVATE_DATA        *Device;
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_DISK_INFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*IdentifyDataSize >= sizeof (Device->NamespaceData)) {
+    Status = EFI_SUCCESS;
+    CopyMem (IdentifyData, &Device->NamespaceData, sizeof (Device->NamespaceData));
+  }
+  *IdentifyDataSize = sizeof (Device->NamespaceData);
+  return Status;
+}
+
+/**
+  Provides sense data information for the controller type.
+
+  This function is used to get sense data.
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function is used to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.h b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.h
new file mode 100644
index 0000000000..f07cfe3474
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.h
@@ -0,0 +1,129 @@
+/** @file
+  Header file for EFI_DISK_INFO_PROTOCOL interface.
+
+Copyright (c) 2013, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _NVME_DISKINFO_H_
+#define _NVME_DISKINFO_H_
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with NVME interface GUID.
+
+  @param[in]  Device  The pointer of NVME_DEVICE_PRIVATE_DATA.
+
+**/
+VOID
+InitializeDiskInfo (
+  IN  NVME_DEVICE_PRIVATE_DATA    *Device
+  );
+
+
+/**
+  Provides inquiry information for the controller type.
+
+  This function is used to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  );
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  );
+
+/**
+  Provides sense data information for the controller type.
+
+  This function is used to get sense data.
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  );
+
+
+/**
+  This function is used to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.inf b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.inf
new file mode 100644
index 0000000000..00acf2b5cf
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.inf
@@ -0,0 +1,79 @@
+## @file
+#  NVM Express Host Controller Module.
+#
+#  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+#  NVM Express specification.
+#
+#  Copyright (c) 2013 - 2015, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php.
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = NvmExpressDxe
+  MODULE_UNI_FILE                = NvmExpressDxe.uni
+  FILE_GUID                      = 5BE3BDF4-53CF-46a3-A6A9-73C34A6E5EE3
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = NvmExpressDriverEntry
+  UNLOAD_IMAGE                   = NvmExpressUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gNvmExpressDriverBinding
+#  COMPONENT_NAME                =  gNvmExpressComponentName
+#  COMPONENT_NAME2               =  gNvmExpressComponentName2
+
+[Sources]
+  NvmExpressBlockIo.c
+  NvmExpressBlockIo.h
+  ComponentName.c
+  NvmExpress.c
+  NvmExpress.h
+  NvmExpressDiskInfo.c
+  NvmExpressDiskInfo.h
+  NvmExpressHci.c
+  NvmExpressHci.h
+  NvmExpressPassthru.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  BaseMemoryLib
+  BaseLib
+  DebugLib
+  DevicePathLib
+  MemoryAllocationLib
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  UefiLib
+  PrintLib
+
+[Protocols]
+  gEfiPciIoProtocolGuid                       ## TO_START
+  ## BY_START
+  ## TO_START
+  gEfiDevicePathProtocolGuid
+  gEfiNvmExpressPassThruProtocolGuid          ## BY_START
+  gEfiBlockIoProtocolGuid                     ## BY_START
+  gEfiDiskInfoProtocolGuid                    ## BY_START
+  gEfiStorageSecurityCommandProtocolGuid      ## BY_START
+  gEfiDriverSupportedEfiVersionProtocolGuid   ## PRODUCES
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER ## SOMETIMES_CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  NvmExpressDxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.uni b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.uni
new file mode 100644
index 0000000000..36fbc2acfe
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxeExtra.uni
new file mode 100644
index 0000000000..2f8bf5922d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.c
new file mode 100644
index 0000000000..f6b6288f65
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.c
@@ -0,0 +1,981 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, 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 "NvmExpress.h"
+
+/**
+  Read Nvm Express controller capability register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Cap              The buffer used to store capability register content.
+
+  @return EFI_SUCCESS      Successfully read the controller capability register content.
+  @return EFI_DEVICE_ERROR Fail to read the controller capability register.
+
+**/
+EFI_STATUS
+ReadNvmeControllerCapabilities (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_CAP                         *Cap
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_CAP_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned64 ((UINT64*)Cap, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express controller configuration register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Cc               The buffer used to store configuration register content.
+
+  @return EFI_SUCCESS      Successfully read the controller configuration register content.
+  @return EFI_DEVICE_ERROR Fail to read the controller configuration register.
+
+**/
+EFI_STATUS
+ReadNvmeControllerConfiguration (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_CC                          *Cc
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_CC_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned32 ((UINT32*)Cc, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Write Nvm Express controller configuration register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Cc               The buffer used to store the content to be written into configuration register.
+
+  @return EFI_SUCCESS      Successfully write data into the controller configuration register.
+  @return EFI_DEVICE_ERROR Fail to write data into the controller configuration register.
+
+**/
+EFI_STATUS
+WriteNvmeControllerConfiguration (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_CC                          *Cc
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Data   = ReadUnaligned32 ((UINT32*)Cc);
+  Status = PciIo->Mem.Write (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_CC_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Cc.En: %d\n", Cc->En));
+  DEBUG ((EFI_D_INFO, "Cc.Css: %d\n", Cc->Css));
+  DEBUG ((EFI_D_INFO, "Cc.Mps: %d\n", Cc->Mps));
+  DEBUG ((EFI_D_INFO, "Cc.Ams: %d\n", Cc->Ams));
+  DEBUG ((EFI_D_INFO, "Cc.Shn: %d\n", Cc->Shn));
+  DEBUG ((EFI_D_INFO, "Cc.Iosqes: %d\n", Cc->Iosqes));
+  DEBUG ((EFI_D_INFO, "Cc.Iocqes: %d\n", Cc->Iocqes));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express controller status register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Csts             The buffer used to store status register content.
+
+  @return EFI_SUCCESS      Successfully read the controller status register content.
+  @return EFI_DEVICE_ERROR Fail to read the controller status register.
+
+**/
+EFI_STATUS
+ReadNvmeControllerStatus (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_CSTS                        *Csts
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_CSTS_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned32 ((UINT32*)Csts, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express admin queue attributes register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Aqa              The buffer used to store admin queue attributes register content.
+
+  @return EFI_SUCCESS      Successfully read the admin queue attributes register content.
+  @return EFI_DEVICE_ERROR Fail to read the admin queue attributes register.
+
+**/
+EFI_STATUS
+ReadNvmeAdminQueueAttributes (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_AQA                         *Aqa
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_AQA_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned32 ((UINT32*)Aqa, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Write Nvm Express admin queue attributes register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Aqa              The buffer used to store the content to be written into admin queue attributes register.
+
+  @return EFI_SUCCESS      Successfully write data into the admin queue attributes register.
+  @return EFI_DEVICE_ERROR Fail to write data into the admin queue attributes register.
+
+**/
+EFI_STATUS
+WriteNvmeAdminQueueAttributes (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_AQA                         *Aqa
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Data   = ReadUnaligned32 ((UINT32*)Aqa);
+  Status = PciIo->Mem.Write (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_AQA_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Aqa.Asqs: %d\n", Aqa->Asqs));
+  DEBUG ((EFI_D_INFO, "Aqa.Acqs: %d\n", Aqa->Acqs));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express admin submission queue base address register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Asq              The buffer used to store admin submission queue base address register content.
+
+  @return EFI_SUCCESS      Successfully read the admin submission queue base address register content.
+  @return EFI_DEVICE_ERROR Fail to read the admin submission queue base address register.
+
+**/
+EFI_STATUS
+ReadNvmeAdminSubmissionQueueBaseAddress (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_ASQ                         *Asq
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_ASQ_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned64 ((UINT64*)Asq, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Write Nvm Express admin submission queue base address register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Asq              The buffer used to store the content to be written into admin submission queue base address register.
+
+  @return EFI_SUCCESS      Successfully write data into the admin submission queue base address register.
+  @return EFI_DEVICE_ERROR Fail to write data into the admin submission queue base address register.
+
+**/
+EFI_STATUS
+WriteNvmeAdminSubmissionQueueBaseAddress (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_ASQ                         *Asq
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+  Data   = ReadUnaligned64 ((UINT64*)Asq);
+
+  Status = PciIo->Mem.Write (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_ASQ_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Asq.Asqb: %lx\n", Asq->Asqb));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express admin completion queue base address register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Acq              The buffer used to store admin completion queue base address register content.
+
+  @return EFI_SUCCESS      Successfully read the admin completion queue base address register content.
+  @return EFI_DEVICE_ERROR Fail to read the admin completion queue base address register.
+
+**/
+EFI_STATUS
+ReadNvmeAdminCompletionQueueBaseAddress (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_ACQ                         *Acq
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_ACQ_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned64 ((UINT64*)Acq, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Write Nvm Express admin completion queue base address register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Acq              The buffer used to store the content to be written into admin completion queue base address register.
+
+  @return EFI_SUCCESS      Successfully write data into the admin completion queue base address register.
+  @return EFI_DEVICE_ERROR Fail to write data into the admin completion queue base address register.
+
+**/
+EFI_STATUS
+WriteNvmeAdminCompletionQueueBaseAddress (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_ACQ                         *Acq
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+  Data   = ReadUnaligned64 ((UINT64*)Acq);
+
+  Status = PciIo->Mem.Write (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_ACQ_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Acq.Acqb: %lxh\n", Acq->Acqb));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Disable the Nvm Express controller.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @return EFI_SUCCESS      Successfully disable the controller.
+  @return EFI_DEVICE_ERROR Fail to disable the controller.
+
+**/
+EFI_STATUS
+NvmeDisableController (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private
+  )
+{
+  NVME_CC                Cc;
+  NVME_CSTS              Csts;
+  EFI_STATUS             Status;
+  UINT32                 Index;
+  UINT8                  Timeout;
+
+  //
+  // Read Controller Configuration Register.
+  //
+  Status = ReadNvmeControllerConfiguration (Private, &Cc);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  Cc.En = 0;
+
+  //
+  // Disable the controller.
+  //
+  Status = WriteNvmeControllerConfiguration (Private, &Cc);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Cap.To specifies max delay time in 500ms increments for Csts.Rdy to transition from 1 to 0 after
+  // Cc.Enable transition from 1 to 0. Loop produces a 1 millisecond delay per itteration, up to 500 * Cap.To.
+  //
+  if (Private->Cap.To == 0) {
+    Timeout = 1;
+  } else {
+    Timeout = Private->Cap.To;
+  }
+
+  for(Index = (Timeout * 500); Index != 0; --Index) {
+    gBS->Stall(1000);
+
+    //
+    // Check if the controller is initialized
+    //
+    Status = ReadNvmeControllerStatus (Private, &Csts);
+
+    if (EFI_ERROR(Status)) {
+      return Status;
+    }
+
+    if (Csts.Rdy == 0) {
+      break;
+    }
+  }
+
+  if (Index == 0) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  DEBUG ((EFI_D_INFO, "NVMe controller is disabled with status [%r].\n", Status));
+  return Status;
+}
+
+/**
+  Enable the Nvm Express controller.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @return EFI_SUCCESS      Successfully enable the controller.
+  @return EFI_DEVICE_ERROR Fail to enable the controller.
+  @return EFI_TIMEOUT      Fail to enable the controller in given time slot.
+
+**/
+EFI_STATUS
+NvmeEnableController (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private
+  )
+{
+  NVME_CC                Cc;
+  NVME_CSTS              Csts;
+  EFI_STATUS             Status;
+  UINT32                 Index;
+  UINT8                  Timeout;
+
+  //
+  // Enable the controller.
+  // CC.AMS, CC.MPS and CC.CSS are all set to 0.
+  //
+  ZeroMem (&Cc, sizeof (NVME_CC));
+  Cc.En     = 1;
+  Cc.Iosqes = 6;
+  Cc.Iocqes = 4;
+
+  Status = WriteNvmeControllerConfiguration (Private, &Cc);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Cap.To specifies max delay time in 500ms increments for Csts.Rdy to set after
+  // Cc.Enable. Loop produces a 1 millisecond delay per itteration, up to 500 * Cap.To.
+  //
+  if (Private->Cap.To == 0) {
+    Timeout = 1;
+  } else {
+    Timeout = Private->Cap.To;
+  }
+
+  for(Index = (Timeout * 500); Index != 0; --Index) {
+    gBS->Stall(1000);
+
+    //
+    // Check if the controller is initialized
+    //
+    Status = ReadNvmeControllerStatus (Private, &Csts);
+
+    if (EFI_ERROR(Status)) {
+      return Status;
+    }
+
+    if (Csts.Rdy) {
+      break;
+    }
+  }
+
+  if (Index == 0) {
+    Status = EFI_TIMEOUT;
+  }
+
+  DEBUG ((EFI_D_INFO, "NVMe controller is enabled with status [%r].\n", Status));
+  return Status;
+}
+
+/**
+  Get identify controller data.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Buffer           The buffer used to store the identify controller data.
+
+  @return EFI_SUCCESS      Successfully get the identify controller data.
+  @return EFI_DEVICE_ERROR Fail to get the identify controller data.
+
+**/
+EFI_STATUS
+NvmeIdentifyController (
+  IN NVME_CONTROLLER_PRIVATE_DATA       *Private,
+  IN VOID                               *Buffer
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  Command.Cdw0.Opcode = NVME_ADMIN_IDENTIFY_CMD;
+  //
+  // According to Nvm Express 1.1 spec Figure 38, When not used, the field shall be cleared to 0h.
+  // For the Identify command, the Namespace Identifier is only used for the Namespace data structure.
+  //
+  Command.Nsid        = 0;
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+  CommandPacket.TransferBuffer = Buffer;
+  CommandPacket.TransferLength = sizeof (NVME_ADMIN_CONTROLLER_DATA);
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+  //
+  // Set bit 0 (Cns bit) to 1 to identify a controller
+  //
+  Command.Cdw10                = 1;
+  Command.Flags                = CDW10_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               NVME_CONTROLLER_ID,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Get specified identify namespace data.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  NamespaceId      The specified namespace identifier.
+  @param  Buffer           The buffer used to store the identify namespace data.
+
+  @return EFI_SUCCESS      Successfully get the identify namespace data.
+  @return EFI_DEVICE_ERROR Fail to get the identify namespace data.
+
+**/
+EFI_STATUS
+NvmeIdentifyNamespace (
+  IN NVME_CONTROLLER_PRIVATE_DATA      *Private,
+  IN UINT32                            NamespaceId,
+  IN VOID                              *Buffer
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  Command.Cdw0.Opcode = NVME_ADMIN_IDENTIFY_CMD;
+  Command.Nsid        = NamespaceId;
+  CommandPacket.TransferBuffer = Buffer;
+  CommandPacket.TransferLength = sizeof (NVME_ADMIN_NAMESPACE_DATA);
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+  //
+  // Set bit 0 (Cns bit) to 1 to identify a namespace
+  //
+  CommandPacket.NvmeCmd->Cdw10 = 0;
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               NamespaceId,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Create io completion queue.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @return EFI_SUCCESS      Successfully create io completion queue.
+  @return EFI_DEVICE_ERROR Fail to create io completion queue.
+
+**/
+EFI_STATUS
+NvmeCreateIoCompletionQueue (
+  IN NVME_CONTROLLER_PRIVATE_DATA      *Private
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  NVME_ADMIN_CRIOCQ                        CrIoCq;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+  ZeroMem (&CrIoCq, sizeof(NVME_ADMIN_CRIOCQ));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  Command.Cdw0.Opcode = NVME_ADMIN_CRIOCQ_CMD;
+  CommandPacket.TransferBuffer = Private->CqBufferPciAddr[1];
+  CommandPacket.TransferLength = EFI_PAGE_SIZE;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+
+  CrIoCq.Qid   = NVME_IO_QUEUE;
+  CrIoCq.Qsize = NVME_CCQ_SIZE;
+  CrIoCq.Pc    = 1;
+  CopyMem (&CommandPacket.NvmeCmd->Cdw10, &CrIoCq, sizeof (NVME_ADMIN_CRIOCQ));
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               0,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Create io submission queue.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @return EFI_SUCCESS      Successfully create io submission queue.
+  @return EFI_DEVICE_ERROR Fail to create io submission queue.
+
+**/
+EFI_STATUS
+NvmeCreateIoSubmissionQueue (
+  IN NVME_CONTROLLER_PRIVATE_DATA      *Private
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  NVME_ADMIN_CRIOSQ                        CrIoSq;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+  ZeroMem (&CrIoSq, sizeof(NVME_ADMIN_CRIOSQ));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  Command.Cdw0.Opcode = NVME_ADMIN_CRIOSQ_CMD;
+  CommandPacket.TransferBuffer = Private->SqBufferPciAddr[1];
+  CommandPacket.TransferLength = EFI_PAGE_SIZE;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+
+  CrIoSq.Qid   = NVME_IO_QUEUE;
+  CrIoSq.Qsize = NVME_CSQ_SIZE;
+  CrIoSq.Pc    = 1;
+  CrIoSq.Cqid  = NVME_IO_QUEUE;
+  CrIoSq.Qprio = 0;
+  CopyMem (&CommandPacket.NvmeCmd->Cdw10, &CrIoSq, sizeof (NVME_ADMIN_CRIOSQ));
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               0,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Initialize the Nvm Express controller.
+
+  @param[in] Private                 The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The NVM Express Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+NvmeControllerInit (
+  IN NVME_CONTROLLER_PRIVATE_DATA    *Private
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_PCI_IO_PROTOCOL             *PciIo;
+  UINT64                          Supports;
+  NVME_AQA                        Aqa;
+  NVME_ASQ                        Asq;
+  NVME_ACQ                        Acq;
+
+  //
+  // Save original PCI attributes and enable this controller.
+  //
+  PciIo  = Private->PciIo;
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &Private->PciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status    = PciIo->Attributes (
+                         PciIo,
+                         EfiPciIoAttributeOperationEnable,
+                         Supports,
+                         NULL
+                         );
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_INFO, "NvmeControllerInit: failed to enable controller\n"));
+    return Status;
+  }
+
+  //
+  // Read the Controller Capabilities register and verify that the NVM command set is supported
+  //
+  Status = ReadNvmeControllerCapabilities (Private, &Private->Cap);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Private->Cap.Css != 0x01) {
+    DEBUG ((EFI_D_INFO, "NvmeControllerInit: the controller doesn't support NVMe command set\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Currently the driver only supports 4k page size.
+  //
+  ASSERT ((Private->Cap.Mpsmin + 12) <= EFI_PAGE_SHIFT);
+
+  Private->Cid[0] = 0;
+  Private->Cid[1] = 0;
+
+  Status = NvmeDisableController (Private);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // set number of entries admin submission & completion queues.
+  //
+  Aqa.Asqs  = NVME_ASQ_SIZE;
+  Aqa.Rsvd1 = 0;
+  Aqa.Acqs  = NVME_ACQ_SIZE;
+  Aqa.Rsvd2 = 0;
+
+  //
+  // Address of admin submission queue.
+  //
+  Asq.Rsvd1 = 0;
+  Asq.Asqb  = (UINT64)(UINTN)(Private->BufferPciAddr) >> 12;
+
+  //
+  // Address of admin completion queue.
+  //
+  Acq.Rsvd1 = 0;
+  Acq.Acqb  = (UINT64)(UINTN)(Private->BufferPciAddr + EFI_PAGE_SIZE) >> 12;
+
+  //
+  // Address of I/O submission & completion queue.
+  //
+  Private->SqBuffer[0]        = (NVME_SQ *)(UINTN)(Private->Buffer);
+  Private->SqBufferPciAddr[0] = (NVME_SQ *)(UINTN)(Private->BufferPciAddr);
+  Private->CqBuffer[0]        = (NVME_CQ *)(UINTN)(Private->Buffer + 1 * EFI_PAGE_SIZE);
+  Private->CqBufferPciAddr[0] = (NVME_CQ *)(UINTN)(Private->BufferPciAddr + 1 * EFI_PAGE_SIZE);
+  Private->SqBuffer[1]        = (NVME_SQ *)(UINTN)(Private->Buffer + 2 * EFI_PAGE_SIZE);
+  Private->SqBufferPciAddr[1] = (NVME_SQ *)(UINTN)(Private->BufferPciAddr + 2 * EFI_PAGE_SIZE);
+  Private->CqBuffer[1]        = (NVME_CQ *)(UINTN)(Private->Buffer + 3 * EFI_PAGE_SIZE);
+  Private->CqBufferPciAddr[1] = (NVME_CQ *)(UINTN)(Private->BufferPciAddr + 3 * EFI_PAGE_SIZE);
+
+  DEBUG ((EFI_D_INFO, "Private->Buffer = [%016X]\n", (UINT64)(UINTN)Private->Buffer));
+  DEBUG ((EFI_D_INFO, "Admin Submission Queue size (Aqa.Asqs) = [%08X]\n", Aqa.Asqs));
+  DEBUG ((EFI_D_INFO, "Admin Completion Queue size (Aqa.Acqs) = [%08X]\n", Aqa.Acqs));
+  DEBUG ((EFI_D_INFO, "Admin Submission Queue (SqBuffer[0]) = [%016X]\n", Private->SqBuffer[0]));
+  DEBUG ((EFI_D_INFO, "Admin Completion Queue (CqBuffer[0]) = [%016X]\n", Private->CqBuffer[0]));
+  DEBUG ((EFI_D_INFO, "I/O   Submission Queue (SqBuffer[1]) = [%016X]\n", Private->SqBuffer[1]));
+  DEBUG ((EFI_D_INFO, "I/O   Completion Queue (CqBuffer[1]) = [%016X]\n", Private->CqBuffer[1]));
+
+  //
+  // Program admin queue attributes.
+  //
+  Status = WriteNvmeAdminQueueAttributes (Private, &Aqa);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Program admin submission queue address.
+  //
+  Status = WriteNvmeAdminSubmissionQueueBaseAddress (Private, &Asq);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Program admin completion queue address.
+  //
+  Status = WriteNvmeAdminCompletionQueueBaseAddress (Private, &Acq);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  Status = NvmeEnableController (Private);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Allocate buffer for Identify Controller data
+  //
+  Private->ControllerData = (NVME_ADMIN_CONTROLLER_DATA *)AllocateZeroPool (sizeof(NVME_ADMIN_CONTROLLER_DATA));
+
+  if (Private->ControllerData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Get current Identify Controller Data
+  //
+  Status = NvmeIdentifyController (Private, Private->ControllerData);
+
+  if (EFI_ERROR(Status)) {
+    FreePool(Private->ControllerData);
+    Private->ControllerData = NULL;
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Dump NvmExpress Identify Controller Data
+  //
+  Private->ControllerData->Sn[19] = 0;
+  Private->ControllerData->Mn[39] = 0;
+  DEBUG ((EFI_D_INFO, " == NVME IDENTIFY CONTROLLER DATA ==\n"));
+  DEBUG ((EFI_D_INFO, "    PCI VID   : 0x%x\n", Private->ControllerData->Vid));
+  DEBUG ((EFI_D_INFO, "    PCI SSVID : 0x%x\n", Private->ControllerData->Ssvid));
+  DEBUG ((EFI_D_INFO, "    SN        : %a\n",   (CHAR8 *)(Private->ControllerData->Sn)));
+  DEBUG ((EFI_D_INFO, "    MN        : %a\n",   (CHAR8 *)(Private->ControllerData->Mn)));
+  DEBUG ((EFI_D_INFO, "    FR        : 0x%x\n", *((UINT64*)Private->ControllerData->Fr)));
+  DEBUG ((EFI_D_INFO, "    RAB       : 0x%x\n", Private->ControllerData->Rab));
+  DEBUG ((EFI_D_INFO, "    IEEE      : 0x%x\n", *(UINT32*)Private->ControllerData->Ieee_oui));
+  DEBUG ((EFI_D_INFO, "    AERL      : 0x%x\n", Private->ControllerData->Aerl));
+  DEBUG ((EFI_D_INFO, "    SQES      : 0x%x\n", Private->ControllerData->Sqes));
+  DEBUG ((EFI_D_INFO, "    CQES      : 0x%x\n", Private->ControllerData->Cqes));
+  DEBUG ((EFI_D_INFO, "    NN        : 0x%x\n", Private->ControllerData->Nn));
+
+  //
+  // Create one I/O completion queue.
+  //
+  Status = NvmeCreateIoCompletionQueue (Private);
+  if (EFI_ERROR(Status)) {
+   return Status;
+  }
+
+  //
+  // Create one I/O Submission queue.
+  //
+  Status = NvmeCreateIoSubmissionQueue (Private);
+  if (EFI_ERROR(Status)) {
+   return Status;
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.h b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.h
new file mode 100644
index 0000000000..06c1db7d8e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.h
@@ -0,0 +1,809 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _NVME_HCI_H_
+#define _NVME_HCI_H_
+
+#define NVME_BAR                 0
+
+//
+// controller register offsets
+//
+#define NVME_CAP_OFFSET          0x0000  // Controller Capabilities
+#define NVME_VER_OFFSET          0x0008  // Version
+#define NVME_INTMS_OFFSET        0x000c  // Interrupt Mask Set
+#define NVME_INTMC_OFFSET        0x0010  // Interrupt Mask Clear
+#define NVME_CC_OFFSET           0x0014  // Controller Configuration
+#define NVME_CSTS_OFFSET         0x001c  // Controller Status
+#define NVME_NSSR_OFFSET         0x0020  // NVM Subsystem Reset
+#define NVME_AQA_OFFSET          0x0024  // Admin Queue Attributes
+#define NVME_ASQ_OFFSET          0x0028  // Admin Submission Queue Base Address
+#define NVME_ACQ_OFFSET          0x0030  // Admin Completion Queue Base Address
+#define NVME_SQ0_OFFSET          0x1000  // Submission Queue 0 (admin) Tail Doorbell
+#define NVME_CQ0_OFFSET          0x1004  // Completion Queue 0 (admin) Head Doorbell
+
+//
+// These register offsets are defined as 0x1000 + (N * (4 << CAP.DSTRD))
+// Get the doorbell stride bit shift value from the controller capabilities.
+//
+#define NVME_SQTDBL_OFFSET(QID, DSTRD)    0x1000 + ((2 * (QID)) * (4 << (DSTRD)))       // Submission Queue y (NVM) Tail Doorbell
+#define NVME_CQHDBL_OFFSET(QID, DSTRD)    0x1000 + (((2 * (QID)) + 1) * (4 << (DSTRD))) // Completion Queue y (NVM) Head Doorbell
+
+
+#pragma pack(1)
+
+//
+// 3.1.1 Offset 00h: CAP - Controller Capabilities
+//
+typedef struct {
+  UINT16 Mqes;      // Maximum Queue Entries Supported
+  UINT8  Cqr:1;     // Contiguous Queues Required
+  UINT8  Ams:2;     // Arbitration Mechanism Supported
+  UINT8  Rsvd1:5;
+  UINT8  To;        // Timeout
+  UINT16 Dstrd:4;
+  UINT16 Nssrs:1;   // NVM Subsystem Reset Supported NSSRS
+  UINT16 Css:4;     // Command Sets Supported - Bit 37
+  UINT16 Rsvd3:7;
+  UINT8  Mpsmin:4;
+  UINT8  Mpsmax:4;
+  UINT8  Rsvd4;
+} NVME_CAP;
+
+//
+// 3.1.2 Offset 08h: VS - Version
+//
+typedef struct {
+  UINT16 Mnr;       // Minor version number
+  UINT16 Mjr;       // Major version number
+} NVME_VER;
+
+//
+// 3.1.5 Offset 14h: CC - Controller Configuration
+//
+typedef struct {
+  UINT16 En:1;       // Enable
+  UINT16 Rsvd1:3;
+  UINT16 Css:3;      // I/O Command Set Selected
+  UINT16 Mps:4;      // Memory Page Size
+  UINT16 Ams:3;      // Arbitration Mechanism Selected
+  UINT16 Shn:2;      // Shutdown Notification
+  UINT8  Iosqes:4;   // I/O Submission Queue Entry Size
+  UINT8  Iocqes:4;   // I/O Completion Queue Entry Size
+  UINT8  Rsvd2;
+} NVME_CC;
+
+//
+// 3.1.6 Offset 1Ch: CSTS - Controller Status
+//
+typedef struct {
+  UINT32 Rdy:1;      // Ready
+  UINT32 Cfs:1;      // Controller Fatal Status
+  UINT32 Shst:2;     // Shutdown Status
+  UINT32 Nssro:1;    // NVM Subsystem Reset Occurred
+  UINT32 Rsvd1:27;
+} NVME_CSTS;
+
+//
+// 3.1.8 Offset 24h: AQA - Admin Queue Attributes
+//
+typedef struct {
+  UINT16 Asqs:12;    // Submission Queue Size
+  UINT16 Rsvd1:4;
+  UINT16 Acqs:12;    // Completion Queue Size
+  UINT16 Rsvd2:4;
+} NVME_AQA;
+
+//
+// 3.1.9 Offset 28h: ASQ - Admin Submission Queue Base Address
+//
+typedef struct {
+  UINT64 Rsvd1:12;
+  UINT64 Asqb:52;    // Admin Submission Queue Base Address
+} NVME_ASQ;
+
+//
+// 3.1.10 Offset 30h: ACQ - Admin Completion Queue Base Address
+//
+typedef struct {
+  UINT64 Rsvd1:12;
+  UINT64 Acqb:52;    // Admin Completion Queue Base Address
+} NVME_ACQ;
+
+//
+// 3.1.11 Offset (1000h + ((2y) * (4 << CAP.DSTRD))): SQyTDBL - Submission Queue y Tail Doorbell
+//
+typedef struct {
+  UINT16 Sqt;
+  UINT16 Rsvd1;
+} NVME_SQTDBL;
+
+//
+// 3.1.12 Offset (1000h + ((2y + 1) * (4 << CAP.DSTRD))): CQyHDBL - Completion Queue y Head Doorbell
+//
+typedef struct {
+  UINT16 Cqh;
+  UINT16 Rsvd1;
+} NVME_CQHDBL;
+
+//
+// NVM command set structures
+//
+// Read Command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting Sector Address */
+  //
+  // CDW 12
+  //
+  UINT16 Nlb;                 /* Number of Sectors */
+  UINT16 Rsvd1:10;
+  UINT16 Prinfo:4;            /* Protection Info Check */
+  UINT16 Fua:1;               /* Force Unit Access */
+  UINT16 Lr:1;                /* Limited Retry */
+  //
+  // CDW 13
+  //
+  UINT32 Af:4;                /* Access Frequency */
+  UINT32 Al:2;                /* Access Latency */
+  UINT32 Sr:1;                /* Sequential Request */
+  UINT32 In:1;                /* Incompressible */
+  UINT32 Rsvd2:24;
+  //
+  // CDW 14
+  //
+  UINT32 Eilbrt;              /* Expected Initial Logical Block Reference Tag */
+  //
+  // CDW 15
+  //
+  UINT16 Elbat;               /* Expected Logical Block Application Tag */
+  UINT16 Elbatm;              /* Expected Logical Block Application Tag Mask */
+} NVME_READ;
+
+//
+// Write Command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting Sector Address */
+  //
+  // CDW 12
+  //
+  UINT16 Nlb;                 /* Number of Sectors */
+  UINT16 Rsvd1:10;
+  UINT16 Prinfo:4;            /* Protection Info Check */
+  UINT16 Fua:1;               /* Force Unit Access */
+  UINT16 Lr:1;                /* Limited Retry */
+  //
+  // CDW 13
+  //
+  UINT32 Af:4;                /* Access Frequency */
+  UINT32 Al:2;                /* Access Latency */
+  UINT32 Sr:1;                /* Sequential Request */
+  UINT32 In:1;                /* Incompressible */
+  UINT32 Rsvd2:24;
+  //
+  // CDW 14
+  //
+  UINT32 Ilbrt;               /* Initial Logical Block Reference Tag */
+  //
+  // CDW 15
+  //
+  UINT16 Lbat;                /* Logical Block Application Tag */
+  UINT16 Lbatm;               /* Logical Block Application Tag Mask */
+} NVME_WRITE;
+
+//
+// Flush
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Flush;               /* Flush */
+} NVME_FLUSH;
+
+//
+// Write Uncorrectable command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting LBA */
+  //
+  // CDW 12
+  //
+  UINT32 Nlb:16;              /* Number of  Logical Blocks */
+  UINT32 Rsvd1:16;
+} NVME_WRITE_UNCORRECTABLE;
+
+//
+// Write Zeroes command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting LBA */
+  //
+  // CDW 12
+  //
+  UINT16 Nlb;                 /* Number of Logical Blocks */
+  UINT16 Rsvd1:10;
+  UINT16 Prinfo:4;            /* Protection Info Check */
+  UINT16 Fua:1;               /* Force Unit Access */
+  UINT16 Lr:1;                /* Limited Retry */
+  //
+  // CDW 13
+  //
+  UINT32 Rsvd2;
+  //
+  // CDW 14
+  //
+  UINT32 Ilbrt;               /* Initial Logical Block Reference Tag */
+  //
+  // CDW 15
+  //
+  UINT16 Lbat;                /* Logical Block Application Tag */
+  UINT16 Lbatm;               /* Logical Block Application Tag Mask */
+} NVME_WRITE_ZEROES;
+
+//
+// Compare command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting LBA */
+  //
+  // CDW 12
+  //
+  UINT16 Nlb;                 /* Number of Logical Blocks */
+  UINT16 Rsvd1:10;
+  UINT16 Prinfo:4;            /* Protection Info Check */
+  UINT16 Fua:1;               /* Force Unit Access */
+  UINT16 Lr:1;                /* Limited Retry */
+  //
+  // CDW 13
+  //
+  UINT32 Rsvd2;
+  //
+  // CDW 14
+  //
+  UINT32 Eilbrt;              /* Expected Initial Logical Block Reference Tag */
+  //
+  // CDW 15
+  //
+  UINT16 Elbat;               /* Expected Logical Block Application Tag */
+  UINT16 Elbatm;              /* Expected Logical Block Application Tag Mask */
+} NVME_COMPARE;
+
+typedef union {
+  NVME_READ                   Read;
+  NVME_WRITE                  Write;
+  NVME_FLUSH                  Flush;
+  NVME_WRITE_UNCORRECTABLE    WriteUncorrectable;
+  NVME_WRITE_ZEROES           WriteZeros;
+  NVME_COMPARE                Compare;
+} NVME_CMD;
+
+typedef struct {
+  UINT16 Mp;                /* Maximum Power */
+  UINT8  Rsvd1;             /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Mps:1;             /* Max Power Scale */
+  UINT8  Nops:1;            /* Non-Operational State */
+  UINT8  Rsvd2:6;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT32 Enlat;             /* Entry Latency */
+  UINT32 Exlat;             /* Exit Latency */
+  UINT8  Rrt:5;             /* Relative Read Throughput */
+  UINT8  Rsvd3:3;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Rrl:5;             /* Relative Read Leatency */
+  UINT8  Rsvd4:3;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Rwt:5;             /* Relative Write Throughput */
+  UINT8  Rsvd5:3;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Rwl:5;             /* Relative Write Leatency */
+  UINT8  Rsvd6:3;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Rsvd7[16];         /* Reserved as of Nvm Express 1.1 Spec */
+} NVME_PSDESCRIPTOR;
+
+//
+//  Identify Controller Data
+//
+typedef struct {
+  //
+  // Controller Capabilities and Features 0-255
+  //
+  UINT16 Vid;                 /* PCI Vendor ID */
+  UINT16 Ssvid;               /* PCI sub-system vendor ID */
+  UINT8  Sn[20];              /* Product serial number */
+
+  UINT8  Mn[40];              /* Proeduct model number */
+  UINT8  Fr[8];               /* Firmware Revision */
+  UINT8  Rab;                 /* Recommended Arbitration Burst */
+  UINT8  Ieee_oui[3];         /* Organization Unique Identifier */
+  UINT8  Cmic;                /* Multi-interface Capabilities */
+  UINT8  Mdts;                /* Maximum Data Transfer Size */
+  UINT8  Cntlid[2];           /* Controller ID */
+  UINT8  Rsvd1[176];          /* Reserved as of Nvm Express 1.1 Spec */
+  //
+  // Admin Command Set Attributes
+  //
+  UINT16 Oacs;                /* Optional Admin Command Support */
+    #define NAMESPACE_MANAGEMENT_SUPPORTED  BIT3
+    #define FW_DOWNLOAD_ACTIVATE_SUPPORTED  BIT2
+    #define FORMAT_NVM_SUPPORTED            BIT1
+    #define SECURITY_SEND_RECEIVE_SUPPORTED BIT0
+  UINT8  Acl;                 /* Abort Command Limit */
+  UINT8  Aerl;                /* Async Event Request Limit */
+  UINT8  Frmw;                /* Firmware updates */
+  UINT8  Lpa;                 /* Log Page Attributes */
+  UINT8  Elpe;                /* Error Log Page Entries */
+  UINT8  Npss;                /* Number of Power States Support */
+  UINT8  Avscc;               /* Admin Vendor Specific Command Configuration */
+  UINT8  Apsta;               /* Autonomous Power State Transition Attributes */
+  UINT8  Rsvd2[246];          /* Reserved as of Nvm Express 1.1 Spec */
+  //
+  // NVM Command Set Attributes
+  //
+  UINT8  Sqes;                /* Submission Queue Entry Size */
+  UINT8  Cqes;                /* Completion Queue Entry Size */
+  UINT16 Rsvd3;               /* Reserved as of Nvm Express 1.1 Spec */
+  UINT32 Nn;                  /* Number of Namespaces */
+  UINT16 Oncs;                /* Optional NVM Command Support */
+  UINT16 Fuses;               /* Fused Operation Support */
+  UINT8  Fna;                 /* Format NVM Attributes */
+  UINT8  Vwc;                 /* Volatile Write Cache */
+  UINT16 Awun;                /* Atomic Write Unit Normal */
+  UINT16 Awupf;               /* Atomic Write Unit Power Fail */
+  UINT8  Nvscc;               /* NVM Vendor Specific Command Configuration */
+  UINT8  Rsvd4;               /* Reserved as of Nvm Express 1.1 Spec */
+  UINT16 Acwu;                /* Atomic Compare & Write Unit */
+  UINT16 Rsvd5;               /* Reserved as of Nvm Express 1.1 Spec */
+  UINT32 Sgls;                /* SGL Support  */
+  UINT8  Rsvd6[164];          /* Reserved as of Nvm Express 1.1 Spec */
+  //
+  // I/O Command set Attributes
+  //
+  UINT8 Rsvd7[1344];          /* Reserved as of Nvm Express 1.1 Spec */
+  //
+  // Power State Descriptors
+  //
+  NVME_PSDESCRIPTOR PsDescriptor[32];
+
+  UINT8  VendorData[1024];    /* Vendor specific data */
+} NVME_ADMIN_CONTROLLER_DATA;
+
+typedef struct {
+  UINT16 Ms;                /* Metadata Size */
+  UINT8  Lbads;             /* LBA Data Size */
+  UINT8  Rp:2;              /* Relative Performance */
+    #define LBAF_RP_BEST      00b
+    #define LBAF_RP_BETTER    01b
+    #define LBAF_RP_GOOD      10b
+    #define LBAF_RP_DEGRADED  11b
+  UINT8  Rsvd1:6;           /* Reserved as of Nvm Express 1.1 Spec */
+} NVME_LBAFORMAT;
+
+//
+// Identify Namespace Data
+//
+typedef struct {
+  //
+  // NVM Command Set Specific
+  //
+  UINT64 Nsze;                /* Namespace Size (total number of blocks in formatted namespace) */
+  UINT64 Ncap;                /* Namespace Capacity (max number of logical blocks) */
+  UINT64 Nuse;                /* Namespace Utilization */
+  UINT8  Nsfeat;              /* Namespace Features */
+  UINT8  Nlbaf;               /* Number of LBA Formats */
+  UINT8  Flbas;               /* Formatted LBA size */
+  UINT8  Mc;                  /* Metadata Capabilities */
+  UINT8  Dpc;                 /* End-to-end Data Protection capabilities */
+  UINT8  Dps;                 /* End-to-end Data Protection Type Settings */
+  UINT8  Nmic;                /* Namespace Multi-path I/O and Namespace Sharing Capabilities */
+  UINT8  Rescap;              /* Reservation Capabilities */
+  UINT8  Rsvd1[88];           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT64 Eui64;               /* IEEE Extended Unique Identifier */
+  //
+  // LBA Format
+  //
+  NVME_LBAFORMAT LbaFormat[16];
+
+  UINT8 Rsvd2[192];           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8 VendorData[3712];     /* Vendor specific data */
+} NVME_ADMIN_NAMESPACE_DATA;
+
+//
+// NvmExpress Admin Identify Cmd
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Cns:2;
+  UINT32 Rsvd1:30;
+} NVME_ADMIN_IDENTIFY;
+
+//
+// NvmExpress Admin Create I/O Completion Queue
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Qid:16;              /* Queue Identifier */
+  UINT32 Qsize:16;            /* Queue Size */
+
+  //
+  // CDW 11
+  //
+  UINT32 Pc:1;                /* Physically Contiguous */
+  UINT32 Ien:1;               /* Interrupts Enabled */
+  UINT32 Rsvd1:14;            /* reserved as of Nvm Express 1.1 Spec */
+  UINT32 Iv:16;               /* Interrupt Vector for MSI-X or MSI*/
+} NVME_ADMIN_CRIOCQ;
+
+//
+// NvmExpress Admin Create I/O Submission Queue
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Qid:16;              /* Queue Identifier */
+  UINT32 Qsize:16;            /* Queue Size */
+
+  //
+  // CDW 11
+  //
+  UINT32 Pc:1;                /* Physically Contiguous */
+  UINT32 Qprio:2;             /* Queue Priority */
+  UINT32 Rsvd1:13;            /* Reserved as of Nvm Express 1.1 Spec */
+  UINT32 Cqid:16;             /* Completion Queue ID */
+} NVME_ADMIN_CRIOSQ;
+
+//
+// NvmExpress Admin Delete I/O Completion Queue
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT16 Qid;
+  UINT16 Rsvd1;
+} NVME_ADMIN_DEIOCQ;
+
+//
+// NvmExpress Admin Delete I/O Submission Queue
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT16 Qid;
+  UINT16 Rsvd1;
+} NVME_ADMIN_DEIOSQ;
+
+//
+// NvmExpress Admin Abort Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Sqid:16;             /* Submission Queue identifier */
+  UINT32 Cid:16;              /* Command Identifier */
+} NVME_ADMIN_ABORT;
+
+//
+// NvmExpress Admin Firmware Activate Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Fs:3;                /* Submission Queue identifier */
+  UINT32 Aa:2;                /* Command Identifier */
+  UINT32 Rsvd1:27;
+} NVME_ADMIN_FIRMWARE_ACTIVATE;
+
+//
+// NvmExpress Admin Firmware Image Download Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Numd;                /* Number of Dwords */
+  //
+  // CDW 11
+  //
+  UINT32 Ofst;                /* Offset */
+} NVME_ADMIN_FIRMWARE_IMAGE_DOWNLOAD;
+
+//
+// NvmExpress Admin Get Features Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Fid:8;                /* Feature Identifier */
+  UINT32 Sel:3;                /* Select */
+  UINT32 Rsvd1:21;
+} NVME_ADMIN_GET_FEATURES;
+
+//
+// NvmExpress Admin Get Log Page Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Lid:8;               /* Log Page Identifier */
+    #define LID_ERROR_INFO   0x1
+    #define LID_SMART_INFO   0x2
+    #define LID_FW_SLOT_INFO 0x3
+  UINT32 Rsvd1:8;
+  UINT32 Numd:12;             /* Number of Dwords */
+  UINT32 Rsvd2:4;             /* Reserved as of Nvm Express 1.1 Spec */
+} NVME_ADMIN_GET_LOG_PAGE;
+
+//
+// NvmExpress Admin Set Features Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Fid:8;               /* Feature Identifier */
+  UINT32 Rsvd1:23;
+  UINT32 Sv:1;                /* Save */
+} NVME_ADMIN_SET_FEATURES;
+
+//
+// NvmExpress Admin Format NVM Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Lbaf:4;              /* LBA Format */
+  UINT32 Ms:1;                /* Metadata Settings */
+  UINT32 Pi:3;                /* Protection Information */
+  UINT32 Pil:1;               /* Protection Information Location */
+  UINT32 Ses:3;               /* Secure Erase Settings */
+  UINT32 Rsvd1:20;
+} NVME_ADMIN_FORMAT_NVM;
+
+//
+// NvmExpress Admin Security Receive Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Rsvd1:8;
+  UINT32 Spsp:16;             /* SP Specific */
+  UINT32 Secp:8;              /* Security Protocol */
+  //
+  // CDW 11
+  //
+  UINT32 Al;                  /* Allocation Length */
+} NVME_ADMIN_SECURITY_RECEIVE;
+
+//
+// NvmExpress Admin Security Send Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Rsvd1:8;
+  UINT32 Spsp:16;             /* SP Specific */
+  UINT32 Secp:8;              /* Security Protocol */
+  //
+  // CDW 11
+  //
+  UINT32 Tl;                  /* Transfer Length */
+} NVME_ADMIN_SECURITY_SEND;
+
+typedef union {
+  NVME_ADMIN_IDENTIFY                   Identify;
+  NVME_ADMIN_CRIOCQ                     CrIoCq;
+  NVME_ADMIN_CRIOSQ                     CrIoSq;
+  NVME_ADMIN_DEIOCQ                     DeIoCq;
+  NVME_ADMIN_DEIOSQ                     DeIoSq;
+  NVME_ADMIN_ABORT                      Abort;
+  NVME_ADMIN_FIRMWARE_ACTIVATE          Activate;
+  NVME_ADMIN_FIRMWARE_IMAGE_DOWNLOAD    FirmwareImageDownload;
+  NVME_ADMIN_GET_FEATURES               GetFeatures;
+  NVME_ADMIN_GET_LOG_PAGE               GetLogPage;
+  NVME_ADMIN_SET_FEATURES               SetFeatures;
+  NVME_ADMIN_FORMAT_NVM                 FormatNvm;
+  NVME_ADMIN_SECURITY_RECEIVE           SecurityReceive;
+  NVME_ADMIN_SECURITY_SEND              SecuritySend;
+} NVME_ADMIN_CMD;
+
+typedef struct {
+  UINT32 Cdw10;
+  UINT32 Cdw11;
+  UINT32 Cdw12;
+  UINT32 Cdw13;
+  UINT32 Cdw14;
+  UINT32 Cdw15;
+} NVME_RAW;
+
+typedef union {
+  NVME_ADMIN_CMD Admin;   // Union of Admin commands
+  NVME_CMD       Nvm;     // Union of Nvm commands
+  NVME_RAW       Raw;
+} NVME_PAYLOAD;
+
+//
+// Submission Queue
+//
+typedef struct {
+  //
+  // CDW 0, Common to all comnmands
+  //
+  UINT8  Opc;               // Opcode
+  UINT8  Fuse:2;            // Fused Operation
+  UINT8  Rsvd1:5;
+  UINT8  Psdt:1;            // PRP or SGL for Data Transfer
+  UINT16 Cid;               // Command Identifier
+
+  //
+  // CDW 1
+  //
+  UINT32 Nsid;              // Namespace Identifier
+
+  //
+  // CDW 2,3
+  //
+  UINT64 Rsvd2;
+
+  //
+  // CDW 4,5
+  //
+  UINT64 Mptr;              // Metadata Pointer
+
+  //
+  // CDW 6-9
+  //
+  UINT64 Prp[2];            // First and second PRP entries
+
+  NVME_PAYLOAD Payload;
+
+} NVME_SQ;
+
+//
+// Completion Queue
+//
+typedef struct {
+  //
+  // CDW 0
+  //
+  UINT32 Dword0;
+  //
+  // CDW 1
+  //
+  UINT32 Rsvd1;
+  //
+  // CDW 2
+  //
+  UINT16 Sqhd;              // Submission Queue Head Pointer
+  UINT16 Sqid;              // Submission Queue Identifier
+  //
+  // CDW 3
+  //
+  UINT16 Cid;               // Command Identifier
+  UINT16 Pt:1;              // Phase Tag
+  UINT16 Sc:8;              // Status Code
+  UINT16 Sct:3;             // Status Code Type
+  UINT16 Rsvd2:2;
+  UINT16 Mo:1;              // More
+  UINT16 Dnr:1;             // Do Not Retry
+} NVME_CQ;
+
+//
+// Nvm Express Admin cmd opcodes
+//
+#define NVME_ADMIN_DEIOSQ_CMD                0x00
+#define NVME_ADMIN_CRIOSQ_CMD                0x01
+#define NVME_ADMIN_GET_LOG_PAGE_CMD          0x02
+#define NVME_ADMIN_DEIOCQ_CMD                0x04
+#define NVME_ADMIN_CRIOCQ_CMD                0x05
+#define NVME_ADMIN_IDENTIFY_CMD              0x06
+#define NVME_ADMIN_ABORT_CMD                 0x08
+#define NVME_ADMIN_SET_FEATURES_CMD          0x09
+#define NVME_ADMIN_GET_FEATURES_CMD          0x0A
+#define NVME_ADMIN_ASYNC_EVENT_REQUEST_CMD   0x0C
+#define NVME_ADMIN_NAMESACE_MANAGEMENT_CMD   0x0D
+#define NVME_ADMIN_FW_COMMIT_CMD             0x10
+#define NVME_ADMIN_FW_IAMGE_DOWNLOAD_CMD     0x11
+#define NVME_ADMIN_NAMESACE_ATTACHMENT_CMD   0x15
+#define NVME_ADMIN_FORMAT_NVM_CMD            0x80
+#define NVME_ADMIN_SECURITY_SEND_CMD         0x81
+#define NVME_ADMIN_SECURITY_RECEIVE_CMD      0x82
+
+#define NVME_IO_FLUSH_OPC                    0
+#define NVME_IO_WRITE_OPC                    1
+#define NVME_IO_READ_OPC                     2
+
+//
+// Offset from the beginning of private data queue buffer
+//
+#define NVME_ASQ_BUF_OFFSET                  EFI_PAGE_SIZE
+
+/**
+  Initialize the Nvm Express controller.
+
+  @param[in] Private                 The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The NVM Express Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+NvmeControllerInit (
+  IN NVME_CONTROLLER_PRIVATE_DATA    *Private
+  );
+
+/**
+  Get identify controller data.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Buffer           The buffer used to store the identify controller data.
+
+  @return EFI_SUCCESS      Successfully get the identify controller data.
+  @return EFI_DEVICE_ERROR Fail to get the identify controller data.
+
+**/
+EFI_STATUS
+NvmeIdentifyController (
+  IN NVME_CONTROLLER_PRIVATE_DATA       *Private,
+  IN VOID                               *Buffer
+  );
+
+/**
+  Get specified identify namespace data.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  NamespaceId      The specified namespace identifier.
+  @param  Buffer           The buffer used to store the identify namespace data.
+
+  @return EFI_SUCCESS      Successfully get the identify namespace data.
+  @return EFI_DEVICE_ERROR Fail to get the identify namespace data.
+
+**/
+EFI_STATUS
+NvmeIdentifyNamespace (
+  IN NVME_CONTROLLER_PRIVATE_DATA      *Private,
+  IN UINT32                            NamespaceId,
+  IN VOID                              *Buffer
+  );
+
+#pragma pack()
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressPassthru.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressPassthru.c
new file mode 100644
index 0000000000..f9871527dd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressPassthru.c
@@ -0,0 +1,907 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  (C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+  Copyright (c) 2013 - 2015, 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 "NvmExpress.h"
+
+/**
+  Dump the execution status from a given completion queue entry.
+
+  @param[in]     Cq               A pointer to the NVME_CQ item.
+
+**/
+VOID
+NvmeDumpStatus (
+  IN NVME_CQ             *Cq
+  )
+{
+  DEBUG ((EFI_D_VERBOSE, "Dump NVMe Completion Entry Status from [0x%x]:\n", Cq));
+
+  DEBUG ((EFI_D_VERBOSE, "  SQ Identifier : [0x%x], Phase Tag : [%d], Cmd Identifier : [0x%x]\n", Cq->Sqid, Cq->Pt, Cq->Cid));
+
+  DEBUG ((EFI_D_VERBOSE, "  NVMe Cmd Execution Result - "));
+
+  switch (Cq->Sct) {
+    case 0x0:
+      switch (Cq->Sc) {
+        case 0x0:
+          DEBUG ((EFI_D_VERBOSE, "Successful Completion\n"));
+          break;
+        case 0x1:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Command Opcode\n"));
+          break;
+        case 0x2:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Field in Command\n"));
+          break;
+        case 0x3:
+          DEBUG ((EFI_D_VERBOSE, "Command ID Conflict\n"));
+          break;
+        case 0x4:
+          DEBUG ((EFI_D_VERBOSE, "Data Transfer Error\n"));
+          break;
+        case 0x5:
+          DEBUG ((EFI_D_VERBOSE, "Commands Aborted due to Power Loss Notification\n"));
+          break;
+        case 0x6:
+          DEBUG ((EFI_D_VERBOSE, "Internal Device Error\n"));
+          break;
+        case 0x7:
+          DEBUG ((EFI_D_VERBOSE, "Command Abort Requested\n"));
+          break;
+        case 0x8:
+          DEBUG ((EFI_D_VERBOSE, "Command Aborted due to SQ Deletion\n"));
+          break;
+        case 0x9:
+          DEBUG ((EFI_D_VERBOSE, "Command Aborted due to Failed Fused Command\n"));
+          break;
+        case 0xA:
+          DEBUG ((EFI_D_VERBOSE, "Command Aborted due to Missing Fused Command\n"));
+          break;
+        case 0xB:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Namespace or Format\n"));
+          break;
+        case 0xC:
+          DEBUG ((EFI_D_VERBOSE, "Command Sequence Error\n"));
+          break;
+        case 0xD:
+          DEBUG ((EFI_D_VERBOSE, "Invalid SGL Last Segment Descriptor\n"));
+          break;
+        case 0xE:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Number of SGL Descriptors\n"));
+          break;
+        case 0xF:
+          DEBUG ((EFI_D_VERBOSE, "Data SGL Length Invalid\n"));
+          break;
+        case 0x10:
+          DEBUG ((EFI_D_VERBOSE, "Metadata SGL Length Invalid\n"));
+          break;
+        case 0x11:
+          DEBUG ((EFI_D_VERBOSE, "SGL Descriptor Type Invalid\n"));
+          break;
+        case 0x80:
+          DEBUG ((EFI_D_VERBOSE, "LBA Out of Range\n"));
+          break;
+        case 0x81:
+          DEBUG ((EFI_D_VERBOSE, "Capacity Exceeded\n"));
+          break;
+        case 0x82:
+          DEBUG ((EFI_D_VERBOSE, "Namespace Not Ready\n"));
+          break;
+        case 0x83:
+          DEBUG ((EFI_D_VERBOSE, "Reservation Conflict\n"));
+          break;
+      }
+      break;
+
+    case 0x1:
+      switch (Cq->Sc) {
+        case 0x0:
+          DEBUG ((EFI_D_VERBOSE, "Completion Queue Invalid\n"));
+          break;
+        case 0x1:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Queue Identifier\n"));
+          break;
+        case 0x2:
+          DEBUG ((EFI_D_VERBOSE, "Maximum Queue Size Exceeded\n"));
+          break;
+        case 0x3:
+          DEBUG ((EFI_D_VERBOSE, "Abort Command Limit Exceeded\n"));
+          break;
+        case 0x5:
+          DEBUG ((EFI_D_VERBOSE, "Asynchronous Event Request Limit Exceeded\n"));
+          break;
+        case 0x6:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Firmware Slot\n"));
+          break;
+        case 0x7:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Firmware Image\n"));
+          break;
+        case 0x8:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Interrupt Vector\n"));
+          break;
+        case 0x9:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Log Page\n"));
+          break;
+        case 0xA:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Format\n"));
+          break;
+        case 0xB:
+          DEBUG ((EFI_D_VERBOSE, "Firmware Application Requires Conventional Reset\n"));
+          break;
+        case 0xC:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Queue Deletion\n"));
+          break;
+        case 0xD:
+          DEBUG ((EFI_D_VERBOSE, "Feature Identifier Not Saveable\n"));
+          break;
+        case 0xE:
+          DEBUG ((EFI_D_VERBOSE, "Feature Not Changeable\n"));
+          break;
+        case 0xF:
+          DEBUG ((EFI_D_VERBOSE, "Feature Not Namespace Specific\n"));
+          break;
+        case 0x10:
+          DEBUG ((EFI_D_VERBOSE, "Firmware Application Requires NVM Subsystem Reset\n"));
+          break;
+        case 0x80:
+          DEBUG ((EFI_D_VERBOSE, "Conflicting Attributes\n"));
+          break;
+        case 0x81:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Protection Information\n"));
+          break;
+        case 0x82:
+          DEBUG ((EFI_D_VERBOSE, "Attempted Write to Read Only Range\n"));
+          break;
+      }
+      break;
+
+    case 0x2:
+      switch (Cq->Sc) {
+        case 0x80:
+          DEBUG ((EFI_D_VERBOSE, "Write Fault\n"));
+          break;
+        case 0x81:
+          DEBUG ((EFI_D_VERBOSE, "Unrecovered Read Error\n"));
+          break;
+        case 0x82:
+          DEBUG ((EFI_D_VERBOSE, "End-to-end Guard Check Error\n"));
+          break;
+        case 0x83:
+          DEBUG ((EFI_D_VERBOSE, "End-to-end Application Tag Check Error\n"));
+          break;
+        case 0x84:
+          DEBUG ((EFI_D_VERBOSE, "End-to-end Reference Tag Check Error\n"));
+          break;
+        case 0x85:
+          DEBUG ((EFI_D_VERBOSE, "Compare Failure\n"));
+          break;
+        case 0x86:
+          DEBUG ((EFI_D_VERBOSE, "Access Denied\n"));
+          break;
+      }
+      break;
+
+    default:
+      break;
+  }
+}
+
+/**
+  Create PRP lists for data transfer which is larger than 2 memory pages.
+  Note here we calcuate the number of required PRP lists and allocate them at one time.
+
+  @param[in]     PciIo               A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param[in]     PhysicalAddr        The physical base address of data buffer.
+  @param[in]     Pages               The number of pages to be transfered.
+  @param[out]    PrpListHost         The host base address of PRP lists.
+  @param[in,out] PrpListNo           The number of PRP List.
+  @param[out]    Mapping             The mapping value returned from PciIo.Map().
+
+  @retval The pointer to the first PRP List of the PRP lists.
+
+**/
+VOID*
+NvmeCreatePrpList (
+  IN     EFI_PCI_IO_PROTOCOL          *PciIo,
+  IN     EFI_PHYSICAL_ADDRESS         PhysicalAddr,
+  IN     UINTN                        Pages,
+     OUT VOID                         **PrpListHost,
+  IN OUT UINTN                        *PrpListNo,
+     OUT VOID                         **Mapping
+  )
+{
+  UINTN                       PrpEntryNo;
+  UINT64                      PrpListBase;
+  UINTN                       PrpListIndex;
+  UINTN                       PrpEntryIndex;
+  UINT64                      Remainder;
+  EFI_PHYSICAL_ADDRESS        PrpListPhyAddr;
+  UINTN                       Bytes;
+  EFI_STATUS                  Status;
+
+  //
+  // The number of Prp Entry in a memory page.
+  //
+  PrpEntryNo = EFI_PAGE_SIZE / sizeof (UINT64);
+
+  //
+  // Calculate total PrpList number.
+  //
+  *PrpListNo = (UINTN)DivU64x64Remainder ((UINT64)Pages, (UINT64)PrpEntryNo - 1, &Remainder);
+  if (*PrpListNo == 0) {
+    *PrpListNo = 1;
+  } else if ((Remainder != 0) && (Remainder != 1)) {
+    *PrpListNo += 1;
+  } else if (Remainder == 1) {
+    Remainder = PrpEntryNo;
+  } else if (Remainder == 0) {
+    Remainder = PrpEntryNo - 1;
+  }
+
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    *PrpListNo,
+                    PrpListHost,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (*PrpListNo);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    *PrpListHost,
+                    &Bytes,
+                    &PrpListPhyAddr,
+                    Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (*PrpListNo))) {
+    DEBUG ((EFI_D_ERROR, "NvmeCreatePrpList: create PrpList failure!\n"));
+    goto EXIT;
+  }
+  //
+  // Fill all PRP lists except of last one.
+  //
+  ZeroMem (*PrpListHost, Bytes);
+  for (PrpListIndex = 0; PrpListIndex < *PrpListNo - 1; ++PrpListIndex) {
+    PrpListBase = *(UINT64*)PrpListHost + PrpListIndex * EFI_PAGE_SIZE;
+
+    for (PrpEntryIndex = 0; PrpEntryIndex < PrpEntryNo; ++PrpEntryIndex) {
+      if (PrpEntryIndex != PrpEntryNo - 1) {
+        //
+        // Fill all PRP entries except of last one.
+        //
+        *((UINT64*)(UINTN)PrpListBase + PrpEntryIndex) = PhysicalAddr;
+        PhysicalAddr += EFI_PAGE_SIZE;
+      } else {
+        //
+        // Fill last PRP entries with next PRP List pointer.
+        //
+        *((UINT64*)(UINTN)PrpListBase + PrpEntryIndex) = PrpListPhyAddr + (PrpListIndex + 1) * EFI_PAGE_SIZE;
+      }
+    }
+  }
+  //
+  // Fill last PRP list.
+  //
+  PrpListBase = *(UINT64*)PrpListHost + PrpListIndex * EFI_PAGE_SIZE;
+  for (PrpEntryIndex = 0; PrpEntryIndex < Remainder; ++PrpEntryIndex) {
+    *((UINT64*)(UINTN)PrpListBase + PrpEntryIndex) = PhysicalAddr;
+    PhysicalAddr += EFI_PAGE_SIZE;
+  }
+
+  return (VOID*)(UINTN)PrpListPhyAddr;
+
+EXIT:
+  PciIo->FreeBuffer (PciIo, *PrpListNo, *PrpListHost);
+  return NULL;
+}
+
+
+/**
+  Sends an NVM Express Command Packet to an NVM Express controller or namespace. This function supports
+  both blocking I/O and non-blocking I/O. The blocking I/O functionality is required, and the non-blocking
+  I/O functionality is optional.
+
+
+  @param[in]     This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]     NamespaceId         A 32 bit namespace ID as defined in the NVMe specification to which the NVM Express Command
+                                     Packet will be sent.  A value of 0 denotes the NVM Express controller, a value of all 0xFF's
+                                     (all bytes are 0xFF) in the namespace ID specifies that the command packet should be sent to
+                                     all valid namespaces.
+  @param[in,out] Packet              A pointer to the NVM Express Command Packet.
+  @param[in]     Event               If non-blocking I/O is not supported then Event is ignored, and blocking I/O is performed.
+                                     If Event is NULL, then blocking I/O is performed. If Event is not NULL and non-blocking I/O
+                                     is supported, then non-blocking I/O is performed, and Event will be signaled when the NVM
+                                     Express Command Packet completes.
+
+  @retval EFI_SUCCESS                The NVM Express Command Packet was sent by the host. TransferLength bytes were transferred
+                                     to, or from DataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The NVM Express Command Packet was not executed. The number of bytes that could be transferred
+                                     is returned in TransferLength.
+  @retval EFI_NOT_READY              The NVM Express Command Packet could not be sent because the controller is not ready. The caller
+                                     may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting to send the NVM Express Command Packet.
+  @retval EFI_INVALID_PARAMETER      NamespaceId or the contents of EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET are invalid. The NVM
+                                     Express Command Packet was not sent, so no additional status information is available.
+  @retval EFI_UNSUPPORTED            The command described by the NVM Express Command Packet is not supported by the NVM Express
+                                     controller. The NVM Express Command Packet was not sent so no additional status information
+                                     is available.
+  @retval EFI_TIMEOUT                A timeout occurred while waiting for the NVM Express Command Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressPassThru (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     UINT32                                      NamespaceId,
+  IN OUT EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET    *Packet,
+  IN     EFI_EVENT                                   Event OPTIONAL
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_STATUS                    Status;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  NVME_SQ                       *Sq;
+  NVME_CQ                       *Cq;
+  UINT8                         QueueType;
+  UINT32                        Bytes;
+  UINT16                        Offset;
+  EFI_EVENT                     TimerEvent;
+  EFI_PCI_IO_PROTOCOL_OPERATION Flag;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  VOID                          *MapData;
+  VOID                          *MapMeta;
+  VOID                          *MapPrpList;
+  UINTN                         MapLength;
+  UINT64                        *Prp;
+  VOID                          *PrpListHost;
+  UINTN                         PrpListNo;
+  UINT32                        Data;
+
+  //
+  // check the data fields in Packet parameter.
+  //
+  if ((This == NULL) || (Packet == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Packet->NvmeCmd == NULL) || (Packet->NvmeCompletion == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Packet->QueueType != NVME_ADMIN_QUEUE && Packet->QueueType != NVME_IO_QUEUE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private     = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (This);
+  PciIo       = Private->PciIo;
+  MapData     = NULL;
+  MapMeta     = NULL;
+  MapPrpList  = NULL;
+  PrpListHost = NULL;
+  PrpListNo   = 0;
+  Prp         = NULL;
+  TimerEvent  = NULL;
+  Status      = EFI_SUCCESS;
+
+  QueueType = Packet->QueueType;
+  Sq  = Private->SqBuffer[QueueType] + Private->SqTdbl[QueueType].Sqt;
+  Cq  = Private->CqBuffer[QueueType] + Private->CqHdbl[QueueType].Cqh;
+
+  if (Packet->NvmeCmd->Nsid != NamespaceId) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (Sq, sizeof (NVME_SQ));
+  Sq->Opc  = (UINT8)Packet->NvmeCmd->Cdw0.Opcode;
+  Sq->Fuse = (UINT8)Packet->NvmeCmd->Cdw0.FusedOperation;
+  Sq->Cid  = Private->Cid[QueueType]++;
+  Sq->Nsid = Packet->NvmeCmd->Nsid;
+
+  //
+  // Currently we only support PRP for data transfer, SGL is NOT supported.
+  //
+  ASSERT (Sq->Psdt == 0);
+  if (Sq->Psdt != 0) {
+    DEBUG ((EFI_D_ERROR, "NvmExpressPassThru: doesn't support SGL mechanism\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  Sq->Prp[0] = (UINT64)(UINTN)Packet->TransferBuffer;
+  //
+  // If the NVMe cmd has data in or out, then mapping the user buffer to the PCI controller specific addresses.
+  // Note here we don't handle data buffer for CreateIOSubmitionQueue and CreateIOCompletionQueue cmds because
+  // these two cmds are special which requires their data buffer must support simultaneous access by both the
+  // processor and a PCI Bus Master. It's caller's responsbility to ensure this.
+  //
+  if (((Sq->Opc & (BIT0 | BIT1)) != 0) && (Sq->Opc != NVME_ADMIN_CRIOCQ_CMD) && (Sq->Opc != NVME_ADMIN_CRIOSQ_CMD)) {
+    if ((Sq->Opc & BIT0) != 0) {
+      Flag = EfiPciIoOperationBusMasterRead;
+    } else {
+      Flag = EfiPciIoOperationBusMasterWrite;
+    }
+
+    MapLength = Packet->TransferLength;
+    Status = PciIo->Map (
+                      PciIo,
+                      Flag,
+                      Packet->TransferBuffer,
+                      &MapLength,
+                      &PhyAddr,
+                      &MapData
+                      );
+    if (EFI_ERROR (Status) || (Packet->TransferLength != MapLength)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Sq->Prp[0] = PhyAddr;
+    Sq->Prp[1] = 0;
+
+    MapLength = Packet->MetadataLength;
+    if(Packet->MetadataBuffer != NULL) {
+      MapLength = Packet->MetadataLength;
+      Status = PciIo->Map (
+                        PciIo,
+                        Flag,
+                        Packet->MetadataBuffer,
+                        &MapLength,
+                        &PhyAddr,
+                        &MapMeta
+                        );
+      if (EFI_ERROR (Status) || (Packet->MetadataLength != MapLength)) {
+        PciIo->Unmap (
+                 PciIo,
+                 MapData
+                 );
+
+        return EFI_OUT_OF_RESOURCES;
+      }
+      Sq->Mptr = PhyAddr;
+    }
+  }
+  //
+  // If the buffer size spans more than two memory pages (page size as defined in CC.Mps),
+  // then build a PRP list in the second PRP submission queue entry.
+  //
+  Offset = ((UINT16)Sq->Prp[0]) & (EFI_PAGE_SIZE - 1);
+  Bytes  = Packet->TransferLength;
+
+  if ((Offset + Bytes) > (EFI_PAGE_SIZE * 2)) {
+    //
+    // Create PrpList for remaining data buffer.
+    //
+    PhyAddr = (Sq->Prp[0] + EFI_PAGE_SIZE) & ~(EFI_PAGE_SIZE - 1);
+    Prp = NvmeCreatePrpList (PciIo, PhyAddr, EFI_SIZE_TO_PAGES(Offset + Bytes) - 1, &PrpListHost, &PrpListNo, &MapPrpList);
+    if (Prp == NULL) {
+      goto EXIT;
+    }
+
+    Sq->Prp[1] = (UINT64)(UINTN)Prp;
+  } else if ((Offset + Bytes) > EFI_PAGE_SIZE) {
+    Sq->Prp[1] = (Sq->Prp[0] + EFI_PAGE_SIZE) & ~(EFI_PAGE_SIZE - 1);
+  }
+
+  if(Packet->NvmeCmd->Flags & CDW2_VALID) {
+    Sq->Rsvd2 = (UINT64)Packet->NvmeCmd->Cdw2;
+  }
+  if(Packet->NvmeCmd->Flags & CDW3_VALID) {
+    Sq->Rsvd2 |= LShiftU64 ((UINT64)Packet->NvmeCmd->Cdw3, 32);
+  }
+  if(Packet->NvmeCmd->Flags & CDW10_VALID) {
+    Sq->Payload.Raw.Cdw10 = Packet->NvmeCmd->Cdw10;
+  }
+  if(Packet->NvmeCmd->Flags & CDW11_VALID) {
+    Sq->Payload.Raw.Cdw11 = Packet->NvmeCmd->Cdw11;
+  }
+  if(Packet->NvmeCmd->Flags & CDW12_VALID) {
+    Sq->Payload.Raw.Cdw12 = Packet->NvmeCmd->Cdw12;
+  }
+  if(Packet->NvmeCmd->Flags & CDW13_VALID) {
+    Sq->Payload.Raw.Cdw13 = Packet->NvmeCmd->Cdw13;
+  }
+  if(Packet->NvmeCmd->Flags & CDW14_VALID) {
+    Sq->Payload.Raw.Cdw14 = Packet->NvmeCmd->Cdw14;
+  }
+  if(Packet->NvmeCmd->Flags & CDW15_VALID) {
+    Sq->Payload.Raw.Cdw15 = Packet->NvmeCmd->Cdw15;
+  }
+
+  //
+  // Ring the submission queue doorbell.
+  //
+  Private->SqTdbl[QueueType].Sqt ^= 1;
+  Data = ReadUnaligned32 ((UINT32*)&Private->SqTdbl[QueueType]);
+  PciIo->Mem.Write (
+               PciIo,
+               EfiPciIoWidthUint32,
+               NVME_BAR,
+               NVME_SQTDBL_OFFSET(QueueType, Private->Cap.Dstrd),
+               1,
+               &Data
+               );
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &TimerEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  Status = gBS->SetTimer(TimerEvent, TimerRelative, Packet->CommandTimeout);
+
+  if (EFI_ERROR(Status)) {
+    goto EXIT;
+  }
+
+  //
+  // Wait for completion queue to get filled in.
+  //
+  Status = EFI_TIMEOUT;
+  while (EFI_ERROR (gBS->CheckEvent (TimerEvent))) {
+    if (Cq->Pt != Private->Pt[QueueType]) {
+      Status = EFI_SUCCESS;
+      break;
+    }
+  }
+
+  //
+  // Check the NVMe cmd execution result
+  //
+  if (Status != EFI_TIMEOUT) {
+    if ((Cq->Sct == 0) && (Cq->Sc == 0)) {
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_DEVICE_ERROR;
+      //
+      // Copy the Respose Queue entry for this command to the callers response buffer
+      //
+      CopyMem(Packet->NvmeCompletion, Cq, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+    
+      //
+      // Dump every completion entry status for debugging.
+      //
+      DEBUG_CODE_BEGIN();
+        NvmeDumpStatus(Cq);
+      DEBUG_CODE_END();
+    }
+  }
+
+  if ((Private->CqHdbl[QueueType].Cqh ^= 1) == 0) {
+    Private->Pt[QueueType] ^= 1;
+  }
+
+  Data = ReadUnaligned32 ((UINT32*)&Private->CqHdbl[QueueType]);
+  PciIo->Mem.Write (
+               PciIo,
+               EfiPciIoWidthUint32,
+               NVME_BAR,
+               NVME_CQHDBL_OFFSET(QueueType, Private->Cap.Dstrd),
+               1,
+               &Data
+               );
+
+EXIT:
+  if (MapData != NULL) {
+    PciIo->Unmap (
+             PciIo,
+             MapData
+             );
+  }
+
+  if (MapMeta != NULL) {
+    PciIo->Unmap (
+             PciIo,
+             MapMeta
+             );
+  }
+
+  if (MapPrpList != NULL) {
+    PciIo->Unmap (
+             PciIo,
+             MapPrpList
+             );
+  }
+
+  if (Prp != NULL) {
+    PciIo->FreeBuffer (PciIo, PrpListNo, PrpListHost);
+  }
+
+  if (TimerEvent != NULL) {
+    gBS->CloseEvent (TimerEvent);
+  }
+  return Status;
+}
+
+/**
+  Used to retrieve the next namespace ID for this NVM Express controller.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.GetNextNamespace() function retrieves the next valid
+  namespace ID on this NVM Express controller.
+
+  If on input the value pointed to by NamespaceId is 0xFFFFFFFF, then the first valid namespace
+  ID defined on the NVM Express controller is returned in the location pointed to by NamespaceId
+  and a status of EFI_SUCCESS is returned.
+
+  If on input the value pointed to by NamespaceId is an invalid namespace ID other than 0xFFFFFFFF,
+  then EFI_INVALID_PARAMETER is returned.
+
+  If on input the value pointed to by NamespaceId is a valid namespace ID, then the next valid
+  namespace ID on the NVM Express controller is returned in the location pointed to by NamespaceId,
+  and EFI_SUCCESS is returned.
+
+  If the value pointed to by NamespaceId is the namespace ID of the last namespace on the NVM
+  Express controller, then EFI_NOT_FOUND is returned.
+
+  @param[in]     This           A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in,out] NamespaceId    On input, a pointer to a legal NamespaceId for an NVM Express
+                                namespace present on the NVM Express controller. On output, a
+                                pointer to the next NamespaceId of an NVM Express namespace on
+                                an NVM Express controller. An input value of 0xFFFFFFFF retrieves
+                                the first NamespaceId for an NVM Express namespace present on an
+                                NVM Express controller.
+
+  @retval EFI_SUCCESS           The Namespace ID of the next Namespace was returned.
+  @retval EFI_NOT_FOUND         There are no more namespaces defined on this controller.
+  @retval EFI_INVALID_PARAMETER NamespaceId is an invalid value other than 0xFFFFFFFF.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressGetNextNamespace (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN OUT UINT32                                      *NamespaceId
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA     *Private;
+  NVME_ADMIN_NAMESPACE_DATA        *NamespaceData;
+  UINT32                           NextNamespaceId;
+  EFI_STATUS                       Status;
+
+  if ((This == NULL) || (NamespaceId == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  NamespaceData = NULL;
+  Status        = EFI_NOT_FOUND;
+
+  Private = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (This);
+  //
+  // If the NamespaceId input value is 0xFFFFFFFF, then get the first valid namespace ID
+  //
+  if (*NamespaceId == 0xFFFFFFFF) {
+    //
+    // Start with the first namespace ID
+    //
+    NextNamespaceId = 1;
+    //
+    // Allocate buffer for Identify Namespace data.
+    //
+    NamespaceData = (NVME_ADMIN_NAMESPACE_DATA *)AllocateZeroPool (sizeof (NVME_ADMIN_NAMESPACE_DATA));
+
+    if (NamespaceData == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    Status = NvmeIdentifyNamespace (Private, NextNamespaceId, NamespaceData);
+    if (EFI_ERROR(Status)) {
+      goto Done;
+    }
+
+    *NamespaceId = NextNamespaceId;
+  } else {
+    if (*NamespaceId >= Private->ControllerData->Nn) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    NextNamespaceId = *NamespaceId + 1;
+    //
+    // Allocate buffer for Identify Namespace data.
+    //
+    NamespaceData = (NVME_ADMIN_NAMESPACE_DATA *)AllocateZeroPool (sizeof (NVME_ADMIN_NAMESPACE_DATA));
+    if (NamespaceData == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    Status = NvmeIdentifyNamespace (Private, NextNamespaceId, NamespaceData);
+    if (EFI_ERROR(Status)) {
+      goto Done;
+    }
+
+    *NamespaceId = NextNamespaceId;
+  }
+
+Done:
+  if (NamespaceData != NULL) {
+    FreePool(NamespaceData);
+  }
+
+  return Status;
+}
+
+/**
+  Used to translate a device path node to a namespace ID.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.GetNamespace() function determines the namespace ID associated with the
+  namespace described by DevicePath.
+
+  If DevicePath is a device path node type that the NVM Express Pass Thru driver supports, then the NVM Express
+  Pass Thru driver will attempt to translate the contents DevicePath into a namespace ID.
+
+  If this translation is successful, then that namespace ID is returned in NamespaceId, and EFI_SUCCESS is returned
+
+  @param[in]  This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]  DevicePath          A pointer to the device path node that describes an NVM Express namespace on
+                                  the NVM Express controller.
+  @param[out] NamespaceId         The NVM Express namespace ID contained in the device path node.
+
+  @retval EFI_SUCCESS             DevicePath was successfully translated to NamespaceId.
+  @retval EFI_INVALID_PARAMETER   If DevicePath or NamespaceId are NULL, then EFI_INVALID_PARAMETER is returned.
+  @retval EFI_UNSUPPORTED         If DevicePath is not a device path node type that the NVM Express Pass Thru driver
+                                  supports, then EFI_UNSUPPORTED is returned.
+  @retval EFI_NOT_FOUND           If DevicePath is a device path node type that the NVM Express Pass Thru driver
+                                  supports, but there is not a valid translation from DevicePath to a namespace ID,
+                                  then EFI_NOT_FOUND is returned.
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressGetNamespace (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     EFI_DEVICE_PATH_PROTOCOL                    *DevicePath,
+     OUT UINT32                                      *NamespaceId
+  )
+{
+  NVME_NAMESPACE_DEVICE_PATH       *Node;
+
+  if ((This == NULL) || (DevicePath == NULL) || (NamespaceId == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DevicePath->Type != MESSAGING_DEVICE_PATH) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Node = (NVME_NAMESPACE_DEVICE_PATH *)DevicePath;
+
+  if (DevicePath->SubType == MSG_NVME_NAMESPACE_DP) {
+    if (DevicePathNodeLength(DevicePath) != sizeof(NVME_NAMESPACE_DEVICE_PATH)) {
+      return EFI_NOT_FOUND;
+    }
+
+    *NamespaceId = Node->NamespaceId;
+
+    return EFI_SUCCESS;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+}
+
+/**
+  Used to allocate and build a device path node for an NVM Express namespace on an NVM Express controller.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.BuildDevicePath() function allocates and builds a single device
+  path node for the NVM Express namespace specified by NamespaceId.
+
+  If the NamespaceId is not valid, then EFI_NOT_FOUND is returned.
+
+  If DevicePath is NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If there are not enough resources to allocate the device path node, then EFI_OUT_OF_RESOURCES is returned.
+
+  Otherwise, DevicePath is allocated with the boot service AllocatePool(), the contents of DevicePath are
+  initialized to describe the NVM Express namespace specified by NamespaceId, and EFI_SUCCESS is returned.
+
+  @param[in]     This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]     NamespaceId         The NVM Express namespace ID  for which a device path node is to be
+                                     allocated and built. Caller must set the NamespaceId to zero if the
+                                     device path node will contain a valid UUID.
+  @param[in,out] DevicePath          A pointer to a single device path node that describes the NVM Express
+                                     namespace specified by NamespaceId. This function is responsible for
+                                     allocating the buffer DevicePath with the boot service AllocatePool().
+                                     It is the caller's responsibility to free DevicePath when the caller
+                                     is finished with DevicePath.
+  @retval EFI_SUCCESS                The device path node that describes the NVM Express namespace specified
+                                     by NamespaceId was allocated and returned in DevicePath.
+  @retval EFI_NOT_FOUND              The NamespaceId is not valid.
+  @retval EFI_INVALID_PARAMETER      DevicePath is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources to allocate the DevicePath node.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressBuildDevicePath (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     UINT32                                      NamespaceId,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL                    **DevicePath
+  )
+{
+  NVME_NAMESPACE_DEVICE_PATH     *Node;
+  NVME_CONTROLLER_PRIVATE_DATA   *Private;
+  EFI_STATUS                     Status;
+  NVME_ADMIN_NAMESPACE_DATA      *NamespaceData;
+
+  //
+  // Validate parameters
+  //
+  if ((This == NULL) || (DevicePath == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (NamespaceId == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  Status  = EFI_SUCCESS;
+  Private = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (This);
+
+  Node = (NVME_NAMESPACE_DEVICE_PATH *)AllocateZeroPool (sizeof (NVME_NAMESPACE_DEVICE_PATH));
+  if (Node == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Node->Header.Type    = MESSAGING_DEVICE_PATH;
+  Node->Header.SubType = MSG_NVME_NAMESPACE_DP;
+  SetDevicePathNodeLength (&Node->Header, sizeof (NVME_NAMESPACE_DEVICE_PATH));
+  Node->NamespaceId    = NamespaceId;
+
+  //
+  // Allocate a buffer for Identify Namespace data.
+  //
+  NamespaceData = NULL;
+  NamespaceData = AllocateZeroPool(sizeof (NVME_ADMIN_NAMESPACE_DATA));
+  if(NamespaceData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+
+  //
+  // Get UUID from specified Identify Namespace data.
+  //
+  Status = NvmeIdentifyNamespace (
+             Private,
+             NamespaceId,
+             (VOID *)NamespaceData
+             );
+
+  if (EFI_ERROR(Status)) {
+    goto Exit;
+  }
+
+  Node->NamespaceUuid = NamespaceData->Eui64;
+
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *)Node;
+
+Exit:
+  if(NamespaceData != NULL) {
+    FreePool (NamespaceData);
+  }
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Node);
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.c
new file mode 100644
index 0000000000..580e8bcc44
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.c
@@ -0,0 +1,176 @@
+/** @file
+  EFI Component Name functions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2011, 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 "PciBus.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gPciBusComponentName = {
+  PciBusComponentNameGetDriverName,
+  PciBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gPciBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) PciBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) PciBusComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mPciBusDriverNameTable[] = {
+  { "eng;en", (CHAR16 *) L"PCI Bus Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mPciBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gPciBusComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.h
new file mode 100644
index 0000000000..e74851ed35
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.h
@@ -0,0 +1,152 @@
+/** @file
+  EFI Component Name functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+
+#ifndef _EFI_PCI_BUS_COMPONENT_NAME_H_
+#define _EFI_PCI_BUS_COMPONENT_NAME_H_
+
+extern EFI_COMPONENT_NAME_PROTOCOL   gPciBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gPciBusComponentName2;
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.c
new file mode 100644
index 0000000000..beecf79a35
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.c
@@ -0,0 +1,409 @@
+/** @file
+  Driver Binding functions for PCI Bus module.
+
+  Single PCI bus driver instance will manager all PCI Root Bridges in one EFI based firmware,
+  since all PCI Root Bridges' resources need to be managed together.
+  Supported() function will try to get PCI Root Bridge IO Protocol.
+  Start() function will get PCI Host Bridge Resource Allocation Protocol to manage all
+  PCI Root Bridges. So it means platform needs install PCI Root Bridge IO protocol for each
+  PCI Root Bus and install PCI Host Bridge Resource Allocation Protocol.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+//
+// PCI Bus Driver Global Variables
+//
+EFI_DRIVER_BINDING_PROTOCOL                   gPciBusDriverBinding = {
+  PciBusDriverBindingSupported,
+  PciBusDriverBindingStart,
+  PciBusDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+EFI_HANDLE                                    gPciHostBrigeHandles[PCI_MAX_HOST_BRIDGE_NUM];
+EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL  *gEfiIncompatiblePciDeviceSupport = NULL;
+UINTN                                         gPciHostBridgeNumber = 0;
+BOOLEAN                                       gFullEnumeration     = TRUE;
+UINT64                                        gAllOne              = 0xFFFFFFFFFFFFFFFFULL;
+UINT64                                        gAllZero             = 0;
+
+EFI_PCI_PLATFORM_PROTOCOL                     *gPciPlatformProtocol;
+EFI_PCI_OVERRIDE_PROTOCOL                     *gPciOverrideProtocol;
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_PCI_HOTPLUG_REQUEST_PROTOCOL mPciHotPlugRequest = {
+  PciHotPlugRequestNotify
+};
+
+/**
+  The Entry Point for PCI Bus module. The user code starts with this function.
+
+  Installs driver module protocols and. Creates virtual device handles for ConIn,
+  ConOut, and StdErr. Installs Simple Text In protocol, Simple Text In Ex protocol,
+  Simple Pointer protocol, Absolute Pointer protocol on those virtual handlers.
+  Installs Graphics Output protocol and/or UGA Draw protocol if needed.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurred when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusEntryPoint (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  EFI_HANDLE  Handle;
+
+  //
+  // Initializes PCI devices pool
+  //
+  InitializePciDevicePool ();
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gPciBusDriverBinding,
+             ImageHandle,
+             &gPciBusComponentName,
+             &gPciBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  if (FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    //
+    // If Hot Plug is supported, install EFI PCI Hot Plug Request protocol.
+    //
+    Handle = NULL;
+    Status = gBS->InstallProtocolInterface (
+                    &Handle,
+                    &gEfiPciHotPlugRequestProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &mPciHotPlugRequest
+                    );
+  }
+
+  return Status;
+}
+
+/**
+  Test to see if this driver supports ControllerHandle. Any ControllerHandle
+  than contains a gEfiPciRootBridgeIoProtocolGuid protocol can be supported.
+
+  @param  This                Protocol instance pointer.
+  @param  Controller          Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child.
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval EFI_ALREADY_STARTED This driver is already running on this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_DEVICE_PATH_PROTOCOL        *ParentDevicePath;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+  EFI_DEV_PATH_PTR                Node;
+
+  //
+  // Check RemainingDevicePath validation
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node, 
+    // if yes, go on checking other conditions
+    //
+    if (!IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node,
+      // check its validation
+      //
+      Node.DevPath = RemainingDevicePath;
+      if (Node.DevPath->Type != HARDWARE_DEVICE_PATH ||
+          Node.DevPath->SubType != HW_PCI_DP         ||
+          DevicePathNodeLength(Node.DevPath) != sizeof(PCI_DEVICE_PATH)) {
+        return EFI_UNSUPPORTED;
+      }
+    }
+  }
+
+  //
+  // Check if Pci Root Bridge IO protocol is installed by platform
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiPciRootBridgeIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close protocol, don't use device path protocol in the Support() function
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiDevicePathProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start this driver on ControllerHandle and enumerate Pci bus and start
+  all device under PCI bus.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child.
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle.
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle.
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+
+  //
+  // Check RemainingDevicePath validation
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node, 
+    // if yes, return EFI_SUCCESS
+    //
+    if (IsDevicePathEnd (RemainingDevicePath)) {
+      return EFI_SUCCESS;
+    }
+  }
+
+  gBS->LocateProtocol (
+         &gEfiIncompatiblePciDeviceSupportProtocolGuid,
+         NULL,
+         (VOID **) &gEfiIncompatiblePciDeviceSupport
+         );
+
+  //
+  // If PCI Platform protocol is available, get it now.
+  // If the platform implements this, it must be installed before BDS phase
+  //
+  gPciPlatformProtocol = NULL;
+  gBS->LocateProtocol (
+        &gEfiPciPlatformProtocolGuid,
+        NULL,
+        (VOID **) &gPciPlatformProtocol
+        );
+
+  //
+  // If PCI Platform protocol doesn't exist, try to Pci Override Protocol.
+  //
+  if (gPciPlatformProtocol == NULL) { 
+    gPciOverrideProtocol = NULL;
+    gBS->LocateProtocol (
+          &gEfiPciOverrideProtocolGuid,
+          NULL,
+          (VOID **) &gPciOverrideProtocol
+          );
+  }  
+
+  if (PcdGetBool (PcdPciDisableBusEnumeration)) {
+    gFullEnumeration = FALSE;
+  } else {
+    gFullEnumeration = (BOOLEAN) ((SearchHostBridgeHandle (Controller) ? FALSE : TRUE));
+  }
+
+  //
+  // Open Device Path Protocol for PCI root bridge
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );  
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Report Status Code to indicate PCI bus starts
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_PCI | EFI_IOB_PC_INIT),
+    ParentDevicePath
+    );
+
+  //
+  // Enumerate the entire host bridge
+  // After enumeration, a database that records all the device information will be created
+  //
+  //
+  Status = PciEnumerator (Controller);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Start all the devices under the entire host bridge.
+  //
+  StartPciDevices (Controller);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This              Protocol instance pointer.
+  @param  Controller        Handle of device to stop driver on.
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle.
+  @retval other             This driver was not removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+  BOOLEAN     AllChildrenStopped;
+
+  if (NumberOfChildren == 0) {
+    //
+    // Close the bus driver
+    //
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiPciRootBridgeIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    DestroyRootBridgeByHandle (
+      Controller
+      );
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Stop all the children
+  //
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    //
+    // De register all the pci device
+    //
+    Status = DeRegisterPciDevice (Controller, ChildHandleBuffer[Index]);
+
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.h
new file mode 100644
index 0000000000..b99d3181ac
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.h
@@ -0,0 +1,404 @@
+/** @file
+  Header files and data structures needed by PCI Bus module.
+
+Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+
+#ifndef _EFI_PCI_BUS_H_
+#define _EFI_PCI_BUS_H_
+
+#include <PiDxe.h>
+
+#include <Protocol/LoadedImage.h>
+#include <Protocol/PciHostBridgeResourceAllocation.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/LoadFile2.h>
+#include <Protocol/PciRootBridgeIo.h>
+#include <Protocol/PciHotPlugRequest.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PciPlatform.h>
+#include <Protocol/PciHotPlugInit.h>
+#include <Protocol/Decompress.h>
+#include <Protocol/BusSpecificDriverOverride.h>
+#include <Protocol/IncompatiblePciDeviceSupport.h>
+#include <Protocol/PciOverride.h>
+#include <Protocol/PciEnumerationComplete.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PcdLib.h>
+#include <Library/PeCoffLib.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/PeImage.h>
+#include <IndustryStandard/Acpi.h>
+
+typedef struct _PCI_IO_DEVICE              PCI_IO_DEVICE;
+typedef struct _PCI_BAR                    PCI_BAR;
+
+#define EFI_PCI_RID(Bus, Device, Function)  (((UINT32)Bus << 8) + ((UINT32)Device << 3) + (UINT32)Function)
+#define EFI_PCI_BUS_OF_RID(RID)             ((UINT32)RID >> 8)
+
+#define     EFI_PCI_IOV_POLICY_ARI           0x0001
+#define     EFI_PCI_IOV_POLICY_SRIOV         0x0002
+#define     EFI_PCI_IOV_POLICY_MRIOV         0x0004
+
+typedef enum {
+  PciBarTypeUnknown = 0,
+  PciBarTypeIo16,
+  PciBarTypeIo32,
+  PciBarTypeMem32,
+  PciBarTypePMem32,
+  PciBarTypeMem64,
+  PciBarTypePMem64,
+  PciBarTypeIo,
+  PciBarTypeMem,
+  PciBarTypeMaxType
+} PCI_BAR_TYPE;
+
+#include "ComponentName.h"
+#include "PciIo.h"
+#include "PciCommand.h"
+#include "PciDeviceSupport.h"
+#include "PciEnumerator.h"
+#include "PciEnumeratorSupport.h"
+#include "PciDriverOverride.h"
+#include "PciRomTable.h"
+#include "PciOptionRomSupport.h"
+#include "PciPowerManagement.h"
+#include "PciHotPlugSupport.h"
+#include "PciLib.h"
+
+#define VGABASE1  0x3B0
+#define VGALIMIT1 0x3BB
+
+#define VGABASE2  0x3C0
+#define VGALIMIT2 0x3DF
+
+#define ISABASE   0x100
+#define ISALIMIT  0x3FF
+
+//
+// PCI BAR parameters
+//
+struct _PCI_BAR {
+  UINT64        BaseAddress;
+  UINT64        Length;
+  UINT64        Alignment;
+  PCI_BAR_TYPE  BarType;
+  BOOLEAN       Prefetchable;
+  UINT8         MemType;
+  UINT16        Offset;
+};
+
+//
+// defined in PCI Card Specification, 8.0
+//
+#define PCI_CARD_MEMORY_BASE_0                0x1C
+#define PCI_CARD_MEMORY_LIMIT_0               0x20
+#define PCI_CARD_MEMORY_BASE_1                0x24
+#define PCI_CARD_MEMORY_LIMIT_1               0x28
+#define PCI_CARD_IO_BASE_0_LOWER              0x2C
+#define PCI_CARD_IO_BASE_0_UPPER              0x2E
+#define PCI_CARD_IO_LIMIT_0_LOWER             0x30
+#define PCI_CARD_IO_LIMIT_0_UPPER             0x32
+#define PCI_CARD_IO_BASE_1_LOWER              0x34
+#define PCI_CARD_IO_BASE_1_UPPER              0x36
+#define PCI_CARD_IO_LIMIT_1_LOWER             0x38
+#define PCI_CARD_IO_LIMIT_1_UPPER             0x3A
+#define PCI_CARD_BRIDGE_CONTROL               0x3E
+
+#define PCI_CARD_PREFETCHABLE_MEMORY_0_ENABLE BIT8
+#define PCI_CARD_PREFETCHABLE_MEMORY_1_ENABLE BIT9
+
+#define RB_IO_RANGE                           1
+#define RB_MEM32_RANGE                        2
+#define RB_PMEM32_RANGE                       3
+#define RB_MEM64_RANGE                        4
+#define RB_PMEM64_RANGE                       5
+
+#define PPB_BAR_0                             0
+#define PPB_BAR_1                             1
+#define PPB_IO_RANGE                          2
+#define PPB_MEM32_RANGE                       3
+#define PPB_PMEM32_RANGE                      4
+#define PPB_PMEM64_RANGE                      5
+#define PPB_MEM64_RANGE                       0xFF
+
+#define P2C_BAR_0                             0
+#define P2C_MEM_1                             1
+#define P2C_MEM_2                             2
+#define P2C_IO_1                              3
+#define P2C_IO_2                              4
+
+#define EFI_BRIDGE_IO32_DECODE_SUPPORTED      0x0001
+#define EFI_BRIDGE_PMEM32_DECODE_SUPPORTED    0x0002
+#define EFI_BRIDGE_PMEM64_DECODE_SUPPORTED    0x0004
+#define EFI_BRIDGE_IO16_DECODE_SUPPORTED      0x0008
+#define EFI_BRIDGE_PMEM_MEM_COMBINE_SUPPORTED 0x0010
+#define EFI_BRIDGE_MEM64_DECODE_SUPPORTED     0x0020
+#define EFI_BRIDGE_MEM32_DECODE_SUPPORTED     0x0040
+
+#define PCI_MAX_HOST_BRIDGE_NUM               0x0010
+
+//
+// Define option for attribute
+//
+#define EFI_SET_SUPPORTS    0
+#define EFI_SET_ATTRIBUTES  1
+
+#define PCI_IO_DEVICE_SIGNATURE               SIGNATURE_32 ('p', 'c', 'i', 'o')
+
+struct _PCI_IO_DEVICE {
+  UINT32                                    Signature;
+  EFI_HANDLE                                Handle;
+  EFI_PCI_IO_PROTOCOL                       PciIo;
+  LIST_ENTRY                                Link;
+
+  EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL PciDriverOverride;
+  EFI_DEVICE_PATH_PROTOCOL                  *DevicePath;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL           *PciRootBridgeIo;
+  EFI_LOAD_FILE2_PROTOCOL                   LoadFile2;
+
+  //
+  // PCI configuration space header type
+  //
+  PCI_TYPE00                                Pci;
+
+  //
+  // Bus number, Device number, Function number
+  //
+  UINT8                                     BusNumber;
+  UINT8                                     DeviceNumber;
+  UINT8                                     FunctionNumber;
+
+  //
+  // BAR for this PCI Device
+  //
+  PCI_BAR                                   PciBar[PCI_MAX_BAR];
+
+  //
+  // The bridge device this pci device is subject to
+  //
+  PCI_IO_DEVICE                             *Parent;
+
+  //
+  // A linked list for children Pci Device if it is bridge device
+  //
+  LIST_ENTRY                                ChildList;
+
+  //
+  // TURE if the PCI bus driver creates the handle for this PCI device
+  //
+  BOOLEAN                                   Registered;
+
+  //
+  // TRUE if the PCI bus driver successfully allocates the resource required by
+  // this PCI device
+  //
+  BOOLEAN                                   Allocated;
+
+  //
+  // The attribute this PCI device currently set
+  //
+  UINT64                                    Attributes;
+
+  //
+  // The attributes this PCI device actually supports
+  //
+  UINT64                                    Supports;
+
+  //
+  // The resource decode the bridge supports
+  //
+  UINT32                                    Decodes;
+
+  //
+  // TRUE if the ROM image is from the PCI Option ROM BAR
+  //
+  BOOLEAN                                   EmbeddedRom;
+
+  //
+  // The OptionRom Size
+  //
+  UINT64                                    RomSize;
+
+  //
+  // The OptionRom Size
+  //
+  UINT64                                    RomBase;
+
+  //
+  // TRUE if all OpROM (in device or in platform specific position) have been processed
+  //
+  BOOLEAN                                   AllOpRomProcessed;
+
+  //
+  // TRUE if there is any EFI driver in the OptionRom
+  //
+  BOOLEAN                                   BusOverride;
+
+  //
+  // A list tracking reserved resource on a bridge device
+  //
+  LIST_ENTRY                                ReservedResourceList;
+
+  //
+  // A list tracking image handle of platform specific overriding driver
+  //
+  LIST_ENTRY                                OptionRomDriverList;
+
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR         *ResourcePaddingDescriptors;
+  EFI_HPC_PADDING_ATTRIBUTES                PaddingAttributes;
+
+  //
+  // Bus number ranges for a PCI Root Bridge device
+  //
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR         *BusNumberRanges;
+
+  BOOLEAN                                   IsPciExp;
+  //
+  // For SR-IOV
+  //
+  UINT8                                     PciExpressCapabilityOffset;
+  UINT32                                    AriCapabilityOffset;
+  UINT32                                    SrIovCapabilityOffset;
+  UINT32                                    MrIovCapabilityOffset;
+  PCI_BAR                                   VfPciBar[PCI_MAX_BAR];
+  UINT32                                    SystemPageSize;
+  UINT16                                    InitialVFs;
+  UINT16                                    ReservedBusNum;
+  //
+  // Per PCI to PCI Bridge spec, I/O window is 4K aligned,
+  // but some chipsets support non-stardard I/O window aligments less than 4K.
+  // This field is used to support this case.
+  //
+  UINT16                                    BridgeIoAlignment;
+};
+
+#define PCI_IO_DEVICE_FROM_PCI_IO_THIS(a) \
+  CR (a, PCI_IO_DEVICE, PciIo, PCI_IO_DEVICE_SIGNATURE)
+
+#define PCI_IO_DEVICE_FROM_PCI_DRIVER_OVERRIDE_THIS(a) \
+  CR (a, PCI_IO_DEVICE, PciDriverOverride, PCI_IO_DEVICE_SIGNATURE)
+
+#define PCI_IO_DEVICE_FROM_LINK(a) \
+  CR (a, PCI_IO_DEVICE, Link, PCI_IO_DEVICE_SIGNATURE)
+
+#define PCI_IO_DEVICE_FROM_LOAD_FILE2_THIS(a) \
+  CR (a, PCI_IO_DEVICE, LoadFile2, PCI_IO_DEVICE_SIGNATURE)
+
+
+
+//
+// Global Variables
+//
+extern EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL *gEfiIncompatiblePciDeviceSupport;
+extern EFI_DRIVER_BINDING_PROTOCOL                  gPciBusDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL                  gPciBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL                 gPciBusComponentName2;
+extern BOOLEAN                                      gFullEnumeration;
+extern UINTN                                        gPciHostBridgeNumber;
+extern EFI_HANDLE                                   gPciHostBrigeHandles[PCI_MAX_HOST_BRIDGE_NUM];
+extern UINT64                                       gAllOne;
+extern UINT64                                       gAllZero;
+extern EFI_PCI_PLATFORM_PROTOCOL                    *gPciPlatformProtocol;
+extern EFI_PCI_OVERRIDE_PROTOCOL                    *gPciOverrideProtocol;
+extern BOOLEAN                                      mReserveIsaAliases;
+extern BOOLEAN                                      mReserveVgaAliases;
+
+/**
+  Macro that checks whether device is a GFX device.
+
+  @param  _p      Specified device.
+
+  @retval TRUE    Device is a a GFX device.
+  @retval FALSE   Device is not a a GFX device.
+
+**/
+#define IS_PCI_GFX(_p)     IS_CLASS2 (_p, PCI_CLASS_DISPLAY, PCI_CLASS_DISPLAY_OTHER)
+
+/**
+  Test to see if this driver supports ControllerHandle. Any ControllerHandle
+  than contains a gEfiPciRootBridgeIoProtocolGuid protocol can be supported.
+
+  @param  This                Protocol instance pointer.
+  @param  Controller          Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child.
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval EFI_ALREADY_STARTED This driver is already running on this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Start this driver on ControllerHandle and enumerate Pci bus and start
+  all device under PCI bus.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child.
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle.
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle.
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This              Protocol instance pointer.
+  @param  Controller        Handle of device to stop driver on.
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle.
+  @retval other             This driver was not removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.inf b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.inf
new file mode 100644
index 0000000000..330ccc8cbf
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.inf
@@ -0,0 +1,112 @@
+## @file
+#  The PCI bus driver will probe all PCI devices and allocate MMIO and IO space for these devices.
+#  Please use PCD feature flag PcdPciBusHotplugDeviceSupport to enable hot plug supporting.
+#
+#  Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PciBusDxe
+  MODULE_UNI_FILE                = PciBusDxe.uni
+  FILE_GUID                      = 93B80004-9FB3-11d4-9A3A-0090273FC14D
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = PciBusEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gPciBusDriverBinding
+#  COMPONENT_NAME                =  gPciBusComponentName
+#  COMPONENT_NAME2               =  gPciBusComponentName2
+#
+
+[Sources]
+  PciLib.c
+  PciIo.c
+  PciBus.c
+  PciDeviceSupport.c
+  ComponentName.c
+  ComponentName.h
+  PciCommand.c
+  PciResourceSupport.c
+  PciEnumeratorSupport.c
+  PciEnumerator.c
+  PciOptionRomSupport.c
+  PciDriverOverride.c
+  PciPowerManagement.c
+  PciPowerManagement.h
+  PciDriverOverride.h
+  PciRomTable.c
+  PciHotPlugSupport.c
+  PciLib.h
+  PciHotPlugSupport.h
+  PciRomTable.h
+  PciOptionRomSupport.h
+  PciEnumeratorSupport.h
+  PciEnumerator.h
+  PciResourceSupport.h
+  PciDeviceSupport.h
+  PciCommand.h
+  PciIo.h
+  PciBus.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  DevicePathLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  ReportStatusCodeLib
+  BaseMemoryLib
+  UefiLib
+  BaseLib
+  UefiDriverEntryPoint
+  DebugLib
+  PeCoffLib
+
+[Protocols]
+  gEfiPciHotPlugRequestProtocolGuid               ## SOMETIMES_PRODUCES
+  gEfiPciIoProtocolGuid                           ## BY_START
+  gEfiDevicePathProtocolGuid                      ## BY_START
+  gEfiBusSpecificDriverOverrideProtocolGuid       ## BY_START
+  gEfiLoadedImageProtocolGuid                     ## SOMETIMES_CONSUMES
+  gEfiDecompressProtocolGuid                      ## SOMETIMES_CONSUMES
+  gEfiPciHotPlugInitProtocolGuid                  ## SOMETIMES_CONSUMES
+  gEfiPciHostBridgeResourceAllocationProtocolGuid ## TO_START
+  gEfiPciPlatformProtocolGuid                     ## SOMETIMES_CONSUMES
+  gEfiPciOverrideProtocolGuid                     ## SOMETIMES_CONSUMES 
+  gEfiPciEnumerationCompleteProtocolGuid          ## PRODUCES 
+  gEfiPciRootBridgeIoProtocolGuid                 ## TO_START
+  gEfiIncompatiblePciDeviceSupportProtocolGuid    ## SOMETIMES_CONSUMES
+  gEfiLoadFile2ProtocolGuid                       ## SOMETIMES_PRODUCES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciBusHotplugDeviceSupport  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciBridgeIoAlignmentProbe   ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUnalignedPciIoEnable        ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSrIovSystemPageSize         ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSrIovSupport                ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAriSupport                  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMrIovSupport                ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciDisableBusEnumeration    ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PciBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.uni b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.uni
new file mode 100644
index 0000000000..9b34afdc9d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxeExtra.uni
new file mode 100644
index 0000000000..63eff41a74
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.c
new file mode 100644
index 0000000000..0bc1fbfeff
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.c
@@ -0,0 +1,260 @@
+/** @file
+  PCI command register operations supporting functions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+/**
+  Operate the PCI register via PciIo function interface.
+
+  @param PciIoDevice    Pointer to instance of PCI_IO_DEVICE.
+  @param Command        Operator command.
+  @param Offset         The address within the PCI configuration space for the PCI controller.
+  @param Operation      Type of Operation.
+  @param PtrCommand     Return buffer holding old PCI command, if operation is not EFI_SET_REGISTER.
+
+  @return Status of PciIo operation.
+
+**/
+EFI_STATUS
+PciOperateRegister (
+  IN  PCI_IO_DEVICE *PciIoDevice,
+  IN  UINT16        Command,
+  IN  UINT8         Offset,
+  IN  UINT8         Operation,
+  OUT UINT16        *PtrCommand
+  )
+{
+  UINT16              OldCommand;
+  EFI_STATUS          Status;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  OldCommand  = 0;
+  PciIo       = &PciIoDevice->PciIo;
+
+  if (Operation != EFI_SET_REGISTER) {
+    Status = PciIo->Pci.Read (
+                          PciIo,
+                          EfiPciIoWidthUint16,
+                          Offset,
+                          1,
+                          &OldCommand
+                          );
+
+    if (Operation == EFI_GET_REGISTER) {
+      *PtrCommand = OldCommand;
+      return Status;
+    }
+  }
+
+  if (Operation == EFI_ENABLE_REGISTER) {
+    OldCommand = (UINT16) (OldCommand | Command);
+  } else if (Operation == EFI_DISABLE_REGISTER) {
+    OldCommand = (UINT16) (OldCommand & ~(Command));
+  } else {
+    OldCommand = Command;
+  }
+
+  return PciIo->Pci.Write (
+                      PciIo,
+                      EfiPciIoWidthUint16,
+                      Offset,
+                      1,
+                      &OldCommand
+                      );
+}
+
+/**
+  Check the cpability supporting by given device.
+
+  @param PciIoDevice   Pointer to instance of PCI_IO_DEVICE.
+
+  @retval TRUE         Cpability supportted.
+  @retval FALSE        Cpability not supportted.
+
+**/
+BOOLEAN
+PciCapabilitySupport (
+  IN PCI_IO_DEVICE  *PciIoDevice
+  )
+{
+  if ((PciIoDevice->Pci.Hdr.Status & EFI_PCI_STATUS_CAPABILITY) != 0) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Locate capability register block per capability ID.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param CapId             The capability ID.
+  @param Offset            A pointer to the offset returned.
+  @param NextRegBlock      A pointer to the next block returned.
+
+  @retval EFI_SUCCESS      Successfuly located capability register block.
+  @retval EFI_UNSUPPORTED  Pci device does not support capability.
+  @retval EFI_NOT_FOUND    Pci device support but can not find register block.
+
+**/
+EFI_STATUS
+LocateCapabilityRegBlock (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINT8          CapId,
+  IN OUT UINT8      *Offset,
+  OUT UINT8         *NextRegBlock OPTIONAL
+  )
+{
+  UINT8   CapabilityPtr;
+  UINT16  CapabilityEntry;
+  UINT8   CapabilityID;
+
+  //
+  // To check the cpability of this device supports
+  //
+  if (!PciCapabilitySupport (PciIoDevice)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (*Offset != 0) {
+    CapabilityPtr = *Offset;
+  } else {
+
+    CapabilityPtr = 0;
+    if (IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+
+      PciIoDevice->PciIo.Pci.Read (
+                               &PciIoDevice->PciIo,
+                               EfiPciIoWidthUint8,
+                               EFI_PCI_CARDBUS_BRIDGE_CAPABILITY_PTR,
+                               1,
+                               &CapabilityPtr
+                               );
+    } else {
+
+      PciIoDevice->PciIo.Pci.Read (
+                               &PciIoDevice->PciIo,
+                               EfiPciIoWidthUint8,
+                               PCI_CAPBILITY_POINTER_OFFSET,
+                               1,
+                               &CapabilityPtr
+                               );
+    }
+  }
+
+  while ((CapabilityPtr >= 0x40) && ((CapabilityPtr & 0x03) == 0x00)) {
+    PciIoDevice->PciIo.Pci.Read (
+                             &PciIoDevice->PciIo,
+                             EfiPciIoWidthUint16,
+                             CapabilityPtr,
+                             1,
+                             &CapabilityEntry
+                             );
+
+    CapabilityID = (UINT8) CapabilityEntry;
+
+    if (CapabilityID == CapId) {
+      *Offset = CapabilityPtr;
+      if (NextRegBlock != NULL) {
+        *NextRegBlock = (UINT8) (CapabilityEntry >> 8);
+      }
+
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Certain PCI device may incorrectly have capability pointing to itself,
+    // break to avoid dead loop.
+    //
+    if (CapabilityPtr == (UINT8) (CapabilityEntry >> 8)) {
+      break;
+    }
+
+    CapabilityPtr = (UINT8) (CapabilityEntry >> 8);
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Locate PciExpress capability register block per capability ID.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param CapId             The capability ID.
+  @param Offset            A pointer to the offset returned.
+  @param NextRegBlock      A pointer to the next block returned.
+
+  @retval EFI_SUCCESS      Successfuly located capability register block.
+  @retval EFI_UNSUPPORTED  Pci device does not support capability.
+  @retval EFI_NOT_FOUND    Pci device support but can not find register block.
+
+**/
+EFI_STATUS
+LocatePciExpressCapabilityRegBlock (
+  IN     PCI_IO_DEVICE *PciIoDevice,
+  IN     UINT16        CapId,
+  IN OUT UINT32        *Offset,
+     OUT UINT32        *NextRegBlock OPTIONAL
+  )
+{
+  EFI_STATUS           Status;
+  UINT32               CapabilityPtr;
+  UINT32               CapabilityEntry;
+  UINT16               CapabilityID;
+
+  //
+  // To check the capability of this device supports
+  //
+  if (!PciIoDevice->IsPciExp) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (*Offset != 0) {
+    CapabilityPtr = *Offset;
+  } else {
+    CapabilityPtr = EFI_PCIE_CAPABILITY_BASE_OFFSET;
+  }
+
+  while (CapabilityPtr != 0) {
+    //
+    // Mask it to DWORD alignment per PCI spec
+    //
+    CapabilityPtr &= 0xFFC;
+    Status = PciIoDevice->PciIo.Pci.Read (
+                                      &PciIoDevice->PciIo,
+                                      EfiPciIoWidthUint32,
+                                      CapabilityPtr,
+                                      1,
+                                      &CapabilityEntry
+                                      );
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    CapabilityID = (UINT16) CapabilityEntry;
+
+    if (CapabilityID == CapId) {
+      *Offset = CapabilityPtr;
+      if (NextRegBlock != NULL) {
+        *NextRegBlock = (CapabilityEntry >> 20) & 0xFFF;
+      }
+
+      return EFI_SUCCESS;
+    }
+
+    CapabilityPtr = (CapabilityEntry >> 20) & 0xFFF;
+  }
+
+  return EFI_NOT_FOUND;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.h
new file mode 100644
index 0000000000..cc942d0d42
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.h
@@ -0,0 +1,238 @@
+/** @file
+  PCI command register operations supporting functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+
+#ifndef _EFI_PCI_COMMAND_H_
+#define _EFI_PCI_COMMAND_H_
+
+//
+// The PCI Command register bits owned by PCI Bus driver.
+//
+// They should be cleared at the beginning. The other registers
+// are owned by chipset, we should not touch them.
+//
+#define EFI_PCI_COMMAND_BITS_OWNED                          ( \
+                EFI_PCI_COMMAND_IO_SPACE                    | \
+                EFI_PCI_COMMAND_MEMORY_SPACE                | \
+                EFI_PCI_COMMAND_BUS_MASTER                  | \
+                EFI_PCI_COMMAND_MEMORY_WRITE_AND_INVALIDATE | \
+                EFI_PCI_COMMAND_VGA_PALETTE_SNOOP           | \
+                EFI_PCI_COMMAND_FAST_BACK_TO_BACK             \
+                )
+
+//
+// The PCI Bridge Control register bits owned by PCI Bus driver.
+//
+// They should be cleared at the beginning. The other registers
+// are owned by chipset, we should not touch them.
+//
+#define EFI_PCI_BRIDGE_CONTROL_BITS_OWNED                   ( \
+                EFI_PCI_BRIDGE_CONTROL_ISA                  | \
+                EFI_PCI_BRIDGE_CONTROL_VGA                  | \
+                EFI_PCI_BRIDGE_CONTROL_VGA_16               | \
+                EFI_PCI_BRIDGE_CONTROL_FAST_BACK_TO_BACK      \
+                )
+
+//
+// The PCCard Bridge Control register bits owned by PCI Bus driver.
+//
+// They should be cleared at the beginning. The other registers
+// are owned by chipset, we should not touch them.
+//
+#define EFI_PCCARD_BRIDGE_CONTROL_BITS_OWNED                ( \
+                EFI_PCI_BRIDGE_CONTROL_ISA                  | \
+                EFI_PCI_BRIDGE_CONTROL_VGA                  | \
+                EFI_PCI_BRIDGE_CONTROL_FAST_BACK_TO_BACK      \
+                )
+
+
+#define EFI_GET_REGISTER      1
+#define EFI_SET_REGISTER      2
+#define EFI_ENABLE_REGISTER   3
+#define EFI_DISABLE_REGISTER  4
+
+/**
+  Operate the PCI register via PciIo function interface.
+
+  @param PciIoDevice    Pointer to instance of PCI_IO_DEVICE.
+  @param Command        Operator command.
+  @param Offset         The address within the PCI configuration space for the PCI controller.
+  @param Operation      Type of Operation.
+  @param PtrCommand     Return buffer holding old PCI command, if operation is not EFI_SET_REGISTER.
+
+  @return Status of PciIo operation.
+
+**/
+EFI_STATUS
+PciOperateRegister (
+  IN  PCI_IO_DEVICE *PciIoDevice,
+  IN  UINT16        Command,
+  IN  UINT8         Offset,
+  IN  UINT8         Operation,
+  OUT UINT16        *PtrCommand
+  );
+
+/**
+  Check the cpability supporting by given device.
+
+  @param PciIoDevice   Pointer to instance of PCI_IO_DEVICE.
+
+  @retval TRUE         Cpability supportted.
+  @retval FALSE        Cpability not supportted.
+
+**/
+BOOLEAN
+PciCapabilitySupport (
+  IN PCI_IO_DEVICE  *PciIoDevice
+  );
+
+/**
+  Locate capability register block per capability ID.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param CapId             The capability ID.
+  @param Offset            A pointer to the offset returned.
+  @param NextRegBlock      A pointer to the next block returned.
+
+  @retval EFI_SUCCESS      Successfuly located capability register block.
+  @retval EFI_UNSUPPORTED  Pci device does not support capability.
+  @retval EFI_NOT_FOUND    Pci device support but can not find register block.
+
+**/
+EFI_STATUS
+LocateCapabilityRegBlock (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINT8          CapId,
+  IN OUT UINT8      *Offset,
+  OUT UINT8         *NextRegBlock OPTIONAL
+  );
+
+/**
+  Locate PciExpress capability register block per capability ID.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param CapId             The capability ID.
+  @param Offset            A pointer to the offset returned.
+  @param NextRegBlock      A pointer to the next block returned.
+
+  @retval EFI_SUCCESS      Successfuly located capability register block.
+  @retval EFI_UNSUPPORTED  Pci device does not support capability.
+  @retval EFI_NOT_FOUND    Pci device support but can not find register block.
+
+**/
+EFI_STATUS
+LocatePciExpressCapabilityRegBlock (
+  IN     PCI_IO_DEVICE *PciIoDevice,
+  IN     UINT16        CapId,
+  IN OUT UINT32        *Offset,
+     OUT UINT32        *NextRegBlock OPTIONAL
+  );
+
+/**
+  Macro that reads command register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[out]           Pointer to the 16-bit value read from command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_READ_COMMAND_REGISTER(a,b) \
+        PciOperateRegister (a, 0, PCI_COMMAND_OFFSET, EFI_GET_REGISTER, b)
+
+/**
+  Macro that writes command register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The 16-bit value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_SET_COMMAND_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_COMMAND_OFFSET, EFI_SET_REGISTER, NULL)
+
+/**
+  Macro that enables command register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The enabled value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_ENABLE_COMMAND_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_COMMAND_OFFSET, EFI_ENABLE_REGISTER, NULL)
+
+/**
+  Macro that disalbes command register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The disabled value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_DISABLE_COMMAND_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_COMMAND_OFFSET, EFI_DISABLE_REGISTER, NULL)
+
+/**
+  Macro that reads PCI bridge control register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[out]           The 16-bit value read from control register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_READ_BRIDGE_CONTROL_REGISTER(a,b) \
+        PciOperateRegister (a, 0, PCI_BRIDGE_CONTROL_REGISTER_OFFSET, EFI_GET_REGISTER, b)
+
+/**
+  Macro that writes PCI bridge control register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The 16-bit value written into control register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_SET_BRIDGE_CONTROL_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_BRIDGE_CONTROL_REGISTER_OFFSET, EFI_SET_REGISTER, NULL)
+
+/**
+  Macro that enables PCI bridge control register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The enabled value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_ENABLE_BRIDGE_CONTROL_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_BRIDGE_CONTROL_REGISTER_OFFSET, EFI_ENABLE_REGISTER, NULL)
+
+/**
+ Macro that disalbes PCI bridge control register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The disabled value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_DISABLE_BRIDGE_CONTROL_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_BRIDGE_CONTROL_REGISTER_OFFSET, EFI_DISABLE_REGISTER, NULL)
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.c
new file mode 100644
index 0000000000..c0227fa2b6
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.c
@@ -0,0 +1,1155 @@
+/** @file
+  Supporting functions implementaion for PCI devices management.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+//
+// This device structure is serviced as a header.
+// Its next field points to the first root bridge device node.
+//
+LIST_ENTRY  mPciDevicePool;
+
+/**
+  Initialize the PCI devices pool.
+
+**/
+VOID
+InitializePciDevicePool (
+  VOID
+  )
+{
+  InitializeListHead (&mPciDevicePool);
+}
+
+/**
+  Insert a root bridge into PCI device pool.
+
+  @param RootBridge     A pointer to the PCI_IO_DEVICE.
+
+**/
+VOID
+InsertRootBridge (
+  IN PCI_IO_DEVICE      *RootBridge
+  )
+{
+  InsertTailList (&mPciDevicePool, &(RootBridge->Link));
+}
+
+/**
+  This function is used to insert a PCI device node under
+  a bridge.
+
+  @param Bridge         The PCI bridge.
+  @param PciDeviceNode  The PCI device needs inserting.
+
+**/
+VOID
+InsertPciDevice (
+  IN PCI_IO_DEVICE      *Bridge,
+  IN PCI_IO_DEVICE      *PciDeviceNode
+  )
+{
+  InsertTailList (&Bridge->ChildList, &(PciDeviceNode->Link));
+  PciDeviceNode->Parent = Bridge;
+}
+
+/**
+  Destroy root bridge and remove it from deivce tree.
+
+  @param RootBridge     The bridge want to be removed.
+
+**/
+VOID
+DestroyRootBridge (
+  IN PCI_IO_DEVICE      *RootBridge
+  )
+{
+  DestroyPciDeviceTree (RootBridge);
+
+  FreePciDevice (RootBridge);
+}
+
+/**
+  Destroy a pci device node.
+
+  All direct or indirect allocated resource for this node will be freed.
+
+  @param PciIoDevice  A pointer to the PCI_IO_DEVICE to be destoried.
+
+**/
+VOID
+FreePciDevice (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  ASSERT (PciIoDevice != NULL);
+  //
+  // Assume all children have been removed underneath this device
+  //
+  if (PciIoDevice->ResourcePaddingDescriptors != NULL) {
+    FreePool (PciIoDevice->ResourcePaddingDescriptors);
+  }
+
+  if (PciIoDevice->DevicePath != NULL) {
+    FreePool (PciIoDevice->DevicePath);
+  }
+
+  FreePool (PciIoDevice);
+}
+
+/**
+  Destroy all the pci device node under the bridge.
+  Bridge itself is not included.
+
+  @param Bridge      A pointer to the PCI_IO_DEVICE.
+
+**/
+VOID
+DestroyPciDeviceTree (
+  IN PCI_IO_DEVICE      *Bridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  while (!IsListEmpty (&Bridge->ChildList)) {
+
+    CurrentLink = Bridge->ChildList.ForwardLink;
+
+    //
+    // Remove this node from the linked list
+    //
+    RemoveEntryList (CurrentLink);
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      DestroyPciDeviceTree (Temp);
+    }
+
+    FreePciDevice (Temp);
+  }
+}
+
+/**
+  Destroy all device nodes under the root bridge
+  specified by Controller.
+
+  The root bridge itself is also included.
+
+  @param  Controller    Root bridge handle.
+
+  @retval EFI_SUCCESS   Destory all devcie nodes successfully.
+  @retval EFI_NOT_FOUND Cannot find any PCI device under specified
+                        root bridge.
+
+**/
+EFI_STATUS
+DestroyRootBridgeByHandle (
+  IN EFI_HANDLE        Controller
+  )
+{
+
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  CurrentLink = mPciDevicePool.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &mPciDevicePool) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (Temp->Handle == Controller) {
+
+      RemoveEntryList (CurrentLink);
+
+      DestroyPciDeviceTree (Temp);
+
+      FreePciDevice (Temp);
+
+      return EFI_SUCCESS;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  This function registers the PCI IO device.
+
+  It creates a handle for this PCI IO device (if the handle does not exist), attaches
+  appropriate protocols onto the handle, does necessary initialization, and sets up
+  parent/child relationship with its bus controller.
+
+  @param Controller     An EFI handle for the PCI bus controller.
+  @param PciIoDevice    A PCI_IO_DEVICE pointer to the PCI IO device to be registered.
+  @param Handle         A pointer to hold the returned EFI handle for the PCI IO device.
+
+  @retval EFI_SUCCESS   The PCI device is successfully registered.
+  @retval other         An error occurred when registering the PCI device.
+
+**/
+EFI_STATUS
+RegisterPciDevice (
+  IN  EFI_HANDLE          Controller,
+  IN  PCI_IO_DEVICE       *PciIoDevice,
+  OUT EFI_HANDLE          *Handle      OPTIONAL
+  )
+{
+  EFI_STATUS          Status;
+  VOID                *PlatformOpRomBuffer;
+  UINTN               PlatformOpRomSize;
+  UINT8               PciExpressCapRegOffset;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT8               Data8;
+  BOOLEAN             HasEfiImage;
+
+  //
+  // Install the pciio protocol, device path protocol
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &PciIoDevice->Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  PciIoDevice->DevicePath,
+                  &gEfiPciIoProtocolGuid,
+                  &PciIoDevice->PciIo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Detect if PCI Express Device
+  //
+  PciExpressCapRegOffset = 0;
+  Status = LocateCapabilityRegBlock (
+             PciIoDevice,
+             EFI_PCI_CAPABILITY_ID_PCIEXP,
+             &PciExpressCapRegOffset,
+             NULL
+             );
+  if (!EFI_ERROR (Status)) {
+    PciIoDevice->IsPciExp = TRUE;
+  }
+
+  //
+  // Force Interrupt line to "Unknown" or "No Connection"
+  //
+  PciIo = &(PciIoDevice->PciIo);
+  Data8 = PCI_INT_LINE_UNKNOWN;
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x3C, 1, &Data8);
+ 
+  //
+  // Process OpRom
+  //
+  if (!PciIoDevice->AllOpRomProcessed) {
+
+    //
+    // Get the OpRom provided by platform
+    //
+    if (gPciPlatformProtocol != NULL) {
+      Status = gPciPlatformProtocol->GetPciRom (
+                                       gPciPlatformProtocol,
+                                       PciIoDevice->Handle,
+                                       &PlatformOpRomBuffer,
+                                       &PlatformOpRomSize
+                                       );
+      if (!EFI_ERROR (Status)) {
+        PciIoDevice->EmbeddedRom    = FALSE;
+        PciIoDevice->RomSize        = PlatformOpRomSize;
+        PciIoDevice->PciIo.RomSize  = PlatformOpRomSize;
+        PciIoDevice->PciIo.RomImage = PlatformOpRomBuffer;
+        //
+        // For OpROM read from gPciPlatformProtocol:
+        // Add the Rom Image to internal database for later PCI light enumeration
+        //
+        PciRomAddImageMapping (
+          NULL,
+          PciIoDevice->PciRootBridgeIo->SegmentNumber,
+          PciIoDevice->BusNumber,
+          PciIoDevice->DeviceNumber,
+          PciIoDevice->FunctionNumber,
+          (UINT64) (UINTN) PciIoDevice->PciIo.RomImage,
+          PciIoDevice->PciIo.RomSize
+          );
+      }
+    } else if (gPciOverrideProtocol != NULL) {
+      Status = gPciOverrideProtocol->GetPciRom (
+                                       gPciOverrideProtocol,
+                                       PciIoDevice->Handle,
+                                       &PlatformOpRomBuffer,
+                                       &PlatformOpRomSize
+                                       );
+      if (!EFI_ERROR (Status)) {
+        PciIoDevice->EmbeddedRom    = FALSE;
+        PciIoDevice->RomSize        = PlatformOpRomSize;
+        PciIoDevice->PciIo.RomSize  = PlatformOpRomSize;
+        PciIoDevice->PciIo.RomImage = PlatformOpRomBuffer;
+        //
+        // For OpROM read from gPciOverrideProtocol:
+        // Add the Rom Image to internal database for later PCI light enumeration
+        //
+        PciRomAddImageMapping (
+          NULL,
+          PciIoDevice->PciRootBridgeIo->SegmentNumber,
+          PciIoDevice->BusNumber,
+          PciIoDevice->DeviceNumber,
+          PciIoDevice->FunctionNumber,
+          (UINT64) (UINTN) PciIoDevice->PciIo.RomImage,
+          PciIoDevice->PciIo.RomSize
+          );
+      }   
+    }
+  }
+
+  //
+  // Determine if there are EFI images in the option rom
+  //
+  HasEfiImage = ContainEfiImage (PciIoDevice->PciIo.RomImage, PciIoDevice->PciIo.RomSize);
+
+  if (HasEfiImage) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &PciIoDevice->Handle,
+                    &gEfiLoadFile2ProtocolGuid,
+                    &PciIoDevice->LoadFile2,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+             &PciIoDevice->Handle,
+             &gEfiDevicePathProtocolGuid,
+             PciIoDevice->DevicePath,
+             &gEfiPciIoProtocolGuid,
+             &PciIoDevice->PciIo,
+             NULL
+             );
+      return Status;
+    }
+  }
+
+
+  if (!PciIoDevice->AllOpRomProcessed) {
+
+    PciIoDevice->AllOpRomProcessed = TRUE;
+
+    //
+    // Dispatch the EFI OpRom for the PCI device.
+    // The OpRom is got from platform in the above code
+    // or loaded from device in the previous round of bus enumeration
+    //
+    if (HasEfiImage) {
+      ProcessOpRomImage (PciIoDevice);
+    }
+  }
+
+  if (PciIoDevice->BusOverride) {
+    //
+    // Install Bus Specific Driver Override Protocol
+    //
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &PciIoDevice->Handle,
+                    &gEfiBusSpecificDriverOverrideProtocolGuid,
+                    &PciIoDevice->PciDriverOverride,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+             &PciIoDevice->Handle,
+             &gEfiDevicePathProtocolGuid,
+             PciIoDevice->DevicePath,
+             &gEfiPciIoProtocolGuid,
+             &PciIoDevice->PciIo,
+             NULL
+             );
+      if (HasEfiImage) {
+        gBS->UninstallMultipleProtocolInterfaces (
+               &PciIoDevice->Handle,
+               &gEfiLoadFile2ProtocolGuid,
+               &PciIoDevice->LoadFile2,
+               NULL
+               );
+      }
+
+      return Status;
+    }
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &(PciIoDevice->PciRootBridgeIo),
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  PciIoDevice->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Handle != NULL) {
+    *Handle = PciIoDevice->Handle;
+  }
+
+  //
+  // Indicate the pci device is registered
+  //
+  PciIoDevice->Registered = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to remove the whole PCI devices on the specified bridge from
+  the root bridge.
+
+  @param RootBridgeHandle   The root bridge device handle.
+  @param Bridge             The bridge device to be removed.
+
+**/
+VOID
+RemoveAllPciDeviceOnBridge (
+  EFI_HANDLE               RootBridgeHandle,
+  PCI_IO_DEVICE            *Bridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  while (!IsListEmpty (&Bridge->ChildList)) {
+
+    CurrentLink = Bridge->ChildList.ForwardLink;
+    Temp        = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    //
+    // Check if the current node has been deregistered before
+    // If it is not, then deregister it
+    //
+    if (Temp->Registered) {
+      DeRegisterPciDevice (RootBridgeHandle, Temp->Handle);
+    }
+
+    //
+    // Remove this node from the linked list
+    //
+    RemoveEntryList (CurrentLink);
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      RemoveAllPciDeviceOnBridge (RootBridgeHandle, Temp);
+    }
+
+    FreePciDevice (Temp);
+  }
+}
+
+/**
+  This function is used to de-register the PCI IO device.
+
+  That includes un-installing PciIo protocol from the specified PCI
+  device handle.
+
+  @param Controller    An EFI handle for the PCI bus controller.
+  @param Handle        PCI device handle.
+
+  @retval EFI_SUCCESS  The PCI device is successfully de-registered.
+  @retval other        An error occurred when de-registering the PCI device.
+
+**/
+EFI_STATUS
+DeRegisterPciDevice (
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  )
+
+{
+  EFI_PCI_IO_PROTOCOL             *PciIo;
+  EFI_STATUS                      Status;
+  PCI_IO_DEVICE                   *PciIoDevice;
+  PCI_IO_DEVICE                   *Node;
+  LIST_ENTRY                      *CurrentLink;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (PciIo);
+
+    //
+    // If it is already de-registered
+    //
+    if (!PciIoDevice->Registered) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // If it is PPB, first de-register its children
+    //
+
+    if (!IsListEmpty (&PciIoDevice->ChildList)) {
+
+      CurrentLink = PciIoDevice->ChildList.ForwardLink;
+
+      while (CurrentLink != NULL && CurrentLink != &PciIoDevice->ChildList) {
+        Node    = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+        Status  = DeRegisterPciDevice (Controller, Node->Handle);
+
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+
+        CurrentLink = CurrentLink->ForwardLink;
+      }
+    }
+
+    //
+    // Close the child handle
+    //
+    Status = gBS->CloseProtocol (
+                    Controller,
+                    &gEfiPciRootBridgeIoProtocolGuid,
+                    gPciBusDriverBinding.DriverBindingHandle,
+                    Handle
+                    );
+
+    //
+    // Un-install the Device Path protocol and PCI I/O protocol
+    // and Bus Specific Driver Override protocol if needed.
+    //
+    if (PciIoDevice->BusOverride) {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      Handle,
+                      &gEfiDevicePathProtocolGuid,
+                      PciIoDevice->DevicePath,
+                      &gEfiPciIoProtocolGuid,
+                      &PciIoDevice->PciIo,
+                      &gEfiBusSpecificDriverOverrideProtocolGuid,
+                      &PciIoDevice->PciDriverOverride,
+                      NULL
+                      );
+    } else {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      Handle,
+                      &gEfiDevicePathProtocolGuid,
+                      PciIoDevice->DevicePath,
+                      &gEfiPciIoProtocolGuid,
+                      &PciIoDevice->PciIo,
+                      NULL
+                      );
+    }
+
+    if (!EFI_ERROR (Status)) {
+      //
+      // Try to uninstall LoadFile2 protocol if exists
+      //
+      Status = gBS->OpenProtocol (
+                      Handle,
+                      &gEfiLoadFile2ProtocolGuid,
+                      NULL,
+                      gPciBusDriverBinding.DriverBindingHandle,
+                      Controller,
+                      EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                      );
+      if (!EFI_ERROR (Status)) {
+        Status = gBS->UninstallMultipleProtocolInterfaces (
+                        Handle,
+                        &gEfiLoadFile2ProtocolGuid,
+                        &PciIoDevice->LoadFile2,
+                        NULL
+                        );
+      }
+      //
+      // Restore Status
+      //
+      Status = EFI_SUCCESS;
+    }
+
+
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+            Controller,
+            &gEfiPciRootBridgeIoProtocolGuid,
+            (VOID **) &PciRootBridgeIo,
+            gPciBusDriverBinding.DriverBindingHandle,
+            Handle,
+            EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+            );
+      return Status;
+    }
+
+    //
+    // The Device Driver should disable this device after disconnect
+    // so the Pci Bus driver will not touch this device any more.
+    // Restore the register field to the original value
+    //
+    PciIoDevice->Registered = FALSE;
+    PciIoDevice->Handle     = NULL;
+  } else {
+
+    //
+    // Handle may be closed before
+    //
+    return EFI_SUCCESS;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start to manage the PCI device on the specified root bridge or PCI-PCI Bridge.
+
+  @param Controller          The root bridge handle.
+  @param RootBridge          A pointer to the PCI_IO_DEVICE.
+  @param RemainingDevicePath A pointer to the EFI_DEVICE_PATH_PROTOCOL.
+  @param NumberOfChildren    Children number.
+  @param ChildHandleBuffer   A pointer to the child handle buffer.
+
+  @retval EFI_NOT_READY   Device is not allocated.
+  @retval EFI_UNSUPPORTED Device only support PCI-PCI bridge.
+  @retval EFI_NOT_FOUND   Can not find the specific device.
+  @retval EFI_SUCCESS     Success to start Pci devices on bridge.
+
+**/
+EFI_STATUS
+StartPciDevicesOnBridge (
+  IN EFI_HANDLE                          Controller,
+  IN PCI_IO_DEVICE                       *RootBridge,
+  IN EFI_DEVICE_PATH_PROTOCOL            *RemainingDevicePath,
+  IN OUT UINT8                           *NumberOfChildren,
+  IN OUT EFI_HANDLE                      *ChildHandleBuffer
+  )
+
+{
+  PCI_IO_DEVICE             *PciIoDevice;
+  EFI_DEV_PATH_PTR          Node;
+  EFI_DEVICE_PATH_PROTOCOL  *CurrentDevicePath;
+  EFI_STATUS                Status;
+  LIST_ENTRY                *CurrentLink;
+  UINT64                    Supports;
+
+  PciIoDevice = NULL;
+  CurrentLink = RootBridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &RootBridge->ChildList) {
+
+    PciIoDevice = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (RemainingDevicePath != NULL) {
+
+      Node.DevPath = RemainingDevicePath;
+
+      if (Node.Pci->Device != PciIoDevice->DeviceNumber ||
+          Node.Pci->Function != PciIoDevice->FunctionNumber) {
+        CurrentLink = CurrentLink->ForwardLink;
+        continue;
+      }
+
+      //
+      // Check if the device has been assigned with required resource
+      //
+      if (!PciIoDevice->Allocated) {
+        return EFI_NOT_READY;
+      }
+
+      //
+      // Check if the current node has been registered before
+      // If it is not, register it
+      //
+      if (!PciIoDevice->Registered) {
+        Status = RegisterPciDevice (
+                   Controller,
+                   PciIoDevice,
+                   NULL
+                   );
+
+      }
+
+      if (NumberOfChildren != NULL && ChildHandleBuffer != NULL && PciIoDevice->Registered) {
+        ChildHandleBuffer[*NumberOfChildren] = PciIoDevice->Handle;
+        (*NumberOfChildren)++;
+      }
+
+      //
+      // Get the next device path
+      //
+      CurrentDevicePath = NextDevicePathNode (RemainingDevicePath);
+      if (IsDevicePathEnd (CurrentDevicePath)) {
+        return EFI_SUCCESS;
+      }
+
+      //
+      // If it is a PPB
+      //
+      if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+        Status = StartPciDevicesOnBridge (
+                   Controller,
+                   PciIoDevice,
+                   CurrentDevicePath,
+                   NumberOfChildren,
+                   ChildHandleBuffer
+                   );
+
+        PciIoDevice->PciIo.Attributes (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoAttributeOperationSupported,
+                             0,
+                             &Supports
+                             );
+        Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+        PciIoDevice->PciIo.Attributes (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoAttributeOperationEnable,
+                             Supports,
+                             NULL
+                             );
+
+        return Status;
+      } else {
+
+        //
+        // Currently, the PCI bus driver only support PCI-PCI bridge
+        //
+        return EFI_UNSUPPORTED;
+      }
+
+    } else {
+
+      //
+      // If remaining device path is NULL,
+      // try to enable all the pci devices under this bridge
+      //
+      if (!PciIoDevice->Registered && PciIoDevice->Allocated) {
+        Status = RegisterPciDevice (
+                   Controller,
+                   PciIoDevice,
+                   NULL
+                   );
+
+      }
+
+      if (NumberOfChildren != NULL && ChildHandleBuffer != NULL && PciIoDevice->Registered) {
+        ChildHandleBuffer[*NumberOfChildren] = PciIoDevice->Handle;
+        (*NumberOfChildren)++;
+      }
+
+      if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+        Status = StartPciDevicesOnBridge (
+                   Controller,
+                   PciIoDevice,
+                   RemainingDevicePath,
+                   NumberOfChildren,
+                   ChildHandleBuffer
+                   );
+
+        PciIoDevice->PciIo.Attributes (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoAttributeOperationSupported,
+                             0,
+                             &Supports
+                             );
+        Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+        PciIoDevice->PciIo.Attributes (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoAttributeOperationEnable,
+                             Supports,
+                             NULL
+                             );
+
+      }
+
+      CurrentLink = CurrentLink->ForwardLink;
+    }
+  }
+
+  if (PciIoDevice == NULL) {
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Start to manage all the PCI devices it found previously under
+  the entire host bridge.
+
+  @param Controller          The root bridge handle.
+
+  @retval EFI_NOT_READY   Device is not allocated.
+  @retval EFI_SUCCESS     Success to start Pci device on host bridge.
+
+**/
+EFI_STATUS
+StartPciDevices (
+  IN EFI_HANDLE                         Controller
+  )
+{
+  PCI_IO_DEVICE     *RootBridge;
+  EFI_HANDLE        ThisHostBridge;
+  LIST_ENTRY        *CurrentLink;
+
+  RootBridge = GetRootBridgeByHandle (Controller);
+  ASSERT (RootBridge != NULL);
+  ThisHostBridge = RootBridge->PciRootBridgeIo->ParentHandle;
+
+  CurrentLink = mPciDevicePool.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &mPciDevicePool) {
+
+    RootBridge = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    //
+    // Locate the right root bridge to start
+    //
+    if (RootBridge->PciRootBridgeIo->ParentHandle == ThisHostBridge) {
+      StartPciDevicesOnBridge (
+         RootBridge->Handle,
+         RootBridge,
+         NULL,
+         NULL,
+         NULL
+         );
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create root bridge device.
+
+  @param RootBridgeHandle    Specified root bridge hanle.
+
+  @return The crated root bridge device instance, NULL means no
+          root bridge device instance created.
+
+**/
+PCI_IO_DEVICE *
+CreateRootBridge (
+  IN EFI_HANDLE                   RootBridgeHandle
+  )
+{
+  EFI_STATUS                      Status;
+  PCI_IO_DEVICE                   *Dev;
+  EFI_DEVICE_PATH_PROTOCOL        *ParentDevicePath;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  Dev = AllocateZeroPool (sizeof (PCI_IO_DEVICE));
+  if (Dev == NULL) {
+    return NULL;
+  }
+
+  Dev->Signature  = PCI_IO_DEVICE_SIGNATURE;
+  Dev->Handle     = RootBridgeHandle;
+  InitializeListHead (&Dev->ChildList);
+
+  Status = gBS->OpenProtocol (
+                  RootBridgeHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  RootBridgeHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Dev);
+    return NULL;
+  }
+
+  //
+  // Record the root bridge parent device path
+  //
+  Dev->DevicePath = DuplicateDevicePath (ParentDevicePath);
+
+  //
+  // Get the pci root bridge io protocol
+  //
+  Status = gBS->OpenProtocol (
+                  RootBridgeHandle,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  RootBridgeHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePciDevice (Dev);
+    return NULL;
+  }
+
+  Dev->PciRootBridgeIo = PciRootBridgeIo;
+
+  //
+  // Initialize the PCI I/O instance structure
+  //
+  InitializePciIoInstance (Dev);
+  InitializePciDriverOverrideInstance (Dev);
+  InitializePciLoadFile2 (Dev);
+
+  //
+  // Initialize reserved resource list and
+  // option rom driver list
+  //
+  InitializeListHead (&Dev->ReservedResourceList);
+  InitializeListHead (&Dev->OptionRomDriverList);
+
+  return Dev;
+}
+
+/**
+  Get root bridge device instance by specific root bridge handle.
+
+  @param RootBridgeHandle    Given root bridge handle.
+
+  @return The root bridge device instance, NULL means no root bridge
+          device instance found.
+
+**/
+PCI_IO_DEVICE *
+GetRootBridgeByHandle (
+  EFI_HANDLE RootBridgeHandle
+  )
+{
+  PCI_IO_DEVICE   *RootBridgeDev;
+  LIST_ENTRY      *CurrentLink;
+
+  CurrentLink = mPciDevicePool.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &mPciDevicePool) {
+
+    RootBridgeDev = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (RootBridgeDev->Handle == RootBridgeHandle) {
+      return RootBridgeDev;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return NULL;
+}
+
+/**
+  Judege whether Pci device existed.
+
+  @param Bridge       Parent bridege instance.
+  @param PciIoDevice  Device instance.
+
+  @retval TRUE        Pci device existed.
+  @retval FALSE       Pci device did not exist.
+
+**/
+BOOLEAN
+PciDeviceExisted (
+  IN PCI_IO_DEVICE    *Bridge,
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (Temp == PciIoDevice) {
+      return TRUE;
+    }
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      if (PciDeviceExisted (Temp, PciIoDevice)) {
+        return TRUE;
+      }
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return FALSE;
+}
+
+/**
+  Get the active VGA device on the same segment.
+
+  @param VgaDevice    PCI IO instance for the VGA device.
+
+  @return The active VGA device on the same segment.
+
+**/
+PCI_IO_DEVICE *
+ActiveVGADeviceOnTheSameSegment (
+  IN PCI_IO_DEVICE        *VgaDevice
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  CurrentLink = mPciDevicePool.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &mPciDevicePool) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (Temp->PciRootBridgeIo->SegmentNumber == VgaDevice->PciRootBridgeIo->SegmentNumber) {
+
+      Temp = ActiveVGADeviceOnTheRootBridge (Temp);
+
+      if (Temp != NULL) {
+        return Temp;
+      }
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return NULL;
+}
+
+/**
+  Get the active VGA device on the root bridge.
+
+  @param RootBridge  PCI IO instance for the root bridge.
+
+  @return The active VGA device.
+
+**/
+PCI_IO_DEVICE *
+ActiveVGADeviceOnTheRootBridge (
+  IN PCI_IO_DEVICE        *RootBridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  CurrentLink = RootBridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &RootBridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (IS_PCI_VGA(&Temp->Pci) &&
+        (Temp->Attributes &
+         (EFI_PCI_IO_ATTRIBUTE_VGA_MEMORY |
+          EFI_PCI_IO_ATTRIBUTE_VGA_IO     |
+          EFI_PCI_IO_ATTRIBUTE_VGA_IO_16)) != 0) {
+      return Temp;
+    }
+
+    if (IS_PCI_BRIDGE (&Temp->Pci)) {
+
+      Temp = ActiveVGADeviceOnTheRootBridge (Temp);
+
+      if (Temp != NULL) {
+        return Temp;
+      }
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return NULL;
+}
+
+
+/**
+  Get HPC PCI address according to its device path.
+
+  @param RootBridge           Root bridege Io instance.
+  @param RemainingDevicePath  Given searching device path.
+  @param PciAddress           Buffer holding searched result.
+
+  @retval EFI_SUCCESS         PCI address was stored in PciAddress
+  @retval EFI_NOT_FOUND       Can not find the specific device path.
+
+**/
+EFI_STATUS
+GetHpcPciAddressFromRootBridge (
+  IN  PCI_IO_DEVICE                    *RootBridge,
+  IN  EFI_DEVICE_PATH_PROTOCOL         *RemainingDevicePath,
+  OUT UINT64                           *PciAddress
+  )
+{
+  EFI_DEV_PATH_PTR          Node;
+  PCI_IO_DEVICE             *Temp;
+  EFI_DEVICE_PATH_PROTOCOL  *CurrentDevicePath;
+  LIST_ENTRY                *CurrentLink;
+  BOOLEAN                   MisMatch;
+
+  MisMatch          = FALSE;
+
+  CurrentDevicePath = RemainingDevicePath;
+  Node.DevPath      = CurrentDevicePath;
+  Temp              = NULL;
+
+  while (!IsDevicePathEnd (CurrentDevicePath)) {
+
+    CurrentLink   = RootBridge->ChildList.ForwardLink;
+    Node.DevPath  = CurrentDevicePath;
+
+    while (CurrentLink != NULL && CurrentLink != &RootBridge->ChildList) {
+      Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+      if (Node.Pci->Device   == Temp->DeviceNumber &&
+          Node.Pci->Function == Temp->FunctionNumber) {
+        RootBridge = Temp;
+        break;
+      }
+
+      CurrentLink = CurrentLink->ForwardLink;
+    }
+
+    //
+    // Check if we find the bridge
+    //
+    if (CurrentLink == &RootBridge->ChildList) {
+
+      MisMatch = TRUE;
+      break;
+
+    }
+
+    CurrentDevicePath = NextDevicePathNode (CurrentDevicePath);
+  }
+
+  if (MisMatch) {
+
+    CurrentDevicePath = NextDevicePathNode (CurrentDevicePath);
+
+    if (IsDevicePathEnd (CurrentDevicePath)) {
+      *PciAddress = EFI_PCI_ADDRESS (RootBridge->BusNumber, Node.Pci->Device, Node.Pci->Function, 0);
+      return EFI_SUCCESS;
+    }
+
+    return EFI_NOT_FOUND;
+  }
+
+  if (Temp != NULL) {
+    *PciAddress = EFI_PCI_ADDRESS (Temp->BusNumber, Temp->DeviceNumber, Temp->FunctionNumber, 0);
+  } else {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.h
new file mode 100644
index 0000000000..1a01e72b1e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.h
@@ -0,0 +1,289 @@
+/** @file
+  Supporting functions declaration for PCI devices management.
+
+Copyright (c) 2006 - 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_DEVICE_SUPPORT_H_
+#define _EFI_PCI_DEVICE_SUPPORT_H_
+
+/**
+  Initialize the PCI devices pool.
+
+**/
+VOID
+InitializePciDevicePool (
+  VOID
+  );
+
+/**
+  Insert a root bridge into PCI device pool.
+
+  @param RootBridge     A pointer to the PCI_IO_DEVICE.
+
+**/
+VOID
+InsertRootBridge (
+  IN PCI_IO_DEVICE      *RootBridge
+  );
+
+/**
+  This function is used to insert a PCI device node under
+  a bridge.
+
+  @param Bridge         The PCI bridge.
+  @param PciDeviceNode  The PCI device needs inserting.
+
+**/
+VOID
+InsertPciDevice (
+  IN PCI_IO_DEVICE      *Bridge,
+  IN PCI_IO_DEVICE      *PciDeviceNode
+  );
+
+/**
+  Destroy root bridge and remove it from deivce tree.
+
+  @param RootBridge     The bridge want to be removed.
+
+**/
+VOID
+DestroyRootBridge (
+  IN PCI_IO_DEVICE      *RootBridge
+  );
+
+/**
+  Destroy all the pci device node under the bridge.
+  Bridge itself is not included.
+
+  @param Bridge         A pointer to the PCI_IO_DEVICE.
+
+**/
+VOID
+DestroyPciDeviceTree (
+  IN PCI_IO_DEVICE      *Bridge
+  );
+
+/**
+  Destroy all device nodes under the root bridge
+  specified by Controller.
+
+  The root bridge itself is also included.
+
+  @param  Controller    Root bridge handle.
+
+  @retval EFI_SUCCESS   Destory all devcie nodes successfully.
+  @retval EFI_NOT_FOUND Cannot find any PCI device under specified
+                        root bridge.
+
+**/
+EFI_STATUS
+DestroyRootBridgeByHandle (
+  IN EFI_HANDLE        Controller
+  );
+
+/**
+  This function registers the PCI IO device.
+
+  It creates a handle for this PCI IO device (if the handle does not exist), attaches
+  appropriate protocols onto the handle, does necessary initialization, and sets up
+  parent/child relationship with its bus controller.
+
+  @param Controller     An EFI handle for the PCI bus controller.
+  @param PciIoDevice    A PCI_IO_DEVICE pointer to the PCI IO device to be registered.
+  @param Handle         A pointer to hold the returned EFI handle for the PCI IO device.
+
+  @retval EFI_SUCCESS   The PCI device is successfully registered.
+  @retval other         An error occurred when registering the PCI device.
+
+**/
+EFI_STATUS
+RegisterPciDevice (
+  IN  EFI_HANDLE          Controller,
+  IN  PCI_IO_DEVICE       *PciIoDevice,
+  OUT EFI_HANDLE          *Handle      OPTIONAL
+  );
+
+/**
+  This function is used to remove the whole PCI devices on the specified bridge from
+  the root bridge.
+
+  @param RootBridgeHandle   The root bridge device handle.
+  @param Bridge             The bridge device to be removed.
+
+**/
+VOID
+RemoveAllPciDeviceOnBridge (
+  EFI_HANDLE               RootBridgeHandle,
+  PCI_IO_DEVICE            *Bridge
+  );
+
+/**
+  This function is used to de-register the PCI IO device.
+
+  That includes un-installing PciIo protocol from the specified PCI
+  device handle.
+
+  @param Controller    An EFI handle for the PCI bus controller.
+  @param Handle        PCI device handle.
+
+  @retval EFI_SUCCESS  The PCI device is successfully de-registered.
+  @retval other        An error occurred when de-registering the PCI device.
+
+**/
+EFI_STATUS
+DeRegisterPciDevice (
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  );
+
+/**
+  Start to manage the PCI device on the specified root bridge or PCI-PCI Bridge.
+
+  @param Controller          The root bridge handle.
+  @param RootBridge          A pointer to the PCI_IO_DEVICE.
+  @param RemainingDevicePath A pointer to the EFI_DEVICE_PATH_PROTOCOL.
+  @param NumberOfChildren    Children number.
+  @param ChildHandleBuffer   A pointer to the child handle buffer.
+
+  @retval EFI_NOT_READY   Device is not allocated.
+  @retval EFI_UNSUPPORTED Device only support PCI-PCI bridge.
+  @retval EFI_NOT_FOUND   Can not find the specific device.
+  @retval EFI_SUCCESS     Success to start Pci devices on bridge.
+
+**/
+EFI_STATUS
+StartPciDevicesOnBridge (
+  IN EFI_HANDLE                          Controller,
+  IN PCI_IO_DEVICE                       *RootBridge,
+  IN EFI_DEVICE_PATH_PROTOCOL            *RemainingDevicePath,
+  IN OUT UINT8                           *NumberOfChildren,
+  IN OUT EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Start to manage all the PCI devices it found previously under
+  the entire host bridge.
+
+  @param Controller          The root bridge handle.
+
+  @retval EFI_NOT_READY   Device is not allocated.
+  @retval EFI_SUCCESS     Success to start Pci device on host bridge.
+
+**/
+EFI_STATUS
+StartPciDevices (
+  IN EFI_HANDLE                         Controller
+  );
+
+/**
+  Create root bridge device.
+
+  @param RootBridgeHandle    Specified root bridge hanle.
+
+  @return The crated root bridge device instance, NULL means no
+          root bridge device instance created.
+
+**/
+PCI_IO_DEVICE *
+CreateRootBridge (
+  IN EFI_HANDLE                   RootBridgeHandle
+  );
+
+/**
+  Get root bridge device instance by specific root bridge handle.
+
+  @param RootBridgeHandle    Given root bridge handle.
+
+  @return The root bridge device instance, NULL means no root bridge
+          device instance found.
+
+**/
+PCI_IO_DEVICE *
+GetRootBridgeByHandle (
+  EFI_HANDLE RootBridgeHandle
+  );
+
+
+/**
+  Judege whether Pci device existed.
+
+  @param Bridge       Parent bridege instance.
+  @param PciIoDevice  Device instance.
+
+  @retval TRUE        Pci device existed.
+  @retval FALSE       Pci device did not exist.
+
+**/
+BOOLEAN
+PciDeviceExisted (
+  IN PCI_IO_DEVICE    *Bridge,
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  Get the active VGA device on the same segment.
+
+  @param VgaDevice    PCI IO instance for the VGA device.
+
+  @return The active VGA device on the same segment.
+
+**/
+PCI_IO_DEVICE *
+ActiveVGADeviceOnTheSameSegment (
+  IN PCI_IO_DEVICE        *VgaDevice
+  );
+
+/**
+  Get the active VGA device on the root bridge.
+
+  @param RootBridge  PCI IO instance for the root bridge.
+
+  @return The active VGA device.
+
+**/
+PCI_IO_DEVICE *
+ActiveVGADeviceOnTheRootBridge (
+  IN PCI_IO_DEVICE        *RootBridge
+  );
+
+/**
+  Get HPC PCI address according to its device path.
+
+  @param RootBridge           Root bridege Io instance.
+  @param RemainingDevicePath  Given searching device path.
+  @param PciAddress           Buffer holding searched result.
+
+  @retval EFI_SUCCESS         PCI address was stored in PciAddress.
+  @retval EFI_NOT_FOUND       Can not find the specific device path.
+
+**/
+EFI_STATUS
+GetHpcPciAddressFromRootBridge (
+  IN  PCI_IO_DEVICE                    *RootBridge,
+  IN  EFI_DEVICE_PATH_PROTOCOL         *RemainingDevicePath,
+  OUT UINT64                           *PciAddress
+  );
+
+/**
+  Destroy a pci device node.
+
+  All direct or indirect allocated resource for this node will be freed.
+
+  @param PciIoDevice  A pointer to the PCI_IO_DEVICE to be destoried.
+
+**/
+VOID
+FreePciDevice (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.c
new file mode 100644
index 0000000000..97f45e42d0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.c
@@ -0,0 +1,143 @@
+/** @file
+  Functions implementation for Bus Specific Driver Override protoocl.
+
+Copyright (c) 2006 - 2009, 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 "PciBus.h"
+
+/**
+  Initializes a PCI Driver Override Instance.
+
+  @param  PciIoDevice   PCI Device instance.
+
+**/
+VOID
+InitializePciDriverOverrideInstance (
+  IN OUT PCI_IO_DEVICE          *PciIoDevice
+  )
+{
+  PciIoDevice->PciDriverOverride.GetDriver = GetDriver;
+}
+
+
+/**
+  Uses a bus specific algorithm to retrieve a driver image handle for a controller.
+
+  @param  This                  A pointer to the EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL instance.
+  @param  DriverImageHandle     On input, a pointer to the previous driver image handle returned
+                                by GetDriver(). On output, a pointer to the next driver
+                                image handle. Passing in a NULL, will return the first driver
+                                image handle.
+
+  @retval EFI_SUCCESS           A bus specific override driver is returned in DriverImageHandle.
+  @retval EFI_NOT_FOUND         The end of the list of override drivers was reached.
+                                A bus specific override driver is not returned in DriverImageHandle.
+  @retval EFI_INVALID_PARAMETER DriverImageHandle is not a handle that was returned on a
+                                previous call to GetDriver().
+
+**/
+EFI_STATUS
+EFIAPI
+GetDriver (
+  IN EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL              *This,
+  IN OUT EFI_HANDLE                                         *DriverImageHandle
+  )
+{
+  PCI_IO_DEVICE             *PciIoDevice;
+  LIST_ENTRY                *CurrentLink;
+  PCI_DRIVER_OVERRIDE_LIST  *Node;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_DRIVER_OVERRIDE_THIS (This);
+
+  CurrentLink = PciIoDevice->OptionRomDriverList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &PciIoDevice->OptionRomDriverList) {
+
+    Node = DRIVER_OVERRIDE_FROM_LINK (CurrentLink);
+
+    if (*DriverImageHandle == NULL) {
+
+      *DriverImageHandle = Node->DriverImageHandle;
+      return EFI_SUCCESS;
+    }
+
+    if (*DriverImageHandle == Node->DriverImageHandle) {
+
+      if (CurrentLink->ForwardLink == &PciIoDevice->OptionRomDriverList ||
+          CurrentLink->ForwardLink == NULL) {
+        return EFI_NOT_FOUND;
+      }
+
+      //
+      // Get next node
+      //
+      Node                = DRIVER_OVERRIDE_FROM_LINK (CurrentLink->ForwardLink);
+      *DriverImageHandle  = Node->DriverImageHandle;
+      return EFI_SUCCESS;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Add an overriding driver image.
+
+  @param PciIoDevice        Instance of PciIo device.
+  @param DriverImageHandle  new added driver image.
+
+  @retval EFI_SUCCESS          Successfully added driver.
+  @retval EFI_OUT_OF_RESOURCES No memory resource for new driver instance.
+  @retval other                Some error occurred when locating gEfiLoadedImageProtocolGuid.
+
+**/
+EFI_STATUS
+AddDriver (
+  IN PCI_IO_DEVICE     *PciIoDevice,
+  IN EFI_HANDLE        DriverImageHandle
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_LOADED_IMAGE_PROTOCOL     *LoadedImage;
+  PE_COFF_LOADER_IMAGE_CONTEXT  ImageContext;
+  PCI_DRIVER_OVERRIDE_LIST      *Node;
+
+  Status = gBS->HandleProtocol (DriverImageHandle, &gEfiLoadedImageProtocolGuid, (VOID **) &LoadedImage);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Node = AllocatePool (sizeof (PCI_DRIVER_OVERRIDE_LIST));
+  if (Node == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Node->Signature         = DRIVER_OVERRIDE_SIGNATURE;
+  Node->DriverImageHandle = DriverImageHandle;
+
+  InsertTailList (&PciIoDevice->OptionRomDriverList, &(Node->Link));
+
+  PciIoDevice->BusOverride  = TRUE;
+
+  ImageContext.Handle    = LoadedImage->ImageBase;
+  ImageContext.ImageRead = PeCoffLoaderImageReadFromMemory;
+
+  //
+  // Get information about the image
+  //
+  PeCoffLoaderGetImageInfo (&ImageContext);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.h
new file mode 100644
index 0000000000..bf8efff8f1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.h
@@ -0,0 +1,86 @@
+/** @file
+  Functions declaration for Bus Specific Driver Override protoocl.
+
+Copyright (c) 2006 - 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+
+#ifndef _EFI_PCI_DRIVER_OVERRRIDE_H_
+#define _EFI_PCI_DRIVER_OVERRRIDE_H_
+
+#define DRIVER_OVERRIDE_SIGNATURE SIGNATURE_32 ('d', 'r', 'o', 'v')
+
+//
+// PCI driver override driver image list
+//
+typedef struct {
+  UINT32          Signature;
+  LIST_ENTRY      Link;
+  EFI_HANDLE      DriverImageHandle;
+} PCI_DRIVER_OVERRIDE_LIST;
+
+
+#define DRIVER_OVERRIDE_FROM_LINK(a) \
+  CR (a, PCI_DRIVER_OVERRIDE_LIST, Link, DRIVER_OVERRIDE_SIGNATURE)
+
+/**
+  Initializes a PCI Driver Override Instance.
+
+  @param  PciIoDevice   PCI Device instance.
+
+**/
+VOID
+InitializePciDriverOverrideInstance (
+  IN OUT PCI_IO_DEVICE          *PciIoDevice
+  );
+
+/**
+  Add an overriding driver image.
+
+  @param PciIoDevice        Instance of PciIo device.
+  @param DriverImageHandle  new added driver image.
+
+  @retval EFI_SUCCESS          Successfully added driver.
+  @retval EFI_OUT_OF_RESOURCES No memory resource for new driver instance.
+  @retval other                Some error occurred when locating gEfiLoadedImageProtocolGuid.
+
+**/
+EFI_STATUS
+AddDriver (
+  IN PCI_IO_DEVICE     *PciIoDevice,
+  IN EFI_HANDLE        DriverImageHandle
+  );
+
+
+/**
+  Uses a bus specific algorithm to retrieve a driver image handle for a controller.
+
+  @param  This                  A pointer to the EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL instance.
+  @param  DriverImageHandle     On input, a pointer to the previous driver image handle returned
+                                by GetDriver(). On output, a pointer to the next driver
+                                image handle. Passing in a NULL, will return the first driver
+                                image handle.
+
+  @retval EFI_SUCCESS           A bus specific override driver is returned in DriverImageHandle.
+  @retval EFI_NOT_FOUND         The end of the list of override drivers was reached.
+                                A bus specific override driver is not returned in DriverImageHandle.
+  @retval EFI_INVALID_PARAMETER DriverImageHandle is not a handle that was returned on a
+                                previous call to GetDriver().
+
+**/
+EFI_STATUS
+EFIAPI
+GetDriver (
+  IN EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL              *This,
+  IN OUT EFI_HANDLE                                         *DriverImageHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.c
new file mode 100644
index 0000000000..7329143136
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.c
@@ -0,0 +1,2251 @@
+/** @file
+  PCI eunmeration implementation on entire PCI bus system for PCI Bus module.
+
+Copyright (c) 2006 - 2013, 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 "PciBus.h"
+
+/**
+  This routine is used to enumerate entire pci bus system
+  in a given platform.
+
+  @param Controller  Parent controller handle.
+
+  @retval EFI_SUCCESS    PCI enumeration finished successfully.
+  @retval other          Some error occurred when enumerating the pci bus system.
+
+**/
+EFI_STATUS
+PciEnumerator (
+  IN EFI_HANDLE                    Controller
+  )
+{
+  EFI_HANDLE                                        HostBridgeHandle;
+  EFI_STATUS                                        Status;
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL                   *PciRootBridgeIo;
+
+  //
+  // If PCI bus has already done the full enumeration, never do it again
+  //
+  if (!gFullEnumeration) {
+    return PciEnumeratorLight (Controller);
+  }
+
+  //
+  // Get the rootbridge Io protocol to find the host bridge handle
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the host bridge handle
+  //
+  HostBridgeHandle = PciRootBridgeIo->ParentHandle;
+
+  //
+  // Get the pci host bridge resource allocation protocol
+  //
+  Status = gBS->OpenProtocol (
+                  HostBridgeHandle,
+                  &gEfiPciHostBridgeResourceAllocationProtocolGuid,
+                  (VOID **) &PciResAlloc,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Notify the pci bus enumeration is about to begin
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeBeginEnumeration);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Start the bus allocation phase
+  //
+  Status = PciHostBridgeEnumerator (PciResAlloc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Submit the resource request
+  //
+  Status = PciHostBridgeResourceAllocator (PciResAlloc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Notify the pci bus enumeration is about to complete
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeEndEnumeration);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Process P2C
+  //
+  Status = PciHostBridgeP2CProcess (PciResAlloc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Process attributes for devices on this host bridge
+  //
+  Status = PciHostBridgeDeviceAttribute (PciResAlloc);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gFullEnumeration = FALSE;
+
+  Status = gBS->InstallProtocolInterface (
+                  &HostBridgeHandle,
+                  &gEfiPciEnumerationCompleteProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Enumerate PCI root bridge.
+
+  @param PciResAlloc   Pointer to protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+  @param RootBridgeDev Instance of root bridge device.
+
+  @retval EFI_SUCCESS  Successfully enumerated root bridge.
+  @retval other        Failed to enumerate root bridge.
+
+**/
+EFI_STATUS
+PciRootBridgeEnumerator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc,
+  IN PCI_IO_DEVICE                                     *RootBridgeDev
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration1;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration2;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration3;
+  UINT8                             SubBusNumber;
+  UINT8                             StartBusNumber;
+  UINT8                             PaddedBusRange;
+  EFI_HANDLE                        RootBridgeHandle;
+  UINT8                             Desc;
+  UINT64                            AddrLen;
+  UINT64                            AddrRangeMin;
+
+  SubBusNumber    = 0;
+  StartBusNumber  = 0;
+  PaddedBusRange  = 0;
+
+  //
+  // Get the root bridge handle
+  //
+  RootBridgeHandle = RootBridgeDev->Handle;
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_IO_BUS_PCI | EFI_IOB_PCI_BUS_ENUM,
+    RootBridgeDev->DevicePath
+    );
+
+  //
+  // Get the Bus information
+  //
+  Status = PciResAlloc->StartBusEnumeration (
+                          PciResAlloc,
+                          RootBridgeHandle,
+                          (VOID **) &Configuration
+                          );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Configuration == NULL || Configuration->Desc == ACPI_END_TAG_DESCRIPTOR) {
+    return EFI_INVALID_PARAMETER;
+  }
+  RootBridgeDev->BusNumberRanges = Configuration;
+
+  //
+  // Sort the descriptors in ascending order
+  //
+  for (Configuration1 = Configuration; Configuration1->Desc != ACPI_END_TAG_DESCRIPTOR; Configuration1++) {
+    Configuration2 = Configuration1;
+    for (Configuration3 = Configuration1 + 1; Configuration3->Desc != ACPI_END_TAG_DESCRIPTOR; Configuration3++) {
+      if (Configuration2->AddrRangeMin > Configuration3->AddrRangeMin) {
+        Configuration2 = Configuration3;
+      }
+    }
+    //
+    // All other fields other than AddrRangeMin and AddrLen are ignored in a descriptor,
+    // so only need to swap these two fields.
+    //
+    if (Configuration2 != Configuration1) {
+      AddrRangeMin = Configuration1->AddrRangeMin;
+      Configuration1->AddrRangeMin = Configuration2->AddrRangeMin;
+      Configuration2->AddrRangeMin = AddrRangeMin;
+      
+      AddrLen = Configuration1->AddrLen;
+      Configuration1->AddrLen = Configuration2->AddrLen;
+      Configuration2->AddrLen = AddrLen;
+    }
+  }
+
+  //
+  // Get the bus number to start with
+  //
+  StartBusNumber = (UINT8) (Configuration->AddrRangeMin);
+
+  //
+  // Initialize the subordinate bus number
+  //
+  SubBusNumber = StartBusNumber;
+
+  //
+  // Reset all assigned PCI bus number
+  //
+  ResetAllPpbBusNumber (
+    RootBridgeDev,
+    StartBusNumber
+  );
+
+  //
+  // Assign bus number
+  //
+  Status = PciScanBus (
+            RootBridgeDev,
+            StartBusNumber,
+            &SubBusNumber,
+            &PaddedBusRange
+            );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+
+  //
+  // Assign max bus number scanned
+  //
+
+  Status = PciAllocateBusNumber (RootBridgeDev, SubBusNumber, PaddedBusRange, &SubBusNumber);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }  
+
+  //
+  // Find the bus range which contains the higest bus number, then returns the number of buses
+  // that should be decoded.
+  //
+  while (Configuration->AddrRangeMin + Configuration->AddrLen - 1 < SubBusNumber) {
+    Configuration++;
+  }
+  AddrLen = Configuration->AddrLen;
+  Configuration->AddrLen = SubBusNumber - Configuration->AddrRangeMin + 1;
+
+  //
+  // Save the Desc field of the next descriptor. Mark the next descriptor as an END descriptor.
+  //
+  Configuration++;
+  Desc = Configuration->Desc;
+  Configuration->Desc = ACPI_END_TAG_DESCRIPTOR;
+  
+  //
+  // Set bus number
+  //
+  Status = PciResAlloc->SetBusNumbers (
+                          PciResAlloc,
+                          RootBridgeHandle,
+                          RootBridgeDev->BusNumberRanges
+                          );
+
+  //
+  // Restore changed fields
+  //
+  Configuration->Desc = Desc;
+  (Configuration - 1)->AddrLen = AddrLen;
+  
+  return Status;
+}
+
+/**
+  This routine is used to process all PCI devices' Option Rom
+  on a certain root bridge.
+
+  @param Bridge     Given parent's root bridge.
+  @param RomBase    Base address of ROM driver loaded from.
+  @param MaxLength  Maximum rom size.
+
+**/
+VOID
+ProcessOptionRom (
+  IN PCI_IO_DEVICE *Bridge,
+  IN UINT64        RomBase,
+  IN UINT64        MaxLength
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  //
+  // Go through bridges to reach all devices
+  //
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (!IsListEmpty (&Temp->ChildList)) {
+
+      //
+      // Go further to process the option rom under this bridge
+      //
+      ProcessOptionRom (Temp, RomBase, MaxLength);
+    }
+
+    if (Temp->RomSize != 0 && Temp->RomSize <= MaxLength) {
+
+      //
+      // Load and process the option rom
+      //
+      LoadOpRomImage (Temp, RomBase);
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+}
+
+/**
+  This routine is used to assign bus number to the given PCI bus system
+
+  @param Bridge             Parent root bridge instance.
+  @param StartBusNumber     Number of beginning.
+  @param SubBusNumber       The number of sub bus.
+
+  @retval EFI_SUCCESS       Successfully assigned bus number.
+  @retval EFI_DEVICE_ERROR  Failed to assign bus number.
+
+**/
+EFI_STATUS
+PciAssignBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  OUT UINT8                             *SubBusNumber
+  )
+{
+  EFI_STATUS                      Status;
+  PCI_TYPE00                      Pci;
+  UINT8                           Device;
+  UINT8                           Func;
+  UINT64                          Address;
+  UINTN                           SecondBus;
+  UINT16                          Register;
+  UINT8                           Register8;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  PciRootBridgeIo = Bridge->PciRootBridgeIo;
+
+  SecondBus       = 0;
+  Register        = 0;
+
+  *SubBusNumber = StartBusNumber;
+
+  //
+  // First check to see whether the parent is ppb
+  //
+  for (Device = 0; Device <= PCI_MAX_DEVICE; Device++) {
+    for (Func = 0; Func <= PCI_MAX_FUNC; Func++) {
+
+      //
+      // Check to see whether a pci device is present
+      //
+      Status = PciDevicePresent (
+                PciRootBridgeIo,
+                &Pci,
+                StartBusNumber,
+                Device,
+                Func
+                );
+
+      if (!EFI_ERROR (Status)   &&
+          (IS_PCI_BRIDGE (&Pci) || IS_CARDBUS_BRIDGE (&Pci))) {
+
+        //
+        // Reserved one bus for cardbus bridge
+        //
+        Status = PciAllocateBusNumber (Bridge, *SubBusNumber, 1, SubBusNumber);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        SecondBus = *SubBusNumber;
+
+        Register  = (UINT16) ((SecondBus << 8) | (UINT16) StartBusNumber);
+
+        Address   = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0x18);
+
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint16,
+                                        Address,
+                                        1,
+                                        &Register
+                                        );
+
+        //
+        // Initialize SubBusNumber to SecondBus
+        //
+        Address = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0x1A);
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint8,
+                                        Address,
+                                        1,
+                                        SubBusNumber
+                                        );
+        //
+        // If it is PPB, resursively search down this bridge
+        //
+        if (IS_PCI_BRIDGE (&Pci)) {
+
+          Register8 = 0xFF;
+          Status = PciRootBridgeIo->Pci.Write (
+                                          PciRootBridgeIo,
+                                          EfiPciWidthUint8,
+                                          Address,
+                                          1,
+                                          &Register8
+                                          );
+
+          Status = PciAssignBusNumber (
+                    Bridge,
+                    (UINT8) (SecondBus),
+                    SubBusNumber
+                    );
+
+          if (EFI_ERROR (Status)) {
+            return EFI_DEVICE_ERROR;
+          }
+        }
+
+        //
+        // Set the current maximum bus number under the PPB
+        //
+        Address = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0x1A);
+
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint8,
+                                        Address,
+                                        1,
+                                        SubBusNumber
+                                        );
+
+      }
+
+      if (Func == 0 && !IS_PCI_MULTI_FUNC (&Pci)) {
+
+        //
+        // Skip sub functions, this is not a multi function device
+        //
+        Func = PCI_MAX_FUNC;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This routine is used to determine the root bridge attribute by interfacing
+  the host bridge resource allocation protocol.
+
+  @param PciResAlloc    Protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL
+  @param RootBridgeDev  Root bridge instance
+
+  @retval EFI_SUCCESS  Successfully got root bridge's attribute.
+  @retval other        Failed to get attribute.
+
+**/
+EFI_STATUS
+DetermineRootBridgeAttributes (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc,
+  IN PCI_IO_DEVICE                                    *RootBridgeDev
+  )
+{
+  UINT64      Attributes;
+  EFI_STATUS  Status;
+  EFI_HANDLE  RootBridgeHandle;
+
+  Attributes        = 0;
+  RootBridgeHandle  = RootBridgeDev->Handle;
+
+  //
+  // Get root bridge attribute by calling into pci host bridge resource allocation protocol
+  //
+  Status = PciResAlloc->GetAllocAttributes (
+                          PciResAlloc,
+                          RootBridgeHandle,
+                          &Attributes
+                          );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Here is the point where PCI bus driver calls HOST bridge allocation protocol
+  // Currently we hardcoded for ea815
+  //
+  if ((Attributes & EFI_PCI_HOST_BRIDGE_COMBINE_MEM_PMEM) != 0) {
+    RootBridgeDev->Decodes |= EFI_BRIDGE_PMEM_MEM_COMBINE_SUPPORTED;
+  }
+
+  if ((Attributes & EFI_PCI_HOST_BRIDGE_MEM64_DECODE) != 0) {
+    RootBridgeDev->Decodes |= EFI_BRIDGE_MEM64_DECODE_SUPPORTED;
+    RootBridgeDev->Decodes |= EFI_BRIDGE_PMEM64_DECODE_SUPPORTED;
+  }
+
+  RootBridgeDev->Decodes |= EFI_BRIDGE_MEM32_DECODE_SUPPORTED;
+  RootBridgeDev->Decodes |= EFI_BRIDGE_PMEM32_DECODE_SUPPORTED;
+  RootBridgeDev->Decodes |= EFI_BRIDGE_IO16_DECODE_SUPPORTED;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get Max Option Rom size on specified bridge.
+
+  @param Bridge    Given bridge device instance.
+
+  @return Max size of option rom needed.
+
+**/
+UINT64
+GetMaxOptionRomSize (
+  IN PCI_IO_DEVICE   *Bridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+  UINT64          MaxOptionRomSize;
+  UINT64          TempOptionRomSize;
+
+  MaxOptionRomSize = 0;
+
+  //
+  // Go through bridges to reach all devices
+  //
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (!IsListEmpty (&Temp->ChildList)) {
+
+      //
+      // Get max option rom size under this bridge
+      //
+      TempOptionRomSize = GetMaxOptionRomSize (Temp);
+
+      //
+      // Compare with the option rom size of the bridge
+      // Get the larger one
+      //
+      if (Temp->RomSize > TempOptionRomSize) {
+        TempOptionRomSize = Temp->RomSize;
+      }
+
+    } else {
+
+      //
+      // For devices get the rom size directly
+      //
+      TempOptionRomSize = Temp->RomSize;
+    }
+
+    //
+    // Get the largest rom size on this bridge
+    //
+    if (TempOptionRomSize > MaxOptionRomSize) {
+      MaxOptionRomSize = TempOptionRomSize;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return MaxOptionRomSize;
+}
+
+/**
+  Process attributes of devices on this host bridge
+
+  @param PciResAlloc Protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS   Successfully process attribute.
+  @retval EFI_NOT_FOUND Can not find the specific root bridge device.
+  @retval other         Failed to determine the root bridge device's attribute.
+
+**/
+EFI_STATUS
+PciHostBridgeDeviceAttribute (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  )
+{
+  EFI_HANDLE    RootBridgeHandle;
+  PCI_IO_DEVICE *RootBridgeDev;
+  EFI_STATUS    Status;
+
+  RootBridgeHandle = NULL;
+
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+    //
+    // Get RootBridg Device by handle
+    //
+    RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Set the attributes for devcies behind the Root Bridge
+    //
+    Status = DetermineDeviceAttribute (RootBridgeDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get resource allocation status from the ACPI resource descriptor.
+
+  @param AcpiConfig       Point to Acpi configuration table.
+  @param IoResStatus      Return the status of I/O resource.
+  @param Mem32ResStatus   Return the status of 32-bit Memory resource.
+  @param PMem32ResStatus  Return the status of 32-bit Prefetchable Memory resource.
+  @param Mem64ResStatus   Return the status of 64-bit Memory resource.
+  @param PMem64ResStatus  Return the status of 64-bit Prefetchable Memory resource.
+
+**/
+VOID
+GetResourceAllocationStatus (
+  VOID        *AcpiConfig,
+  OUT UINT64  *IoResStatus,
+  OUT UINT64  *Mem32ResStatus,
+  OUT UINT64  *PMem32ResStatus,
+  OUT UINT64  *Mem64ResStatus,
+  OUT UINT64  *PMem64ResStatus
+  )
+{
+  UINT8                             *Temp;
+  UINT64                            ResStatus;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *ACPIAddressDesc;
+
+  Temp = (UINT8 *) AcpiConfig;
+
+  while (*Temp == ACPI_ADDRESS_SPACE_DESCRIPTOR) {
+
+    ACPIAddressDesc       = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Temp;
+    ResStatus = ACPIAddressDesc->AddrTranslationOffset;
+
+    switch (ACPIAddressDesc->ResType) {
+    case 0:
+      if (ACPIAddressDesc->AddrSpaceGranularity == 32) {
+        if (ACPIAddressDesc->SpecificFlag == 0x06) {
+          //
+          // Pmem32
+          //
+          *PMem32ResStatus = ResStatus;
+        } else {
+          //
+          // Mem32
+          //
+          *Mem32ResStatus = ResStatus;
+        }
+      }
+
+      if (ACPIAddressDesc->AddrSpaceGranularity == 64) {
+        if (ACPIAddressDesc->SpecificFlag == 0x06) {
+          //
+          // PMem64
+          //
+          *PMem64ResStatus = ResStatus;
+        } else {
+          //
+          // Mem64
+          //
+          *Mem64ResStatus = ResStatus;
+        }
+      }
+
+      break;
+
+    case 1:
+      //
+      // Io
+      //
+      *IoResStatus = ResStatus;
+      break;
+
+    default:
+      break;
+    }
+
+    Temp += sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR);
+  }
+}
+
+/**
+  Remove a PCI device from device pool and mark its bar.
+
+  @param PciDevice Instance of Pci device.
+
+  @retval EFI_SUCCESS Successfully remove the PCI device.
+  @retval EFI_ABORTED Pci device is a root bridge or a PCI-PCI bridge.
+
+**/
+EFI_STATUS
+RejectPciDevice (
+  IN PCI_IO_DEVICE       *PciDevice
+  )
+{
+  PCI_IO_DEVICE   *Bridge;
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+
+  //
+  // Remove the padding resource from a bridge
+  //
+  if ( IS_PCI_BRIDGE(&PciDevice->Pci) &&
+       PciDevice->ResourcePaddingDescriptors != NULL ) {
+    FreePool (PciDevice->ResourcePaddingDescriptors);
+    PciDevice->ResourcePaddingDescriptors = NULL;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Skip RB and PPB
+  //
+  if (IS_PCI_BRIDGE (&PciDevice->Pci) || (PciDevice->Parent == NULL)) {
+    return EFI_ABORTED;
+  }
+
+  if (IS_CARDBUS_BRIDGE (&PciDevice->Pci)) {
+    //
+    // Get the root bridge device
+    //
+    Bridge = PciDevice;
+    while (Bridge->Parent != NULL) {
+      Bridge = Bridge->Parent;
+    }
+
+    RemoveAllPciDeviceOnBridge (Bridge->Handle, PciDevice);
+
+    //
+    // Mark its bar
+    //
+    InitializeP2C (PciDevice);
+  }
+
+  //
+  // Remove the device
+  //
+  Bridge      = PciDevice->Parent;
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (Temp == PciDevice) {
+      InitializePciDevice (Temp);
+      RemoveEntryList (CurrentLink);
+      return EFI_SUCCESS;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_ABORTED;
+}
+
+/**
+  Determine whethter a PCI device can be rejected.
+
+  @param  PciResNode Pointer to Pci resource node instance.
+
+  @retval TRUE  The PCI device can be rejected.
+  @retval TRUE  The PCI device cannot be rejected.
+
+**/
+BOOLEAN
+IsRejectiveDevice (
+  IN  PCI_RESOURCE_NODE   *PciResNode
+  )
+{
+  PCI_IO_DEVICE *Temp;
+
+  Temp = PciResNode->PciDev;
+
+  //
+  // Ensure the device is present
+  //
+  if (Temp == NULL) {
+    return FALSE;
+  }
+
+  //
+  // PPB and RB should go ahead
+  //
+  if (IS_PCI_BRIDGE (&Temp->Pci) || (Temp->Parent == NULL)) {
+    return TRUE;
+  }
+
+  //
+  // Skip device on Bus0
+  //
+  if ((Temp->Parent != NULL) && (Temp->BusNumber == 0)) {
+    return FALSE;
+  }
+
+  //
+  // Skip VGA
+  //
+  if (IS_PCI_VGA (&Temp->Pci)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Compare two resource nodes and get the larger resource consumer.
+
+  @param PciResNode1  resource node 1 want to be compared
+  @param PciResNode2  resource node 2 want to be compared
+
+  @return Larger resource node.
+
+**/
+PCI_RESOURCE_NODE *
+GetLargerConsumerDevice (
+  IN  PCI_RESOURCE_NODE   *PciResNode1,
+  IN  PCI_RESOURCE_NODE   *PciResNode2
+  )
+{
+  if (PciResNode2 == NULL) {
+    return PciResNode1;
+  }
+
+  if ((IS_PCI_BRIDGE(&(PciResNode2->PciDev->Pci)) || (PciResNode2->PciDev->Parent == NULL)) \
+       && (PciResNode2->ResourceUsage != PciResUsagePadding) )
+  {
+    return PciResNode1;
+  }
+
+  if (PciResNode1 == NULL) {
+    return PciResNode2;
+  }
+
+  if ((PciResNode1->Length) > (PciResNode2->Length)) {
+    return PciResNode1;
+  }
+
+  return PciResNode2;
+}
+
+
+/**
+  Get the max resource consumer in the host resource pool.
+
+  @param ResPool  Pointer to resource pool node.
+
+  @return The max resource consumer in the host resource pool.
+
+**/
+PCI_RESOURCE_NODE *
+GetMaxResourceConsumerDevice (
+  IN  PCI_RESOURCE_NODE   *ResPool
+  )
+{
+  PCI_RESOURCE_NODE *Temp;
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *PciResNode;
+  PCI_RESOURCE_NODE *PPBResNode;
+
+  PciResNode  = NULL;
+
+  CurrentLink = ResPool->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &ResPool->ChildList) {
+
+    Temp = RESOURCE_NODE_FROM_LINK (CurrentLink);
+
+    if (!IsRejectiveDevice (Temp)) {
+      CurrentLink = CurrentLink->ForwardLink;
+      continue;
+    }
+
+    if ((IS_PCI_BRIDGE (&(Temp->PciDev->Pci)) || (Temp->PciDev->Parent == NULL)) \
+          && (Temp->ResourceUsage != PciResUsagePadding))
+    {
+      PPBResNode  = GetMaxResourceConsumerDevice (Temp);
+      PciResNode  = GetLargerConsumerDevice (PciResNode, PPBResNode);
+    } else {
+      PciResNode = GetLargerConsumerDevice (PciResNode, Temp);
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return PciResNode;
+}
+
+/**
+  Adjust host bridge allocation so as to reduce resource requirement
+
+  @param IoPool           Pointer to instance of I/O resource Node.
+  @param Mem32Pool        Pointer to instance of 32-bit memory resource Node.
+  @param PMem32Pool       Pointer to instance of 32-bit Prefetchable memory resource node.
+  @param Mem64Pool        Pointer to instance of 64-bit memory resource node.
+  @param PMem64Pool       Pointer to instance of 64-bit Prefetchable memory resource node.
+  @param IoResStatus      Status of I/O resource Node.
+  @param Mem32ResStatus   Status of 32-bit memory resource Node.
+  @param PMem32ResStatus  Status of 32-bit Prefetchable memory resource node.
+  @param Mem64ResStatus   Status of 64-bit memory resource node.
+  @param PMem64ResStatus  Status of 64-bit Prefetchable memory resource node.
+
+  @retval EFI_SUCCESS     Successfully adjusted resoruce on host bridge.
+  @retval EFI_ABORTED     Host bridge hasn't this resource type or no resource be adjusted.
+
+**/
+EFI_STATUS
+PciHostBridgeAdjustAllocation (
+  IN  PCI_RESOURCE_NODE   *IoPool,
+  IN  PCI_RESOURCE_NODE   *Mem32Pool,
+  IN  PCI_RESOURCE_NODE   *PMem32Pool,
+  IN  PCI_RESOURCE_NODE   *Mem64Pool,
+  IN  PCI_RESOURCE_NODE   *PMem64Pool,
+  IN  UINT64              IoResStatus,
+  IN  UINT64              Mem32ResStatus,
+  IN  UINT64              PMem32ResStatus,
+  IN  UINT64              Mem64ResStatus,
+  IN  UINT64              PMem64ResStatus
+  )
+{
+  BOOLEAN                               AllocationAjusted;
+  PCI_RESOURCE_NODE                     *PciResNode;
+  PCI_RESOURCE_NODE                     *ResPool[5];
+  PCI_IO_DEVICE                         *RemovedPciDev[5];
+  UINT64                                ResStatus[5];
+  UINTN                                 RemovedPciDevNum;
+  UINTN                                 DevIndex;
+  UINTN                                 ResType;
+  EFI_STATUS                            Status;
+  EFI_RESOURCE_ALLOC_FAILURE_ERROR_DATA_PAYLOAD AllocFailExtendedData;
+
+  PciResNode = NULL;
+  ZeroMem (RemovedPciDev, 5 * sizeof (PCI_IO_DEVICE *));
+  RemovedPciDevNum  = 0;
+
+  ResPool[0]        = IoPool;
+  ResPool[1]        = Mem32Pool;
+  ResPool[2]        = PMem32Pool;
+  ResPool[3]        = Mem64Pool;
+  ResPool[4]        = PMem64Pool;
+
+  ResStatus[0]      = IoResStatus;
+  ResStatus[1]      = Mem32ResStatus;
+  ResStatus[2]      = PMem32ResStatus;
+  ResStatus[3]      = Mem64ResStatus;
+  ResStatus[4]      = PMem64ResStatus;
+
+  AllocationAjusted = FALSE;
+
+  for (ResType = 0; ResType < 5; ResType++) {
+
+    if (ResStatus[ResType] == EFI_RESOURCE_SATISFIED) {
+      continue;
+    }
+
+    if (ResStatus[ResType] == EFI_RESOURCE_NOT_SATISFIED) {
+      //
+      // Host bridge hasn't this resource type
+      //
+      return EFI_ABORTED;
+    }
+
+    //
+    // Hostbridge hasn't enough resource
+    //
+    PciResNode = GetMaxResourceConsumerDevice (ResPool[ResType]);
+    if (PciResNode == NULL) {
+      continue;
+    }
+
+    //
+    // Check if the device has been removed before
+    //
+    for (DevIndex = 0; DevIndex < RemovedPciDevNum; DevIndex++) {
+      if (PciResNode->PciDev == RemovedPciDev[DevIndex]) {
+        break;
+      }
+    }
+
+    if (DevIndex != RemovedPciDevNum) {
+      continue;
+    }
+
+    //
+    // Remove the device if it isn't in the array
+    //
+    Status = RejectPciDevice (PciResNode->PciDev);
+    if (Status == EFI_SUCCESS) {
+      DEBUG ((
+        EFI_D_ERROR,
+        "PciBus: [%02x|%02x|%02x] was rejected due to resource confliction.\n",
+        PciResNode->PciDev->BusNumber, PciResNode->PciDev->DeviceNumber, PciResNode->PciDev->FunctionNumber
+        ));
+
+      //
+      // Raise the EFI_IOB_EC_RESOURCE_CONFLICT status code
+      //
+      //
+      // Have no way to get ReqRes, AllocRes & Bar here
+      //
+      ZeroMem (&AllocFailExtendedData, sizeof (AllocFailExtendedData));
+      AllocFailExtendedData.DevicePathSize = (UINT16) sizeof (EFI_DEVICE_PATH_PROTOCOL);
+      AllocFailExtendedData.DevicePath     = (UINT8 *) PciResNode->PciDev->DevicePath;
+      AllocFailExtendedData.Bar            = PciResNode->Bar;
+
+      REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+            EFI_PROGRESS_CODE,
+            EFI_IO_BUS_PCI | EFI_IOB_EC_RESOURCE_CONFLICT,
+            (VOID *) &AllocFailExtendedData,
+            sizeof (AllocFailExtendedData)
+            );
+
+      //
+      // Add it to the array and indicate at least a device has been rejected
+      //
+      RemovedPciDev[RemovedPciDevNum++] = PciResNode->PciDev;
+      AllocationAjusted                 = TRUE;
+    }
+  }
+  //
+  // End for
+  //
+
+  if (AllocationAjusted) {
+    return EFI_SUCCESS;
+  } else {
+    return EFI_ABORTED;
+  }
+}
+
+/**
+  Summary requests for all resource type, and contruct ACPI resource
+  requestor instance.
+
+  @param Bridge           detecting bridge
+  @param IoNode           Pointer to instance of I/O resource Node
+  @param Mem32Node        Pointer to instance of 32-bit memory resource Node
+  @param PMem32Node       Pointer to instance of 32-bit Pmemory resource node
+  @param Mem64Node        Pointer to instance of 64-bit memory resource node
+  @param PMem64Node       Pointer to instance of 64-bit Pmemory resource node
+  @param Config           Output buffer holding new constructed APCI resource requestor
+
+  @retval EFI_SUCCESS           Successfully constructed ACPI resource.
+  @retval EFI_OUT_OF_RESOURCES  No memory availabe.
+
+**/
+EFI_STATUS
+ConstructAcpiResourceRequestor (
+  IN PCI_IO_DEVICE      *Bridge,
+  IN PCI_RESOURCE_NODE  *IoNode,
+  IN PCI_RESOURCE_NODE  *Mem32Node,
+  IN PCI_RESOURCE_NODE  *PMem32Node,
+  IN PCI_RESOURCE_NODE  *Mem64Node,
+  IN PCI_RESOURCE_NODE  *PMem64Node,
+  OUT VOID              **Config
+  )
+{
+  UINT8                             NumConfig;
+  UINT8                             Aperture;
+  UINT8                             *Configuration;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Ptr;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *PtrEnd;
+
+  NumConfig = 0;
+  Aperture  = 0;
+
+  *Config  = NULL;
+
+  //
+  // if there is io request, add to the io aperture
+  //
+  if (ResourceRequestExisted (IoNode)) {
+    NumConfig++;
+    Aperture |= 0x01;
+  }
+
+  //
+  // if there is mem32 request, add to the mem32 aperture
+  //
+  if (ResourceRequestExisted (Mem32Node)) {
+    NumConfig++;
+    Aperture |= 0x02;
+  }
+
+  //
+  // if there is pmem32 request, add to the pmem32 aperture
+  //
+  if (ResourceRequestExisted (PMem32Node)) {
+    NumConfig++;
+    Aperture |= 0x04;
+  }
+
+  //
+  // if there is mem64 request, add to the mem64 aperture
+  //
+  if (ResourceRequestExisted (Mem64Node)) {
+    NumConfig++;
+    Aperture |= 0x08;
+  }
+
+  //
+  // if there is pmem64 request, add to the pmem64 aperture
+  //
+  if (ResourceRequestExisted (PMem64Node)) {
+    NumConfig++;
+    Aperture |= 0x10;
+  }
+
+  if (NumConfig != 0) {
+
+    //
+    // If there is at least one type of resource request,
+    // allocate a acpi resource node
+    //
+    Configuration = AllocateZeroPool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) * NumConfig + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+    if (Configuration == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Ptr = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration;
+
+    //
+    // Deal with io aperture
+    //
+    if ((Aperture & 0x01) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Io
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_IO;
+      //
+      // non ISA range
+      //
+      Ptr->SpecificFlag = 1;
+      Ptr->AddrLen      = IoNode->Length;
+      Ptr->AddrRangeMax = IoNode->Alignment;
+
+      Ptr++;
+    }
+    //
+    // Deal with mem32 aperture
+    //
+    if ((Aperture & 0x02) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Mem
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // Nonprefechable
+      //
+      Ptr->SpecificFlag = 0;
+      //
+      // 32 bit
+      //
+      Ptr->AddrSpaceGranularity = 32;
+      Ptr->AddrLen      = Mem32Node->Length;
+      Ptr->AddrRangeMax = Mem32Node->Alignment;
+
+      Ptr++;
+    }
+
+    //
+    // Deal with Pmem32 aperture
+    //
+    if ((Aperture & 0x04) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Mem
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // prefechable
+      //
+      Ptr->SpecificFlag = 0x6;
+      //
+      // 32 bit
+      //
+      Ptr->AddrSpaceGranularity = 32;
+      Ptr->AddrLen      = PMem32Node->Length;
+      Ptr->AddrRangeMax = PMem32Node->Alignment;
+
+      Ptr++;
+    }
+    //
+    // Deal with mem64 aperture
+    //
+    if ((Aperture & 0x08) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Mem
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // nonprefechable
+      //
+      Ptr->SpecificFlag = 0;
+      //
+      // 64 bit
+      //
+      Ptr->AddrSpaceGranularity = 64;
+      Ptr->AddrLen      = Mem64Node->Length;
+      Ptr->AddrRangeMax = Mem64Node->Alignment;
+
+      Ptr++;
+    }
+    //
+    // Deal with Pmem64 aperture
+    //
+    if ((Aperture & 0x10) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Mem
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // prefechable
+      //
+      Ptr->SpecificFlag = 0x06;
+      //
+      // 64 bit
+      //
+      Ptr->AddrSpaceGranularity = 64;
+      Ptr->AddrLen      = PMem64Node->Length;
+      Ptr->AddrRangeMax = PMem64Node->Alignment;
+
+      Ptr++;
+    }
+
+    //
+    // put the checksum
+    //
+    PtrEnd            = (EFI_ACPI_END_TAG_DESCRIPTOR *) Ptr;
+
+    PtrEnd->Desc      = ACPI_END_TAG_DESCRIPTOR;
+    PtrEnd->Checksum  = 0;
+
+  } else {
+
+    //
+    // If there is no resource request
+    //
+    Configuration = AllocateZeroPool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+    if (Configuration == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Ptr               = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) (Configuration);
+    Ptr->Desc         = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+
+    PtrEnd            = (EFI_ACPI_END_TAG_DESCRIPTOR *) (Ptr + 1);
+    PtrEnd->Desc      = ACPI_END_TAG_DESCRIPTOR;
+    PtrEnd->Checksum  = 0;
+  }
+
+  *Config = Configuration;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get resource base from an acpi configuration descriptor.
+
+  @param Config       An acpi configuration descriptor.
+  @param IoBase       Output of I/O resource base address.
+  @param Mem32Base    Output of 32-bit memory base address.
+  @param PMem32Base   Output of 32-bit prefetchable memory base address.
+  @param Mem64Base    Output of 64-bit memory base address.
+  @param PMem64Base   Output of 64-bit prefetchable memory base address.
+
+**/
+VOID
+GetResourceBase (
+  IN VOID     *Config,
+  OUT UINT64  *IoBase,
+  OUT UINT64  *Mem32Base,
+  OUT UINT64  *PMem32Base,
+  OUT UINT64  *Mem64Base,
+  OUT UINT64  *PMem64Base
+  )
+{
+  UINT8                             *Temp;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Ptr;
+  UINT64                            ResStatus;
+
+  ASSERT (Config != NULL);
+
+  *IoBase     = 0xFFFFFFFFFFFFFFFFULL;
+  *Mem32Base  = 0xFFFFFFFFFFFFFFFFULL;
+  *PMem32Base = 0xFFFFFFFFFFFFFFFFULL;
+  *Mem64Base  = 0xFFFFFFFFFFFFFFFFULL;
+  *PMem64Base = 0xFFFFFFFFFFFFFFFFULL;
+
+  Temp        = (UINT8 *) Config;
+
+  while (*Temp == ACPI_ADDRESS_SPACE_DESCRIPTOR) {
+
+    Ptr       = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Temp;
+    ResStatus = Ptr->AddrTranslationOffset;
+
+    if (ResStatus == EFI_RESOURCE_SATISFIED) {
+
+      switch (Ptr->ResType) {
+
+      //
+      // Memory type aperture
+      //
+      case 0:
+
+        //
+        // Check to see the granularity
+        //
+        if (Ptr->AddrSpaceGranularity == 32) {
+          if ((Ptr->SpecificFlag & 0x06) != 0) {
+            *PMem32Base = Ptr->AddrRangeMin;
+          } else {
+            *Mem32Base = Ptr->AddrRangeMin;
+          }
+        }
+
+        if (Ptr->AddrSpaceGranularity == 64) {
+          if ((Ptr->SpecificFlag & 0x06) != 0) {
+            *PMem64Base = Ptr->AddrRangeMin;
+          } else {
+            *Mem64Base = Ptr->AddrRangeMin;
+          }
+        }
+        break;
+
+      case 1:
+
+        //
+        // Io type aperture
+        //
+        *IoBase = Ptr->AddrRangeMin;
+        break;
+
+      default:
+        break;
+
+      }
+      //
+      // End switch
+      //
+    }
+    //
+    // End for
+    //
+    Temp += sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR);
+  }
+}
+
+/**
+  Enumerate pci bridge, allocate resource and determine attribute
+  for devices on this bridge.
+
+  @param BridgeDev    Pointer to instance of bridge device.
+
+  @retval EFI_SUCCESS Successfully enumerated PCI bridge.
+  @retval other       Failed to enumerate.
+
+**/
+EFI_STATUS
+PciBridgeEnumerator (
+  IN PCI_IO_DEVICE                                     *BridgeDev
+  )
+{
+  UINT8               SubBusNumber;
+  UINT8               StartBusNumber;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  EFI_STATUS          Status;
+
+  SubBusNumber    = 0;
+  StartBusNumber  = 0;
+  PciIo           = &(BridgeDev->PciIo);
+  Status          = PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0x19, 1, &StartBusNumber);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PciAssignBusNumber (
+            BridgeDev,
+            StartBusNumber,
+            &SubBusNumber
+            );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PciPciDeviceInfoCollector (BridgeDev, StartBusNumber);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PciBridgeResourceAllocator (BridgeDev);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = DetermineDeviceAttribute (BridgeDev);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Allocate all kinds of resource for PCI bridge.
+
+  @param  Bridge      Pointer to bridge instance.
+
+  @retval EFI_SUCCESS Successfully allocated resource for PCI bridge.
+  @retval other       Failed to allocate resource for bridge.
+
+**/
+EFI_STATUS
+PciBridgeResourceAllocator (
+  IN PCI_IO_DEVICE  *Bridge
+  )
+{
+  PCI_RESOURCE_NODE *IoBridge;
+  PCI_RESOURCE_NODE *Mem32Bridge;
+  PCI_RESOURCE_NODE *PMem32Bridge;
+  PCI_RESOURCE_NODE *Mem64Bridge;
+  PCI_RESOURCE_NODE *PMem64Bridge;
+  UINT64            IoBase;
+  UINT64            Mem32Base;
+  UINT64            PMem32Base;
+  UINT64            Mem64Base;
+  UINT64            PMem64Base;
+  EFI_STATUS        Status;
+
+  IoBridge = CreateResourceNode (
+               Bridge,
+               0,
+               Bridge->BridgeIoAlignment,
+               0,
+               PciBarTypeIo16,
+               PciResUsageTypical
+               );
+
+  Mem32Bridge = CreateResourceNode (
+                  Bridge,
+                  0,
+                  0xFFFFF,
+                  0,
+                  PciBarTypeMem32,
+                  PciResUsageTypical
+                  );
+
+  PMem32Bridge = CreateResourceNode (
+                   Bridge,
+                   0,
+                   0xFFFFF,
+                   0,
+                   PciBarTypePMem32,
+                   PciResUsageTypical
+                   );
+
+  Mem64Bridge = CreateResourceNode (
+                  Bridge,
+                  0,
+                  0xFFFFF,
+                  0,
+                  PciBarTypeMem64,
+                  PciResUsageTypical
+                  );
+
+  PMem64Bridge = CreateResourceNode (
+                   Bridge,
+                   0,
+                   0xFFFFF,
+                   0,
+                   PciBarTypePMem64,
+                   PciResUsageTypical
+                   );
+
+  //
+  // Create resourcemap by going through all the devices subject to this root bridge
+  //
+  CreateResourceMap (
+    Bridge,
+    IoBridge,
+    Mem32Bridge,
+    PMem32Bridge,
+    Mem64Bridge,
+    PMem64Bridge
+    );
+
+  Status = GetResourceBaseFromBridge (
+             Bridge,
+             &IoBase,
+             &Mem32Base,
+             &PMem32Base,
+             &Mem64Base,
+             &PMem64Base
+             );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Program IO resources
+  //
+  ProgramResource (
+    IoBase,
+    IoBridge
+    );
+
+  //
+  // Program Mem32 resources
+  //
+  ProgramResource (
+    Mem32Base,
+    Mem32Bridge
+    );
+
+  //
+  // Program PMem32 resources
+  //
+  ProgramResource (
+    PMem32Base,
+    PMem32Bridge
+    );
+
+  //
+  // Program Mem64 resources
+  //
+  ProgramResource (
+    Mem64Base,
+    Mem64Bridge
+    );
+
+  //
+  // Program PMem64 resources
+  //
+  ProgramResource (
+    PMem64Base,
+    PMem64Bridge
+    );
+
+  DestroyResourceTree (IoBridge);
+  DestroyResourceTree (Mem32Bridge);
+  DestroyResourceTree (PMem32Bridge);
+  DestroyResourceTree (PMem64Bridge);
+  DestroyResourceTree (Mem64Bridge);
+
+  gBS->FreePool (IoBridge);
+  gBS->FreePool (Mem32Bridge);
+  gBS->FreePool (PMem32Bridge);
+  gBS->FreePool (PMem64Bridge);
+  gBS->FreePool (Mem64Bridge);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get resource base address for a pci bridge device.
+
+  @param Bridge     Given Pci driver instance.
+  @param IoBase     Output for base address of I/O type resource.
+  @param Mem32Base  Output for base address of 32-bit memory type resource.
+  @param PMem32Base Ooutput for base address of 32-bit Pmemory type resource.
+  @param Mem64Base  Output for base address of 64-bit memory type resource.
+  @param PMem64Base Output for base address of 64-bit Pmemory type resource.
+
+  @retval EFI_SUCCESS           Successfully got resource base address.
+  @retval EFI_OUT_OF_RESOURCES  PCI bridge is not available.
+
+**/
+EFI_STATUS
+GetResourceBaseFromBridge (
+  IN  PCI_IO_DEVICE *Bridge,
+  OUT UINT64        *IoBase,
+  OUT UINT64        *Mem32Base,
+  OUT UINT64        *PMem32Base,
+  OUT UINT64        *Mem64Base,
+  OUT UINT64        *PMem64Base
+  )
+{
+  if (!Bridge->Allocated) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  *IoBase     = gAllOne;
+  *Mem32Base  = gAllOne;
+  *PMem32Base = gAllOne;
+  *Mem64Base  = gAllOne;
+  *PMem64Base = gAllOne;
+
+  if (IS_PCI_BRIDGE (&Bridge->Pci)) {
+
+    if (Bridge->PciBar[PPB_IO_RANGE].Length > 0) {
+      *IoBase = Bridge->PciBar[PPB_IO_RANGE].BaseAddress;
+    }
+
+    if (Bridge->PciBar[PPB_MEM32_RANGE].Length > 0) {
+      *Mem32Base = Bridge->PciBar[PPB_MEM32_RANGE].BaseAddress;
+    }
+
+    if (Bridge->PciBar[PPB_PMEM32_RANGE].Length > 0) {
+      *PMem32Base = Bridge->PciBar[PPB_PMEM32_RANGE].BaseAddress;
+    }
+
+    if (Bridge->PciBar[PPB_PMEM64_RANGE].Length > 0) {
+      *PMem64Base = Bridge->PciBar[PPB_PMEM64_RANGE].BaseAddress;
+    } else {
+      *PMem64Base = gAllOne;
+    }
+
+  }
+
+  if (IS_CARDBUS_BRIDGE (&Bridge->Pci)) {
+    if (Bridge->PciBar[P2C_IO_1].Length > 0) {
+      *IoBase = Bridge->PciBar[P2C_IO_1].BaseAddress;
+    } else {
+      if (Bridge->PciBar[P2C_IO_2].Length > 0) {
+        *IoBase = Bridge->PciBar[P2C_IO_2].BaseAddress;
+      }
+    }
+
+    if (Bridge->PciBar[P2C_MEM_1].Length > 0) {
+      if (Bridge->PciBar[P2C_MEM_1].BarType == PciBarTypePMem32) {
+        *PMem32Base = Bridge->PciBar[P2C_MEM_1].BaseAddress;
+      }
+
+      if (Bridge->PciBar[P2C_MEM_1].BarType == PciBarTypeMem32) {
+        *Mem32Base = Bridge->PciBar[P2C_MEM_1].BaseAddress;
+      }
+    }
+
+    if (Bridge->PciBar[P2C_MEM_2].Length > 0) {
+      if (Bridge->PciBar[P2C_MEM_2].BarType == PciBarTypePMem32) {
+        *PMem32Base = Bridge->PciBar[P2C_MEM_2].BaseAddress;
+      }
+
+      if (Bridge->PciBar[P2C_MEM_2].BarType == PciBarTypeMem32) {
+        *Mem32Base = Bridge->PciBar[P2C_MEM_2].BaseAddress;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+   These are the notifications from the PCI bus driver that it is about to enter a certain
+   phase of the PCI enumeration process.
+
+   This member function can be used to notify the host bridge driver to perform specific actions,
+   including any chipset-specific initialization, so that the chipset is ready to enter the next phase.
+   Eight notification points are defined at this time. See belows:
+   EfiPciHostBridgeBeginEnumeration       Resets the host bridge PCI apertures and internal data
+                                          structures. The PCI enumerator should issue this notification
+                                          before starting a fresh enumeration process. Enumeration cannot
+                                          be restarted after sending any other notification such as
+                                          EfiPciHostBridgeBeginBusAllocation.
+   EfiPciHostBridgeBeginBusAllocation     The bus allocation phase is about to begin. No specific action is
+                                          required here. This notification can be used to perform any
+                                          chipset-specific programming.
+   EfiPciHostBridgeEndBusAllocation       The bus allocation and bus programming phase is complete. No
+                                          specific action is required here. This notification can be used to
+                                          perform any chipset-specific programming.
+   EfiPciHostBridgeBeginResourceAllocation
+                                          The resource allocation phase is about to begin. No specific
+                                          action is required here. This notification can be used to perform
+                                          any chipset-specific programming.
+   EfiPciHostBridgeAllocateResources      Allocates resources per previously submitted requests for all the PCI
+                                          root bridges. These resource settings are returned on the next call to
+                                          GetProposedResources(). Before calling NotifyPhase() with a Phase of
+                                          EfiPciHostBridgeAllocateResource, the PCI bus enumerator is responsible
+                                          for gathering I/O and memory requests for
+                                          all the PCI root bridges and submitting these requests using
+                                          SubmitResources(). This function pads the resource amount
+                                          to suit the root bridge hardware, takes care of dependencies between
+                                          the PCI root bridges, and calls the Global Coherency Domain (GCD)
+                                          with the allocation request. In the case of padding, the allocated range
+                                          could be bigger than what was requested.
+   EfiPciHostBridgeSetResources           Programs the host bridge hardware to decode previously allocated
+                                          resources (proposed resources) for all the PCI root bridges. After the
+                                          hardware is programmed, reassigning resources will not be supported.
+                                          The bus settings are not affected.
+   EfiPciHostBridgeFreeResources          Deallocates resources that were previously allocated for all the PCI
+                                          root bridges and resets the I/O and memory apertures to their initial
+                                          state. The bus settings are not affected. If the request to allocate
+                                          resources fails, the PCI enumerator can use this notification to
+                                          deallocate previous resources, adjust the requests, and retry
+                                          allocation.
+   EfiPciHostBridgeEndResourceAllocation  The resource allocation phase is completed. No specific action is
+                                          required here. This notification can be used to perform any chipsetspecific
+                                          programming.
+
+   @param[in] PciResAlloc         The instance pointer of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL
+   @param[in] Phase               The phase during enumeration
+
+   @retval EFI_NOT_READY          This phase cannot be entered at this time. For example, this error
+                                  is valid for a Phase of EfiPciHostBridgeAllocateResources if
+                                  SubmitResources() has not been called for one or more
+                                  PCI root bridges before this call
+   @retval EFI_DEVICE_ERROR       Programming failed due to a hardware error. This error is valid
+                                  for a Phase of EfiPciHostBridgeSetResources.
+   @retval EFI_INVALID_PARAMETER  Invalid phase parameter
+   @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a lack of resources.
+                                  This error is valid for a Phase of EfiPciHostBridgeAllocateResources if the
+                                  previously submitted resource requests cannot be fulfilled or
+                                  were only partially fulfilled.
+   @retval EFI_SUCCESS            The notification was accepted without any errors.
+
+**/
+EFI_STATUS
+NotifyPhase (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc,
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PHASE       Phase
+  )
+{
+  EFI_HANDLE                      HostBridgeHandle;
+  EFI_HANDLE                      RootBridgeHandle;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+  EFI_STATUS                      Status;
+
+  HostBridgeHandle  = NULL;
+  RootBridgeHandle  = NULL;
+  if (gPciPlatformProtocol != NULL) {
+    //
+    // Get Host Bridge Handle.
+    //
+    PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle);
+
+    //
+    // Get the rootbridge Io protocol to find the host bridge handle
+    //
+    Status = gBS->HandleProtocol (
+                    RootBridgeHandle,
+                    &gEfiPciRootBridgeIoProtocolGuid,
+                    (VOID **) &PciRootBridgeIo
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return EFI_NOT_FOUND;
+    }
+
+    HostBridgeHandle = PciRootBridgeIo->ParentHandle;
+
+    //
+    // Call PlatformPci::PlatformNotify() if the protocol is present.
+    //
+    gPciPlatformProtocol->PlatformNotify (
+                            gPciPlatformProtocol,
+                            HostBridgeHandle,
+                            Phase,
+                            ChipsetEntry
+                            );
+  } else if (gPciOverrideProtocol != NULL){
+    //
+    // Get Host Bridge Handle.
+    //
+    PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle);
+
+    //
+    // Get the rootbridge Io protocol to find the host bridge handle
+    //
+    Status = gBS->HandleProtocol (
+                    RootBridgeHandle,
+                    &gEfiPciRootBridgeIoProtocolGuid,
+                    (VOID **) &PciRootBridgeIo
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return EFI_NOT_FOUND;
+    }
+
+    HostBridgeHandle = PciRootBridgeIo->ParentHandle;
+
+    //
+    // Call PlatformPci::PhaseNotify() if the protocol is present.
+    //
+    gPciOverrideProtocol->PlatformNotify (
+                            gPciOverrideProtocol,
+                            HostBridgeHandle,
+                            Phase,
+                            ChipsetEntry
+                            );
+  }  
+
+  Status = PciResAlloc->NotifyPhase (
+                          PciResAlloc,
+                          Phase
+                          );
+
+  if (gPciPlatformProtocol != NULL) {
+    //
+    // Call PlatformPci::PlatformNotify() if the protocol is present.
+    //
+    gPciPlatformProtocol->PlatformNotify (
+                            gPciPlatformProtocol,
+                            HostBridgeHandle,
+                            Phase,
+                            ChipsetExit
+                            );
+
+  } else if (gPciOverrideProtocol != NULL) {
+    //
+    // Call PlatformPci::PhaseNotify() if the protocol is present.
+    //
+    gPciOverrideProtocol->PlatformNotify (
+                            gPciOverrideProtocol,
+                            HostBridgeHandle,
+                            Phase,
+                            ChipsetExit
+                            );
+  }
+
+  return Status;
+}
+
+/**
+  Provides the hooks from the PCI bus driver to every PCI controller (device/function) at various
+  stages of the PCI enumeration process that allow the host bridge driver to preinitialize individual
+  PCI controllers before enumeration.
+
+  This function is called during the PCI enumeration process. No specific action is expected from this
+  member function. It allows the host bridge driver to preinitialize individual PCI controllers before
+  enumeration.
+
+  @param Bridge            Pointer to the EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL instance.
+  @param Bus               The bus number of the pci device.
+  @param Device            The device number of the pci device.
+  @param Func              The function number of the pci device.
+  @param Phase             The phase of the PCI device enumeration.
+
+  @retval EFI_SUCCESS              The requested parameters were returned.
+  @retval EFI_INVALID_PARAMETER    RootBridgeHandle is not a valid root bridge handle.
+  @retval EFI_INVALID_PARAMETER    Phase is not a valid phase that is defined in
+                                   EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE.
+  @retval EFI_DEVICE_ERROR         Programming failed due to a hardware error. The PCI enumerator should
+                                   not enumerate this device, including its child devices if it is a PCI-to-PCI
+                                   bridge.
+
+**/
+EFI_STATUS
+PreprocessController (
+  IN PCI_IO_DEVICE                                    *Bridge,
+  IN UINT8                                            Bus,
+  IN UINT8                                            Device,
+  IN UINT8                                            Func,
+  IN EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE     Phase
+  )
+{
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS       RootBridgePciAddress;
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc;
+  EFI_HANDLE                                        RootBridgeHandle;
+  EFI_HANDLE                                        HostBridgeHandle;
+  EFI_STATUS                                        Status;
+
+  //
+  // Get the host bridge handle
+  //
+  HostBridgeHandle = Bridge->PciRootBridgeIo->ParentHandle;
+
+  //
+  // Get the pci host bridge resource allocation protocol
+  //
+  Status = gBS->OpenProtocol (
+                  HostBridgeHandle,
+                  &gEfiPciHostBridgeResourceAllocationProtocolGuid,
+                  (VOID **) &PciResAlloc,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get Root Brige Handle
+  //
+  while (Bridge->Parent != NULL) {
+    Bridge = Bridge->Parent;
+  }
+
+  RootBridgeHandle                      = Bridge->Handle;
+
+  RootBridgePciAddress.Register         = 0;
+  RootBridgePciAddress.Function         = Func;
+  RootBridgePciAddress.Device           = Device;
+  RootBridgePciAddress.Bus              = Bus;
+  RootBridgePciAddress.ExtendedRegister = 0;
+
+  if (gPciPlatformProtocol != NULL) {
+    //
+    // Call PlatformPci::PrepController() if the protocol is present.
+    //
+    gPciPlatformProtocol->PlatformPrepController (
+                            gPciPlatformProtocol,
+                            HostBridgeHandle,
+                            RootBridgeHandle,
+                            RootBridgePciAddress,
+                            Phase,
+                            ChipsetEntry
+                            );
+  } else if (gPciOverrideProtocol != NULL) {
+    //
+    // Call PlatformPci::PrepController() if the protocol is present.
+    //
+    gPciOverrideProtocol->PlatformPrepController (
+                            gPciOverrideProtocol,
+                            HostBridgeHandle,
+                            RootBridgeHandle,
+                            RootBridgePciAddress,
+                            Phase,
+                            ChipsetEntry
+                            );
+  }
+
+  Status = PciResAlloc->PreprocessController (
+                          PciResAlloc,
+                          RootBridgeHandle,
+                          RootBridgePciAddress,
+                          Phase
+                          );
+
+  if (gPciPlatformProtocol != NULL) {
+    //
+    // Call PlatformPci::PrepController() if the protocol is present.
+    //
+    gPciPlatformProtocol->PlatformPrepController (
+                            gPciPlatformProtocol,
+                            HostBridgeHandle,
+                            RootBridgeHandle,
+                            RootBridgePciAddress,
+                            Phase,
+                            ChipsetExit
+                            );
+  } else if (gPciOverrideProtocol != NULL) {
+    //
+    // Call PlatformPci::PrepController() if the protocol is present.
+    //
+    gPciOverrideProtocol->PlatformPrepController (
+                            gPciOverrideProtocol,
+                            HostBridgeHandle,
+                            RootBridgeHandle,
+                            RootBridgePciAddress,
+                            Phase,
+                            ChipsetExit
+                            );
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function allows the PCI bus driver to be notified to act as requested when a hot-plug event has
+  happened on the hot-plug controller. Currently, the operations include add operation and remove operation..
+
+  @param This                 A pointer to the hot plug request protocol.
+  @param Operation            The operation the PCI bus driver is requested to make.
+  @param Controller           The handle of the hot-plug controller.
+  @param RemainingDevicePath  The remaining device path for the PCI-like hot-plug device.
+  @param NumberOfChildren     The number of child handles.
+                              For a add operation, it is an output parameter.
+                              For a remove operation, it's an input parameter.
+  @param ChildHandleBuffer    The buffer which contains the child handles.
+
+  @retval EFI_INVALID_PARAMETER  Operation is not a legal value.
+                                 Controller is NULL or not a valid handle.
+                                 NumberOfChildren is NULL.
+                                 ChildHandleBuffer is NULL while Operation is add.
+  @retval EFI_OUT_OF_RESOURCES   There are no enough resources to start the devices.
+  @retval EFI_NOT_FOUND          Can not find bridge according to controller handle.
+  @retval EFI_SUCCESS            The handles for the specified device have been created or destroyed
+                                 as requested, and for an add operation, the new handles are
+                                 returned in ChildHandleBuffer.
+**/
+EFI_STATUS
+EFIAPI
+PciHotPlugRequestNotify (
+  IN EFI_PCI_HOTPLUG_REQUEST_PROTOCOL * This,
+  IN EFI_PCI_HOTPLUG_OPERATION        Operation,
+  IN EFI_HANDLE                       Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL         * RemainingDevicePath OPTIONAL,
+  IN OUT UINT8                        *NumberOfChildren,
+  IN OUT EFI_HANDLE                   * ChildHandleBuffer
+  )
+{
+  PCI_IO_DEVICE       *Bridge;
+  PCI_IO_DEVICE       *Temp;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINTN               Index;
+  EFI_HANDLE          RootBridgeHandle;
+  EFI_STATUS          Status;
+
+  //
+  // Check input parameter validity
+  //
+  if ((Controller == NULL) || (NumberOfChildren == NULL)){
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Operation != EfiPciHotPlugRequestAdd) && (Operation != EfiPciHotplugRequestRemove)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Operation == EfiPciHotPlugRequestAdd){
+    if (ChildHandleBuffer == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  } else if ((Operation == EfiPciHotplugRequestRemove) && (*NumberOfChildren != 0)) {
+    if (ChildHandleBuffer == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Bridge = PCI_IO_DEVICE_FROM_PCI_IO_THIS (PciIo);
+
+  //
+  // Get root bridge handle
+  //
+  Temp = Bridge;
+  while (Temp->Parent != NULL) {
+    Temp = Temp->Parent;
+  }
+
+  RootBridgeHandle = Temp->Handle;
+
+  if (Operation == EfiPciHotPlugRequestAdd) {
+    //
+    // Report Status Code to indicate hot plug happens
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_IO_BUS_PCI | EFI_IOB_PC_HOTPLUG),
+      Temp->DevicePath
+      );
+
+    if (NumberOfChildren != NULL) {
+      *NumberOfChildren = 0;
+    }
+
+    if (IsListEmpty (&Bridge->ChildList)) {
+
+      Status = PciBridgeEnumerator (Bridge);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+
+    Status = StartPciDevicesOnBridge (
+              RootBridgeHandle,
+              Bridge,
+              RemainingDevicePath,
+              NumberOfChildren,
+              ChildHandleBuffer
+              );
+
+    return Status;
+  }
+
+  if (Operation == EfiPciHotplugRequestRemove) {
+
+    if (*NumberOfChildren == 0) {
+      //
+      // Remove all devices on the bridge
+      //
+      RemoveAllPciDeviceOnBridge (RootBridgeHandle, Bridge);
+      return EFI_SUCCESS;
+
+    }
+
+    for (Index = 0; Index < *NumberOfChildren; Index++) {
+      //
+      // De register all the pci device
+      //
+      Status = DeRegisterPciDevice (RootBridgeHandle, ChildHandleBuffer[Index]);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+    }
+    //
+    // End for
+    //
+    return EFI_SUCCESS;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Search hostbridge according to given handle
+
+  @param RootBridgeHandle  Host bridge handle.
+
+  @retval TRUE             Found host bridge handle.
+  @retval FALSE            Not found hot bridge handle.
+
+**/
+BOOLEAN
+SearchHostBridgeHandle (
+  IN EFI_HANDLE RootBridgeHandle
+  )
+{
+  EFI_HANDLE                      HostBridgeHandle;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+  UINTN                           Index;
+  EFI_STATUS                      Status;
+
+  //
+  // Get the rootbridge Io protocol to find the host bridge handle
+  //
+  Status = gBS->OpenProtocol (
+                  RootBridgeHandle,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  RootBridgeHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  HostBridgeHandle = PciRootBridgeIo->ParentHandle;
+  for (Index = 0; Index < gPciHostBridgeNumber; Index++) {
+    if (HostBridgeHandle == gPciHostBrigeHandles[Index]) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Add host bridge handle to global variable for enumerating.
+
+  @param HostBridgeHandle   Host bridge handle.
+
+  @retval EFI_SUCCESS       Successfully added host bridge.
+  @retval EFI_ABORTED       Host bridge is NULL, or given host bridge
+                            has been in host bridge list.
+
+**/
+EFI_STATUS
+AddHostBridgeEnumerator (
+  IN EFI_HANDLE HostBridgeHandle
+  )
+{
+  UINTN Index;
+
+  if (HostBridgeHandle == NULL) {
+    return EFI_ABORTED;
+  }
+
+  for (Index = 0; Index < gPciHostBridgeNumber; Index++) {
+    if (HostBridgeHandle == gPciHostBrigeHandles[Index]) {
+      return EFI_ABORTED;
+    }
+  }
+
+  if (Index < PCI_MAX_HOST_BRIDGE_NUM) {
+    gPciHostBrigeHandles[Index] = HostBridgeHandle;
+    gPciHostBridgeNumber++;
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.h
new file mode 100644
index 0000000000..ff164d1f09
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.h
@@ -0,0 +1,519 @@
+/** @file
+  PCI bus enumeration logic function declaration for PCI bus module.
+
+Copyright (c) 2006 - 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_ENUMERATOR_H_
+#define _EFI_PCI_ENUMERATOR_H_
+
+#include "PciResourceSupport.h"
+
+/**
+  This routine is used to enumerate entire pci bus system
+  in a given platform.
+
+  @param Controller  Parent controller handle.
+
+  @retval EFI_SUCCESS    PCI enumeration finished successfully.
+  @retval other          Some error occurred when enumerating the pci bus system.
+
+**/
+EFI_STATUS
+PciEnumerator (
+  IN EFI_HANDLE                    Controller
+  );
+
+/**
+  Enumerate PCI root bridge.
+
+  @param PciResAlloc   Pointer to protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+  @param RootBridgeDev Instance of root bridge device.
+
+  @retval EFI_SUCCESS  Successfully enumerated root bridge.
+  @retval other        Failed to enumerate root bridge.
+
+**/
+EFI_STATUS
+PciRootBridgeEnumerator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc,
+  IN PCI_IO_DEVICE                                     *RootBridgeDev
+  );
+
+/**
+  This routine is used to process all PCI devices' Option Rom
+  on a certain root bridge.
+
+  @param Bridge     Given parent's root bridge.
+  @param RomBase    Base address of ROM driver loaded from.
+  @param MaxLength  Maximum rom size.
+
+**/
+VOID
+ProcessOptionRom (
+  IN PCI_IO_DEVICE *Bridge,
+  IN UINT64        RomBase,
+  IN UINT64        MaxLength
+  );
+
+/**
+  This routine is used to assign bus number to the given PCI bus system
+
+  @param Bridge             Parent root bridge instance.
+  @param StartBusNumber     Number of beginning.
+  @param SubBusNumber       The number of sub bus.
+
+  @retval EFI_SUCCESS       Successfully assigned bus number.
+  @retval EFI_DEVICE_ERROR  Failed to assign bus number.
+
+**/
+EFI_STATUS
+PciAssignBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  OUT UINT8                             *SubBusNumber
+  );
+
+/**
+  This routine is used to determine the root bridge attribute by interfacing
+  the host bridge resource allocation protocol.
+
+  @param PciResAlloc    Protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL
+  @param RootBridgeDev  Root bridge instance
+
+  @retval EFI_SUCCESS  Successfully got root bridge's attribute.
+  @retval other        Failed to get attribute.
+
+**/
+EFI_STATUS
+DetermineRootBridgeAttributes (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc,
+  IN PCI_IO_DEVICE                                    *RootBridgeDev
+  );
+
+/**
+  Get Max Option Rom size on specified bridge.
+
+  @param Bridge    Given bridge device instance.
+
+  @return Max size of option rom needed.
+
+**/
+UINT64
+GetMaxOptionRomSize (
+  IN PCI_IO_DEVICE   *Bridge
+  );
+
+/**
+  Process attributes of devices on this host bridge
+
+  @param PciResAlloc Protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS   Successfully process attribute.
+  @retval EFI_NOT_FOUND Can not find the specific root bridge device.
+  @retval other         Failed to determine the root bridge device's attribute.
+
+**/
+EFI_STATUS
+PciHostBridgeDeviceAttribute (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  );
+
+/**
+  Get resource allocation status from the ACPI resource descriptor.
+
+  @param AcpiConfig       Point to Acpi configuration table.
+  @param IoResStatus      Return the status of I/O resource.
+  @param Mem32ResStatus   Return the status of 32-bit Memory resource.
+  @param PMem32ResStatus  Return the status of 32-bit Prefetchable Memory resource.
+  @param Mem64ResStatus   Return the status of 64-bit Memory resource.
+  @param PMem64ResStatus  Return the status of 64-bit Prefetchable Memory resource.
+
+**/
+VOID
+GetResourceAllocationStatus (
+  VOID        *AcpiConfig,
+  OUT UINT64  *IoResStatus,
+  OUT UINT64  *Mem32ResStatus,
+  OUT UINT64  *PMem32ResStatus,
+  OUT UINT64  *Mem64ResStatus,
+  OUT UINT64  *PMem64ResStatus
+  );
+
+/**
+  Remove a PCI device from device pool and mark its bar.
+
+  @param PciDevice Instance of Pci device.
+
+  @retval EFI_SUCCESS Successfully remove the PCI device.
+  @retval EFI_ABORTED Pci device is a root bridge or a PCI-PCI bridge.
+
+**/
+EFI_STATUS
+RejectPciDevice (
+  IN PCI_IO_DEVICE       *PciDevice
+  );
+
+/**
+  Determine whethter a PCI device can be rejected.
+
+  @param  PciResNode Pointer to Pci resource node instance.
+
+  @retval TRUE  The PCI device can be rejected.
+  @retval TRUE  The PCI device cannot be rejected.
+
+**/
+BOOLEAN
+IsRejectiveDevice (
+  IN  PCI_RESOURCE_NODE   *PciResNode
+  );
+
+/**
+  Compare two resource nodes and get the larger resource consumer.
+
+  @param PciResNode1  resource node 1 want to be compared
+  @param PciResNode2  resource node 2 want to be compared
+
+  @return Larger resource node.
+
+**/
+PCI_RESOURCE_NODE *
+GetLargerConsumerDevice (
+  IN  PCI_RESOURCE_NODE   *PciResNode1,
+  IN  PCI_RESOURCE_NODE   *PciResNode2
+  );
+
+/**
+  Get the max resource consumer in the host resource pool.
+
+  @param ResPool  Pointer to resource pool node.
+
+  @return The max resource consumer in the host resource pool.
+
+**/
+PCI_RESOURCE_NODE *
+GetMaxResourceConsumerDevice (
+  IN  PCI_RESOURCE_NODE   *ResPool
+  );
+
+/**
+  Adjust host bridge allocation so as to reduce resource requirement
+
+  @param IoPool           Pointer to instance of I/O resource Node.
+  @param Mem32Pool        Pointer to instance of 32-bit memory resource Node.
+  @param PMem32Pool       Pointer to instance of 32-bit Prefetchable memory resource node.
+  @param Mem64Pool        Pointer to instance of 64-bit memory resource node.
+  @param PMem64Pool       Pointer to instance of 64-bit Prefetchable memory resource node.
+  @param IoResStatus      Status of I/O resource Node.
+  @param Mem32ResStatus   Status of 32-bit memory resource Node.
+  @param PMem32ResStatus  Status of 32-bit Prefetchable memory resource node.
+  @param Mem64ResStatus   Status of 64-bit memory resource node.
+  @param PMem64ResStatus  Status of 64-bit Prefetchable memory resource node.
+
+  @retval EFI_SUCCESS     Successfully adjusted resoruce on host bridge.
+  @retval EFI_ABORTED     Host bridge hasn't this resource type or no resource be adjusted.
+
+**/
+EFI_STATUS
+PciHostBridgeAdjustAllocation (
+  IN  PCI_RESOURCE_NODE   *IoPool,
+  IN  PCI_RESOURCE_NODE   *Mem32Pool,
+  IN  PCI_RESOURCE_NODE   *PMem32Pool,
+  IN  PCI_RESOURCE_NODE   *Mem64Pool,
+  IN  PCI_RESOURCE_NODE   *PMem64Pool,
+  IN  UINT64              IoResStatus,
+  IN  UINT64              Mem32ResStatus,
+  IN  UINT64              PMem32ResStatus,
+  IN  UINT64              Mem64ResStatus,
+  IN  UINT64              PMem64ResStatus
+  );
+
+/**
+  Summary requests for all resource type, and contruct ACPI resource
+  requestor instance.
+
+  @param Bridge           detecting bridge
+  @param IoNode           Pointer to instance of I/O resource Node
+  @param Mem32Node        Pointer to instance of 32-bit memory resource Node
+  @param PMem32Node       Pointer to instance of 32-bit Pmemory resource node
+  @param Mem64Node        Pointer to instance of 64-bit memory resource node
+  @param PMem64Node       Pointer to instance of 64-bit Pmemory resource node
+  @param Config           Output buffer holding new constructed APCI resource requestor
+
+  @retval EFI_SUCCESS           Successfully constructed ACPI resource.
+  @retval EFI_OUT_OF_RESOURCES  No memory availabe.
+
+**/
+EFI_STATUS
+ConstructAcpiResourceRequestor (
+  IN PCI_IO_DEVICE      *Bridge,
+  IN PCI_RESOURCE_NODE  *IoNode,
+  IN PCI_RESOURCE_NODE  *Mem32Node,
+  IN PCI_RESOURCE_NODE  *PMem32Node,
+  IN PCI_RESOURCE_NODE  *Mem64Node,
+  IN PCI_RESOURCE_NODE  *PMem64Node,
+  OUT VOID              **Config
+  );
+
+/**
+  Get resource base from an acpi configuration descriptor.
+
+  @param Config       An acpi configuration descriptor.
+  @param IoBase       Output of I/O resource base address.
+  @param Mem32Base    Output of 32-bit memory base address.
+  @param PMem32Base   Output of 32-bit prefetchable memory base address.
+  @param Mem64Base    Output of 64-bit memory base address.
+  @param PMem64Base   Output of 64-bit prefetchable memory base address.
+
+**/
+VOID
+GetResourceBase (
+  IN VOID     *Config,
+  OUT UINT64  *IoBase,
+  OUT UINT64  *Mem32Base,
+  OUT UINT64  *PMem32Base,
+  OUT UINT64  *Mem64Base,
+  OUT UINT64  *PMem64Base
+  );
+
+/**
+  Enumerate pci bridge, allocate resource and determine attribute
+  for devices on this bridge.
+
+  @param BridgeDev    Pointer to instance of bridge device.
+
+  @retval EFI_SUCCESS Successfully enumerated PCI bridge.
+  @retval other       Failed to enumerate.
+
+**/
+EFI_STATUS
+PciBridgeEnumerator (
+  IN PCI_IO_DEVICE                                     *BridgeDev
+  );
+
+/**
+  Allocate all kinds of resource for PCI bridge.
+
+  @param  Bridge      Pointer to bridge instance.
+
+  @retval EFI_SUCCESS Successfully allocated resource for PCI bridge.
+  @retval other       Failed to allocate resource for bridge.
+
+**/
+EFI_STATUS
+PciBridgeResourceAllocator (
+  IN PCI_IO_DEVICE  *Bridge
+  );
+
+/**
+  Get resource base address for a pci bridge device.
+
+  @param Bridge     Given Pci driver instance.
+  @param IoBase     Output for base address of I/O type resource.
+  @param Mem32Base  Output for base address of 32-bit memory type resource.
+  @param PMem32Base Ooutput for base address of 32-bit Pmemory type resource.
+  @param Mem64Base  Output for base address of 64-bit memory type resource.
+  @param PMem64Base Output for base address of 64-bit Pmemory type resource.
+
+  @retval EFI_SUCCESS           Successfully got resource base address.
+  @retval EFI_OUT_OF_RESOURCES  PCI bridge is not available.
+
+**/
+EFI_STATUS
+GetResourceBaseFromBridge (
+  IN  PCI_IO_DEVICE *Bridge,
+  OUT UINT64        *IoBase,
+  OUT UINT64        *Mem32Base,
+  OUT UINT64        *PMem32Base,
+  OUT UINT64        *Mem64Base,
+  OUT UINT64        *PMem64Base
+  );
+
+/**
+  Process Option Rom on this host bridge
+
+  @param PciResAlloc Pointer to instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_NOT_FOUND Can not find the root bridge instance.
+  @retval EFI_SUCCESS   Success process.
+**/
+EFI_STATUS
+PciHostBridgeP2CProcess (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  );
+
+/**
+   These are the notifications from the PCI bus driver that it is about to enter a certain
+   phase of the PCI enumeration process.
+
+   This member function can be used to notify the host bridge driver to perform specific actions,
+   including any chipset-specific initialization, so that the chipset is ready to enter the next phase.
+   Eight notification points are defined at this time. See belows:
+   EfiPciHostBridgeBeginEnumeration       Resets the host bridge PCI apertures and internal data
+                                          structures. The PCI enumerator should issue this notification
+                                          before starting a fresh enumeration process. Enumeration cannot
+                                          be restarted after sending any other notification such as
+                                          EfiPciHostBridgeBeginBusAllocation.
+   EfiPciHostBridgeBeginBusAllocation     The bus allocation phase is about to begin. No specific action is
+                                          required here. This notification can be used to perform any
+                                          chipset-specific programming.
+   EfiPciHostBridgeEndBusAllocation       The bus allocation and bus programming phase is complete. No
+                                          specific action is required here. This notification can be used to
+                                          perform any chipset-specific programming.
+   EfiPciHostBridgeBeginResourceAllocation
+                                          The resource allocation phase is about to begin. No specific
+                                          action is required here. This notification can be used to perform
+                                          any chipset-specific programming.
+   EfiPciHostBridgeAllocateResources      Allocates resources per previously submitted requests for all the PCI
+                                          root bridges. These resource settings are returned on the next call to
+                                          GetProposedResources(). Before calling NotifyPhase() with a Phase of
+                                          EfiPciHostBridgeAllocateResource, the PCI bus enumerator is responsible
+                                          for gathering I/O and memory requests for
+                                          all the PCI root bridges and submitting these requests using
+                                          SubmitResources(). This function pads the resource amount
+                                          to suit the root bridge hardware, takes care of dependencies between
+                                          the PCI root bridges, and calls the Global Coherency Domain (GCD)
+                                          with the allocation request. In the case of padding, the allocated range
+                                          could be bigger than what was requested.
+   EfiPciHostBridgeSetResources           Programs the host bridge hardware to decode previously allocated
+                                          resources (proposed resources) for all the PCI root bridges. After the
+                                          hardware is programmed, reassigning resources will not be supported.
+                                          The bus settings are not affected.
+   EfiPciHostBridgeFreeResources          Deallocates resources that were previously allocated for all the PCI
+                                          root bridges and resets the I/O and memory apertures to their initial
+                                          state. The bus settings are not affected. If the request to allocate
+                                          resources fails, the PCI enumerator can use this notification to
+                                          deallocate previous resources, adjust the requests, and retry
+                                          allocation.
+   EfiPciHostBridgeEndResourceAllocation  The resource allocation phase is completed. No specific action is
+                                          required here. This notification can be used to perform any chipsetspecific
+                                          programming.
+
+   @param[in] PciResAlloc         The instance pointer of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL
+   @param[in] Phase               The phase during enumeration
+
+   @retval EFI_NOT_READY          This phase cannot be entered at this time. For example, this error
+                                  is valid for a Phase of EfiPciHostBridgeAllocateResources if
+                                  SubmitResources() has not been called for one or more
+                                  PCI root bridges before this call
+   @retval EFI_DEVICE_ERROR       Programming failed due to a hardware error. This error is valid
+                                  for a Phase of EfiPciHostBridgeSetResources.
+   @retval EFI_INVALID_PARAMETER  Invalid phase parameter
+   @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a lack of resources.
+                                  This error is valid for a Phase of EfiPciHostBridgeAllocateResources if the
+                                  previously submitted resource requests cannot be fulfilled or
+                                  were only partially fulfilled.
+   @retval EFI_SUCCESS            The notification was accepted without any errors.
+
+**/
+EFI_STATUS
+NotifyPhase (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc,
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PHASE       Phase
+  );
+
+/**
+  Provides the hooks from the PCI bus driver to every PCI controller (device/function) at various
+  stages of the PCI enumeration process that allow the host bridge driver to preinitialize individual
+  PCI controllers before enumeration.
+
+  This function is called during the PCI enumeration process. No specific action is expected from this
+  member function. It allows the host bridge driver to preinitialize individual PCI controllers before
+  enumeration.
+
+  @param Bridge            Pointer to the EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL instance.
+  @param Bus               The bus number of the pci device.
+  @param Device            The device number of the pci device.
+  @param Func              The function number of the pci device.
+  @param Phase             The phase of the PCI device enumeration.
+
+  @retval EFI_SUCCESS              The requested parameters were returned.
+  @retval EFI_INVALID_PARAMETER    RootBridgeHandle is not a valid root bridge handle.
+  @retval EFI_INVALID_PARAMETER    Phase is not a valid phase that is defined in
+                                   EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE.
+  @retval EFI_DEVICE_ERROR         Programming failed due to a hardware error. The PCI enumerator should
+                                    not enumerate this device, including its child devices if it is a PCI-to-PCI
+                                    bridge.
+
+**/
+EFI_STATUS
+PreprocessController (
+  IN PCI_IO_DEVICE                                  *Bridge,
+  IN UINT8                                          Bus,
+  IN UINT8                                          Device,
+  IN UINT8                                          Func,
+  IN EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE   Phase
+  );
+
+/**
+  This function allows the PCI bus driver to be notified to act as requested when a hot-plug event has
+  happened on the hot-plug controller. Currently, the operations include add operation and remove operation..
+
+  @param This                 A pointer to the hot plug request protocol.
+  @param Operation            The operation the PCI bus driver is requested to make.
+  @param Controller           The handle of the hot-plug controller.
+  @param RemainingDevicePath  The remaining device path for the PCI-like hot-plug device.
+  @param NumberOfChildren     The number of child handles.
+                              For a add operation, it is an output parameter.
+                              For a remove operation, it's an input parameter.
+  @param ChildHandleBuffer    The buffer which contains the child handles.
+
+  @retval EFI_INVALID_PARAMETER  Operation is not a legal value.
+                                 Controller is NULL or not a valid handle.
+                                 NumberOfChildren is NULL.
+                                 ChildHandleBuffer is NULL while Operation is add.
+  @retval EFI_OUT_OF_RESOURCES   There are no enough resources to start the devices.
+  @retval EFI_NOT_FOUND          Can not find bridge according to controller handle.
+  @retval EFI_SUCCESS            The handles for the specified device have been created or destroyed
+                                 as requested, and for an add operation, the new handles are
+                                 returned in ChildHandleBuffer.
+**/
+EFI_STATUS
+EFIAPI
+PciHotPlugRequestNotify (
+  IN EFI_PCI_HOTPLUG_REQUEST_PROTOCOL * This,
+  IN EFI_PCI_HOTPLUG_OPERATION        Operation,
+  IN EFI_HANDLE                       Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL         * RemainingDevicePath OPTIONAL,
+  IN OUT UINT8                        *NumberOfChildren,
+  IN OUT EFI_HANDLE                   * ChildHandleBuffer
+  );
+
+/**
+  Search hostbridge according to given handle
+
+  @param RootBridgeHandle  Host bridge handle.
+
+  @retval TRUE             Found host bridge handle.
+  @retval FALSE            Not found hot bridge handle.
+
+**/
+BOOLEAN
+SearchHostBridgeHandle (
+  IN EFI_HANDLE RootBridgeHandle
+  );
+
+/**
+  Add host bridge handle to global variable for enumerating.
+
+  @param HostBridgeHandle   Host bridge handle.
+
+  @retval EFI_SUCCESS       Successfully added host bridge.
+  @retval EFI_ABORTED       Host bridge is NULL, or given host bridge
+                            has been in host bridge list.
+
+**/
+EFI_STATUS
+AddHostBridgeEnumerator (
+  IN EFI_HANDLE HostBridgeHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.c
new file mode 100644
index 0000000000..b070eb1b5f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.c
@@ -0,0 +1,2717 @@
+/** @file
+  PCI emumeration support functions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+extern CHAR16  *mBarTypeStr[];
+
+/**
+  This routine is used to check whether the pci device is present.
+
+  @param PciRootBridgeIo   Pointer to instance of EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Pci               Output buffer for PCI device configuration space.
+  @param Bus               PCI bus NO.
+  @param Device            PCI device NO.
+  @param Func              PCI Func NO.
+
+  @retval EFI_NOT_FOUND    PCI device not present.
+  @retval EFI_SUCCESS      PCI device is found.
+
+**/
+EFI_STATUS
+PciDevicePresent (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL     *PciRootBridgeIo,
+  OUT PCI_TYPE00                          *Pci,
+  IN  UINT8                               Bus,
+  IN  UINT8                               Device,
+  IN  UINT8                               Func
+  )
+{
+  UINT64      Address;
+  EFI_STATUS  Status;
+
+  //
+  // Create PCI address map in terms of Bus, Device and Func
+  //
+  Address = EFI_PCI_ADDRESS (Bus, Device, Func, 0);
+
+  //
+  // Read the Vendor ID register
+  //
+  Status = PciRootBridgeIo->Pci.Read (
+                                  PciRootBridgeIo,
+                                  EfiPciWidthUint32,
+                                  Address,
+                                  1,
+                                  Pci
+                                  );
+
+  if (!EFI_ERROR (Status) && (Pci->Hdr).VendorId != 0xffff) {
+    //
+    // Read the entire config header for the device
+    //
+    Status = PciRootBridgeIo->Pci.Read (
+                                    PciRootBridgeIo,
+                                    EfiPciWidthUint32,
+                                    Address,
+                                    sizeof (PCI_TYPE00) / sizeof (UINT32),
+                                    Pci
+                                    );
+
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Collect all the resource information under this root bridge.
+
+  A database that records all the information about pci device subject to this
+  root bridge will then be created.
+
+  @param Bridge         Parent bridge instance.
+  @param StartBusNumber Bus number of begining.
+
+  @retval EFI_SUCCESS   PCI device is found.
+  @retval other         Some error occurred when reading PCI bridge information.
+
+**/
+EFI_STATUS
+PciPciDeviceInfoCollector (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber
+  )
+{
+  EFI_STATUS          Status;
+  PCI_TYPE00          Pci;
+  UINT8               Device;
+  UINT8               Func;
+  UINT8               SecBus;
+  PCI_IO_DEVICE       *PciIoDevice;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  Status  = EFI_SUCCESS;
+  SecBus  = 0;
+
+  for (Device = 0; Device <= PCI_MAX_DEVICE; Device++) {
+
+    for (Func = 0; Func <= PCI_MAX_FUNC; Func++) {
+
+      //
+      // Check to see whether PCI device is present
+      //
+      Status = PciDevicePresent (
+                 Bridge->PciRootBridgeIo,
+                 &Pci,
+                 (UINT8) StartBusNumber,
+                 (UINT8) Device,
+                 (UINT8) Func
+                 );
+      if (!EFI_ERROR (Status)) {
+
+        //
+        // Call back to host bridge function
+        //
+        PreprocessController (Bridge, (UINT8) StartBusNumber, Device, Func, EfiPciBeforeResourceCollection);
+
+        //
+        // Collect all the information about the PCI device discovered
+        //
+        Status = PciSearchDevice (
+                   Bridge,
+                   &Pci,
+                   (UINT8) StartBusNumber,
+                   Device,
+                   Func,
+                   &PciIoDevice
+                   );
+
+        //
+        // Recursively scan PCI busses on the other side of PCI-PCI bridges
+        //
+        //
+        if (!EFI_ERROR (Status) && (IS_PCI_BRIDGE (&Pci) || IS_CARDBUS_BRIDGE (&Pci))) {
+
+          //
+          // If it is PPB, we need to get the secondary bus to continue the enumeration
+          //
+          PciIo   = &(PciIoDevice->PciIo);
+
+          Status  = PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET, 1, &SecBus);
+
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+
+          //
+          // Get resource padding for PPB
+          //
+          GetResourcePaddingPpb (PciIoDevice);
+
+          //
+          // Deep enumerate the next level bus
+          //
+          Status = PciPciDeviceInfoCollector (
+                     PciIoDevice,
+                     (UINT8) (SecBus)
+                     );
+
+        }
+
+        if (Func == 0 && !IS_PCI_MULTI_FUNC (&Pci)) {
+
+          //
+          // Skip sub functions, this is not a multi function device
+          //
+          Func = PCI_MAX_FUNC;
+        }
+      }
+
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Seach required device and create PCI device instance.
+
+  @param Bridge     Parent bridge instance.
+  @param Pci        Input PCI device information block.
+  @param Bus        PCI bus NO.
+  @param Device     PCI device NO.
+  @param Func       PCI func  NO.
+  @param PciDevice  Output of searched PCI device instance.
+
+  @retval EFI_SUCCESS           Successfully created PCI device instance.
+  @retval EFI_OUT_OF_RESOURCES  Cannot get PCI device information.
+
+**/
+EFI_STATUS
+PciSearchDevice (
+  IN  PCI_IO_DEVICE                         *Bridge,
+  IN  PCI_TYPE00                            *Pci,
+  IN  UINT8                                 Bus,
+  IN  UINT8                                 Device,
+  IN  UINT8                                 Func,
+  OUT PCI_IO_DEVICE                         **PciDevice
+  )
+{
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = NULL;
+
+  DEBUG ((
+    EFI_D_INFO,
+    "PciBus: Discovered %s @ [%02x|%02x|%02x]\n",
+    IS_PCI_BRIDGE (Pci) ?     L"PPB" :
+    IS_CARDBUS_BRIDGE (Pci) ? L"P2C" :
+                              L"PCI",
+    Bus, Device, Func
+    ));
+
+  if (!IS_PCI_BRIDGE (Pci)) {
+
+    if (IS_CARDBUS_BRIDGE (Pci)) {
+      PciIoDevice = GatherP2CInfo (
+                      Bridge,
+                      Pci,
+                      Bus,
+                      Device,
+                      Func
+                      );
+      if ((PciIoDevice != NULL) && gFullEnumeration) {
+        InitializeP2C (PciIoDevice);
+      }
+    } else {
+
+      //
+      // Create private data for Pci Device
+      //
+      PciIoDevice = GatherDeviceInfo (
+                      Bridge,
+                      Pci,
+                      Bus,
+                      Device,
+                      Func
+                      );
+
+    }
+
+  } else {
+
+    //
+    // Create private data for PPB
+    //
+    PciIoDevice = GatherPpbInfo (
+                    Bridge,
+                    Pci,
+                    Bus,
+                    Device,
+                    Func
+                    );
+
+    //
+    // Special initialization for PPB including making the PPB quiet
+    //
+    if ((PciIoDevice != NULL) && gFullEnumeration) {
+      InitializePpb (PciIoDevice);
+    }
+  }
+
+  if (PciIoDevice == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Update the bar information for this PCI device so as to support some specific device
+  //
+  UpdatePciInfo (PciIoDevice);
+
+  if (PciIoDevice->DevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Detect this function has option rom
+  //
+  if (gFullEnumeration) {
+
+    if (!IS_CARDBUS_BRIDGE (Pci)) {
+
+      GetOpRomInfo (PciIoDevice);
+
+    }
+
+    ResetPowerManagementFeature (PciIoDevice);
+
+  }
+
+  //
+  // Insert it into a global tree for future reference
+  //
+  InsertPciDevice (Bridge, PciIoDevice);
+
+  //
+  // Determine PCI device attributes
+  //
+
+  if (PciDevice != NULL) {
+    *PciDevice = PciIoDevice;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Dump the PPB padding resource information.
+
+  @param PciIoDevice     PCI IO instance.
+  @param ResourceType    The desired resource type to dump.
+                         PciBarTypeUnknown means to dump all types of resources.
+**/
+VOID
+DumpPpbPaddingResource (
+  IN PCI_IO_DEVICE                    *PciIoDevice,
+  IN PCI_BAR_TYPE                     ResourceType
+  )
+{
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  PCI_BAR_TYPE                      Type;
+
+  if (PciIoDevice->ResourcePaddingDescriptors == NULL) {
+    return;
+  }
+
+  if (ResourceType == PciBarTypeIo16 || ResourceType == PciBarTypeIo32) {
+    ResourceType = PciBarTypeIo;
+  }
+
+  for (Descriptor = PciIoDevice->ResourcePaddingDescriptors; Descriptor->Desc != ACPI_END_TAG_DESCRIPTOR; Descriptor++) {
+
+    Type = PciBarTypeUnknown;
+    if (Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR && Descriptor->ResType == ACPI_ADDRESS_SPACE_TYPE_IO) {
+      Type = PciBarTypeIo;
+    } else if (Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR && Descriptor->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+
+      if (Descriptor->AddrSpaceGranularity == 32) {
+        //
+        // prefechable
+        //
+        if (Descriptor->SpecificFlag == EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) {
+          Type = PciBarTypePMem32;
+        }
+
+        //
+        // Non-prefechable
+        //
+        if (Descriptor->SpecificFlag == 0) {
+          Type = PciBarTypeMem32;
+        }
+      }
+
+      if (Descriptor->AddrSpaceGranularity == 64) {
+        //
+        // prefechable
+        //
+        if (Descriptor->SpecificFlag == EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) {
+          Type = PciBarTypePMem64;
+        }
+
+        //
+        // Non-prefechable
+        //
+        if (Descriptor->SpecificFlag == 0) {
+          Type = PciBarTypeMem64;
+        }
+      }
+    }
+
+    if ((Type != PciBarTypeUnknown) && ((ResourceType == PciBarTypeUnknown) || (ResourceType == Type))) {
+      DEBUG ((
+        EFI_D_INFO,
+        "   Padding: Type = %s; Alignment = 0x%lx;\tLength = 0x%lx\n",
+        mBarTypeStr[Type], Descriptor->AddrRangeMax, Descriptor->AddrLen
+        ));
+    }
+  }
+
+}
+
+/**
+  Dump the PCI BAR information.
+
+  @param PciIoDevice     PCI IO instance.
+**/
+VOID
+DumpPciBars (
+  IN PCI_IO_DEVICE                    *PciIoDevice
+  )
+{
+  UINTN                               Index;
+
+  for (Index = 0; Index < PCI_MAX_BAR; Index++) {
+    if (PciIoDevice->PciBar[Index].BarType == PciBarTypeUnknown) {
+      continue;
+    }
+
+    DEBUG ((
+      EFI_D_INFO,
+      "   BAR[%d]: Type = %s; Alignment = 0x%lx;\tLength = 0x%lx;\tOffset = 0x%02x\n",
+      Index, mBarTypeStr[MIN (PciIoDevice->PciBar[Index].BarType, PciBarTypeMaxType)],
+      PciIoDevice->PciBar[Index].Alignment, PciIoDevice->PciBar[Index].Length, PciIoDevice->PciBar[Index].Offset
+      ));
+  }
+
+  for (Index = 0; Index < PCI_MAX_BAR; Index++) {
+    if ((PciIoDevice->VfPciBar[Index].BarType == PciBarTypeUnknown) && (PciIoDevice->VfPciBar[Index].Length == 0)) {
+      continue;
+    }
+
+    DEBUG ((
+      EFI_D_INFO,
+      " VFBAR[%d]: Type = %s; Alignment = 0x%lx;\tLength = 0x%lx;\tOffset = 0x%02x\n",
+      Index, mBarTypeStr[MIN (PciIoDevice->VfPciBar[Index].BarType, PciBarTypeMaxType)],
+      PciIoDevice->VfPciBar[Index].Alignment, PciIoDevice->VfPciBar[Index].Length, PciIoDevice->VfPciBar[Index].Offset
+      ));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+}
+
+/**
+  Create PCI device instance for PCI device.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherDeviceInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  )
+{
+  UINTN                           Offset;
+  UINTN                           BarIndex;
+  PCI_IO_DEVICE                   *PciIoDevice;
+
+  PciIoDevice = CreatePciIoDevice (
+                  Bridge,
+                  Pci,
+                  Bus,
+                  Device,
+                  Func
+                  );
+
+  if (PciIoDevice == NULL) {
+    return NULL;
+  }
+
+  //
+  // If it is a full enumeration, disconnect the device in advance
+  //
+  if (gFullEnumeration) {
+
+    PCI_DISABLE_COMMAND_REGISTER (PciIoDevice, EFI_PCI_COMMAND_BITS_OWNED);
+
+  }
+
+  //
+  // Start to parse the bars
+  //
+  for (Offset = 0x10, BarIndex = 0; Offset <= 0x24 && BarIndex < PCI_MAX_BAR; BarIndex++) {
+    Offset = PciParseBar (PciIoDevice, Offset, BarIndex);
+  }
+
+  //
+  // Parse the SR-IOV VF bars
+  //
+  if (PcdGetBool (PcdSrIovSupport) && PciIoDevice->SrIovCapabilityOffset != 0) {
+    for (Offset = PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_BAR0, BarIndex = 0;
+         Offset <= PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_BAR5;
+         BarIndex++) {
+
+      ASSERT (BarIndex < PCI_MAX_BAR);
+      Offset = PciIovParseVfBar (PciIoDevice, Offset, BarIndex);
+    }
+  }
+
+  DEBUG_CODE (DumpPciBars (PciIoDevice););
+  return PciIoDevice;
+}
+
+/**
+  Create PCI device instance for PCI-PCI bridge.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherPpbInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  )
+{
+  PCI_IO_DEVICE                   *PciIoDevice;
+  EFI_STATUS                      Status;
+  UINT8                           Value;
+  EFI_PCI_IO_PROTOCOL             *PciIo;
+  UINT8                           Temp;
+  UINT32                          PMemBaseLimit;
+  UINT16                          PrefetchableMemoryBase;
+  UINT16                          PrefetchableMemoryLimit;
+
+  PciIoDevice = CreatePciIoDevice (
+                  Bridge,
+                  Pci,
+                  Bus,
+                  Device,
+                  Func
+                  );
+
+  if (PciIoDevice == NULL) {
+    return NULL;
+  }
+
+  if (gFullEnumeration) {
+    PCI_DISABLE_COMMAND_REGISTER (PciIoDevice, EFI_PCI_COMMAND_BITS_OWNED);
+
+    //
+    // Initalize the bridge control register
+    //
+    PCI_DISABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, EFI_PCI_BRIDGE_CONTROL_BITS_OWNED);
+
+  }
+
+  //
+  // PPB can have two BARs
+  //
+  if (PciParseBar (PciIoDevice, 0x10, PPB_BAR_0) == 0x14) {
+    //
+    // Not 64-bit bar
+    //
+    PciParseBar (PciIoDevice, 0x14, PPB_BAR_1);
+  }
+
+  PciIo = &PciIoDevice->PciIo;
+
+  //
+  // Test whether it support 32 decode or not
+  //
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Temp);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &gAllOne);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Value);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Temp);
+
+  if (Value != 0) {
+    if ((Value & 0x01) != 0) {
+      PciIoDevice->Decodes |= EFI_BRIDGE_IO32_DECODE_SUPPORTED;
+    } else {
+      PciIoDevice->Decodes |= EFI_BRIDGE_IO16_DECODE_SUPPORTED;
+    }
+  }
+
+  //
+  // if PcdPciBridgeIoAlignmentProbe is TRUE, PCI bus driver probes
+  // PCI bridge supporting non-stardard I/O window alignment less than 4K.
+  //
+
+  PciIoDevice->BridgeIoAlignment = 0xFFF;
+  if (FeaturePcdGet (PcdPciBridgeIoAlignmentProbe)) {
+    //
+    // Check any bits of bit 3-1 of I/O Base Register are writable.
+    // if so, it is assumed non-stardard I/O window alignment is supported by this bridge.
+    // Per spec, bit 3-1 of I/O Base Register are reserved bits, so its content can't be assumed.
+    //
+    Value = (UINT8)(Temp ^ (BIT3 | BIT2 | BIT1));
+    PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Value);
+    PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Value);
+    PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Temp);
+    Value = (UINT8)((Value ^ Temp) & (BIT3 | BIT2 | BIT1));
+    switch (Value) {
+      case BIT3:
+        PciIoDevice->BridgeIoAlignment = 0x7FF;
+        break;
+      case BIT3 | BIT2:
+        PciIoDevice->BridgeIoAlignment = 0x3FF;
+        break;
+      case BIT3 | BIT2 | BIT1:
+        PciIoDevice->BridgeIoAlignment = 0x1FF;
+        break;
+    }
+  }
+
+  Status = BarExisted (
+            PciIoDevice,
+            0x24,
+            NULL,
+            &PMemBaseLimit
+            );
+
+  //
+  // Test if it supports 64 memory or not
+  //
+  // The bottom 4 bits of both the Prefetchable Memory Base and Prefetchable Memory Limit
+  // registers:
+  //   0 - the bridge supports only 32 bit addresses.
+  //   1 - the bridge supports 64-bit addresses.
+  //
+  PrefetchableMemoryBase = (UINT16)(PMemBaseLimit & 0xffff);
+  PrefetchableMemoryLimit = (UINT16)(PMemBaseLimit >> 16);
+  if (!EFI_ERROR (Status) &&
+      (PrefetchableMemoryBase & 0x000f) == 0x0001 &&
+      (PrefetchableMemoryLimit & 0x000f) == 0x0001) {
+    Status = BarExisted (
+              PciIoDevice,
+              0x28,
+              NULL,
+              NULL
+              );
+
+    if (!EFI_ERROR (Status)) {
+      PciIoDevice->Decodes |= EFI_BRIDGE_PMEM32_DECODE_SUPPORTED;
+      PciIoDevice->Decodes |= EFI_BRIDGE_PMEM64_DECODE_SUPPORTED;
+    } else {
+      PciIoDevice->Decodes |= EFI_BRIDGE_PMEM32_DECODE_SUPPORTED;
+    }
+  }
+
+  //
+  // Memory 32 code is required for ppb
+  //
+  PciIoDevice->Decodes |= EFI_BRIDGE_MEM32_DECODE_SUPPORTED;
+
+  GetResourcePaddingPpb (PciIoDevice);
+
+  DEBUG_CODE (
+    DumpPpbPaddingResource (PciIoDevice, PciBarTypeUnknown);
+    DumpPciBars (PciIoDevice);
+  );
+
+  return PciIoDevice;
+}
+
+
+/**
+  Create PCI device instance for PCI Card bridge device.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherP2CInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  )
+{
+  PCI_IO_DEVICE                   *PciIoDevice;
+
+  PciIoDevice = CreatePciIoDevice (
+                  Bridge,
+                  Pci,
+                  Bus,
+                  Device,
+                  Func
+                  );
+
+  if (PciIoDevice == NULL) {
+    return NULL;
+  }
+
+  if (gFullEnumeration) {
+    PCI_DISABLE_COMMAND_REGISTER (PciIoDevice, EFI_PCI_COMMAND_BITS_OWNED);
+
+    //
+    // Initalize the bridge control register
+    //
+    PCI_DISABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, EFI_PCCARD_BRIDGE_CONTROL_BITS_OWNED);
+  }
+
+  //
+  // P2C only has one bar that is in 0x10
+  //
+  PciParseBar (PciIoDevice, 0x10, P2C_BAR_0);
+
+  //
+  // Read PciBar information from the bar register
+  //
+  GetBackPcCardBar (PciIoDevice);
+  PciIoDevice->Decodes = EFI_BRIDGE_MEM32_DECODE_SUPPORTED  |
+                         EFI_BRIDGE_PMEM32_DECODE_SUPPORTED |
+                         EFI_BRIDGE_IO32_DECODE_SUPPORTED;
+
+  DEBUG_CODE (DumpPciBars (PciIoDevice););
+
+  return PciIoDevice;
+}
+
+/**
+  Create device path for pci deivce.
+
+  @param ParentDevicePath  Parent bridge's path.
+  @param PciIoDevice       Pci device instance.
+
+  @return Device path protocol instance for specific pci device.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+CreatePciDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL *ParentDevicePath,
+  IN  PCI_IO_DEVICE            *PciIoDevice
+  )
+{
+
+  PCI_DEVICE_PATH PciNode;
+
+  //
+  // Create PCI device path
+  //
+  PciNode.Header.Type     = HARDWARE_DEVICE_PATH;
+  PciNode.Header.SubType  = HW_PCI_DP;
+  SetDevicePathNodeLength (&PciNode.Header, sizeof (PciNode));
+
+  PciNode.Device          = PciIoDevice->DeviceNumber;
+  PciNode.Function        = PciIoDevice->FunctionNumber;
+  PciIoDevice->DevicePath = AppendDevicePathNode (ParentDevicePath, &PciNode.Header);
+
+  return PciIoDevice->DevicePath;
+}
+
+/**
+  Check whether the PCI IOV VF bar is existed or not.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param Offset            The offset.
+  @param BarLengthValue    The bar length value returned.
+  @param OriginalBarValue  The original bar value returned.
+
+  @retval EFI_NOT_FOUND    The bar doesn't exist.
+  @retval EFI_SUCCESS      The bar exist.
+
+**/
+EFI_STATUS
+VfBarExisted (
+  IN PCI_IO_DEVICE *PciIoDevice,
+  IN UINTN         Offset,
+  OUT UINT32       *BarLengthValue,
+  OUT UINT32       *OriginalBarValue
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT32              OriginalValue;
+  UINT32              Value;
+  EFI_TPL             OldTpl;
+
+  //
+  // Ensure it is called properly
+  //
+  ASSERT (PciIoDevice->SrIovCapabilityOffset != 0);
+  if (PciIoDevice->SrIovCapabilityOffset == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  PciIo = &PciIoDevice->PciIo;
+
+  //
+  // Preserve the original value
+  //
+
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, (UINT32)Offset, 1, &OriginalValue);
+
+  //
+  // Raise TPL to high level to disable timer interrupt while the BAR is probed
+  //
+  OldTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, (UINT32)Offset, 1, &gAllOne);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, (UINT32)Offset, 1, &Value);
+
+  //
+  // Write back the original value
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, (UINT32)Offset, 1, &OriginalValue);
+
+  //
+  // Restore TPL to its original level
+  //
+  gBS->RestoreTPL (OldTpl);
+
+  if (BarLengthValue != NULL) {
+    *BarLengthValue = Value;
+  }
+
+  if (OriginalBarValue != NULL) {
+    *OriginalBarValue = OriginalValue;
+  }
+
+  if (Value == 0) {
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Check whether the bar is existed or not.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param Offset            The offset.
+  @param BarLengthValue    The bar length value returned.
+  @param OriginalBarValue  The original bar value returned.
+
+  @retval EFI_NOT_FOUND    The bar doesn't exist.
+  @retval EFI_SUCCESS      The bar exist.
+
+**/
+EFI_STATUS
+BarExisted (
+  IN  PCI_IO_DEVICE *PciIoDevice,
+  IN  UINTN         Offset,
+  OUT UINT32        *BarLengthValue,
+  OUT UINT32        *OriginalBarValue
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT32              OriginalValue;
+  UINT32              Value;
+  EFI_TPL             OldTpl;
+
+  PciIo = &PciIoDevice->PciIo;
+
+  //
+  // Preserve the original value
+  //
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, (UINT8) Offset, 1, &OriginalValue);
+
+  //
+  // Raise TPL to high level to disable timer interrupt while the BAR is probed
+  //
+  OldTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, (UINT8) Offset, 1, &gAllOne);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, (UINT8) Offset, 1, &Value);
+
+  //
+  // Write back the original value
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, (UINT8) Offset, 1, &OriginalValue);
+
+  //
+  // Restore TPL to its original level
+  //
+  gBS->RestoreTPL (OldTpl);
+
+  if (BarLengthValue != NULL) {
+    *BarLengthValue = Value;
+  }
+
+  if (OriginalBarValue != NULL) {
+    *OriginalBarValue = OriginalValue;
+  }
+
+  if (Value == 0) {
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Test whether the device can support given attributes.
+
+  @param PciIoDevice      Pci device instance.
+  @param Command          Input command register value, and
+                          returned supported register value.
+  @param BridgeControl    Inout bridge control value for PPB or P2C, and
+                          returned supported bridge control value.
+  @param OldCommand       Returned and stored old command register offset.
+  @param OldBridgeControl Returned and stored old Bridge control value for PPB or P2C.
+
+**/
+VOID
+PciTestSupportedAttribute (
+  IN     PCI_IO_DEVICE                      *PciIoDevice,
+  IN OUT UINT16                             *Command,
+  IN OUT UINT16                             *BridgeControl,
+     OUT UINT16                             *OldCommand,
+     OUT UINT16                             *OldBridgeControl
+  )
+{
+  EFI_TPL OldTpl;
+
+  //
+  // Preserve the original value
+  //
+  PCI_READ_COMMAND_REGISTER (PciIoDevice, OldCommand);
+
+  //
+  // Raise TPL to high level to disable timer interrupt while the BAR is probed
+  //
+  OldTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+  PCI_SET_COMMAND_REGISTER (PciIoDevice, *Command);
+  PCI_READ_COMMAND_REGISTER (PciIoDevice, Command);
+
+  //
+  // Write back the original value
+  //
+  PCI_SET_COMMAND_REGISTER (PciIoDevice, *OldCommand);
+
+  //
+  // Restore TPL to its original level
+  //
+  gBS->RestoreTPL (OldTpl);
+
+  if (IS_PCI_BRIDGE (&PciIoDevice->Pci) || IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+
+    //
+    // Preserve the original value
+    //
+    PCI_READ_BRIDGE_CONTROL_REGISTER (PciIoDevice, OldBridgeControl);
+
+    //
+    // Raise TPL to high level to disable timer interrupt while the BAR is probed
+    //
+    OldTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+    PCI_SET_BRIDGE_CONTROL_REGISTER (PciIoDevice, *BridgeControl);
+    PCI_READ_BRIDGE_CONTROL_REGISTER (PciIoDevice, BridgeControl);
+
+    //
+    // Write back the original value
+    //
+    PCI_SET_BRIDGE_CONTROL_REGISTER (PciIoDevice, *OldBridgeControl);
+
+    //
+    // Restore TPL to its original level
+    //
+    gBS->RestoreTPL (OldTpl);
+
+  } else {
+    *OldBridgeControl = 0;
+    *BridgeControl    = 0;
+  }
+}
+
+/**
+  Set the supported or current attributes of a PCI device.
+
+  @param PciIoDevice    Structure pointer for PCI device.
+  @param Command        Command register value.
+  @param BridgeControl  Bridge control value for PPB or P2C.
+  @param Option         Make a choice of EFI_SET_SUPPORTS or EFI_SET_ATTRIBUTES.
+
+**/
+VOID
+PciSetDeviceAttribute (
+  IN PCI_IO_DEVICE                      *PciIoDevice,
+  IN UINT16                             Command,
+  IN UINT16                             BridgeControl,
+  IN UINTN                              Option
+  )
+{
+  UINT64  Attributes;
+
+  Attributes = 0;
+
+  if ((Command & EFI_PCI_COMMAND_IO_SPACE) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_IO;
+  }
+
+  if ((Command & EFI_PCI_COMMAND_MEMORY_SPACE) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_MEMORY;
+  }
+
+  if ((Command & EFI_PCI_COMMAND_BUS_MASTER) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_BUS_MASTER;
+  }
+
+  if ((Command & EFI_PCI_COMMAND_VGA_PALETTE_SNOOP) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO;
+  }
+
+  if ((BridgeControl & EFI_PCI_BRIDGE_CONTROL_ISA) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_ISA_IO;
+  }
+
+  if ((BridgeControl & EFI_PCI_BRIDGE_CONTROL_VGA) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_IO;
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_MEMORY;
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO;
+  }
+
+  if ((BridgeControl & EFI_PCI_BRIDGE_CONTROL_VGA_16) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_IO_16;
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16;
+  }
+
+  if (Option == EFI_SET_SUPPORTS) {
+
+    Attributes |= (UINT64) (EFI_PCI_IO_ATTRIBUTE_MEMORY_WRITE_COMBINE |
+                  EFI_PCI_IO_ATTRIBUTE_MEMORY_CACHED        |
+                  EFI_PCI_IO_ATTRIBUTE_MEMORY_DISABLE       |
+                  EFI_PCI_IO_ATTRIBUTE_EMBEDDED_DEVICE      |
+                  EFI_PCI_IO_ATTRIBUTE_EMBEDDED_ROM         |
+                  EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE);
+
+    if (IS_PCI_LPC (&PciIoDevice->Pci)) {
+        Attributes |= EFI_PCI_IO_ATTRIBUTE_ISA_MOTHERBOARD_IO;
+        Attributes |= (mReserveIsaAliases ? (UINT64) EFI_PCI_IO_ATTRIBUTE_ISA_IO : \
+                                            (UINT64) EFI_PCI_IO_ATTRIBUTE_ISA_IO_16);
+    }
+
+    if (IS_PCI_BRIDGE (&PciIoDevice->Pci) || IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+      //
+      // For bridge, it should support IDE attributes
+      //
+      Attributes |= EFI_PCI_IO_ATTRIBUTE_IDE_SECONDARY_IO;
+      Attributes |= EFI_PCI_IO_ATTRIBUTE_IDE_PRIMARY_IO;
+
+      if (mReserveVgaAliases) {
+        Attributes &= ~(UINT64)(EFI_PCI_IO_ATTRIBUTE_VGA_IO_16 | \
+                                EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16);
+      } else {
+        Attributes &= ~(UINT64)(EFI_PCI_IO_ATTRIBUTE_VGA_IO | \
+                                EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO);
+      }
+    } else {
+
+      if (IS_PCI_IDE (&PciIoDevice->Pci)) {
+        Attributes |= EFI_PCI_IO_ATTRIBUTE_IDE_SECONDARY_IO;
+        Attributes |= EFI_PCI_IO_ATTRIBUTE_IDE_PRIMARY_IO;
+      }
+
+      if (IS_PCI_VGA (&PciIoDevice->Pci)) {
+        Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_MEMORY;
+        Attributes |= (mReserveVgaAliases ? (UINT64) EFI_PCI_IO_ATTRIBUTE_VGA_IO : \
+                                            (UINT64) EFI_PCI_IO_ATTRIBUTE_VGA_IO_16);
+      }
+    }
+
+    PciIoDevice->Supports = Attributes;
+    PciIoDevice->Supports &= ( (PciIoDevice->Parent->Supports) | \
+                               EFI_PCI_IO_ATTRIBUTE_IO | EFI_PCI_IO_ATTRIBUTE_MEMORY | \
+                               EFI_PCI_IO_ATTRIBUTE_BUS_MASTER );
+
+  } else {
+    //
+    // When this attribute is clear, the RomImage and RomSize fields in the PCI IO were
+    // initialized based on the PCI option ROM found through the ROM BAR of the PCI controller.
+    // When this attribute is set, the PCI option ROM described by the RomImage and RomSize
+    // fields is not from the the ROM BAR of the PCI controller.
+    //
+    if (!PciIoDevice->EmbeddedRom) {
+      Attributes |= EFI_PCI_IO_ATTRIBUTE_EMBEDDED_ROM;
+    }
+    PciIoDevice->Attributes = Attributes;
+  }
+}
+
+/**
+  Determine if the device can support Fast Back to Back attribute.
+
+  @param PciIoDevice  Pci device instance.
+  @param StatusIndex  Status register value.
+
+  @retval EFI_SUCCESS       This device support Fast Back to Back attribute.
+  @retval EFI_UNSUPPORTED   This device doesn't support Fast Back to Back attribute.
+
+**/
+EFI_STATUS
+GetFastBackToBackSupport (
+  IN PCI_IO_DEVICE                      *PciIoDevice,
+  IN UINT8                              StatusIndex
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  EFI_STATUS          Status;
+  UINT32              StatusRegister;
+
+  //
+  // Read the status register
+  //
+  PciIo   = &PciIoDevice->PciIo;
+  Status  = PciIo->Pci.Read (PciIo, EfiPciIoWidthUint16, StatusIndex, 1, &StatusRegister);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check the Fast B2B bit
+  //
+  if ((StatusRegister & EFI_PCI_FAST_BACK_TO_BACK_CAPABLE) != 0) {
+    return EFI_SUCCESS;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+}
+
+/**
+  Process the option ROM for all the children of the specified parent PCI device.
+  It can only be used after the first full Option ROM process.
+
+  @param PciIoDevice Pci device instance.
+
+**/
+VOID
+ProcessOptionRomLight (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  )
+{
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+
+  //
+  // For RootBridge, PPB , P2C, go recursively to traverse all its children
+  //
+  CurrentLink = PciIoDevice->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &PciIoDevice->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      ProcessOptionRomLight (Temp);
+    }
+
+    PciRomGetImageMapping (Temp);
+
+    //
+    // The OpRom has already been processed in the first round
+    //
+    Temp->AllOpRomProcessed = TRUE;
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+}
+
+/**
+  Determine the related attributes of all devices under a Root Bridge.
+
+  @param PciIoDevice   PCI device instance.
+
+**/
+EFI_STATUS
+DetermineDeviceAttribute (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  )
+{
+  UINT16          Command;
+  UINT16          BridgeControl;
+  UINT16          OldCommand;
+  UINT16          OldBridgeControl;
+  BOOLEAN         FastB2BSupport;
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+  EFI_STATUS      Status;
+
+  //
+  // For Root Bridge, just copy it by RootBridgeIo proctocol
+  // so as to keep consistent with the actual attribute
+  //
+  if (PciIoDevice->Parent == NULL) {
+    Status = PciIoDevice->PciRootBridgeIo->GetAttributes (
+                                            PciIoDevice->PciRootBridgeIo,
+                                            &PciIoDevice->Supports,
+                                            &PciIoDevice->Attributes
+                                            );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Assume the PCI Root Bridge supports DAC
+    //
+    PciIoDevice->Supports |= (UINT64)(EFI_PCI_IO_ATTRIBUTE_EMBEDDED_DEVICE |
+                              EFI_PCI_IO_ATTRIBUTE_EMBEDDED_ROM |
+                              EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE);
+
+  } else {
+
+    //
+    // Set the attributes to be checked for common PCI devices and PPB or P2C
+    // Since some devices only support part of them, it is better to set the
+    // attribute according to its command or bridge control register
+    //
+    Command = EFI_PCI_COMMAND_IO_SPACE     |
+              EFI_PCI_COMMAND_MEMORY_SPACE |
+              EFI_PCI_COMMAND_BUS_MASTER   |
+              EFI_PCI_COMMAND_VGA_PALETTE_SNOOP;
+
+    BridgeControl = EFI_PCI_BRIDGE_CONTROL_ISA | EFI_PCI_BRIDGE_CONTROL_VGA | EFI_PCI_BRIDGE_CONTROL_VGA_16;
+
+    //
+    // Test whether the device can support attributes above
+    //
+    PciTestSupportedAttribute (PciIoDevice, &Command, &BridgeControl, &OldCommand, &OldBridgeControl);
+
+    //
+    // Set the supported attributes for specified PCI device
+    //
+    PciSetDeviceAttribute (PciIoDevice, Command, BridgeControl, EFI_SET_SUPPORTS);
+
+    //
+    // Set the current attributes for specified PCI device
+    //
+    PciSetDeviceAttribute (PciIoDevice, OldCommand, OldBridgeControl, EFI_SET_ATTRIBUTES);
+
+    //
+    // Enable other supported attributes but not defined in PCI_IO_PROTOCOL
+    //
+    PCI_ENABLE_COMMAND_REGISTER (PciIoDevice, EFI_PCI_COMMAND_MEMORY_WRITE_AND_INVALIDATE);
+  }
+
+  FastB2BSupport = TRUE;
+
+  //
+  // P2C can not support FB2B on the secondary side
+  //
+  if (IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+    FastB2BSupport = FALSE;
+  }
+
+  //
+  // For RootBridge, PPB , P2C, go recursively to traverse all its children
+  //
+  CurrentLink = PciIoDevice->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &PciIoDevice->ChildList) {
+
+    Temp    = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    Status  = DetermineDeviceAttribute (Temp);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Detect Fast Bact to Bact support for the device under the bridge
+    //
+    Status = GetFastBackToBackSupport (Temp, PCI_PRIMARY_STATUS_OFFSET);
+    if (FastB2BSupport && EFI_ERROR (Status)) {
+      FastB2BSupport = FALSE;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+  //
+  // Set or clear Fast Back to Back bit for the whole bridge
+  //
+  if (!IsListEmpty (&PciIoDevice->ChildList)) {
+
+    if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+
+      Status = GetFastBackToBackSupport (PciIoDevice, PCI_BRIDGE_STATUS_REGISTER_OFFSET);
+
+      if (EFI_ERROR (Status) || (!FastB2BSupport)) {
+        FastB2BSupport = FALSE;
+        PCI_DISABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, EFI_PCI_BRIDGE_CONTROL_FAST_BACK_TO_BACK);
+      } else {
+        PCI_ENABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, EFI_PCI_BRIDGE_CONTROL_FAST_BACK_TO_BACK);
+      }
+    }
+
+    CurrentLink = PciIoDevice->ChildList.ForwardLink;
+    while (CurrentLink != NULL && CurrentLink != &PciIoDevice->ChildList) {
+      Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+      if (FastB2BSupport) {
+        PCI_ENABLE_COMMAND_REGISTER (Temp, EFI_PCI_COMMAND_FAST_BACK_TO_BACK);
+      } else {
+        PCI_DISABLE_COMMAND_REGISTER (Temp, EFI_PCI_COMMAND_FAST_BACK_TO_BACK);
+      }
+
+      CurrentLink = CurrentLink->ForwardLink;
+    }
+  }
+  //
+  // End for IsListEmpty
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  This routine is used to update the bar information for those incompatible PCI device.
+
+  @param PciIoDevice      Input Pci device instance. Output Pci device instance with updated
+                          Bar information.
+
+  @retval EFI_SUCCESS     Successfully updated bar information.
+  @retval EFI_UNSUPPORTED Given PCI device doesn't belong to incompatible PCI device list.
+
+**/
+EFI_STATUS
+UpdatePciInfo (
+  IN OUT PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             BarIndex;
+  UINTN                             BarEndIndex;
+  BOOLEAN                           SetFlag;
+  VOID                              *Configuration;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Ptr;
+
+  Configuration = NULL;
+  Status        = EFI_SUCCESS;
+
+  if (gEfiIncompatiblePciDeviceSupport == NULL) {
+    //
+    // It can only be supported after the Incompatible PCI Device
+    // Support Protocol has been installed
+    //
+    Status = gBS->LocateProtocol (
+                    &gEfiIncompatiblePciDeviceSupportProtocolGuid,
+                    NULL,
+                    (VOID **) &gEfiIncompatiblePciDeviceSupport
+                    );
+  }
+  if (Status == EFI_SUCCESS) {
+      //
+      // Check whether the device belongs to incompatible devices from protocol or not
+      // If it is , then get its special requirement in the ACPI table
+      //
+      Status = gEfiIncompatiblePciDeviceSupport->CheckDevice (
+                                                   gEfiIncompatiblePciDeviceSupport,
+                                                   PciIoDevice->Pci.Hdr.VendorId,
+                                                   PciIoDevice->Pci.Hdr.DeviceId,
+                                                   PciIoDevice->Pci.Hdr.RevisionID,
+                                                   PciIoDevice->Pci.Device.SubsystemVendorID,
+                                                   PciIoDevice->Pci.Device.SubsystemID,
+                                                   &Configuration
+                                                   );
+
+  }
+
+  if (EFI_ERROR (Status) || Configuration == NULL ) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Update PCI device information from the ACPI table
+  //
+  Ptr = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration;
+
+  while (Ptr->Desc != ACPI_END_TAG_DESCRIPTOR) {
+
+    if (Ptr->Desc != ACPI_ADDRESS_SPACE_DESCRIPTOR) {
+      //
+      // The format is not support
+      //
+      break;
+    }
+
+    BarIndex    = (UINTN) Ptr->AddrTranslationOffset;
+    BarEndIndex = BarIndex;
+
+    //
+    // Update all the bars in the device
+    //
+    if (BarIndex == PCI_BAR_ALL) {
+      BarIndex    = 0;
+      BarEndIndex = PCI_MAX_BAR - 1;
+    }
+
+    if (BarIndex > PCI_MAX_BAR) {
+      Ptr++;
+      continue;
+    }
+
+    for (; BarIndex <= BarEndIndex; BarIndex++) {
+      SetFlag = FALSE;
+      switch (Ptr->ResType) {
+      case ACPI_ADDRESS_SPACE_TYPE_MEM:
+
+        //
+        // Make sure the bar is memory type
+        //
+        if (CheckBarType (PciIoDevice, (UINT8) BarIndex, PciBarTypeMem)) {
+          SetFlag = TRUE;
+        }
+        break;
+
+      case ACPI_ADDRESS_SPACE_TYPE_IO:
+
+        //
+        // Make sure the bar is IO type
+        //
+        if (CheckBarType (PciIoDevice, (UINT8) BarIndex, PciBarTypeIo)) {
+          SetFlag = TRUE;
+        }
+        break;
+      }
+
+      if (SetFlag) {
+
+        //
+        // Update the new alignment for the device
+        //
+        SetNewAlign (&(PciIoDevice->PciBar[BarIndex].Alignment), Ptr->AddrRangeMax);
+
+        //
+        // Update the new length for the device
+        //
+        if (Ptr->AddrLen != PCI_BAR_NOCHANGE) {
+          PciIoDevice->PciBar[BarIndex].Length = Ptr->AddrLen;
+        }
+      }
+    }
+
+    Ptr++;
+  }
+
+  FreePool (Configuration);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This routine will update the alignment with the new alignment.
+
+  @param Alignment    Input Old alignment. Output updated alignment.
+  @param NewAlignment New alignment.
+
+**/
+VOID
+SetNewAlign (
+  IN OUT UINT64     *Alignment,
+  IN     UINT64     NewAlignment
+  )
+{
+  UINT64  OldAlignment;
+  UINTN   ShiftBit;
+
+  //
+  // The new alignment is the same as the original,
+  // so skip it
+  //
+  if (NewAlignment == PCI_BAR_OLD_ALIGN) {
+    return ;
+  }
+  //
+  // Check the validity of the parameter
+  //
+   if (NewAlignment != PCI_BAR_EVEN_ALIGN  &&
+       NewAlignment != PCI_BAR_SQUAD_ALIGN &&
+       NewAlignment != PCI_BAR_DQUAD_ALIGN ) {
+    *Alignment = NewAlignment;
+    return ;
+  }
+
+  OldAlignment  = (*Alignment) + 1;
+  ShiftBit      = 0;
+
+  //
+  // Get the first non-zero hex value of the length
+  //
+  while ((OldAlignment & 0x0F) == 0x00) {
+    OldAlignment = RShiftU64 (OldAlignment, 4);
+    ShiftBit += 4;
+  }
+
+  //
+  // Adjust the alignment to even, quad or double quad boundary
+  //
+  if (NewAlignment == PCI_BAR_EVEN_ALIGN) {
+    if ((OldAlignment & 0x01) != 0) {
+      OldAlignment = OldAlignment + 2 - (OldAlignment & 0x01);
+    }
+  } else if (NewAlignment == PCI_BAR_SQUAD_ALIGN) {
+    if ((OldAlignment & 0x03) != 0) {
+      OldAlignment = OldAlignment + 4 - (OldAlignment & 0x03);
+    }
+  } else if (NewAlignment == PCI_BAR_DQUAD_ALIGN) {
+    if ((OldAlignment & 0x07) != 0) {
+      OldAlignment = OldAlignment + 8 - (OldAlignment & 0x07);
+    }
+  }
+
+  //
+  // Update the old value
+  //
+  NewAlignment  = LShiftU64 (OldAlignment, ShiftBit) - 1;
+  *Alignment    = NewAlignment;
+
+  return ;
+}
+
+/**
+  Parse PCI IOV VF bar information and fill them into PCI device instance.
+
+  @param PciIoDevice  Pci device instance.
+  @param Offset       Bar offset.
+  @param BarIndex     Bar index.
+
+  @return Next bar offset.
+
+**/
+UINTN
+PciIovParseVfBar (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINTN          Offset,
+  IN UINTN          BarIndex
+  )
+{
+  UINT32      Value;
+  UINT32      OriginalValue;
+  UINT32      Mask;
+  EFI_STATUS  Status;
+
+  //
+  // Ensure it is called properly
+  //
+  ASSERT (PciIoDevice->SrIovCapabilityOffset != 0);
+  if (PciIoDevice->SrIovCapabilityOffset == 0) {
+    return 0;
+  }
+
+  OriginalValue = 0;
+  Value         = 0;
+
+  Status = VfBarExisted (
+            PciIoDevice,
+            Offset,
+            &Value,
+            &OriginalValue
+            );
+
+  if (EFI_ERROR (Status)) {
+    PciIoDevice->VfPciBar[BarIndex].BaseAddress = 0;
+    PciIoDevice->VfPciBar[BarIndex].Length      = 0;
+    PciIoDevice->VfPciBar[BarIndex].Alignment   = 0;
+
+    //
+    // Scan all the BARs anyway
+    //
+    PciIoDevice->VfPciBar[BarIndex].Offset = (UINT16) Offset;
+    return Offset + 4;
+  }
+
+  PciIoDevice->VfPciBar[BarIndex].Offset = (UINT16) Offset;
+  if ((Value & 0x01) != 0) {
+    //
+    // Device I/Os. Impossible
+    //
+    ASSERT (FALSE);
+    return Offset + 4;
+
+  } else {
+
+    Mask  = 0xfffffff0;
+
+    PciIoDevice->VfPciBar[BarIndex].BaseAddress = OriginalValue & Mask;
+
+    switch (Value & 0x07) {
+
+    //
+    //memory space; anywhere in 32 bit address space
+    //
+    case 0x00:
+      if ((Value & 0x08) != 0) {
+        PciIoDevice->VfPciBar[BarIndex].BarType = PciBarTypePMem32;
+      } else {
+        PciIoDevice->VfPciBar[BarIndex].BarType = PciBarTypeMem32;
+      }
+
+      PciIoDevice->VfPciBar[BarIndex].Length    = (~(Value & Mask)) + 1;
+      PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->VfPciBar[BarIndex].Length - 1;
+
+      //
+      // Adjust Length
+      //
+      PciIoDevice->VfPciBar[BarIndex].Length = MultU64x32 (PciIoDevice->VfPciBar[BarIndex].Length, PciIoDevice->InitialVFs);
+      //
+      // Adjust Alignment
+      //
+      if (PciIoDevice->VfPciBar[BarIndex].Alignment < PciIoDevice->SystemPageSize - 1) {
+        PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->SystemPageSize - 1;
+      }
+
+      break;
+
+    //
+    // memory space; anywhere in 64 bit address space
+    //
+    case 0x04:
+      if ((Value & 0x08) != 0) {
+        PciIoDevice->VfPciBar[BarIndex].BarType = PciBarTypePMem64;
+      } else {
+        PciIoDevice->VfPciBar[BarIndex].BarType = PciBarTypeMem64;
+      }
+
+      //
+      // According to PCI 2.2,if the bar indicates a memory 64 decoding, next bar
+      // is regarded as an extension for the first bar. As a result
+      // the sizing will be conducted on combined 64 bit value
+      // Here just store the masked first 32bit value for future size
+      // calculation
+      //
+      PciIoDevice->VfPciBar[BarIndex].Length    = Value & Mask;
+      PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->VfPciBar[BarIndex].Length - 1;
+
+      if (PciIoDevice->VfPciBar[BarIndex].Alignment < PciIoDevice->SystemPageSize - 1) {
+        PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->SystemPageSize - 1;
+      }
+
+      //
+      // Increment the offset to point to next DWORD
+      //
+      Offset += 4;
+
+      Status = VfBarExisted (
+                PciIoDevice,
+                Offset,
+                &Value,
+                &OriginalValue
+                );
+
+      if (EFI_ERROR (Status)) {
+        return Offset + 4;
+      }
+
+      //
+      // Fix the length to support some spefic 64 bit BAR
+      //
+      Value |= ((UINT32) -1 << HighBitSet32 (Value));
+
+      //
+      // Calculate the size of 64bit bar
+      //
+      PciIoDevice->VfPciBar[BarIndex].BaseAddress |= LShiftU64 ((UINT64) OriginalValue, 32);
+
+      PciIoDevice->VfPciBar[BarIndex].Length    = PciIoDevice->VfPciBar[BarIndex].Length | LShiftU64 ((UINT64) Value, 32);
+      PciIoDevice->VfPciBar[BarIndex].Length    = (~(PciIoDevice->VfPciBar[BarIndex].Length)) + 1;
+      PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->VfPciBar[BarIndex].Length - 1;
+
+      //
+      // Adjust Length
+      //
+      PciIoDevice->VfPciBar[BarIndex].Length = MultU64x32 (PciIoDevice->VfPciBar[BarIndex].Length, PciIoDevice->InitialVFs);
+      //
+      // Adjust Alignment
+      //
+      if (PciIoDevice->VfPciBar[BarIndex].Alignment < PciIoDevice->SystemPageSize - 1) {
+        PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->SystemPageSize - 1;
+      }
+
+      break;
+
+    //
+    // reserved
+    //
+    default:
+      PciIoDevice->VfPciBar[BarIndex].BarType   = PciBarTypeUnknown;
+      PciIoDevice->VfPciBar[BarIndex].Length    = (~(Value & Mask)) + 1;
+      PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->VfPciBar[BarIndex].Length - 1;
+
+      if (PciIoDevice->VfPciBar[BarIndex].Alignment < PciIoDevice->SystemPageSize - 1) {
+        PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->SystemPageSize - 1;
+      }
+
+      break;
+    }
+  }
+  
+  //
+  // Check the length again so as to keep compatible with some special bars
+  //
+  if (PciIoDevice->VfPciBar[BarIndex].Length == 0) {
+    PciIoDevice->VfPciBar[BarIndex].BarType     = PciBarTypeUnknown;
+    PciIoDevice->VfPciBar[BarIndex].BaseAddress = 0;
+    PciIoDevice->VfPciBar[BarIndex].Alignment   = 0;
+  }
+  
+  //
+  // Increment number of bar
+  //
+  return Offset + 4;
+}
+
+/**
+  Parse PCI bar information and fill them into PCI device instance.
+
+  @param PciIoDevice  Pci device instance.
+  @param Offset       Bar offset.
+  @param BarIndex     Bar index.
+
+  @return Next bar offset.
+
+**/
+UINTN
+PciParseBar (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINTN          Offset,
+  IN UINTN          BarIndex
+  )
+{
+  UINT32      Value;
+  UINT32      OriginalValue;
+  UINT32      Mask;
+  EFI_STATUS  Status;
+
+  OriginalValue = 0;
+  Value         = 0;
+
+  Status = BarExisted (
+             PciIoDevice,
+             Offset,
+             &Value,
+             &OriginalValue
+             );
+
+  if (EFI_ERROR (Status)) {
+    PciIoDevice->PciBar[BarIndex].BaseAddress = 0;
+    PciIoDevice->PciBar[BarIndex].Length      = 0;
+    PciIoDevice->PciBar[BarIndex].Alignment   = 0;
+
+    //
+    // Some devices don't fully comply to PCI spec 2.2. So be to scan all the BARs anyway
+    //
+    PciIoDevice->PciBar[BarIndex].Offset = (UINT8) Offset;
+    return Offset + 4;
+  }
+
+  PciIoDevice->PciBar[BarIndex].Offset = (UINT8) Offset;
+  if ((Value & 0x01) != 0) {
+    //
+    // Device I/Os
+    //
+    Mask = 0xfffffffc;
+
+    if ((Value & 0xFFFF0000) != 0) {
+      //
+      // It is a IO32 bar
+      //
+      PciIoDevice->PciBar[BarIndex].BarType   = PciBarTypeIo32;
+      PciIoDevice->PciBar[BarIndex].Length    = ((~(Value & Mask)) + 1);
+      PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+
+    } else {
+      //
+      // It is a IO16 bar
+      //
+      PciIoDevice->PciBar[BarIndex].BarType   = PciBarTypeIo16;
+      PciIoDevice->PciBar[BarIndex].Length    = 0x0000FFFF & ((~(Value & Mask)) + 1);
+      PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+
+    }
+    //
+    // Workaround. Some platforms inplement IO bar with 0 length
+    // Need to treat it as no-bar
+    //
+    if (PciIoDevice->PciBar[BarIndex].Length == 0) {
+      PciIoDevice->PciBar[BarIndex].BarType = (PCI_BAR_TYPE) 0;
+    }
+
+    PciIoDevice->PciBar[BarIndex].Prefetchable  = FALSE;
+    PciIoDevice->PciBar[BarIndex].BaseAddress   = OriginalValue & Mask;
+
+  } else {
+
+    Mask  = 0xfffffff0;
+
+    PciIoDevice->PciBar[BarIndex].BaseAddress = OriginalValue & Mask;
+
+    switch (Value & 0x07) {
+
+    //
+    //memory space; anywhere in 32 bit address space
+    //
+    case 0x00:
+      if ((Value & 0x08) != 0) {
+        PciIoDevice->PciBar[BarIndex].BarType = PciBarTypePMem32;
+      } else {
+        PciIoDevice->PciBar[BarIndex].BarType = PciBarTypeMem32;
+      }
+
+      PciIoDevice->PciBar[BarIndex].Length    = (~(Value & Mask)) + 1;
+      if (PciIoDevice->PciBar[BarIndex].Length < (SIZE_4KB)) {
+        //
+        // Force minimum 4KByte alignment for Virtualization technology for Directed I/O
+        //
+        PciIoDevice->PciBar[BarIndex].Alignment = (SIZE_4KB - 1);
+      } else {
+        PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+      }
+      break;
+
+    //
+    // memory space; anywhere in 64 bit address space
+    //
+    case 0x04:
+      if ((Value & 0x08) != 0) {
+        PciIoDevice->PciBar[BarIndex].BarType = PciBarTypePMem64;
+      } else {
+        PciIoDevice->PciBar[BarIndex].BarType = PciBarTypeMem64;
+      }
+
+      //
+      // According to PCI 2.2,if the bar indicates a memory 64 decoding, next bar
+      // is regarded as an extension for the first bar. As a result
+      // the sizing will be conducted on combined 64 bit value
+      // Here just store the masked first 32bit value for future size
+      // calculation
+      //
+      PciIoDevice->PciBar[BarIndex].Length    = Value & Mask;
+      PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+
+      //
+      // Increment the offset to point to next DWORD
+      //
+      Offset += 4;
+
+      Status = BarExisted (
+                 PciIoDevice,
+                 Offset,
+                 &Value,
+                 &OriginalValue
+                 );
+
+      if (EFI_ERROR (Status)) {
+        //
+        // the high 32 bit does not claim any BAR, we need to re-check the low 32 bit BAR again
+        //
+        if (PciIoDevice->PciBar[BarIndex].Length == 0) {
+          //
+          // some device implement MMIO bar with 0 length, need to treat it as no-bar
+          //
+          PciIoDevice->PciBar[BarIndex].BarType = PciBarTypeUnknown;
+          return Offset + 4;
+        }
+      }
+
+      //
+      // Fix the length to support some spefic 64 bit BAR
+      //
+      if (Value == 0) {
+        DEBUG ((EFI_D_INFO, "[PciBus]BAR probing for upper 32bit of MEM64 BAR returns 0, change to 0xFFFFFFFF.\n"));
+        Value = (UINT32) -1;
+      } else {
+        Value |= ((UINT32)(-1) << HighBitSet32 (Value));
+      }
+
+      //
+      // Calculate the size of 64bit bar
+      //
+      PciIoDevice->PciBar[BarIndex].BaseAddress |= LShiftU64 ((UINT64) OriginalValue, 32);
+
+      PciIoDevice->PciBar[BarIndex].Length    = PciIoDevice->PciBar[BarIndex].Length | LShiftU64 ((UINT64) Value, 32);
+      PciIoDevice->PciBar[BarIndex].Length    = (~(PciIoDevice->PciBar[BarIndex].Length)) + 1;
+      if (PciIoDevice->PciBar[BarIndex].Length < (SIZE_4KB)) {
+        //
+        // Force minimum 4KByte alignment for Virtualization technology for Directed I/O
+        //
+        PciIoDevice->PciBar[BarIndex].Alignment = (SIZE_4KB - 1);
+      } else {
+        PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+      }
+
+      break;
+
+    //
+    // reserved
+    //
+    default:
+      PciIoDevice->PciBar[BarIndex].BarType   = PciBarTypeUnknown;
+      PciIoDevice->PciBar[BarIndex].Length    = (~(Value & Mask)) + 1;
+      if (PciIoDevice->PciBar[BarIndex].Length < (SIZE_4KB)) {
+        //
+        // Force minimum 4KByte alignment for Virtualization technology for Directed I/O
+        //
+        PciIoDevice->PciBar[BarIndex].Alignment = (SIZE_4KB - 1);
+      } else {
+        PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+      }
+      break;
+    }
+  }
+
+  //
+  // Check the length again so as to keep compatible with some special bars
+  //
+  if (PciIoDevice->PciBar[BarIndex].Length == 0) {
+    PciIoDevice->PciBar[BarIndex].BarType     = PciBarTypeUnknown;
+    PciIoDevice->PciBar[BarIndex].BaseAddress = 0;
+    PciIoDevice->PciBar[BarIndex].Alignment   = 0;
+  }
+
+  //
+  // Increment number of bar
+  //
+  return Offset + 4;
+}
+
+/**
+  This routine is used to initialize the bar of a PCI device.
+
+  @param PciIoDevice Pci device instance.
+
+  @note It can be called typically when a device is going to be rejected.
+
+**/
+VOID
+InitializePciDevice (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT8               Offset;
+
+  PciIo = &(PciIoDevice->PciIo);
+
+  //
+  // Put all the resource apertures
+  // Resource base is set to all ones so as to indicate its resource
+  // has not been alloacted
+  //
+  for (Offset = 0x10; Offset <= 0x24; Offset += sizeof (UINT32)) {
+    PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, Offset, 1, &gAllOne);
+  }
+}
+
+/**
+  This routine is used to initialize the bar of a PCI-PCI Bridge device.
+
+  @param  PciIoDevice PCI-PCI bridge device instance.
+
+**/
+VOID
+InitializePpb (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  PciIo = &(PciIoDevice->PciIo);
+
+  //
+  // Put all the resource apertures including IO16
+  // Io32, pMem32, pMem64 to quiescent state
+  // Resource base all ones, Resource limit all zeros
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1D, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x20, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x22, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x24, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x26, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x28, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x2C, 1, &gAllZero);
+
+  //
+  // Don't support use io32 as for now
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x30, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x32, 1, &gAllZero);
+
+  //
+  // Force Interrupt line to zero for cards that come up randomly
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x3C, 1, &gAllZero);
+}
+
+/**
+  This routine is used to initialize the bar of a PCI Card Bridge device.
+
+  @param PciIoDevice  PCI Card bridge device.
+
+**/
+VOID
+InitializeP2C (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  PciIo = &(PciIoDevice->PciIo);
+
+  //
+  // Put all the resource apertures including IO16
+  // Io32, pMem32, pMem64 to quiescent state(
+  // Resource base all ones, Resource limit all zeros
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x1c, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x20, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x24, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x28, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x2c, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x30, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x34, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x38, 1, &gAllZero);
+
+  //
+  // Force Interrupt line to zero for cards that come up randomly
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x3C, 1, &gAllZero);
+}
+
+/**
+  Create and initiliaze general PCI I/O device instance for
+  PCI device/bridge device/hotplug bridge device.
+
+  @param PciRootBridgeIo   Pointer to instance of EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Pci               Input Pci information block.
+  @param Bus               Device Bus NO.
+  @param Device            Device device NO.
+  @param Func              Device func NO.
+
+  @return Instance of PCI device. NULL means no instance created.
+
+**/
+PCI_IO_DEVICE *
+CreatePciIoDevice (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  )
+{
+  PCI_IO_DEVICE        *PciIoDevice;
+  EFI_PCI_IO_PROTOCOL  *PciIo;
+  EFI_STATUS           Status;
+
+  PciIoDevice = AllocateZeroPool (sizeof (PCI_IO_DEVICE));
+  if (PciIoDevice == NULL) {
+    return NULL;
+  }
+
+  PciIoDevice->Signature        = PCI_IO_DEVICE_SIGNATURE;
+  PciIoDevice->Handle           = NULL;
+  PciIoDevice->PciRootBridgeIo  = Bridge->PciRootBridgeIo;
+  PciIoDevice->DevicePath       = NULL;
+  PciIoDevice->BusNumber        = Bus;
+  PciIoDevice->DeviceNumber     = Device;
+  PciIoDevice->FunctionNumber   = Func;
+  PciIoDevice->Decodes          = 0;
+
+  if (gFullEnumeration) {
+    PciIoDevice->Allocated = FALSE;
+  } else {
+    PciIoDevice->Allocated = TRUE;
+  }
+
+  PciIoDevice->Registered         = FALSE;
+  PciIoDevice->Attributes         = 0;
+  PciIoDevice->Supports           = 0;
+  PciIoDevice->BusOverride        = FALSE;
+  PciIoDevice->AllOpRomProcessed  = FALSE;
+
+  PciIoDevice->IsPciExp           = FALSE;
+
+  CopyMem (&(PciIoDevice->Pci), Pci, sizeof (PCI_TYPE01));
+
+  //
+  // Initialize the PCI I/O instance structure
+  //
+  InitializePciIoInstance (PciIoDevice);
+  InitializePciDriverOverrideInstance (PciIoDevice);
+  InitializePciLoadFile2 (PciIoDevice);
+  PciIo = &PciIoDevice->PciIo;
+
+  //
+  // Create a device path for this PCI device and store it into its private data
+  //
+  CreatePciDevicePath (
+    Bridge->DevicePath,
+    PciIoDevice
+    );
+
+  //
+  // Detect if PCI Express Device
+  //
+  PciIoDevice->PciExpressCapabilityOffset = 0;
+  Status = LocateCapabilityRegBlock (
+             PciIoDevice,
+             EFI_PCI_CAPABILITY_ID_PCIEXP,
+             &PciIoDevice->PciExpressCapabilityOffset,
+             NULL
+             );
+  if (!EFI_ERROR (Status)) {
+    PciIoDevice->IsPciExp = TRUE;
+  }
+
+  if (PcdGetBool (PcdAriSupport)) {
+    //
+    // Check if the device is an ARI device.
+    //
+    Status = LocatePciExpressCapabilityRegBlock (
+               PciIoDevice,
+               EFI_PCIE_CAPABILITY_ID_ARI,
+               &PciIoDevice->AriCapabilityOffset,
+               NULL
+               );
+    if (!EFI_ERROR (Status)) {
+      //
+      // We need to enable ARI feature before calculate BusReservation,
+      // because FirstVFOffset and VFStride may change after that.
+      //
+      EFI_PCI_IO_PROTOCOL  *ParentPciIo;
+      UINT32               Data32;
+
+      //
+      // Check if its parent supports ARI forwarding.
+      //
+      ParentPciIo = &Bridge->PciIo;
+      ParentPciIo->Pci.Read (
+                          ParentPciIo, 
+                          EfiPciIoWidthUint32,
+                          Bridge->PciExpressCapabilityOffset + EFI_PCIE_CAPABILITY_DEVICE_CAPABILITIES_2_OFFSET,
+                          1,
+                          &Data32
+                          );
+      if ((Data32 & EFI_PCIE_CAPABILITY_DEVICE_CAPABILITIES_2_ARI_FORWARDING) != 0) {
+        //
+        // ARI forward support in bridge, so enable it.
+        //
+        ParentPciIo->Pci.Read (
+                            ParentPciIo,
+                            EfiPciIoWidthUint32,
+                            Bridge->PciExpressCapabilityOffset + EFI_PCIE_CAPABILITY_DEVICE_CONTROL_2_OFFSET,
+                            1,
+                            &Data32
+                            );
+        if ((Data32 & EFI_PCIE_CAPABILITY_DEVICE_CONTROL_2_ARI_FORWARDING) == 0) {
+          Data32 |= EFI_PCIE_CAPABILITY_DEVICE_CONTROL_2_ARI_FORWARDING;
+          ParentPciIo->Pci.Write (
+                              ParentPciIo,
+                              EfiPciIoWidthUint32,
+                              Bridge->PciExpressCapabilityOffset + EFI_PCIE_CAPABILITY_DEVICE_CONTROL_2_OFFSET,
+                              1,
+                              &Data32
+                              );
+          DEBUG ((
+            EFI_D_INFO,
+            " ARI: forwarding enabled for PPB[%02x:%02x:%02x]\n",
+            Bridge->BusNumber,
+            Bridge->DeviceNumber,
+            Bridge->FunctionNumber
+            ));
+        }
+      }
+
+      DEBUG ((EFI_D_INFO, " ARI: CapOffset = 0x%x\n", PciIoDevice->AriCapabilityOffset));
+    }
+  }
+
+  //
+  // Initialization for SR-IOV
+  //
+
+  if (PcdGetBool (PcdSrIovSupport)) {
+    Status = LocatePciExpressCapabilityRegBlock (
+               PciIoDevice,
+               EFI_PCIE_CAPABILITY_ID_SRIOV,
+               &PciIoDevice->SrIovCapabilityOffset,
+               NULL
+               );
+    if (!EFI_ERROR (Status)) {
+      UINT32    SupportedPageSize;
+      UINT16    VFStride;
+      UINT16    FirstVFOffset;
+      UINT16    Data16;
+      UINT32    PFRid;
+      UINT32    LastVF;
+
+      //
+      // If the SR-IOV device is an ARI device, then Set ARI Capable Hierarchy for the device.
+      //
+      if (PcdGetBool (PcdAriSupport) && PciIoDevice->AriCapabilityOffset != 0) {
+        PciIo->Pci.Read (
+                     PciIo,
+                     EfiPciIoWidthUint16,
+                     PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_CONTROL,
+                     1,
+                     &Data16
+                     );
+        Data16 |= EFI_PCIE_CAPABILITY_ID_SRIOV_CONTROL_ARI_HIERARCHY;
+        PciIo->Pci.Write (
+                     PciIo,
+                     EfiPciIoWidthUint16,
+                     PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_CONTROL,
+                     1,
+                     &Data16
+                     );
+      }
+
+      //
+      // Calculate SystemPageSize
+      //
+
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint32,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_SUPPORTED_PAGE_SIZE,
+                   1,
+                   &SupportedPageSize
+                   );
+      PciIoDevice->SystemPageSize = (PcdGet32 (PcdSrIovSystemPageSize) & SupportedPageSize);
+      ASSERT (PciIoDevice->SystemPageSize != 0);
+
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint32,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_SYSTEM_PAGE_SIZE,
+                   1,
+                   &PciIoDevice->SystemPageSize
+                   );
+      //
+      // Adjust SystemPageSize for Alignment usage later
+      //
+      PciIoDevice->SystemPageSize <<= 12;
+
+      //
+      // Calculate BusReservation for PCI IOV
+      //
+
+      //
+      // Read First FirstVFOffset, InitialVFs, and VFStride
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_FIRSTVF,
+                   1,
+                   &FirstVFOffset
+                   );
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_INITIALVFS,
+                   1,
+                   &PciIoDevice->InitialVFs
+                   );
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_VFSTRIDE,
+                   1,
+                   &VFStride
+                   );
+      //
+      // Calculate LastVF
+      //
+      PFRid = EFI_PCI_RID(Bus, Device, Func);
+      LastVF = PFRid + FirstVFOffset + (PciIoDevice->InitialVFs - 1) * VFStride;
+
+      //
+      // Calculate ReservedBusNum for this PF
+      //
+      PciIoDevice->ReservedBusNum = (UINT16)(EFI_PCI_BUS_OF_RID (LastVF) - Bus + 1);
+
+      DEBUG ((
+        EFI_D_INFO,
+        " SR-IOV: SupportedPageSize = 0x%x; SystemPageSize = 0x%x; FirstVFOffset = 0x%x;\n",
+        SupportedPageSize, PciIoDevice->SystemPageSize >> 12, FirstVFOffset
+        ));
+      DEBUG ((
+        EFI_D_INFO,
+        "         InitialVFs = 0x%x; ReservedBusNum = 0x%x; CapOffset = 0x%x\n",
+        PciIoDevice->InitialVFs, PciIoDevice->ReservedBusNum, PciIoDevice->SrIovCapabilityOffset
+        ));
+    }
+  }
+
+  if (PcdGetBool (PcdMrIovSupport)) {
+    Status = LocatePciExpressCapabilityRegBlock (
+               PciIoDevice,
+               EFI_PCIE_CAPABILITY_ID_MRIOV,
+               &PciIoDevice->MrIovCapabilityOffset,
+               NULL
+               );
+    if (!EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, " MR-IOV: CapOffset = 0x%x\n", PciIoDevice->MrIovCapabilityOffset));
+    }
+  }
+
+  //
+  // Initialize the reserved resource list
+  //
+  InitializeListHead (&PciIoDevice->ReservedResourceList);
+
+  //
+  // Initialize the driver list
+  //
+  InitializeListHead (&PciIoDevice->OptionRomDriverList);
+
+  //
+  // Initialize the child list
+  //
+  InitializeListHead (&PciIoDevice->ChildList);
+
+  return PciIoDevice;
+}
+
+/**
+  This routine is used to enumerate entire pci bus system
+  in a given platform.
+
+  It is only called on the second start on the same Root Bridge.
+
+  @param  Controller     Parent bridge handler.
+
+  @retval EFI_SUCCESS    PCI enumeration finished successfully.
+  @retval other          Some error occurred when enumerating the pci bus system.
+
+**/
+EFI_STATUS
+PciEnumeratorLight (
+  IN EFI_HANDLE                    Controller
+  )
+{
+
+  EFI_STATUS                        Status;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL   *PciRootBridgeIo;
+  PCI_IO_DEVICE                     *RootBridgeDev;
+  UINT16                            MinBus;
+  UINT16                            MaxBus;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptors;
+
+  MinBus      = 0;
+  MaxBus      = PCI_MAX_BUS;
+  Descriptors = NULL;
+
+  //
+  // If this root bridge has been already enumerated, then return successfully
+  //
+  if (GetRootBridgeByHandle (Controller) != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Open pci root bridge io protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    return Status;
+  }
+
+  Status = PciRootBridgeIo->Configuration (PciRootBridgeIo, (VOID **) &Descriptors);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  while (PciGetBusRange (&Descriptors, &MinBus, &MaxBus, NULL) == EFI_SUCCESS) {
+
+    //
+    // Create a device node for root bridge device with a NULL host bridge controller handle
+    //
+    RootBridgeDev = CreateRootBridge (Controller);
+
+    if (RootBridgeDev == NULL) {
+      Descriptors++;
+      continue;
+    }
+
+    //
+    // Record the root bridgeio protocol
+    //
+    RootBridgeDev->PciRootBridgeIo = PciRootBridgeIo;
+
+    Status = PciPciDeviceInfoCollector (
+               RootBridgeDev,
+               (UINT8) MinBus
+               );
+
+    if (!EFI_ERROR (Status)) {
+
+      //
+      // Remove those PCI devices which are rejected when full enumeration
+      //
+      RemoveRejectedPciDevices (RootBridgeDev->Handle, RootBridgeDev);
+
+      //
+      // Process option rom light
+      //
+      ProcessOptionRomLight (RootBridgeDev);
+
+      //
+      // Determine attributes for all devices under this root bridge
+      //
+      DetermineDeviceAttribute (RootBridgeDev);
+
+      //
+      // If successfully, insert the node into device pool
+      //
+      InsertRootBridge (RootBridgeDev);
+    } else {
+
+      //
+      // If unsuccessly, destroy the entire node
+      //
+      DestroyRootBridge (RootBridgeDev);
+    }
+
+    Descriptors++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get bus range from PCI resource descriptor list.
+
+  @param Descriptors  A pointer to the address space descriptor.
+  @param MinBus       The min bus returned.
+  @param MaxBus       The max bus returned.
+  @param BusRange     The bus range returned.
+
+  @retval EFI_SUCCESS    Successfully got bus range.
+  @retval EFI_NOT_FOUND  Can not find the specific bus.
+
+**/
+EFI_STATUS
+PciGetBusRange (
+  IN     EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR  **Descriptors,
+  OUT    UINT16                             *MinBus,
+  OUT    UINT16                             *MaxBus,
+  OUT    UINT16                             *BusRange
+  )
+{
+  while ((*Descriptors)->Desc != ACPI_END_TAG_DESCRIPTOR) {
+    if ((*Descriptors)->ResType == ACPI_ADDRESS_SPACE_TYPE_BUS) {
+      if (MinBus != NULL) {
+        *MinBus = (UINT16) (*Descriptors)->AddrRangeMin;
+      }
+
+      if (MaxBus != NULL) {
+        *MaxBus = (UINT16) (*Descriptors)->AddrRangeMax;
+      }
+
+      if (BusRange != NULL) {
+        *BusRange = (UINT16) (*Descriptors)->AddrLen;
+      }
+
+      return EFI_SUCCESS;
+    }
+
+    (*Descriptors)++;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  This routine can be used to start the root bridge.
+
+  @param RootBridgeDev     Pci device instance.
+
+  @retval EFI_SUCCESS      This device started.
+  @retval other            Failed to get PCI Root Bridge I/O protocol.
+
+**/
+EFI_STATUS
+StartManagingRootBridge (
+  IN PCI_IO_DEVICE *RootBridgeDev
+  )
+{
+  EFI_HANDLE                      RootBridgeHandle;
+  EFI_STATUS                      Status;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  //
+  // Get the root bridge handle
+  //
+  RootBridgeHandle = RootBridgeDev->Handle;
+  PciRootBridgeIo  = NULL;
+
+  //
+  // Get the pci root bridge io protocol
+  //
+  Status = gBS->OpenProtocol (
+                  RootBridgeHandle,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  RootBridgeHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    return Status;
+  }
+
+  //
+  // Store the PciRootBridgeIo protocol into root bridge private data
+  //
+  RootBridgeDev->PciRootBridgeIo = PciRootBridgeIo;
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  This routine can be used to check whether a PCI device should be rejected when light enumeration.
+
+  @param PciIoDevice  Pci device instance.
+
+  @retval TRUE      This device should be rejected.
+  @retval FALSE     This device shouldn't be rejected.
+
+**/
+BOOLEAN
+IsPciDeviceRejected (
+  IN PCI_IO_DEVICE *PciIoDevice
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      TestValue;
+  UINT32      OldValue;
+  UINT32      Mask;
+  UINT8       BarOffset;
+
+  //
+  // PPB should be skip!
+  //
+  if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+    return FALSE;
+  }
+
+  if (IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+    //
+    // Only test base registers for P2C
+    //
+    for (BarOffset = 0x1C; BarOffset <= 0x38; BarOffset += 2 * sizeof (UINT32)) {
+
+      Mask    = (BarOffset < 0x2C) ? 0xFFFFF000 : 0xFFFFFFFC;
+      Status  = BarExisted (PciIoDevice, BarOffset, &TestValue, &OldValue);
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      TestValue = TestValue & Mask;
+      if ((TestValue != 0) && (TestValue == (OldValue & Mask))) {
+        //
+        // The bar isn't programed, so it should be rejected
+        //
+        return TRUE;
+      }
+    }
+
+    return FALSE;
+  }
+
+  for (BarOffset = 0x14; BarOffset <= 0x24; BarOffset += sizeof (UINT32)) {
+    //
+    // Test PCI devices
+    //
+    Status = BarExisted (PciIoDevice, BarOffset, &TestValue, &OldValue);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if ((TestValue & 0x01) != 0) {
+
+      //
+      // IO Bar
+      //
+      Mask      = 0xFFFFFFFC;
+      TestValue = TestValue & Mask;
+      if ((TestValue != 0) && (TestValue == (OldValue & Mask))) {
+        return TRUE;
+      }
+
+    } else {
+
+      //
+      // Mem Bar
+      //
+      Mask      = 0xFFFFFFF0;
+      TestValue = TestValue & Mask;
+
+      if ((TestValue & 0x07) == 0x04) {
+
+        //
+        // Mem64 or PMem64
+        //
+        BarOffset += sizeof (UINT32);
+        if ((TestValue != 0) && (TestValue == (OldValue & Mask))) {
+
+          //
+          // Test its high 32-Bit BAR
+          //
+          Status = BarExisted (PciIoDevice, BarOffset, &TestValue, &OldValue);
+          if (TestValue == OldValue) {
+            return TRUE;
+          }
+        }
+
+      } else {
+
+        //
+        // Mem32 or PMem32
+        //
+        if ((TestValue != 0) && (TestValue == (OldValue & Mask))) {
+          return TRUE;
+        }
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Reset all bus number from specific bridge.
+
+  @param Bridge           Parent specific bridge.
+  @param StartBusNumber   Start bus number.
+
+**/
+VOID
+ResetAllPpbBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber
+  )
+{
+  EFI_STATUS                      Status;
+  PCI_TYPE00                      Pci;
+  UINT8                           Device;
+  UINT32                          Register;
+  UINT8                           Func;
+  UINT64                          Address;
+  UINT8                           SecondaryBus;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  PciRootBridgeIo = Bridge->PciRootBridgeIo;
+
+  for (Device = 0; Device <= PCI_MAX_DEVICE; Device++) {
+    for (Func = 0; Func <= PCI_MAX_FUNC; Func++) {
+
+      //
+      // Check to see whether a pci device is present
+      //
+      Status = PciDevicePresent (
+                 PciRootBridgeIo,
+                 &Pci,
+                 StartBusNumber,
+                 Device,
+                 Func
+                 );
+
+      if (!EFI_ERROR (Status) && (IS_PCI_BRIDGE (&Pci))) {
+
+        Register  = 0;
+        Address   = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0x18);
+        Status    = PciRootBridgeIo->Pci.Read (
+                                           PciRootBridgeIo,
+                                           EfiPciWidthUint32,
+                                           Address,
+                                           1,
+                                           &Register
+                                           );
+        SecondaryBus = (UINT8)(Register >> 8);
+
+        if (SecondaryBus != 0) {
+          ResetAllPpbBusNumber (Bridge, SecondaryBus);
+        }
+
+        //
+        // Reset register 18h, 19h, 1Ah on PCI Bridge
+        //
+        Register &= 0xFF000000;
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint32,
+                                        Address,
+                                        1,
+                                        &Register
+                                        );
+      }
+
+      if (Func == 0 && !IS_PCI_MULTI_FUNC (&Pci)) {
+        //
+        // Skip sub functions, this is not a multi function device
+        //
+        Func = PCI_MAX_FUNC;
+      }
+    }
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.h
new file mode 100644
index 0000000000..42306e9a47
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.h
@@ -0,0 +1,476 @@
+/** @file
+  PCI emumeration support functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_ENUMERATOR_SUPPORT_H_
+#define _EFI_PCI_ENUMERATOR_SUPPORT_H_
+
+/**
+  This routine is used to check whether the pci device is present.
+
+  @param PciRootBridgeIo   Pointer to instance of EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Pci               Output buffer for PCI device configuration space.
+  @param Bus               PCI bus NO.
+  @param Device            PCI device NO.
+  @param Func              PCI Func NO.
+
+  @retval EFI_NOT_FOUND    PCI device not present.
+  @retval EFI_SUCCESS      PCI device is found.
+
+**/
+EFI_STATUS
+PciDevicePresent (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL     *PciRootBridgeIo,
+  OUT PCI_TYPE00                          *Pci,
+  IN  UINT8                               Bus,
+  IN  UINT8                               Device,
+  IN  UINT8                               Func
+  );
+
+/**
+  Collect all the resource information under this root bridge.
+
+  A database that records all the information about pci device subject to this
+  root bridge will then be created.
+
+  @param Bridge         Parent bridge instance.
+  @param StartBusNumber Bus number of begining.
+
+  @retval EFI_SUCCESS   PCI device is found.
+  @retval other         Some error occurred when reading PCI bridge information.
+
+**/
+EFI_STATUS
+PciPciDeviceInfoCollector (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber
+  );
+
+/**
+  Seach required device and create PCI device instance.
+
+  @param Bridge     Parent bridge instance.
+  @param Pci        Input PCI device information block.
+  @param Bus        PCI bus NO.
+  @param Device     PCI device NO.
+  @param Func       PCI func  NO.
+  @param PciDevice  Output of searched PCI device instance.
+
+  @retval EFI_SUCCESS           Successfully created PCI device instance.
+  @retval EFI_OUT_OF_RESOURCES  Cannot get PCI device information.
+
+**/
+EFI_STATUS
+PciSearchDevice (
+  IN  PCI_IO_DEVICE                         *Bridge,
+  IN  PCI_TYPE00                            *Pci,
+  IN  UINT8                                 Bus,
+  IN  UINT8                                 Device,
+  IN  UINT8                                 Func,
+  OUT PCI_IO_DEVICE                         **PciDevice
+  );
+
+/**
+  Create PCI device instance for PCI device.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherDeviceInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  );
+
+/**
+  Create PCI device instance for PCI-PCI bridge.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherPpbInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  );
+
+/**
+  Create PCI device instance for PCI Card bridge device.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherP2CInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  );
+
+/**
+  Create device path for pci deivce.
+
+  @param ParentDevicePath  Parent bridge's path.
+  @param PciIoDevice       Pci device instance.
+
+  @return device path protocol instance for specific pci device.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+CreatePciDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL *ParentDevicePath,
+  IN  PCI_IO_DEVICE            *PciIoDevice
+  );
+
+/**
+  Check whether the PCI IOV VF bar is existed or not.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param Offset            The offset.
+  @param BarLengthValue    The bar length value returned.
+  @param OriginalBarValue  The original bar value returned.
+
+  @retval EFI_NOT_FOUND    The bar doesn't exist.
+  @retval EFI_SUCCESS      The bar exist.
+
+**/
+EFI_STATUS
+VfBarExisted (
+  IN PCI_IO_DEVICE *PciIoDevice,
+  IN UINTN         Offset,
+  OUT UINT32       *BarLengthValue,
+  OUT UINT32       *OriginalBarValue
+  );
+
+/**
+  Check whether the bar is existed or not.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param Offset            The offset.
+  @param BarLengthValue    The bar length value returned.
+  @param OriginalBarValue  The original bar value returned.
+
+  @retval EFI_NOT_FOUND    The bar doesn't exist.
+  @retval EFI_SUCCESS      The bar exist.
+
+**/
+EFI_STATUS
+BarExisted (
+  IN  PCI_IO_DEVICE *PciIoDevice,
+  IN  UINTN         Offset,
+  OUT UINT32        *BarLengthValue,
+  OUT UINT32        *OriginalBarValue
+  );
+
+/**
+  Test whether the device can support given attributes.
+
+  @param PciIoDevice      Pci device instance.
+  @param Command          Input command register value, and
+                          returned supported register value.
+  @param BridgeControl    Inout bridge control value for PPB or P2C, and
+                          returned supported bridge control value.
+  @param OldCommand       Returned and stored old command register offset.
+  @param OldBridgeControl Returned and stored old Bridge control value for PPB or P2C.
+
+**/
+VOID
+PciTestSupportedAttribute (
+  IN     PCI_IO_DEVICE                      *PciIoDevice,
+  IN OUT UINT16                             *Command,
+  IN OUT UINT16                             *BridgeControl,
+     OUT UINT16                             *OldCommand,
+     OUT UINT16                             *OldBridgeControl
+  );
+
+/**
+  Set the supported or current attributes of a PCI device.
+
+  @param PciIoDevice    Structure pointer for PCI device.
+  @param Command        Command register value.
+  @param BridgeControl  Bridge control value for PPB or P2C.
+  @param Option         Make a choice of EFI_SET_SUPPORTS or EFI_SET_ATTRIBUTES.
+
+**/
+VOID
+PciSetDeviceAttribute (
+  IN PCI_IO_DEVICE                      *PciIoDevice,
+  IN UINT16                             Command,
+  IN UINT16                             BridgeControl,
+  IN UINTN                              Option
+  );
+
+/**
+  Determine if the device can support Fast Back to Back attribute.
+
+  @param PciIoDevice  Pci device instance.
+  @param StatusIndex  Status register value.
+
+  @retval EFI_SUCCESS       This device support Fast Back to Back attribute.
+  @retval EFI_UNSUPPORTED   This device doesn't support Fast Back to Back attribute.
+
+**/
+EFI_STATUS
+GetFastBackToBackSupport (
+  IN PCI_IO_DEVICE                      *PciIoDevice,
+  IN UINT8                              StatusIndex
+  );
+
+/**
+  Determine the related attributes of all devices under a Root Bridge.
+
+  @param PciIoDevice   PCI device instance.
+
+**/
+EFI_STATUS
+DetermineDeviceAttribute (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  );
+
+/**
+  This routine is used to update the bar information for those incompatible PCI device.
+
+  @param PciIoDevice      Input Pci device instance. Output Pci device instance with updated
+                          Bar information.
+
+  @retval EFI_SUCCESS     Successfully updated bar information.
+  @retval EFI_UNSUPPORTED Given PCI device doesn't belong to incompatible PCI device list.
+
+**/
+EFI_STATUS
+UpdatePciInfo (
+  IN OUT PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  This routine will update the alignment with the new alignment.
+
+  @param Alignment    Input Old alignment. Output updated alignment.
+  @param NewAlignment New alignment.
+
+**/
+VOID
+SetNewAlign (
+  IN OUT UINT64     *Alignment,
+  IN     UINT64     NewAlignment
+  );
+
+/**
+  Parse PCI bar information and fill them into PCI device instance.
+
+  @param PciIoDevice  Pci device instance.
+  @param Offset       Bar offset.
+  @param BarIndex     Bar index.
+
+  @return Next bar offset.
+
+**/
+UINTN
+PciParseBar (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINTN          Offset,
+  IN UINTN          BarIndex
+  );
+
+/**
+  Parse PCI IOV VF bar information and fill them into PCI device instance.
+
+  @param PciIoDevice  Pci device instance.
+  @param Offset       Bar offset.
+  @param BarIndex     Bar index.
+
+  @return Next bar offset.
+
+**/
+UINTN
+PciIovParseVfBar (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINTN          Offset,
+  IN UINTN          BarIndex
+  );
+
+/**
+  This routine is used to initialize the bar of a PCI device.
+
+  @param PciIoDevice Pci device instance.
+
+  @note It can be called typically when a device is going to be rejected.
+
+**/
+VOID
+InitializePciDevice (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  This routine is used to initialize the bar of a PCI-PCI Bridge device.
+
+  @param  PciIoDevice PCI-PCI bridge device instance.
+
+**/
+VOID
+InitializePpb (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  This routine is used to initialize the bar of a PCI Card Bridge device.
+
+  @param PciIoDevice  PCI Card bridge device.
+
+**/
+VOID
+InitializeP2C (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  Create and initiliaze general PCI I/O device instance for
+  PCI device/bridge device/hotplug bridge device.
+
+  @param Bridge            Parent bridge instance.
+  @param Pci               Input Pci information block.
+  @param Bus               Device Bus NO.
+  @param Device            Device device NO.
+  @param Func              Device func NO.
+
+  @return Instance of PCI device. NULL means no instance created.
+
+**/
+PCI_IO_DEVICE *
+CreatePciIoDevice (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  );
+
+/**
+  This routine is used to enumerate entire pci bus system
+  in a given platform.
+
+  It is only called on the second start on the same Root Bridge.
+
+  @param  Controller     Parent bridge handler.
+
+  @retval EFI_SUCCESS    PCI enumeration finished successfully.
+  @retval other          Some error occurred when enumerating the pci bus system.
+
+**/
+EFI_STATUS
+PciEnumeratorLight (
+  IN EFI_HANDLE                    Controller
+  );
+
+/**
+  Get bus range from PCI resource descriptor list.
+
+  @param Descriptors  A pointer to the address space descriptor.
+  @param MinBus       The min bus returned.
+  @param MaxBus       The max bus returned.
+  @param BusRange     The bus range returned.
+
+  @retval EFI_SUCCESS    Successfully got bus range.
+  @retval EFI_NOT_FOUND  Can not find the specific bus.
+
+**/
+EFI_STATUS
+PciGetBusRange (
+  IN     EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR  **Descriptors,
+  OUT    UINT16                             *MinBus,
+  OUT    UINT16                             *MaxBus,
+  OUT    UINT16                             *BusRange
+  );
+
+/**
+  This routine can be used to start the root bridge.
+
+  @param RootBridgeDev     Pci device instance.
+
+  @retval EFI_SUCCESS      This device started.
+  @retval other            Failed to get PCI Root Bridge I/O protocol.
+
+**/
+EFI_STATUS
+StartManagingRootBridge (
+  IN PCI_IO_DEVICE *RootBridgeDev
+  );
+
+/**
+  This routine can be used to check whether a PCI device should be rejected when light enumeration.
+
+  @param PciIoDevice  Pci device instance.
+
+  @retval TRUE      This device should be rejected.
+  @retval FALSE     This device shouldn't be rejected.
+
+**/
+BOOLEAN
+IsPciDeviceRejected (
+  IN PCI_IO_DEVICE *PciIoDevice
+  );
+
+/**
+  Reset all bus number from specific bridge.
+
+  @param Bridge           Parent specific bridge.
+  @param StartBusNumber   Start bus number.
+
+**/
+VOID
+ResetAllPpbBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber
+  );
+
+/**
+  Dump the PPB padding resource information.
+
+  @param PciIoDevice     PCI IO instance.
+  @param ResourceType    The desired resource type to dump.
+                         PciBarTypeUnknown means to dump all types of resources.
+**/
+VOID
+DumpPpbPaddingResource (
+  IN PCI_IO_DEVICE                    *PciIoDevice,
+  IN PCI_BAR_TYPE                     ResourceType
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.c
new file mode 100644
index 0000000000..257874b8b0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.c
@@ -0,0 +1,394 @@
+/** @file
+  PCI Hot Plug support functions implementation for PCI Bus module..
+
+Copyright (c) 2006 - 2010, 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 "PciBus.h"
+
+EFI_PCI_HOT_PLUG_INIT_PROTOCOL  *gPciHotPlugInit = NULL;
+EFI_HPC_LOCATION                *gPciRootHpcPool = NULL;
+UINTN                           gPciRootHpcCount = 0;
+ROOT_HPC_DATA                   *gPciRootHpcData = NULL;
+
+
+/**
+  Event notification function to set Hot Plug controller status.
+
+  @param  Event                    The event that invoke this function.
+  @param  Context                  The calling context, pointer to ROOT_HPC_DATA.
+
+**/
+VOID
+EFIAPI
+PciHPCInitialized (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  ROOT_HPC_DATA   *HpcData;
+
+  HpcData               = (ROOT_HPC_DATA *) Context;
+  HpcData->Initialized  = TRUE;
+}
+
+/**
+  Compare two device pathes to check if they are exactly same.
+
+  @param DevicePath1    A pointer to the first device path data structure.
+  @param DevicePath2    A pointer to the second device path data structure.
+
+  @retval TRUE    They are same.
+  @retval FALSE   They are not same.
+
+**/
+BOOLEAN
+EfiCompareDevicePath (
+  IN EFI_DEVICE_PATH_PROTOCOL *DevicePath1,
+  IN EFI_DEVICE_PATH_PROTOCOL *DevicePath2
+  )
+{
+  UINTN Size1;
+  UINTN Size2;
+
+  Size1 = GetDevicePathSize (DevicePath1);
+  Size2 = GetDevicePathSize (DevicePath2);
+
+  if (Size1 != Size2) {
+    return FALSE;
+  }
+
+  if (CompareMem (DevicePath1, DevicePath2, Size1) != 0) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Check hot plug support and initialize root hot plug private data.
+
+  If Hot Plug is supported by the platform, call PCI Hot Plug Init protocol
+  to get PCI Hot Plug controller's information and constructor the root hot plug
+  private data structure.
+
+  @retval EFI_SUCCESS           They are same.
+  @retval EFI_UNSUPPORTED       No PCI Hot Plug controler on the platform.
+  @retval EFI_OUT_OF_RESOURCES  No memory to constructor root hot plug private
+                                data structure.
+
+**/
+EFI_STATUS
+InitializeHotPlugSupport (
+  VOID
+  )
+{
+  EFI_STATUS        Status;
+  EFI_HPC_LOCATION  *HpcList;
+  UINTN             HpcCount;
+
+  //
+  // Locate the PciHotPlugInit Protocol
+  // If it doesn't exist, that means there is no
+  // hot plug controller supported on the platform
+  // the PCI Bus driver is running on. HotPlug Support
+  // is an optional feature, so absence of the protocol
+  // won't incur the penalty.
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiPciHotPlugInitProtocolGuid,
+                  NULL,
+                  (VOID **) &gPciHotPlugInit
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = gPciHotPlugInit->GetRootHpcList (
+                              gPciHotPlugInit,
+                              &HpcCount,
+                              &HpcList
+                              );
+
+  if (!EFI_ERROR (Status)) {
+
+    gPciRootHpcPool   = HpcList;
+    gPciRootHpcCount  = HpcCount;
+    gPciRootHpcData   = AllocateZeroPool (sizeof (ROOT_HPC_DATA) * gPciRootHpcCount);
+    if (gPciRootHpcData == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Test whether device path is for root pci hot plug bus.
+
+  @param HpbDevicePath  A pointer to device path data structure to be tested.
+  @param HpIndex        If HpIndex is not NULL, return the index of root hot
+                        plug in global array when TRUE is retuned.
+
+  @retval TRUE          The device path is for root pci hot plug bus.
+  @retval FALSE         The device path is not for root pci hot plug bus.
+
+**/
+BOOLEAN
+IsRootPciHotPlugBus (
+  IN  EFI_DEVICE_PATH_PROTOCOL        *HpbDevicePath,
+  OUT UINTN                           *HpIndex    OPTIONAL
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < gPciRootHpcCount; Index++) {
+
+    if (EfiCompareDevicePath (gPciRootHpcPool[Index].HpbDevicePath, HpbDevicePath)) {
+
+      if (HpIndex != NULL) {
+        *HpIndex = Index;
+      }
+
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Test whether device path is for root pci hot plug controller.
+
+  @param HpcDevicePath  A pointer to device path data structure to be tested.
+  @param HpIndex        If HpIndex is not NULL, return the index of root hot
+                        plug in global array when TRUE is retuned.
+
+  @retval TRUE          The device path is for root pci hot plug controller.
+  @retval FALSE         The device path is not for root pci hot plug controller.
+
+**/
+BOOLEAN
+IsRootPciHotPlugController (
+  IN EFI_DEVICE_PATH_PROTOCOL         *HpcDevicePath,
+  OUT UINTN                           *HpIndex
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < gPciRootHpcCount; Index++) {
+
+    if (EfiCompareDevicePath (gPciRootHpcPool[Index].HpcDevicePath, HpcDevicePath)) {
+
+      if (HpIndex != NULL) {
+        *HpIndex = Index;
+      }
+
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Creating event object for PCI Hot Plug controller.
+
+  @param  HpIndex   Index of hot plug device in global array.
+  @param  Event     The retuned event that invoke this function.
+
+  @return Status of create event invoken.
+
+**/
+EFI_STATUS
+CreateEventForHpc (
+  IN  UINTN      HpIndex,
+  OUT EFI_EVENT  *Event
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  PciHPCInitialized,
+                  gPciRootHpcData + HpIndex,
+                  &((gPciRootHpcData + HpIndex)->Event)
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    *Event = (gPciRootHpcData + HpIndex)->Event;
+  }
+
+  return Status;
+}
+
+/**
+  Wait for all root PCI Hot Plug controller finished initializing.
+
+  @param TimeoutInMicroSeconds  Microseconds to wait for all root HPCs' initialization.
+
+  @retval EFI_SUCCESS           All HPCs initialization finished.
+  @retval EFI_TIMEOUT           Not ALL HPCs initialization finished in Microseconds.
+
+**/
+EFI_STATUS
+AllRootHPCInitialized (
+  IN  UINTN           TimeoutInMicroSeconds
+  )
+{
+  UINT32  Delay;
+  UINTN   Index;
+
+  Delay = (UINT32) ((TimeoutInMicroSeconds / 30) + 1);
+
+  do {
+    for (Index = 0; Index < gPciRootHpcCount; Index++) {
+
+      if (gPciRootHpcData[Index].Found && !gPciRootHpcData[Index].Initialized) {
+        break;
+      }
+    }
+
+    if (Index == gPciRootHpcCount) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 30 microseconds..
+    //
+    gBS->Stall (30);
+
+    Delay--;
+
+  } while (Delay > 0);
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Check whether PCI-PCI bridge has PCI Hot Plug capability register block.
+
+  @param PciIoDevice    A Pointer to the PCI-PCI bridge.
+
+  @retval TRUE    PCI device is HPC.
+  @retval FALSE   PCI device is not HPC.
+
+**/
+BOOLEAN
+IsSHPC (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  )
+{
+
+  EFI_STATUS  Status;
+  UINT8       Offset;
+
+  if (PciIoDevice == NULL) {
+    return FALSE;
+  }
+
+  Offset = 0;
+  Status = LocateCapabilityRegBlock (
+            PciIoDevice,
+            EFI_PCI_CAPABILITY_ID_HOTPLUG,
+            &Offset,
+            NULL
+            );
+
+  //
+  // If the PCI-PCI bridge has the hot plug controller build-in,
+  // then return TRUE;
+  //
+  if (!EFI_ERROR (Status)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Get resource padding if the specified PCI bridge is a hot plug bus.
+
+  @param PciIoDevice    PCI bridge instance.
+
+**/
+VOID
+GetResourcePaddingForHpb (
+  IN PCI_IO_DEVICE      *PciIoDevice
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HPC_STATE                     State;
+  UINT64                            PciAddress;
+  EFI_HPC_PADDING_ATTRIBUTES        Attributes;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptors;
+
+  if (IsPciHotPlugBus (PciIoDevice)) {
+    //
+    // If PCI-PCI bridge device is PCI Hot Plug bus.
+    //
+    PciAddress = EFI_PCI_ADDRESS (PciIoDevice->BusNumber, PciIoDevice->DeviceNumber, PciIoDevice->FunctionNumber, 0);
+    Status = gPciHotPlugInit->GetResourcePadding (
+                                gPciHotPlugInit,
+                                PciIoDevice->DevicePath,
+                                PciAddress,
+                                &State,
+                                (VOID **) &Descriptors,
+                                &Attributes
+                                );
+
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+
+    if ((State & EFI_HPC_STATE_ENABLED) != 0 && (State & EFI_HPC_STATE_INITIALIZED) != 0) {
+      PciIoDevice->ResourcePaddingDescriptors = Descriptors;
+      PciIoDevice->PaddingAttributes          = Attributes;
+    }
+
+    return;
+  }
+}
+
+/**
+  Test whether PCI device is hot plug bus.
+
+  @param PciIoDevice  PCI device instance.
+
+  @retval TRUE    PCI device is a hot plug bus.
+  @retval FALSE   PCI device is not a hot plug bus.
+
+**/
+BOOLEAN
+IsPciHotPlugBus (
+  PCI_IO_DEVICE                       *PciIoDevice
+  )
+{
+  if (IsSHPC (PciIoDevice)) {
+    //
+    // If the PPB has the hot plug controller build-in,
+    // then return TRUE;
+    //
+    return TRUE;
+  }
+
+  //
+  // Otherwise, see if it is a Root HPC
+  //
+  if(IsRootPciHotPlugBus (PciIoDevice->DevicePath, NULL)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.h
new file mode 100644
index 0000000000..1fb9ba9720
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.h
@@ -0,0 +1,190 @@
+/** @file
+  PCI Hot Plug support functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2010, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_HOT_PLUG_SUPPORT_H_
+#define _EFI_PCI_HOT_PLUG_SUPPORT_H_
+
+//
+// stall 1 second, its unit is 100ns
+//
+#define STALL_1_SECOND        1000000
+
+//
+// PCI Hot Plug controller private data
+//
+typedef struct {
+  EFI_EVENT Event;
+  BOOLEAN   Found;
+  BOOLEAN   Initialized;
+  VOID      *Padding;
+} ROOT_HPC_DATA;
+
+//
+// Reference of some global variabes
+//
+extern EFI_PCI_HOT_PLUG_INIT_PROTOCOL *gPciHotPlugInit;
+extern EFI_HPC_LOCATION               *gPciRootHpcPool;
+extern ROOT_HPC_DATA                  *gPciRootHpcData;
+
+/**
+  Event notification function to set Hot Plug controller status.
+
+  @param  Event                    The event that invoke this function.
+  @param  Context                  The calling context, pointer to ROOT_HPC_DATA.
+
+**/
+VOID
+EFIAPI
+PciHPCInitialized (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  );
+
+/**
+  Compare two device pathes to check if they are exactly same.
+
+  @param DevicePath1    A pointer to the first device path data structure.
+  @param DevicePath2    A pointer to the second device path data structure.
+
+  @retval TRUE    They are same.
+  @retval FALSE   They are not same.
+
+**/
+BOOLEAN
+EfiCompareDevicePath (
+  IN EFI_DEVICE_PATH_PROTOCOL *DevicePath1,
+  IN EFI_DEVICE_PATH_PROTOCOL *DevicePath2
+  );
+
+/**
+  Check hot plug support and initialize root hot plug private data.
+
+  If Hot Plug is supported by the platform, call PCI Hot Plug Init protocol
+  to get PCI Hot Plug controller's information and constructor the root hot plug
+  private data structure.
+
+  @retval EFI_SUCCESS           They are same.
+  @retval EFI_UNSUPPORTED       No PCI Hot Plug controler on the platform.
+  @retval EFI_OUT_OF_RESOURCES  No memory to constructor root hot plug private
+                                data structure.
+
+**/
+EFI_STATUS
+InitializeHotPlugSupport (
+  VOID
+  );
+
+/**
+  Test whether PCI device is hot plug bus.
+
+  @param PciIoDevice  PCI device instance.
+
+  @retval TRUE    PCI device is a hot plug bus.
+  @retval FALSE   PCI device is not a hot plug bus.
+
+**/
+BOOLEAN
+IsPciHotPlugBus (
+  PCI_IO_DEVICE                       *PciIoDevice
+  );
+
+/**
+  Test whether device path is for root pci hot plug bus.
+
+  @param HpbDevicePath  A pointer to device path data structure to be tested.
+  @param HpIndex        If HpIndex is not NULL, return the index of root hot
+                        plug in global array when TRUE is retuned.
+
+  @retval TRUE          The device path is for root pci hot plug bus.
+  @retval FALSE         The device path is not for root pci hot plug bus.
+
+**/
+BOOLEAN
+IsRootPciHotPlugBus (
+  IN  EFI_DEVICE_PATH_PROTOCOL        *HpbDevicePath,
+  OUT UINTN                           *HpIndex    OPTIONAL
+  );
+
+/**
+  Test whether device path is for root pci hot plug controller.
+
+  @param HpcDevicePath  A pointer to device path data structure to be tested.
+  @param HpIndex        If HpIndex is not NULL, return the index of root hot
+                        plug in global array when TRUE is retuned.
+
+  @retval TRUE          The device path is for root pci hot plug controller.
+  @retval FALSE         The device path is not for root pci hot plug controller.
+
+**/
+BOOLEAN
+IsRootPciHotPlugController (
+  IN EFI_DEVICE_PATH_PROTOCOL         *HpcDevicePath,
+  OUT UINTN                           *HpIndex
+  );
+
+/**
+  Creating event object for PCI Hot Plug controller.
+
+  @param  HpIndex   Index of hot plug device in global array.
+  @param  Event     The retuned event that invoke this function.
+
+  @return Status of create event invoken.
+
+**/
+EFI_STATUS
+CreateEventForHpc (
+  IN  UINTN      HpIndex,
+  OUT EFI_EVENT  *Event
+  );
+
+/**
+  Wait for all root PCI Hot Plug controller finished initializing.
+
+  @param TimeoutInMicroSeconds  Microseconds to wait for all root HPCs' initialization.
+
+  @retval EFI_SUCCESS           All HPCs initialization finished.
+  @retval EFI_TIMEOUT           Not ALL HPCs initialization finished in Microseconds.
+
+**/
+EFI_STATUS
+AllRootHPCInitialized (
+  IN  UINTN           TimeoutInMicroSeconds
+  );
+
+/**
+  Check whether PCI-PCI bridge has PCI Hot Plug capability register block.
+
+  @param PciIoDevice    A Pointer to the PCI-PCI bridge.
+
+  @retval TRUE    PCI device is HPC.
+  @retval FALSE   PCI device is not HPC.
+
+**/
+BOOLEAN
+IsSHPC (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  );
+
+/**
+  Get resource padding if the specified PCI bridge is a hot plug bus.
+
+  @param PciIoDevice    PCI bridge instance.
+
+**/
+VOID
+GetResourcePaddingForHpb (
+  IN PCI_IO_DEVICE      *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.c
new file mode 100644
index 0000000000..416063268f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.c
@@ -0,0 +1,2043 @@
+/** @file
+  EFI PCI IO protocol functions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+//
+// Pci Io Protocol Interface
+//
+EFI_PCI_IO_PROTOCOL  mPciIoInterface = {
+  PciIoPollMem,
+  PciIoPollIo,
+  {
+    PciIoMemRead,
+    PciIoMemWrite
+  },
+  {
+    PciIoIoRead,
+    PciIoIoWrite
+  },
+  {
+    PciIoConfigRead,
+    PciIoConfigWrite
+  },
+  PciIoCopyMem,
+  PciIoMap,
+  PciIoUnmap,
+  PciIoAllocateBuffer,
+  PciIoFreeBuffer,
+  PciIoFlush,
+  PciIoGetLocation,
+  PciIoAttributes,
+  PciIoGetBarAttributes,
+  PciIoSetBarAttributes,
+  0,
+  NULL
+};
+
+/**
+  Initializes a PCI I/O Instance.
+
+  @param PciIoDevice    Pci device instance.
+
+**/
+VOID
+InitializePciIoInstance (
+  IN PCI_IO_DEVICE               *PciIoDevice
+  )
+{
+  CopyMem (&PciIoDevice->PciIo, &mPciIoInterface, sizeof (EFI_PCI_IO_PROTOCOL));
+}
+
+/**
+  Verifies access to a PCI Base Address Register (BAR).
+
+  @param PciIoDevice  Pci device instance.
+  @param BarIndex     The BAR index of the standard PCI Configuration header to use as the
+                      base address for the memory or I/O operation to perform.
+  @param Type         Operation type could be memory or I/O.
+  @param Width        Signifies the width of the memory or I/O operations.
+  @param Count        The number of memory or I/O operations to perform.
+  @param Offset       The offset within the PCI configuration space for the PCI controller.
+
+  @retval EFI_INVALID_PARAMETER Invalid Width/BarIndex or Bar type.
+  @retval EFI_SUCCESS           Successfully verified.
+
+**/
+EFI_STATUS
+PciIoVerifyBarAccess (
+  IN PCI_IO_DEVICE                   *PciIoDevice,
+  IN UINT8                           BarIndex,
+  IN PCI_BAR_TYPE                    Type,
+  IN IN EFI_PCI_IO_PROTOCOL_WIDTH    Width,
+  IN IN UINTN                        Count,
+  IN UINT64                          *Offset
+  )
+{
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BarIndex == EFI_PCI_IO_PASS_THROUGH_BAR) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // BarIndex 0-5 is legal
+  //
+  if (BarIndex >= PCI_MAX_BAR) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!CheckBarType (PciIoDevice, BarIndex, Type)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If Width is EfiPciIoWidthFifoUintX then convert to EfiPciIoWidthUintX
+  // If Width is EfiPciIoWidthFillUintX then convert to EfiPciIoWidthUintX
+  //
+  if (Width >= EfiPciIoWidthFifoUint8 && Width <= EfiPciIoWidthFifoUint64) {
+    Count = 1;
+  }
+
+  Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & 0x03);
+
+  if ((*Offset + Count * (UINTN)(1 << Width)) - 1 >= PciIoDevice->PciBar[BarIndex].Length) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Offset = *Offset + PciIoDevice->PciBar[BarIndex].BaseAddress;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Verifies access to a PCI Configuration Header.
+
+  @param PciIoDevice  Pci device instance.
+  @param Width        Signifies the width of the memory or I/O operations.
+  @param Count        The number of memory or I/O operations to perform.
+  @param Offset       The offset within the PCI configuration space for the PCI controller.
+
+  @retval EFI_INVALID_PARAMETER  Invalid Width
+  @retval EFI_UNSUPPORTED        Offset overflowed.
+  @retval EFI_SUCCESS            Successfully verified.
+
+**/
+EFI_STATUS
+PciIoVerifyConfigAccess (
+  IN PCI_IO_DEVICE              *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN UINTN                      Count,
+  IN UINT64                     *Offset
+  )
+{
+  UINT64  ExtendOffset;
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If Width is EfiPciIoWidthFillUintX then convert to EfiPciIoWidthUintX
+  //
+  Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & 0x03);
+
+  if (PciIoDevice->IsPciExp) {
+    if ((*Offset + Count * (UINTN)(1 << Width)) - 1 >= PCI_EXP_MAX_CONFIG_OFFSET) {
+      return EFI_UNSUPPORTED;
+    }
+
+    ExtendOffset  = LShiftU64 (*Offset, 32);
+    *Offset       = EFI_PCI_ADDRESS (PciIoDevice->BusNumber, PciIoDevice->DeviceNumber, PciIoDevice->FunctionNumber, 0);
+    *Offset       = (*Offset) | ExtendOffset;
+
+  } else {
+    if ((*Offset + Count * (UINTN)(1 << Width)) - 1 >= PCI_MAX_CONFIG_OFFSET) {
+      return EFI_UNSUPPORTED;
+    }
+
+    *Offset = EFI_PCI_ADDRESS (PciIoDevice->BusNumber, PciIoDevice->DeviceNumber, PciIoDevice->FunctionNumber, *Offset);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads from the memory space of a PCI controller. Returns either when the polling exit criteria is
+  satisfied or after a defined duration.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory operation.
+  @param  Mask                  Mask used for the polling criteria.
+  @param  Value                 The comparison value used for the polling exit criteria.
+  @param  Delay                 The number of 100 ns units to poll.
+  @param  Result                Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS           The last data returned from the access matched the poll exit criteria.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       Offset is not valid for the BarIndex of this PCI controller.
+  @retval EFI_TIMEOUT           Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoPollMem (
+  IN  EFI_PCI_IO_PROTOCOL        *This,
+  IN  EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT8                      BarIndex,
+  IN  UINT64                     Offset,
+  IN  UINT64                     Mask,
+  IN  UINT64                     Value,
+  IN  UINT64                     Delay,
+  OUT UINT64                     *Result
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeMem, Width, 1, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Width > EfiPciIoWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Status  = PciIoMemRead (This, Width, BarIndex, Offset, 1, Result);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      if ((*Result & Mask) == Value || Delay == 0) {
+        return EFI_SUCCESS;
+      }
+      do {
+        //
+        // Stall 10 us = 100 * 100ns
+        //
+        gBS->Stall (10);
+
+        Status  = PciIoMemRead (This, Width, BarIndex, Offset, 1, Result);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        if ((*Result & Mask) == Value) {
+          return EFI_SUCCESS;
+        }
+        if (Delay <= 100) {
+          return EFI_TIMEOUT;
+        }
+        Delay -= 100;
+      } while (TRUE);
+    }
+  }
+  
+  Status = PciIoDevice->PciRootBridgeIo->PollMem (
+                                           PciIoDevice->PciRootBridgeIo,
+                                           (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                           Offset,
+                                           Mask,
+                                           Value,
+                                           Delay,
+                                           Result
+                                           );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Reads from the memory space of a PCI controller. Returns either when the polling exit criteria is
+  satisfied or after a defined duration.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory operation.
+  @param  Mask                  Mask used for the polling criteria.
+  @param  Value                 The comparison value used for the polling exit criteria.
+  @param  Delay                 The number of 100 ns units to poll.
+  @param  Result                Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS           The last data returned from the access matched the poll exit criteria.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       Offset is not valid for the BarIndex of this PCI controller.
+  @retval EFI_TIMEOUT           Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoPollIo (
+  IN  EFI_PCI_IO_PROTOCOL        *This,
+  IN  EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT8                      BarIndex,
+  IN  UINT64                     Offset,
+  IN  UINT64                     Mask,
+  IN  UINT64                     Value,
+  IN  UINT64                     Delay,
+  OUT UINT64                     *Result
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width > EfiPciIoWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeIo, Width, 1, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Status  = PciIoIoRead (This, Width, BarIndex, Offset, 1, Result);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      if ((*Result & Mask) == Value || Delay == 0) {
+        return EFI_SUCCESS;
+      }
+      do {
+        //
+        // Stall 10 us = 100 * 100ns
+        //
+        gBS->Stall (10);
+
+        Status  = PciIoIoRead (This, Width, BarIndex, Offset, 1, Result);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        if ((*Result & Mask) == Value) {
+          return EFI_SUCCESS;
+        }
+        if (Delay <= 100) {
+          return EFI_TIMEOUT;
+        }
+        Delay -= 100;
+      } while (TRUE);
+    }
+  }
+  
+  Status = PciIoDevice->PciRootBridgeIo->PollIo (
+                                           PciIoDevice->PciRootBridgeIo,
+                                           (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                           Offset,
+                                           Mask,
+                                           Value,
+                                           Delay,
+                                           Result
+                                           );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMemRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeMem, Width, Count, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }  
+  
+
+  Status = PciIoDevice->PciRootBridgeIo->Mem.Read (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Offset,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_READ_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMemWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeMem, Width, Count, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }
+
+  Status = PciIoDevice->PciRootBridgeIo->Mem.Write (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Offset,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_WRITE_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoIoRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeIo, Width, Count, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }    
+
+  Status = PciIoDevice->PciRootBridgeIo->Io.Read (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Offset,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_READ_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoIoWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeIo, Width, Count, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }  
+
+  Status = PciIoDevice->PciRootBridgeIo->Io.Write (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Offset,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_WRITE_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in PCI configuration space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the PCI configuration space for the PCI controller.
+  @param  Count                 The number of PCI configuration operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI configuration header of the PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER Buffer is NULL or Width is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoConfigRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT32                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+  UINT64        Address;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Address     = Offset;
+  Status      = PciIoVerifyConfigAccess (PciIoDevice, Width, Count, &Address);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }    
+
+  Status = PciIoDevice->PciRootBridgeIo->Pci.Read (
+                                               PciIoDevice->PciRootBridgeIo,
+                                               (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                               Address,
+                                               Count,
+                                               Buffer
+                                               );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_READ_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in PCI configuration space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the PCI configuration space for the PCI controller.
+  @param  Count                 The number of PCI configuration operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI configuration header of the PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER Buffer is NULL or Width is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoConfigWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT32                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+  UINT64        Address;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Address     = Offset;
+  Status      = PciIoVerifyConfigAccess (PciIoDevice, Width, Count, &Address);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }  
+  
+  Status = PciIoDevice->PciRootBridgeIo->Pci.Write (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Address,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_WRITE_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enables a PCI driver to copy one region of PCI memory space to another region of PCI
+  memory space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  DestBarIndex          The BAR index in the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  DestOffset            The destination offset within the BAR specified by DestBarIndex to
+                                start the memory writes for the copy operation.
+  @param  SrcBarIndex           The BAR index in the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  SrcOffset             The source offset within the BAR specified by SrcBarIndex to start
+                                the memory reads for the copy operation.
+  @param  Count                 The number of memory operations to perform. Bytes moved is Width
+                                size * Count, starting at DestOffset and SrcOffset.
+
+  @retval EFI_SUCCESS           The data was copied from one memory region to another memory region.
+  @retval EFI_UNSUPPORTED       DestBarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       SrcBarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by DestOffset, Width, and Count
+                                is not valid for the PCI BAR specified by DestBarIndex.
+  @retval EFI_UNSUPPORTED       The address range specified by SrcOffset, Width, and Count is
+                                not valid for the PCI BAR specified by SrcBarIndex.
+  @retval EFI_INVALID_PARAMETER Width is invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoCopyMem (
+  IN EFI_PCI_IO_PROTOCOL              *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH    Width,
+  IN     UINT8                        DestBarIndex,
+  IN     UINT64                       DestOffset,
+  IN     UINT8                        SrcBarIndex,
+  IN     UINT64                       SrcOffset,
+  IN     UINTN                        Count
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Width == EfiPciIoWidthFifoUint8  ||
+      Width == EfiPciIoWidthFifoUint16 ||
+      Width == EfiPciIoWidthFifoUint32 ||
+      Width == EfiPciIoWidthFifoUint64 ||
+      Width == EfiPciIoWidthFillUint8  ||
+      Width == EfiPciIoWidthFillUint16 ||
+      Width == EfiPciIoWidthFillUint32 ||
+      Width == EfiPciIoWidthFillUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, DestBarIndex, PciBarTypeMem, Width, Count, &DestOffset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, SrcBarIndex, PciBarTypeMem, Width, Count, &SrcOffset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((SrcOffset & ((1 << (Width & 0x03)) - 1)) != 0 || (DestOffset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }  
+
+  Status = PciIoDevice->PciRootBridgeIo->CopyMem (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                          DestOffset,
+                                          SrcOffset,
+                                          Count
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Provides the PCI controller-specific addresses needed to access system memory.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Operation             Indicates if the bus master is going to read or write to system memory.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  NumberOfBytes         On input the number of bytes to map. On output the number of bytes
+                                that were mapped.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param  Mapping               A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMap (
+  IN     EFI_PCI_IO_PROTOCOL            *This,
+  IN     EFI_PCI_IO_PROTOCOL_OPERATION  Operation,
+  IN     VOID                           *HostAddress,
+  IN OUT UINTN                          *NumberOfBytes,
+  OUT    EFI_PHYSICAL_ADDRESS           *DeviceAddress,
+  OUT    VOID                           **Mapping
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Operation >= EfiPciIoOperationMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (HostAddress == NULL || NumberOfBytes == NULL || DeviceAddress == NULL || Mapping == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((PciIoDevice->Attributes & EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE) != 0) {
+    Operation = (EFI_PCI_IO_PROTOCOL_OPERATION) (Operation + EfiPciOperationBusMasterRead64);
+  }
+
+  Status = PciIoDevice->PciRootBridgeIo->Map (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_OPERATION) Operation,
+                                          HostAddress,
+                                          NumberOfBytes,
+                                          DeviceAddress,
+                                          Mapping
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Mapping               The mapping value returned from Map().
+
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoUnmap (
+  IN  EFI_PCI_IO_PROTOCOL  *This,
+  IN  VOID                 *Mapping
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Status = PciIoDevice->PciRootBridgeIo->Unmap (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          Mapping
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Allocates pages that are suitable for an EfiPciIoOperationBusMasterCommonBuffer
+  mapping.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.
+  @param  Pages                 The number of pages to allocate.
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoAllocateBuffer (
+  IN  EFI_PCI_IO_PROTOCOL   *This,
+  IN  EFI_ALLOCATE_TYPE     Type,
+  IN  EFI_MEMORY_TYPE       MemoryType,
+  IN  UINTN                 Pages,
+  OUT VOID                  **HostAddress,
+  IN  UINT64                Attributes
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  if ((Attributes &
+      (~(EFI_PCI_ATTRIBUTE_MEMORY_WRITE_COMBINE | EFI_PCI_ATTRIBUTE_MEMORY_CACHED))) != 0){
+    return EFI_UNSUPPORTED;
+  }
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((PciIoDevice->Attributes & EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE) != 0) {
+    Attributes |= EFI_PCI_ATTRIBUTE_DUAL_ADDRESS_CYCLE;
+  }
+
+  Status = PciIoDevice->PciRootBridgeIo->AllocateBuffer (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          Type,
+                                          MemoryType,
+                                          Pages,
+                                          HostAddress,
+                                          Attributes
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Pages                 The number of pages to free.
+  @param  HostAddress           The base system memory address of the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoFreeBuffer (
+  IN  EFI_PCI_IO_PROTOCOL   *This,
+  IN  UINTN                 Pages,
+  IN  VOID                  *HostAddress
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Status = PciIoDevice->PciRootBridgeIo->FreeBuffer (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          Pages,
+                                          HostAddress
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Flushes all PCI posted write transactions from a PCI host bridge to system memory.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The PCI posted write transactions were flushed from the PCI host
+                                bridge to system memory.
+  @retval EFI_DEVICE_ERROR      The PCI posted write transactions were not flushed from the PCI
+                                host bridge due to a hardware error.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoFlush (
+  IN  EFI_PCI_IO_PROTOCOL  *This
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Status = PciIoDevice->PciRootBridgeIo->Flush (
+                                           PciIoDevice->PciRootBridgeIo
+                                           );
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Retrieves this PCI controller's current PCI bus number, device number, and function number.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  SegmentNumber         The PCI controller's current PCI segment number.
+  @param  BusNumber             The PCI controller's current PCI bus number.
+  @param  DeviceNumber          The PCI controller's current PCI device number.
+  @param  FunctionNumber        The PCI controller's current PCI function number.
+
+  @retval EFI_SUCCESS           The PCI controller location was returned.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoGetLocation (
+  IN  EFI_PCI_IO_PROTOCOL  *This,
+  OUT UINTN                *Segment,
+  OUT UINTN                *Bus,
+  OUT UINTN                *Device,
+  OUT UINTN                *Function
+  )
+{
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if (Segment == NULL || Bus == NULL || Device == NULL || Function == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Segment  = PciIoDevice->PciRootBridgeIo->SegmentNumber;
+  *Bus      = PciIoDevice->BusNumber;
+  *Device   = PciIoDevice->DeviceNumber;
+  *Function = PciIoDevice->FunctionNumber;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check BAR type for PCI resource.
+
+  @param PciIoDevice   PCI device instance.
+  @param BarIndex      The BAR index of the standard PCI Configuration header to use as the
+                       base address for the memory or I/O operation to perform.
+  @param BarType       Memory or I/O.
+
+  @retval TRUE         Pci device's bar type is same with input BarType.
+  @retval TRUE         Pci device's bar type is not same with input BarType.
+
+**/
+BOOLEAN
+CheckBarType (
+  IN PCI_IO_DEVICE          *PciIoDevice,
+  IN UINT8                  BarIndex,
+  IN PCI_BAR_TYPE           BarType
+  )
+{
+  switch (BarType) {
+
+  case PciBarTypeMem:
+
+    if (PciIoDevice->PciBar[BarIndex].BarType != PciBarTypeMem32  &&
+        PciIoDevice->PciBar[BarIndex].BarType != PciBarTypePMem32 &&
+        PciIoDevice->PciBar[BarIndex].BarType != PciBarTypePMem64 &&
+        PciIoDevice->PciBar[BarIndex].BarType != PciBarTypeMem64    ) {
+      return FALSE;
+    }
+
+    return TRUE;
+
+  case PciBarTypeIo:
+    if (PciIoDevice->PciBar[BarIndex].BarType != PciBarTypeIo32 &&
+        PciIoDevice->PciBar[BarIndex].BarType != PciBarTypeIo16){
+      return FALSE;
+    }
+
+    return TRUE;
+
+  default:
+    break;
+  }
+
+  return FALSE;
+}
+
+/**
+  Set/Disable new attributes to a Root Bridge.
+
+  @param  PciIoDevice  Pci device instance.
+  @param  Attributes   New attribute want to be set.
+  @param  Operation    Set or Disable.
+
+  @retval  EFI_UNSUPPORTED  If root bridge does not support change attribute.
+  @retval  EFI_SUCCESS      Successfully set new attributs.
+
+**/
+EFI_STATUS
+ModifyRootBridgeAttributes (
+  IN  PCI_IO_DEVICE                            *PciIoDevice,
+  IN  UINT64                                   Attributes,
+  IN  EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation
+  )
+{
+  UINT64      PciRootBridgeSupports;
+  UINT64      PciRootBridgeAttributes;
+  UINT64      NewPciRootBridgeAttributes;
+  EFI_STATUS  Status;
+
+  //
+  // Get the current attributes of this PCI device's PCI Root Bridge
+  //
+  Status = PciIoDevice->PciRootBridgeIo->GetAttributes (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          &PciRootBridgeSupports,
+                                          &PciRootBridgeAttributes
+                                          );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Mask off attributes not supported by PCI root bridge.
+  //
+  Attributes &= ~(UINT64)(EFI_PCI_IO_ATTRIBUTE_EMBEDDED_DEVICE |
+                          EFI_PCI_IO_ATTRIBUTE_EMBEDDED_ROM |
+                          EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE);
+
+  //
+  // Record the new attribute of the Root Bridge
+  //
+  if (Operation == EfiPciIoAttributeOperationEnable) {
+    NewPciRootBridgeAttributes = PciRootBridgeAttributes | Attributes;
+  } else {
+    NewPciRootBridgeAttributes = PciRootBridgeAttributes & (~Attributes);
+  }
+
+  //
+  // Call the PCI Root Bridge to attempt to modify the attributes
+  //
+  if ((NewPciRootBridgeAttributes ^ PciRootBridgeAttributes) != 0) {
+
+    Status = PciIoDevice->PciRootBridgeIo->SetAttributes (
+                                            PciIoDevice->PciRootBridgeIo,
+                                            NewPciRootBridgeAttributes,
+                                            NULL,
+                                            NULL
+                                            );
+    if (EFI_ERROR (Status)) {
+      //
+      // The PCI Root Bridge could not modify the attributes, so return the error.
+      //
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  //
+  // Also update the attributes for this Root Bridge structure
+  //
+  PciIoDevice->Attributes = NewPciRootBridgeAttributes;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether this device can be enable/disable to snoop.
+
+  @param PciIoDevice  Pci device instance.
+  @param Operation    Enable/Disable.
+
+  @retval EFI_UNSUPPORTED  Pci device is not GFX device or not support snoop.
+  @retval EFI_SUCCESS      Snoop can be supported.
+
+**/
+EFI_STATUS
+SupportPaletteSnoopAttributes (
+  IN PCI_IO_DEVICE                            *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation
+  )
+{
+  PCI_IO_DEVICE *Temp;
+  UINT16        VGACommand;
+
+  //
+  // Snoop attribute can be only modified by GFX
+  //
+  if (!IS_PCI_GFX (&PciIoDevice->Pci)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get the boot VGA on the same segement
+  //
+  Temp = ActiveVGADeviceOnTheSameSegment (PciIoDevice);
+
+  if (Temp == NULL) {
+    //
+    // If there is no VGA device on the segement, set
+    // this graphics card to decode the palette range
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Check these two agents are on the same path
+  //
+  if (!PciDevicesOnTheSamePath (Temp, PciIoDevice)) {
+    //
+    // they are not on the same path, so snoop can be enabled or disabled
+    //
+    return EFI_SUCCESS;
+  }
+  //
+  // Check if they are on the same bus
+  //
+  if (Temp->Parent == PciIoDevice->Parent) {
+
+    PCI_READ_COMMAND_REGISTER (Temp, &VGACommand);
+
+    //
+    // If they are on the same bus, either one can
+    // be set to snoop, the other set to decode
+    //
+    if ((VGACommand & EFI_PCI_COMMAND_VGA_PALETTE_SNOOP) != 0) {
+      //
+      // VGA has set to snoop, so GFX can be only set to disable snoop
+      //
+      if (Operation == EfiPciIoAttributeOperationEnable) {
+        return EFI_UNSUPPORTED;
+      }
+    } else {
+      //
+      // VGA has disabled to snoop, so GFX can be only enabled
+      //
+      if (Operation == EfiPciIoAttributeOperationDisable) {
+        return EFI_UNSUPPORTED;
+      }
+    }
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // If they are on  the same path but on the different bus
+  // The first agent is set to snoop, the second one set to
+  // decode
+  //
+
+  if (Temp->BusNumber < PciIoDevice->BusNumber) {
+    //
+    // GFX should be set to decode
+    //
+    if (Operation == EfiPciIoAttributeOperationDisable) {
+      PCI_ENABLE_COMMAND_REGISTER (Temp, EFI_PCI_COMMAND_VGA_PALETTE_SNOOP);
+      Temp->Attributes |= EFI_PCI_COMMAND_VGA_PALETTE_SNOOP;
+    } else {
+      return EFI_UNSUPPORTED;
+    }
+
+  } else {
+    //
+    // GFX should be set to snoop
+    //
+    if (Operation == EfiPciIoAttributeOperationEnable) {
+      PCI_DISABLE_COMMAND_REGISTER (Temp, EFI_PCI_COMMAND_VGA_PALETTE_SNOOP);
+      Temp->Attributes &= (~(UINT64)EFI_PCI_COMMAND_VGA_PALETTE_SNOOP);
+    } else {
+      return EFI_UNSUPPORTED;
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Performs an operation on the attributes that this PCI controller supports. The operations include
+  getting the set of supported attributes, retrieving the current attributes, setting the current
+  attributes, enabling attributes, and disabling attributes.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Operation             The operation to perform on the attributes for this PCI controller.
+  @param  Attributes            The mask of attributes that are used for Set, Enable, and Disable
+                                operations.
+  @param  Result                A pointer to the result mask of attributes that are returned for the Get
+                                and Supported operations.
+
+  @retval EFI_SUCCESS           The operation on the PCI controller's attributes was completed.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED       one or more of the bits set in
+                                Attributes are not supported by this PCI controller or one of
+                                its parent bridges when Operation is Set, Enable or Disable.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoAttributes (
+  IN EFI_PCI_IO_PROTOCOL                       * This,
+  IN  EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation,
+  IN  UINT64                                   Attributes,
+  OUT UINT64                                   *Result OPTIONAL
+  )
+{
+  EFI_STATUS    Status;
+
+  PCI_IO_DEVICE *PciIoDevice;
+  PCI_IO_DEVICE *UpStreamBridge;
+  PCI_IO_DEVICE *Temp;
+
+  UINT64        Supports;
+  UINT64        UpStreamAttributes;
+  UINT16        BridgeControl;
+  UINT16        Command;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  switch (Operation) {
+  case EfiPciIoAttributeOperationGet:
+    if (Result == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    *Result = PciIoDevice->Attributes;
+    return EFI_SUCCESS;
+
+  case EfiPciIoAttributeOperationSupported:
+    if (Result == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    *Result = PciIoDevice->Supports;
+    return EFI_SUCCESS;
+
+  case EfiPciIoAttributeOperationSet:
+    Status = PciIoDevice->PciIo.Attributes (
+                                  &(PciIoDevice->PciIo),
+                                  EfiPciIoAttributeOperationEnable,
+                                  Attributes,
+                                  NULL
+                                  );
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+
+    Status = PciIoDevice->PciIo.Attributes (
+                                  &(PciIoDevice->PciIo),
+                                  EfiPciIoAttributeOperationDisable,
+                                  (~Attributes) & (PciIoDevice->Supports),
+                                  NULL
+                                  );
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+
+    return EFI_SUCCESS;
+
+  case EfiPciIoAttributeOperationEnable:
+  case EfiPciIoAttributeOperationDisable:
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Just a trick for ENABLE attribute
+  // EFI_PCI_DEVICE_ENABLE is not defined in UEFI spec, which is the internal usage.
+  // So, this logic doesn't confrom to UEFI spec, which should be removed.
+  // But this trick logic is still kept for some binary drivers that depend on it.
+  //
+  if ((Attributes & EFI_PCI_DEVICE_ENABLE) == EFI_PCI_DEVICE_ENABLE) {
+    Attributes &= (PciIoDevice->Supports);
+
+    //
+    // Raise the EFI_P_PC_ENABLE Status code
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      EFI_IO_BUS_PCI | EFI_P_PC_ENABLE,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  //
+  // Check VGA and VGA16, they can not be set at the same time
+  //
+  if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_IO | EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO)) != 0) {
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_IO_16 | EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16)) != 0) {
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  //
+  // If no attributes can be supported, then return.
+  // Otherwise, set the attributes that it can support.
+  //
+  Supports = (PciIoDevice->Supports) & Attributes;
+  if (Supports != Attributes) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // For Root Bridge, just call RootBridgeIo to set attributes;
+  //
+  if (PciIoDevice->Parent == NULL) {
+    Status = ModifyRootBridgeAttributes (PciIoDevice, Attributes, Operation);
+    return Status;
+  }
+
+  Command       = 0;
+  BridgeControl = 0;
+
+  //
+  // For PPB & P2C, set relevant attribute bits
+  //
+  if (IS_PCI_BRIDGE (&PciIoDevice->Pci) || IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_IO | EFI_PCI_IO_ATTRIBUTE_VGA_IO_16)) != 0) {
+      BridgeControl |= EFI_PCI_BRIDGE_CONTROL_VGA;
+    }
+
+    if ((Attributes & EFI_PCI_IO_ATTRIBUTE_ISA_IO) != 0) {
+      BridgeControl |= EFI_PCI_BRIDGE_CONTROL_ISA;
+    }
+
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO | EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16)) != 0) {
+      Command |= EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO;
+    }
+
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16 | EFI_PCI_IO_ATTRIBUTE_VGA_IO_16)) != 0) {
+      BridgeControl |= EFI_PCI_BRIDGE_CONTROL_VGA_16;
+    }
+
+  } else {
+    //
+    // Do with the attributes on VGA
+    // Only for VGA's legacy resource, we just can enable once.
+    //
+    if ((Attributes &
+        (EFI_PCI_IO_ATTRIBUTE_VGA_IO    |
+         EFI_PCI_IO_ATTRIBUTE_VGA_IO_16 |
+         EFI_PCI_IO_ATTRIBUTE_VGA_MEMORY)) != 0) {
+      //
+      // Check if a VGA has been enabled before enabling a new one
+      //
+      if (Operation == EfiPciIoAttributeOperationEnable) {
+        //
+        // Check if there have been an active VGA device on the same segment
+        //
+        Temp = ActiveVGADeviceOnTheSameSegment (PciIoDevice);
+        if (Temp != NULL && Temp != PciIoDevice) {
+          //
+          // An active VGA has been detected, so can not enable another
+          //
+          return EFI_UNSUPPORTED;
+        }
+      }
+    }
+
+    //
+    // Do with the attributes on GFX
+    //
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO | EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16)) != 0) {
+
+      if (Operation == EfiPciIoAttributeOperationEnable) {
+        //
+        // Check if snoop can be enabled in current configuration
+        //
+        Status = SupportPaletteSnoopAttributes (PciIoDevice, Operation);
+
+        if (EFI_ERROR (Status)) {
+
+          //
+          // Enable operation is forbidden, so mask the bit in attributes
+          // so as to keep consistent with the actual Status
+          //
+          // Attributes &= (~EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO);
+          //
+          //
+          //
+          return EFI_UNSUPPORTED;
+
+        }
+      }
+
+      //
+      // It can be supported, so get ready to set the bit
+      //
+      Command |= EFI_PCI_COMMAND_VGA_PALETTE_SNOOP;
+    }
+  }
+
+  if ((Attributes & EFI_PCI_IO_ATTRIBUTE_IO) != 0) {
+    Command |= EFI_PCI_COMMAND_IO_SPACE;
+  }
+
+  if ((Attributes & EFI_PCI_IO_ATTRIBUTE_MEMORY) != 0) {
+    Command |= EFI_PCI_COMMAND_MEMORY_SPACE;
+  }
+
+  if ((Attributes & EFI_PCI_IO_ATTRIBUTE_BUS_MASTER) != 0) {
+    Command |= EFI_PCI_COMMAND_BUS_MASTER;
+  }
+  //
+  // The upstream bridge should be also set to revelant attribute
+  // expect for IO, Mem and BusMaster
+  //
+  UpStreamAttributes = Attributes &
+                       (~(EFI_PCI_IO_ATTRIBUTE_IO     |
+                          EFI_PCI_IO_ATTRIBUTE_MEMORY |
+                          EFI_PCI_IO_ATTRIBUTE_BUS_MASTER
+                          )
+                        );
+  UpStreamBridge = PciIoDevice->Parent;
+
+  if (Operation == EfiPciIoAttributeOperationEnable) {
+    //
+    // Enable relevant attributes to command register and bridge control register
+    //
+    Status = PCI_ENABLE_COMMAND_REGISTER (PciIoDevice, Command);
+    if (BridgeControl != 0) {
+      Status = PCI_ENABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, BridgeControl);
+    }
+
+    PciIoDevice->Attributes |= Attributes;
+
+    //
+    // Enable attributes of the upstream bridge
+    //
+    Status = UpStreamBridge->PciIo.Attributes (
+                                    &(UpStreamBridge->PciIo),
+                                    EfiPciIoAttributeOperationEnable,
+                                    UpStreamAttributes,
+                                    NULL
+                                    );
+  } else {
+
+    //
+    // Disable relevant attributes to command register and bridge control register
+    //
+    Status = PCI_DISABLE_COMMAND_REGISTER (PciIoDevice, Command);
+    if (BridgeControl != 0) {
+      Status = PCI_DISABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, BridgeControl);
+    }
+
+    PciIoDevice->Attributes &= (~Attributes);
+    Status = EFI_SUCCESS;
+
+  }
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Gets the attributes that this PCI controller supports setting on a BAR using
+  SetBarAttributes(), and retrieves the list of resource descriptors for a BAR.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for resource range. The legal range for this field is 0..5.
+  @param  Supports              A pointer to the mask of attributes that this PCI controller supports
+                                setting for this BAR with SetBarAttributes().
+  @param  Resources             A pointer to the ACPI 2.0 resource descriptors that describe the current
+                                configuration of this BAR of the PCI controller.
+
+  @retval EFI_SUCCESS           If Supports is not NULL, then the attributes that the PCI
+                                controller supports are returned in Supports. If Resources
+                                is not NULL, then the ACPI 2.0 resource descriptors that the PCI
+                                controller is currently using are returned in Resources.
+  @retval EFI_INVALID_PARAMETER Both Supports and Attributes are NULL.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources available to allocate
+                                Resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoGetBarAttributes (
+  IN EFI_PCI_IO_PROTOCOL             * This,
+  IN  UINT8                          BarIndex,
+  OUT UINT64                         *Supports, OPTIONAL
+  OUT VOID                           **Resources OPTIONAL
+  )
+{
+  UINT8                             *Configuration;
+  PCI_IO_DEVICE                     *PciIoDevice;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *AddressSpace;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if (Supports == NULL && Resources == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((BarIndex >= PCI_MAX_BAR) || (PciIoDevice->PciBar[BarIndex].BarType == PciBarTypeUnknown)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // This driver does not support modifications to the WRITE_COMBINE or
+  // CACHED attributes for BAR ranges.
+  //
+  if (Supports != NULL) {
+    *Supports = PciIoDevice->Supports & EFI_PCI_IO_ATTRIBUTE_MEMORY_CACHED & EFI_PCI_IO_ATTRIBUTE_MEMORY_WRITE_COMBINE;
+  }
+
+  if (Resources != NULL) {
+    Configuration = AllocateZeroPool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+    if (Configuration == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    AddressSpace = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration;
+
+    AddressSpace->Desc         = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+    AddressSpace->Len          = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+
+    AddressSpace->AddrRangeMin = PciIoDevice->PciBar[BarIndex].BaseAddress;
+    AddressSpace->AddrLen      = PciIoDevice->PciBar[BarIndex].Length;
+    AddressSpace->AddrRangeMax = PciIoDevice->PciBar[BarIndex].Alignment;
+
+    switch (PciIoDevice->PciBar[BarIndex].BarType) {
+    case PciBarTypeIo16:
+    case PciBarTypeIo32:
+      //
+      // Io
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_IO;
+      break;
+
+    case PciBarTypeMem32:
+      //
+      // Mem
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // 32 bit
+      //
+      AddressSpace->AddrSpaceGranularity = 32;
+      break;
+
+    case PciBarTypePMem32:
+      //
+      // Mem
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // prefechable
+      //
+      AddressSpace->SpecificFlag = 0x6;
+      //
+      // 32 bit
+      //
+      AddressSpace->AddrSpaceGranularity = 32;
+      break;
+
+    case PciBarTypeMem64:
+      //
+      // Mem
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // 64 bit
+      //
+      AddressSpace->AddrSpaceGranularity = 64;
+      break;
+
+    case PciBarTypePMem64:
+      //
+      // Mem
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // prefechable
+      //
+      AddressSpace->SpecificFlag = 0x6;
+      //
+      // 64 bit
+      //
+      AddressSpace->AddrSpaceGranularity = 64;
+      break;
+
+    default:
+      break;
+    }
+
+    //
+    // put the checksum
+    //
+    End           = (EFI_ACPI_END_TAG_DESCRIPTOR *) (AddressSpace + 1);
+    End->Desc     = ACPI_END_TAG_DESCRIPTOR;
+    End->Checksum = 0;
+
+    *Resources    = Configuration;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets the attributes for a range of a BAR on a PCI controller.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Attributes            The mask of attributes to set for the resource range specified by
+                                BarIndex, Offset, and Length.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for resource range. The legal range for this field is 0..5.
+  @param  Offset                A pointer to the BAR relative base address of the resource range to be
+                                modified by the attributes specified by Attributes.
+  @param  Length                A pointer to the length of the resource range to be modified by the
+                                attributes specified by Attributes.
+
+  @retval EFI_SUCCESS           The set of attributes specified by Attributes for the resource
+                                range specified by BarIndex, Offset, and Length were
+                                set on the PCI controller, and the actual resource range is returned
+                                in Offset and Length.
+  @retval EFI_INVALID_PARAMETER Offset or Length is NULL.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to set the attributes on the
+                                resource range specified by BarIndex, Offset, and
+                                Length.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoSetBarAttributes (
+  IN EFI_PCI_IO_PROTOCOL              *This,
+  IN     UINT64                       Attributes,
+  IN     UINT8                        BarIndex,
+  IN OUT UINT64                       *Offset,
+  IN OUT UINT64                       *Length
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+  UINT64        NonRelativeOffset;
+  UINT64        Supports;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  //
+  // Make sure Offset and Length are not NULL
+  //
+  if (Offset == NULL || Length == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (PciIoDevice->PciBar[BarIndex].BarType == PciBarTypeUnknown) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // This driver does not support setting the WRITE_COMBINE or the CACHED attributes.
+  // If Attributes is not 0, then return EFI_UNSUPPORTED.
+  //
+  Supports = PciIoDevice->Supports & EFI_PCI_IO_ATTRIBUTE_MEMORY_CACHED & EFI_PCI_IO_ATTRIBUTE_MEMORY_WRITE_COMBINE;
+
+  if (Attributes != (Attributes & Supports)) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Attributes must be supported.  Make sure the BAR range describd by BarIndex, Offset, and
+  // Length are valid for this PCI device.
+  //
+  NonRelativeOffset = *Offset;
+  Status = PciIoVerifyBarAccess (
+            PciIoDevice,
+            BarIndex,
+            PciBarTypeMem,
+            EfiPciIoWidthUint8,
+            (UINT32) *Length,
+            &NonRelativeOffset
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Program parent bridge's attribute recurrently.
+
+  @param PciIoDevice  Child Pci device instance
+  @param Operation    The operation to perform on the attributes for this PCI controller.
+  @param Attributes   The mask of attributes that are used for Set, Enable, and Disable
+                      operations.
+
+  @retval EFI_SUCCESS           The operation on the PCI controller's attributes was completed.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED       one or more of the bits set in
+                                Attributes are not supported by this PCI controller or one of
+                                its parent bridges when Operation is Set, Enable or Disable.
+
+**/
+EFI_STATUS
+UpStreamBridgesAttributes (
+  IN PCI_IO_DEVICE                            *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation,
+  IN UINT64                                   Attributes
+  )
+{
+  PCI_IO_DEVICE       *Parent;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  Parent = PciIoDevice->Parent;
+
+  while (Parent != NULL && IS_PCI_BRIDGE (&Parent->Pci)) {
+
+    //
+    // Get the PciIo Protocol
+    //
+    PciIo = &Parent->PciIo;
+
+    PciIo->Attributes (PciIo, Operation, Attributes, NULL);
+
+    Parent = Parent->Parent;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Test whether two Pci devices has same parent bridge.
+
+  @param PciDevice1  The first pci device for testing.
+  @param PciDevice2  The second pci device for testing.
+
+  @retval TRUE       Two Pci device has the same parent bridge.
+  @retval FALSE      Two Pci device has not the same parent bridge.
+
+**/
+BOOLEAN
+PciDevicesOnTheSamePath (
+  IN PCI_IO_DEVICE        *PciDevice1,
+  IN PCI_IO_DEVICE        *PciDevice2
+  )
+{
+  BOOLEAN   Existed1;
+  BOOLEAN   Existed2;
+
+  if (PciDevice1->Parent == PciDevice2->Parent) {
+    return TRUE;
+  }
+
+  Existed1 = PciDeviceExisted (PciDevice1->Parent, PciDevice2);
+  Existed2 = PciDeviceExisted (PciDevice2->Parent, PciDevice1);
+
+  return (BOOLEAN) (Existed1 || Existed2);
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.h
new file mode 100644
index 0000000000..ac2def5acf
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.h
@@ -0,0 +1,687 @@
+/** @file
+  EFI PCI IO protocol functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_IO_PROTOCOL_H_
+#define _EFI_PCI_IO_PROTOCOL_H_
+
+/**
+  Initializes a PCI I/O Instance.
+
+  @param PciIoDevice    Pci device instance.
+
+**/
+VOID
+InitializePciIoInstance (
+  IN PCI_IO_DEVICE               *PciIoDevice
+  );
+
+/**
+  Verifies access to a PCI Base Address Register (BAR).
+
+  @param PciIoDevice  Pci device instance.
+  @param BarIndex     The BAR index of the standard PCI Configuration header to use as the
+                      base address for the memory or I/O operation to perform.
+  @param Type         Operation type could be memory or I/O.
+  @param Width        Signifies the width of the memory or I/O operations.
+  @param Count        The number of memory or I/O operations to perform.
+  @param Offset       The offset within the PCI configuration space for the PCI controller.
+
+  @retval EFI_INVALID_PARAMETER Invalid Width/BarIndex or Bar type.
+  @retval EFI_SUCCESS           Successfully verified.
+
+**/
+EFI_STATUS
+PciIoVerifyBarAccess (
+  IN PCI_IO_DEVICE                   *PciIoDevice,
+  IN UINT8                           BarIndex,
+  IN PCI_BAR_TYPE                    Type,
+  IN IN EFI_PCI_IO_PROTOCOL_WIDTH    Width,
+  IN IN UINTN                        Count,
+  IN UINT64                          *Offset
+  );
+
+/**
+  Verifies access to a PCI Configuration Header.
+
+  @param PciIoDevice  Pci device instance.
+  @param Width        Signifies the width of the memory or I/O operations.
+  @param Count        The number of memory or I/O operations to perform.
+  @param Offset       The offset within the PCI configuration space for the PCI controller.
+
+  @retval EFI_INVALID_PARAMETER  Invalid Width
+  @retval EFI_UNSUPPORTED        Offset overflowed.
+  @retval EFI_SUCCESS            Successfully verified.
+
+**/
+EFI_STATUS
+PciIoVerifyConfigAccess (
+  IN PCI_IO_DEVICE              *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN UINTN                      Count,
+  IN UINT64                     *Offset
+  );
+
+/**
+  Reads from the memory space of a PCI controller. Returns either when the polling exit criteria is
+  satisfied or after a defined duration.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory operation.
+  @param  Mask                  Mask used for the polling criteria.
+  @param  Value                 The comparison value used for the polling exit criteria.
+  @param  Delay                 The number of 100 ns units to poll.
+  @param  Result                Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS           The last data returned from the access matched the poll exit criteria.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       Offset is not valid for the BarIndex of this PCI controller.
+  @retval EFI_TIMEOUT           Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoPollMem (
+  IN  EFI_PCI_IO_PROTOCOL        *This,
+  IN  EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT8                      BarIndex,
+  IN  UINT64                     Offset,
+  IN  UINT64                     Mask,
+  IN  UINT64                     Value,
+  IN  UINT64                     Delay,
+  OUT UINT64                     *Result
+  );
+
+/**
+  Reads from the memory space of a PCI controller. Returns either when the polling exit criteria is
+  satisfied or after a defined duration.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory operation.
+  @param  Mask                  Mask used for the polling criteria.
+  @param  Value                 The comparison value used for the polling exit criteria.
+  @param  Delay                 The number of 100 ns units to poll.
+  @param  Result                Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS           The last data returned from the access matched the poll exit criteria.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       Offset is not valid for the BarIndex of this PCI controller.
+  @retval EFI_TIMEOUT           Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoPollIo (
+  IN  EFI_PCI_IO_PROTOCOL        *This,
+  IN  EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT8                      BarIndex,
+  IN  UINT64                     Offset,
+  IN  UINT64                     Mask,
+  IN  UINT64                     Value,
+  IN  UINT64                     Delay,
+  OUT UINT64                     *Result
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMemRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMemWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoIoRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoIoWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in PCI configuration space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the PCI configuration space for the PCI controller.
+  @param  Count                 The number of PCI configuration operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI configuration header of the PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER Buffer is NULL or Width is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoConfigRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT32                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in PCI configuration space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the PCI configuration space for the PCI controller.
+  @param  Count                 The number of PCI configuration operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI configuration header of the PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER Buffer is NULL or Width is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoConfigWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT32                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enables a PCI driver to copy one region of PCI memory space to another region of PCI
+  memory space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  DestBarIndex          The BAR index in the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  DestOffset            The destination offset within the BAR specified by DestBarIndex to
+                                start the memory writes for the copy operation.
+  @param  SrcBarIndex           The BAR index in the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  SrcOffset             The source offset within the BAR specified by SrcBarIndex to start
+                                the memory reads for the copy operation.
+  @param  Count                 The number of memory operations to perform. Bytes moved is Width
+                                size * Count, starting at DestOffset and SrcOffset.
+
+  @retval EFI_SUCCESS           The data was copied from one memory region to another memory region.
+  @retval EFI_UNSUPPORTED       DestBarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       SrcBarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by DestOffset, Width, and Count
+                                is not valid for the PCI BAR specified by DestBarIndex.
+  @retval EFI_UNSUPPORTED       The address range specified by SrcOffset, Width, and Count is
+                                not valid for the PCI BAR specified by SrcBarIndex.
+  @retval EFI_INVALID_PARAMETER Width is invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoCopyMem (
+  IN EFI_PCI_IO_PROTOCOL              *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH    Width,
+  IN     UINT8                        DestBarIndex,
+  IN     UINT64                       DestOffset,
+  IN     UINT8                        SrcBarIndex,
+  IN     UINT64                       SrcOffset,
+  IN     UINTN                        Count
+  );
+
+/**
+  Provides the PCI controller-specific addresses needed to access system memory.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Operation             Indicates if the bus master is going to read or write to system memory.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  NumberOfBytes         On input the number of bytes to map. On output the number of bytes
+                                that were mapped.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param  Mapping               A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMap (
+  IN     EFI_PCI_IO_PROTOCOL            *This,
+  IN     EFI_PCI_IO_PROTOCOL_OPERATION  Operation,
+  IN     VOID                           *HostAddress,
+  IN OUT UINTN                          *NumberOfBytes,
+  OUT    EFI_PHYSICAL_ADDRESS           *DeviceAddress,
+  OUT    VOID                           **Mapping
+  );
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Mapping               The mapping value returned from Map().
+
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoUnmap (
+  IN  EFI_PCI_IO_PROTOCOL  *This,
+  IN  VOID                 *Mapping
+  );
+
+/**
+  Allocates pages that are suitable for an EfiPciIoOperationBusMasterCommonBuffer
+  mapping.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.
+  @param  Pages                 The number of pages to allocate.
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoAllocateBuffer (
+  IN  EFI_PCI_IO_PROTOCOL   *This,
+  IN  EFI_ALLOCATE_TYPE     Type,
+  IN  EFI_MEMORY_TYPE       MemoryType,
+  IN  UINTN                 Pages,
+  OUT VOID                  **HostAddress,
+  IN  UINT64                Attributes
+  );
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Pages                 The number of pages to free.
+  @param  HostAddress           The base system memory address of the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoFreeBuffer (
+  IN  EFI_PCI_IO_PROTOCOL   *This,
+  IN  UINTN                 Pages,
+  IN  VOID                  *HostAddress
+  );
+
+/**
+  Flushes all PCI posted write transactions from a PCI host bridge to system memory.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The PCI posted write transactions were flushed from the PCI host
+                                bridge to system memory.
+  @retval EFI_DEVICE_ERROR      The PCI posted write transactions were not flushed from the PCI
+                                host bridge due to a hardware error.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoFlush (
+  IN  EFI_PCI_IO_PROTOCOL  *This
+  );
+
+/**
+  Retrieves this PCI controller's current PCI bus number, device number, and function number.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  SegmentNumber         The PCI controller's current PCI segment number.
+  @param  BusNumber             The PCI controller's current PCI bus number.
+  @param  DeviceNumber          The PCI controller's current PCI device number.
+  @param  FunctionNumber        The PCI controller's current PCI function number.
+
+  @retval EFI_SUCCESS           The PCI controller location was returned.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoGetLocation (
+  IN  EFI_PCI_IO_PROTOCOL  *This,
+  OUT UINTN                *Segment,
+  OUT UINTN                *Bus,
+  OUT UINTN                *Device,
+  OUT UINTN                *Function
+  );
+
+/**
+  Check BAR type for PCI resource.
+
+  @param PciIoDevice   PCI device instance.
+  @param BarIndex      The BAR index of the standard PCI Configuration header to use as the
+                       base address for the memory or I/O operation to perform.
+  @param BarType       Memory or I/O.
+
+  @retval TRUE         Pci device's bar type is same with input BarType.
+  @retval TRUE         Pci device's bar type is not same with input BarType.
+
+**/
+BOOLEAN
+CheckBarType (
+  IN PCI_IO_DEVICE          *PciIoDevice,
+  IN UINT8                  BarIndex,
+  IN PCI_BAR_TYPE           BarType
+  );
+
+/**
+  Set/Disable new attributes to a Root Bridge.
+
+  @param  PciIoDevice  Pci device instance.
+  @param  Attributes   New attribute want to be set.
+  @param  Operation    Set or Disable.
+
+  @retval  EFI_UNSUPPORTED  If root bridge does not support change attribute.
+  @retval  EFI_SUCCESS      Successfully set new attributs.
+
+**/
+EFI_STATUS
+ModifyRootBridgeAttributes (
+  IN  PCI_IO_DEVICE                            *PciIoDevice,
+  IN  UINT64                                   Attributes,
+  IN  EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation
+  );
+
+/**
+  Check whether this device can be enable/disable to snoop.
+
+  @param PciIoDevice  Pci device instance.
+  @param Operation    Enable/Disable.
+
+  @retval EFI_UNSUPPORTED  Pci device is not GFX device or not support snoop.
+  @retval EFI_SUCCESS      Snoop can be supported.
+
+**/
+EFI_STATUS
+SupportPaletteSnoopAttributes (
+  IN PCI_IO_DEVICE                            *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation
+  );
+
+/**
+  Performs an operation on the attributes that this PCI controller supports. The operations include
+  getting the set of supported attributes, retrieving the current attributes, setting the current
+  attributes, enabling attributes, and disabling attributes.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Operation             The operation to perform on the attributes for this PCI controller.
+  @param  Attributes            The mask of attributes that are used for Set, Enable, and Disable
+                                operations.
+  @param  Result                A pointer to the result mask of attributes that are returned for the Get
+                                and Supported operations.
+
+  @retval EFI_SUCCESS           The operation on the PCI controller's attributes was completed.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED       one or more of the bits set in
+                                Attributes are not supported by this PCI controller or one of
+                                its parent bridges when Operation is Set, Enable or Disable.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoAttributes (
+  IN EFI_PCI_IO_PROTOCOL                       * This,
+  IN  EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation,
+  IN  UINT64                                   Attributes,
+  OUT UINT64                                   *Result OPTIONAL
+  );
+
+/**
+  Gets the attributes that this PCI controller supports setting on a BAR using
+  SetBarAttributes(), and retrieves the list of resource descriptors for a BAR.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for resource range. The legal range for this field is 0..5.
+  @param  Supports              A pointer to the mask of attributes that this PCI controller supports
+                                setting for this BAR with SetBarAttributes().
+  @param  Resources             A pointer to the ACPI 2.0 resource descriptors that describe the current
+                                configuration of this BAR of the PCI controller.
+
+  @retval EFI_SUCCESS           If Supports is not NULL, then the attributes that the PCI
+                                controller supports are returned in Supports. If Resources
+                                is not NULL, then the ACPI 2.0 resource descriptors that the PCI
+                                controller is currently using are returned in Resources.
+  @retval EFI_INVALID_PARAMETER Both Supports and Attributes are NULL.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources available to allocate
+                                Resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoGetBarAttributes (
+  IN EFI_PCI_IO_PROTOCOL             * This,
+  IN  UINT8                          BarIndex,
+  OUT UINT64                         *Supports, OPTIONAL
+  OUT VOID                           **Resources OPTIONAL
+  );
+
+/**
+  Sets the attributes for a range of a BAR on a PCI controller.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Attributes            The mask of attributes to set for the resource range specified by
+                                BarIndex, Offset, and Length.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for resource range. The legal range for this field is 0..5.
+  @param  Offset                A pointer to the BAR relative base address of the resource range to be
+                                modified by the attributes specified by Attributes.
+  @param  Length                A pointer to the length of the resource range to be modified by the
+                                attributes specified by Attributes.
+
+  @retval EFI_SUCCESS           The set of attributes specified by Attributes for the resource
+                                range specified by BarIndex, Offset, and Length were
+                                set on the PCI controller, and the actual resource range is returned
+                                in Offset and Length.
+  @retval EFI_INVALID_PARAMETER Offset or Length is NULL.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to set the attributes on the
+                                resource range specified by BarIndex, Offset, and
+                                Length.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoSetBarAttributes (
+  IN EFI_PCI_IO_PROTOCOL              *This,
+  IN     UINT64                       Attributes,
+  IN     UINT8                        BarIndex,
+  IN OUT UINT64                       *Offset,
+  IN OUT UINT64                       *Length
+  );
+
+/**
+  Program parent bridge's attribute recurrently.
+
+  @param PciIoDevice  Child Pci device instance
+  @param Operation    The operation to perform on the attributes for this PCI controller.
+  @param Attributes   The mask of attributes that are used for Set, Enable, and Disable
+                      operations.
+
+  @retval EFI_SUCCESS           The operation on the PCI controller's attributes was completed.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED       one or more of the bits set in
+                                Attributes are not supported by this PCI controller or one of
+                                its parent bridges when Operation is Set, Enable or Disable.
+
+**/
+EFI_STATUS
+UpStreamBridgesAttributes (
+  IN PCI_IO_DEVICE                            *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation,
+  IN UINT64                                   Attributes
+  );
+
+/**
+  Test whether two Pci devices has same parent bridge.
+
+  @param PciDevice1  The first pci device for testing.
+  @param PciDevice2  The second pci device for testing.
+
+  @retval TRUE       Two Pci device has the same parent bridge.
+  @retval FALSE      Two Pci device has not the same parent bridge.
+
+**/
+BOOLEAN
+PciDevicesOnTheSamePath (
+  IN PCI_IO_DEVICE        *PciDevice1,
+  IN PCI_IO_DEVICE        *PciDevice2
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.c
new file mode 100644
index 0000000000..f3407bcb63
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.c
@@ -0,0 +1,1649 @@
+/** @file
+  Internal library implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2016, Intel Corporation. All rights reserved.<BR>
+(C) Copyright 2015 Hewlett Packard Enterprise Development LP<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 "PciBus.h"
+
+GLOBAL_REMOVE_IF_UNREFERENCED
+CHAR16 *mBarTypeStr[] = {
+  L"Unknow",
+  L"  Io16",
+  L"  Io32",
+  L" Mem32",
+  L"PMem32",
+  L" Mem64",
+  L"PMem64",
+  L"    Io",
+  L"   Mem",
+  L"Unknow"
+  };
+
+/**
+  Retrieve the PCI Card device BAR information via PciIo interface.
+
+  @param PciIoDevice        PCI Card device instance.
+
+**/
+VOID
+GetBackPcCardBar (
+  IN  PCI_IO_DEVICE                  *PciIoDevice
+  )
+{
+  UINT32  Address;
+
+  if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    return;
+  }
+
+  //
+  // Read PciBar information from the bar register
+  //
+  if (!gFullEnumeration) {
+    Address = 0;
+    PciIoDevice->PciIo.Pci.Read (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoWidthUint32,
+                             PCI_CARD_MEMORY_BASE_0,
+                             1,
+                             &Address
+                             );
+
+    (PciIoDevice->PciBar)[P2C_MEM_1].BaseAddress  = (UINT64) (Address);
+    (PciIoDevice->PciBar)[P2C_MEM_1].Length       = 0x2000000;
+    (PciIoDevice->PciBar)[P2C_MEM_1].BarType      = PciBarTypeMem32;
+
+    Address = 0;
+    PciIoDevice->PciIo.Pci.Read (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoWidthUint32,
+                             PCI_CARD_MEMORY_BASE_1,
+                             1,
+                             &Address
+                             );
+    (PciIoDevice->PciBar)[P2C_MEM_2].BaseAddress  = (UINT64) (Address);
+    (PciIoDevice->PciBar)[P2C_MEM_2].Length       = 0x2000000;
+    (PciIoDevice->PciBar)[P2C_MEM_2].BarType      = PciBarTypePMem32;
+
+    Address = 0;
+    PciIoDevice->PciIo.Pci.Read (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoWidthUint32,
+                             PCI_CARD_IO_BASE_0_LOWER,
+                             1,
+                             &Address
+                             );
+    (PciIoDevice->PciBar)[P2C_IO_1].BaseAddress = (UINT64) (Address);
+    (PciIoDevice->PciBar)[P2C_IO_1].Length      = 0x100;
+    (PciIoDevice->PciBar)[P2C_IO_1].BarType     = PciBarTypeIo16;
+
+    Address = 0;
+    PciIoDevice->PciIo.Pci.Read (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoWidthUint32,
+                             PCI_CARD_IO_BASE_1_LOWER,
+                             1,
+                             &Address
+                             );
+    (PciIoDevice->PciBar)[P2C_IO_2].BaseAddress = (UINT64) (Address);
+    (PciIoDevice->PciBar)[P2C_IO_2].Length      = 0x100;
+    (PciIoDevice->PciBar)[P2C_IO_2].BarType     = PciBarTypeIo16;
+
+  }
+
+  if (gPciHotPlugInit != NULL && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    GetResourcePaddingForHpb (PciIoDevice);
+  }
+}
+
+/**
+  Remove rejected pci device from specific root bridge
+  handle.
+
+  @param RootBridgeHandle  Specific parent root bridge handle.
+  @param Bridge            Bridge device instance.
+
+**/
+VOID
+RemoveRejectedPciDevices (
+  IN EFI_HANDLE        RootBridgeHandle,
+  IN PCI_IO_DEVICE     *Bridge
+  )
+{
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+  LIST_ENTRY      *LastLink;
+
+  if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    return;
+  }
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (IS_PCI_BRIDGE (&Temp->Pci)) {
+      //
+      // Remove rejected devices recusively
+      //
+      RemoveRejectedPciDevices (RootBridgeHandle, Temp);
+    } else {
+      //
+      // Skip rejection for all PPBs, while detect rejection for others
+      //
+      if (IsPciDeviceRejected (Temp)) {
+
+        //
+        // For P2C, remove all devices on it
+        //
+        if (!IsListEmpty (&Temp->ChildList)) {
+          RemoveAllPciDeviceOnBridge (RootBridgeHandle, Temp);
+        }
+
+        //
+        // Finally remove itself
+        //
+        LastLink = CurrentLink->BackLink;
+        RemoveEntryList (CurrentLink);
+        FreePciDevice (Temp);
+
+        CurrentLink = LastLink;
+      }
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+}
+
+/**
+  Dump the resourc map of the bridge device.
+
+  @param[in] BridgeResource   Resource descriptor of the bridge device.
+**/
+VOID
+DumpBridgeResource (
+  IN PCI_RESOURCE_NODE     *BridgeResource
+  )
+{
+  LIST_ENTRY               *Link;
+  PCI_RESOURCE_NODE        *Resource;
+  PCI_BAR                  *Bar;
+
+  if ((BridgeResource != NULL) && (BridgeResource->Length != 0)) {
+    DEBUG ((
+      EFI_D_INFO, "Type = %s; Base = 0x%lx;\tLength = 0x%lx;\tAlignment = 0x%lx\n",
+      mBarTypeStr[MIN (BridgeResource->ResType, PciBarTypeMaxType)],
+      BridgeResource->PciDev->PciBar[BridgeResource->Bar].BaseAddress,
+      BridgeResource->Length, BridgeResource->Alignment
+      ));
+    for ( Link = GetFirstNode (&BridgeResource->ChildList)
+        ; !IsNull (&BridgeResource->ChildList, Link)
+        ; Link = GetNextNode (&BridgeResource->ChildList, Link)
+        ) {
+      Resource = RESOURCE_NODE_FROM_LINK (Link);
+      if (Resource->ResourceUsage == PciResUsageTypical) {
+        Bar = Resource->Virtual ? Resource->PciDev->VfPciBar : Resource->PciDev->PciBar;
+        DEBUG ((
+          EFI_D_INFO, "   Base = 0x%lx;\tLength = 0x%lx;\tAlignment = 0x%lx;\tOwner = %s [%02x|%02x|%02x:",
+          Bar[Resource->Bar].BaseAddress, Resource->Length, Resource->Alignment,
+          IS_PCI_BRIDGE (&Resource->PciDev->Pci)     ? L"PPB" :
+          IS_CARDBUS_BRIDGE (&Resource->PciDev->Pci) ? L"P2C" :
+                                                       L"PCI",
+          Resource->PciDev->BusNumber, Resource->PciDev->DeviceNumber,
+          Resource->PciDev->FunctionNumber
+          ));
+
+        if ((!IS_PCI_BRIDGE (&Resource->PciDev->Pci) && !IS_CARDBUS_BRIDGE (&Resource->PciDev->Pci)) ||
+            (IS_PCI_BRIDGE (&Resource->PciDev->Pci) && (Resource->Bar < PPB_IO_RANGE)) ||
+            (IS_CARDBUS_BRIDGE (&Resource->PciDev->Pci) && (Resource->Bar < P2C_MEM_1))
+            ) {
+          //
+          // The resource requirement comes from the device itself.
+          //
+          DEBUG ((EFI_D_INFO, "%02x]", Bar[Resource->Bar].Offset));
+        } else {
+          //
+          // The resource requirement comes from the subordinate devices.
+          //
+          DEBUG ((EFI_D_INFO, "**]"));
+        }
+      } else {
+        DEBUG ((EFI_D_INFO, "   Base = Padding;\tLength = 0x%lx;\tAlignment = 0x%lx", Resource->Length, Resource->Alignment));
+      }
+      if (BridgeResource->ResType != Resource->ResType) {
+        DEBUG ((EFI_D_INFO, "; Type = %s", mBarTypeStr[MIN (Resource->ResType, PciBarTypeMaxType)]));
+      }
+      DEBUG ((EFI_D_INFO, "\n"));
+    }
+  }
+}
+
+/**
+  Find the corresponding resource node for the Device in child list of BridgeResource.
+  
+  @param[in]  Device          Pointer to PCI_IO_DEVICE.
+  @param[in]  BridgeResource  Pointer to PCI_RESOURCE_NODE.
+  @param[out] DeviceResources Pointer to a buffer to receive resources for the Device.
+  
+  @return Count of the resource descriptors returned.
+**/
+UINTN
+FindResourceNode (
+  IN  PCI_IO_DEVICE     *Device,
+  IN  PCI_RESOURCE_NODE *BridgeResource,
+  OUT PCI_RESOURCE_NODE **DeviceResources OPTIONAL
+  )
+{
+  LIST_ENTRY               *Link;
+  PCI_RESOURCE_NODE        *Resource;
+  UINTN                    Count;
+
+  Count = 0;
+  for ( Link = BridgeResource->ChildList.ForwardLink
+      ; Link != &BridgeResource->ChildList
+      ; Link = Link->ForwardLink
+      ) {
+    Resource = RESOURCE_NODE_FROM_LINK (Link);
+    if (Resource->PciDev == Device) {
+      if (DeviceResources != NULL) {
+        DeviceResources[Count] = Resource;
+      }
+      Count++;
+    }
+  }
+
+  return Count;
+}
+
+/**
+  Dump the resource map of all the devices under Bridge.
+  
+  @param[in] Bridge        Bridge device instance.
+  @param[in] Resources     Resource descriptors for the bridge device.
+  @param[in] ResourceCount Count of resource descriptors.
+**/
+VOID
+DumpResourceMap (
+  IN PCI_IO_DEVICE     *Bridge,
+  IN PCI_RESOURCE_NODE **Resources,
+  IN UINTN             ResourceCount
+  )
+{
+  EFI_STATUS           Status;
+  LIST_ENTRY           *Link;
+  PCI_IO_DEVICE        *Device;
+  UINTN                Index;
+  CHAR16               *Str;
+  PCI_RESOURCE_NODE    **ChildResources;
+  UINTN                ChildResourceCount;
+
+  DEBUG ((EFI_D_INFO, "PciBus: Resource Map for "));
+
+  Status = gBS->OpenProtocol (
+                  Bridge->Handle,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  NULL,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((
+      EFI_D_INFO, "Bridge [%02x|%02x|%02x]\n",
+      Bridge->BusNumber, Bridge->DeviceNumber, Bridge->FunctionNumber
+      ));
+  } else {
+    Str = ConvertDevicePathToText (
+            DevicePathFromHandle (Bridge->Handle),
+            FALSE,
+            FALSE
+            );
+    DEBUG ((EFI_D_INFO, "Root Bridge %s\n", Str != NULL ? Str : L""));
+    if (Str != NULL) {
+      FreePool (Str);
+    }
+  }
+
+  for (Index = 0; Index < ResourceCount; Index++) {
+    DumpBridgeResource (Resources[Index]);
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  for ( Link = Bridge->ChildList.ForwardLink
+      ; Link != &Bridge->ChildList
+      ; Link = Link->ForwardLink
+      ) {
+    Device = PCI_IO_DEVICE_FROM_LINK (Link);
+    if (IS_PCI_BRIDGE (&Device->Pci)) {
+
+      ChildResourceCount = 0;
+      for (Index = 0; Index < ResourceCount; Index++) {
+        ChildResourceCount += FindResourceNode (Device, Resources[Index], NULL);
+      }
+      ChildResources = AllocatePool (sizeof (PCI_RESOURCE_NODE *) * ChildResourceCount);
+      ASSERT (ChildResources != NULL);
+      ChildResourceCount = 0;
+      for (Index = 0; Index < ResourceCount; Index++) {
+        ChildResourceCount += FindResourceNode (Device, Resources[Index], &ChildResources[ChildResourceCount]);
+      }
+
+      DumpResourceMap (Device, ChildResources, ChildResourceCount);
+      FreePool (ChildResources);
+    }
+  }
+}
+
+/**
+  Submits the I/O and memory resource requirements for the specified PCI Host Bridge.
+
+  @param PciResAlloc  Point to protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS           Successfully finished resource allocation.
+  @retval EFI_NOT_FOUND         Cannot get root bridge instance.
+  @retval EFI_OUT_OF_RESOURCES  Platform failed to program the resources if no hot plug supported.
+  @retval other                 Some error occurred when allocating resources for the PCI Host Bridge.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determine whether need support hotplug.
+
+**/
+EFI_STATUS
+PciHostBridgeResourceAllocator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  )
+{
+  PCI_IO_DEVICE                                  *RootBridgeDev;
+  EFI_HANDLE                                     RootBridgeHandle;
+  VOID                                           *AcpiConfig;
+  EFI_STATUS                                     Status;
+  UINT64                                         IoBase;
+  UINT64                                         Mem32Base;
+  UINT64                                         PMem32Base;
+  UINT64                                         Mem64Base;
+  UINT64                                         PMem64Base;
+  UINT64                                         IoResStatus;
+  UINT64                                         Mem32ResStatus;
+  UINT64                                         PMem32ResStatus;
+  UINT64                                         Mem64ResStatus;
+  UINT64                                         PMem64ResStatus;
+  UINT64                                         MaxOptionRomSize;
+  PCI_RESOURCE_NODE                              *IoBridge;
+  PCI_RESOURCE_NODE                              *Mem32Bridge;
+  PCI_RESOURCE_NODE                              *PMem32Bridge;
+  PCI_RESOURCE_NODE                              *Mem64Bridge;
+  PCI_RESOURCE_NODE                              *PMem64Bridge;
+  PCI_RESOURCE_NODE                              IoPool;
+  PCI_RESOURCE_NODE                              Mem32Pool;
+  PCI_RESOURCE_NODE                              PMem32Pool;
+  PCI_RESOURCE_NODE                              Mem64Pool;
+  PCI_RESOURCE_NODE                              PMem64Pool;
+  BOOLEAN                                        ReAllocate;
+  EFI_DEVICE_HANDLE_EXTENDED_DATA_PAYLOAD        HandleExtendedData;
+  EFI_RESOURCE_ALLOC_FAILURE_ERROR_DATA_PAYLOAD  AllocFailExtendedData;
+
+  //
+  // Reallocate flag
+  //
+  ReAllocate = FALSE;
+
+  //
+  // It may try several times if the resource allocation fails
+  //
+  while (TRUE) {
+    //
+    // Initialize resource pool
+    //
+    InitializeResourcePool (&IoPool, PciBarTypeIo16);
+    InitializeResourcePool (&Mem32Pool, PciBarTypeMem32);
+    InitializeResourcePool (&PMem32Pool, PciBarTypePMem32);
+    InitializeResourcePool (&Mem64Pool, PciBarTypeMem64);
+    InitializeResourcePool (&PMem64Pool, PciBarTypePMem64);
+
+    RootBridgeDev     = NULL;
+    RootBridgeHandle  = 0;
+
+    while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+      //
+      // Get Root Bridge Device by handle
+      //
+      RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+
+      if (RootBridgeDev == NULL) {
+        return EFI_NOT_FOUND;
+      }
+
+      //
+      // Create the entire system resource map from the information collected by
+      // enumerator. Several resource tree was created
+      //
+
+      //
+      // If non-stardard PCI Bridge I/O window alignment is supported,
+      // set I/O aligment to minimum possible alignment for root bridge.
+      //
+      IoBridge = CreateResourceNode (
+                   RootBridgeDev,
+                   0,
+                   FeaturePcdGet (PcdPciBridgeIoAlignmentProbe) ? 0x1FF: 0xFFF,
+                   RB_IO_RANGE,
+                   PciBarTypeIo16,
+                   PciResUsageTypical
+                   );
+
+      Mem32Bridge = CreateResourceNode (
+                      RootBridgeDev,
+                      0,
+                      0xFFFFF,
+                      RB_MEM32_RANGE,
+                      PciBarTypeMem32,
+                      PciResUsageTypical
+                      );
+
+      PMem32Bridge = CreateResourceNode (
+                       RootBridgeDev,
+                       0,
+                       0xFFFFF,
+                       RB_PMEM32_RANGE,
+                       PciBarTypePMem32,
+                       PciResUsageTypical
+                       );
+
+      Mem64Bridge = CreateResourceNode (
+                      RootBridgeDev,
+                      0,
+                      0xFFFFF,
+                      RB_MEM64_RANGE,
+                      PciBarTypeMem64,
+                      PciResUsageTypical
+                      );
+
+      PMem64Bridge = CreateResourceNode (
+                       RootBridgeDev,
+                       0,
+                       0xFFFFF,
+                       RB_PMEM64_RANGE,
+                       PciBarTypePMem64,
+                       PciResUsageTypical
+                       );
+
+      //
+      // Create resourcemap by going through all the devices subject to this root bridge
+      //
+      CreateResourceMap (
+        RootBridgeDev,
+        IoBridge,
+        Mem32Bridge,
+        PMem32Bridge,
+        Mem64Bridge,
+        PMem64Bridge
+        );
+
+      //
+      // Get the max ROM size that the root bridge can process
+      //
+      RootBridgeDev->RomSize = Mem32Bridge->Length;
+
+      //
+      // Skip to enlarge the resource request during realloction
+      //
+      if (!ReAllocate) {
+        //
+        // Get Max Option Rom size for current root bridge
+        //
+        MaxOptionRomSize = GetMaxOptionRomSize (RootBridgeDev);
+
+        //
+        // Enlarger the mem32 resource to accomdate the option rom
+        // if the mem32 resource is not enough to hold the rom
+        //
+        if (MaxOptionRomSize > Mem32Bridge->Length) {
+
+          Mem32Bridge->Length     = MaxOptionRomSize;
+          RootBridgeDev->RomSize  = MaxOptionRomSize;
+
+          //
+          // Alignment should be adjusted as well
+          //
+          if (Mem32Bridge->Alignment < MaxOptionRomSize - 1) {
+            Mem32Bridge->Alignment = MaxOptionRomSize - 1;
+          }
+        }
+      }
+
+      //
+      // Based on the all the resource tree, contruct ACPI resource node to
+      // submit the resource aperture to pci host bridge protocol
+      //
+      Status = ConstructAcpiResourceRequestor (
+                 RootBridgeDev,
+                 IoBridge,
+                 Mem32Bridge,
+                 PMem32Bridge,
+                 Mem64Bridge,
+                 PMem64Bridge,
+                 &AcpiConfig
+                 );
+
+      //
+      // Insert these resource nodes into the database
+      //
+      InsertResourceNode (&IoPool, IoBridge);
+      InsertResourceNode (&Mem32Pool, Mem32Bridge);
+      InsertResourceNode (&PMem32Pool, PMem32Bridge);
+      InsertResourceNode (&Mem64Pool, Mem64Bridge);
+      InsertResourceNode (&PMem64Pool, PMem64Bridge);
+
+      if (Status == EFI_SUCCESS) {
+        //
+        // Submit the resource requirement
+        //
+        Status = PciResAlloc->SubmitResources (
+                                PciResAlloc,
+                                RootBridgeDev->Handle,
+                                AcpiConfig
+                                );
+        //
+        // If SubmitResources returns error, PciBus isn't able to start.
+        // It's a fatal error so assertion is added.
+        //
+        DEBUG ((EFI_D_INFO, "PciBus: HostBridge->SubmitResources() - %r\n", Status));
+        ASSERT_EFI_ERROR (Status);
+      }
+
+      //
+      // Free acpi resource node
+      //
+      if (AcpiConfig != NULL) {
+        FreePool (AcpiConfig);
+      }
+
+      if (EFI_ERROR (Status)) {
+        //
+        // Destroy all the resource tree
+        //
+        DestroyResourceTree (&IoPool);
+        DestroyResourceTree (&Mem32Pool);
+        DestroyResourceTree (&PMem32Pool);
+        DestroyResourceTree (&Mem64Pool);
+        DestroyResourceTree (&PMem64Pool);
+        return Status;
+      }
+    }
+    //
+    // End while, at least one Root Bridge should be found.
+    //
+    ASSERT (RootBridgeDev != NULL);
+
+    //
+    // Notify platform to start to program the resource
+    //
+    Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeAllocateResources);
+    DEBUG ((EFI_D_INFO, "PciBus: HostBridge->NotifyPhase(AllocateResources) - %r\n", Status));
+    if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+      //
+      // If Hot Plug is not supported
+      //
+      if (EFI_ERROR (Status)) {
+        //
+        // Allocation failed, then return
+        //
+        return EFI_OUT_OF_RESOURCES;
+      }
+      //
+      // Allocation succeed.
+      // Get host bridge handle for status report, and then skip the main while
+      //
+      HandleExtendedData.Handle = RootBridgeDev->PciRootBridgeIo->ParentHandle;
+
+      break;
+
+    } else {
+      //
+      // If Hot Plug is supported
+      //
+      if (!EFI_ERROR (Status)) {
+        //
+        // Allocation succeed, then continue the following
+        //
+        break;
+      }
+
+      //
+      // If the resource allocation is unsuccessful, free resources on bridge
+      //
+
+      RootBridgeDev     = NULL;
+      RootBridgeHandle  = 0;
+
+      IoResStatus       = EFI_RESOURCE_SATISFIED;
+      Mem32ResStatus    = EFI_RESOURCE_SATISFIED;
+      PMem32ResStatus   = EFI_RESOURCE_SATISFIED;
+      Mem64ResStatus    = EFI_RESOURCE_SATISFIED;
+      PMem64ResStatus   = EFI_RESOURCE_SATISFIED;
+
+      while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+        //
+        // Get RootBridg Device by handle
+        //
+        RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+        if (RootBridgeDev == NULL) {
+          return EFI_NOT_FOUND;
+        }
+
+        //
+        // Get host bridge handle for status report
+        //
+        HandleExtendedData.Handle = RootBridgeDev->PciRootBridgeIo->ParentHandle;
+
+        //
+        // Get acpi resource node for all the resource types
+        //
+        AcpiConfig = NULL;
+
+        Status = PciResAlloc->GetProposedResources (
+                                PciResAlloc,
+                                RootBridgeDev->Handle,
+                                &AcpiConfig
+                                );
+
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+
+        if (AcpiConfig != NULL) {
+          //
+          // Adjust resource allocation policy for each RB
+          //
+          GetResourceAllocationStatus (
+            AcpiConfig,
+            &IoResStatus,
+            &Mem32ResStatus,
+            &PMem32ResStatus,
+            &Mem64ResStatus,
+            &PMem64ResStatus
+            );
+          FreePool (AcpiConfig);
+        }
+      }
+      //
+      // End while
+      //
+
+      //
+      // Raise the EFI_IOB_EC_RESOURCE_CONFLICT status code
+      //
+      //
+      // It is very difficult to follow the spec here
+      // Device path , Bar index can not be get here
+      //
+      ZeroMem (&AllocFailExtendedData, sizeof (AllocFailExtendedData));
+
+      REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+            EFI_PROGRESS_CODE,
+            EFI_IO_BUS_PCI | EFI_IOB_EC_RESOURCE_CONFLICT,
+            (VOID *) &AllocFailExtendedData,
+            sizeof (AllocFailExtendedData)
+            );
+
+      Status = PciHostBridgeAdjustAllocation (
+                 &IoPool,
+                 &Mem32Pool,
+                 &PMem32Pool,
+                 &Mem64Pool,
+                 &PMem64Pool,
+                 IoResStatus,
+                 Mem32ResStatus,
+                 PMem32ResStatus,
+                 Mem64ResStatus,
+                 PMem64ResStatus
+                 );
+
+      //
+      // Destroy all the resource tree
+      //
+      DestroyResourceTree (&IoPool);
+      DestroyResourceTree (&Mem32Pool);
+      DestroyResourceTree (&PMem32Pool);
+      DestroyResourceTree (&Mem64Pool);
+      DestroyResourceTree (&PMem64Pool);
+
+      NotifyPhase (PciResAlloc, EfiPciHostBridgeFreeResources);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      ReAllocate = TRUE;
+    }
+  }
+  //
+  // End main while
+  //
+
+  //
+  // Raise the EFI_IOB_PCI_RES_ALLOC status code
+  //
+  REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+        EFI_PROGRESS_CODE,
+        EFI_IO_BUS_PCI | EFI_IOB_PCI_RES_ALLOC,
+        (VOID *) &HandleExtendedData,
+        sizeof (HandleExtendedData)
+        );
+
+  //
+  // Notify pci bus driver starts to program the resource
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeSetResources);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  RootBridgeDev     = NULL;
+
+  RootBridgeHandle  = 0;
+
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+    //
+    // Get RootBridg Device by handle
+    //
+    RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Get acpi resource node for all the resource types
+    //
+    AcpiConfig = NULL;
+    Status = PciResAlloc->GetProposedResources (
+                            PciResAlloc,
+                            RootBridgeDev->Handle,
+                            &AcpiConfig
+                            );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the resource base by interpreting acpi resource node
+    //
+    //
+    GetResourceBase (
+      AcpiConfig,
+      &IoBase,
+      &Mem32Base,
+      &PMem32Base,
+      &Mem64Base,
+      &PMem64Base
+      );
+
+    //
+    // Process option rom for this root bridge
+    //
+    ProcessOptionRom (RootBridgeDev, Mem32Base, RootBridgeDev->RomSize);
+
+    //
+    // Create the entire system resource map from the information collected by
+    // enumerator. Several resource tree was created
+    //
+    FindResourceNode (RootBridgeDev, &IoPool, &IoBridge);
+    FindResourceNode (RootBridgeDev, &Mem32Pool, &Mem32Bridge);
+    FindResourceNode (RootBridgeDev, &PMem32Pool, &PMem32Bridge);
+    FindResourceNode (RootBridgeDev, &Mem64Pool, &Mem64Bridge);
+    FindResourceNode (RootBridgeDev, &PMem64Pool, &PMem64Bridge);
+
+    ASSERT (IoBridge     != NULL);
+    ASSERT (Mem32Bridge  != NULL);
+    ASSERT (PMem32Bridge != NULL);
+    ASSERT (Mem64Bridge  != NULL);
+    ASSERT (PMem64Bridge != NULL);
+
+    //
+    // Program IO resources
+    //
+    ProgramResource (
+      IoBase,
+      IoBridge
+      );
+
+    //
+    // Program Mem32 resources
+    //
+    ProgramResource (
+      Mem32Base,
+      Mem32Bridge
+      );
+
+    //
+    // Program PMem32 resources
+    //
+    ProgramResource (
+      PMem32Base,
+      PMem32Bridge
+      );
+
+    //
+    // Program Mem64 resources
+    //
+    ProgramResource (
+      Mem64Base,
+      Mem64Bridge
+      );
+
+    //
+    // Program PMem64 resources
+    //
+    ProgramResource (
+      PMem64Base,
+      PMem64Bridge
+      );
+
+    IoBridge    ->PciDev->PciBar[IoBridge    ->Bar].BaseAddress = IoBase;
+    Mem32Bridge ->PciDev->PciBar[Mem32Bridge ->Bar].BaseAddress = Mem32Base;
+    PMem32Bridge->PciDev->PciBar[PMem32Bridge->Bar].BaseAddress = PMem32Base;
+    Mem64Bridge ->PciDev->PciBar[Mem64Bridge ->Bar].BaseAddress = Mem64Base;
+    PMem64Bridge->PciDev->PciBar[PMem64Bridge->Bar].BaseAddress = PMem64Base;
+
+    //
+    // Dump the resource map for current root bridge
+    //
+    DEBUG_CODE (
+      PCI_RESOURCE_NODE *Resources[5];
+      Resources[0] = IoBridge;
+      Resources[1] = Mem32Bridge;
+      Resources[2] = PMem32Bridge;
+      Resources[3] = Mem64Bridge;
+      Resources[4] = PMem64Bridge;
+      DumpResourceMap (RootBridgeDev, Resources, sizeof (Resources) / sizeof (Resources[0]));
+    );
+
+    FreePool (AcpiConfig);
+  }
+
+  //
+  // Destroy all the resource tree
+  //
+  DestroyResourceTree (&IoPool);
+  DestroyResourceTree (&Mem32Pool);
+  DestroyResourceTree (&PMem32Pool);
+  DestroyResourceTree (&Mem64Pool);
+  DestroyResourceTree (&PMem64Pool);
+
+  //
+  // Notify the resource allocation phase is to end
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeEndResourceAllocation);
+
+  return Status;
+}
+
+/**
+  Allocate NumberOfBuses buses and return the next available PCI bus number.
+
+  @param  Bridge           Bridge device instance.
+  @param  StartBusNumber   Current available PCI bus number.
+  @param  NumberOfBuses    Number of buses enumerated below the StartBusNumber.
+  @param  NextBusNumber    Next available PCI bus number.
+
+  @retval EFI_SUCCESS           Available bus number resource is enough. Next available PCI bus number
+                                is returned in NextBusNumber.
+  @retval EFI_OUT_OF_RESOURCES  Available bus number resource is not enough for allocation.
+
+**/
+EFI_STATUS
+PciAllocateBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  IN UINT8                              NumberOfBuses,
+  OUT UINT8                             *NextBusNumber
+  )
+{
+  PCI_IO_DEVICE                      *RootBridge;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR  *BusNumberRanges;
+  UINT8                              NextNumber;
+  UINT64                             MaxNumberInRange;
+
+  //
+  // Get PCI Root Bridge device
+  //
+  RootBridge = Bridge;
+  while (RootBridge->Parent != NULL) {
+    RootBridge = RootBridge->Parent;
+  }
+
+  //
+  // Get next available PCI bus number
+  //
+  BusNumberRanges = RootBridge->BusNumberRanges;
+  while (BusNumberRanges->Desc != ACPI_END_TAG_DESCRIPTOR) {
+    MaxNumberInRange = BusNumberRanges->AddrRangeMin + BusNumberRanges->AddrLen - 1;
+    if (StartBusNumber >= BusNumberRanges->AddrRangeMin && StartBusNumber <=  MaxNumberInRange) {
+      NextNumber = (UINT8)(StartBusNumber + NumberOfBuses);
+      while (NextNumber > MaxNumberInRange) {
+        ++BusNumberRanges;
+        if (BusNumberRanges->Desc == ACPI_END_TAG_DESCRIPTOR) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+        NextNumber = (UINT8)(NextNumber + (BusNumberRanges->AddrRangeMin - (MaxNumberInRange + 1)));
+        MaxNumberInRange = BusNumberRanges->AddrRangeMin + BusNumberRanges->AddrLen - 1;
+      }
+      *NextBusNumber = NextNumber;
+      return EFI_SUCCESS;
+    }
+    BusNumberRanges++;
+  }
+  return EFI_OUT_OF_RESOURCES;
+}
+
+/**
+  Scan pci bus and assign bus number to the given PCI bus system.
+
+  @param  Bridge           Bridge device instance.
+  @param  StartBusNumber   start point.
+  @param  SubBusNumber     Point to sub bus number.
+  @param  PaddedBusRange   Customized bus number.
+
+  @retval EFI_SUCCESS      Successfully scanned and assigned bus number.
+  @retval other            Some error occurred when scanning pci bus.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determine whether need support hotplug.
+
+**/
+EFI_STATUS
+PciScanBus (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  OUT UINT8                             *SubBusNumber,
+  OUT UINT8                             *PaddedBusRange
+  )
+{
+  EFI_STATUS                        Status;
+  PCI_TYPE00                        Pci;
+  UINT8                             Device;
+  UINT8                             Func;
+  UINT64                            Address;
+  UINT8                             SecondBus;
+  UINT8                             PaddedSubBus;
+  UINT16                            Register;
+  UINTN                             HpIndex;
+  PCI_IO_DEVICE                     *PciDevice;
+  EFI_EVENT                         Event;
+  EFI_HPC_STATE                     State;
+  UINT64                            PciAddress;
+  EFI_HPC_PADDING_ATTRIBUTES        Attributes;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptors;
+  UINT16                            BusRange;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL   *PciRootBridgeIo;
+  BOOLEAN                           BusPadding;
+  UINT32                            TempReservedBusNum;
+
+  PciRootBridgeIo = Bridge->PciRootBridgeIo;
+  SecondBus       = 0;
+  Register        = 0;
+  State           = 0;
+  Attributes      = (EFI_HPC_PADDING_ATTRIBUTES) 0;
+  BusRange        = 0;
+  BusPadding      = FALSE;
+  PciDevice       = NULL;
+  PciAddress      = 0;
+
+  for (Device = 0; Device <= PCI_MAX_DEVICE; Device++) {
+    TempReservedBusNum = 0;
+    for (Func = 0; Func <= PCI_MAX_FUNC; Func++) {
+
+      //
+      // Check to see whether a pci device is present
+      //
+      Status = PciDevicePresent (
+                PciRootBridgeIo,
+                &Pci,
+                StartBusNumber,
+                Device,
+                Func
+                );
+
+      if (EFI_ERROR (Status) && Func == 0) {
+        //
+        // go to next device if there is no Function 0
+        //
+        break;
+      }
+
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      //
+      // Get the PCI device information
+      //
+      Status = PciSearchDevice (
+                Bridge,
+                &Pci,
+                StartBusNumber,
+                Device,
+                Func,
+                &PciDevice
+                );
+
+      ASSERT (!EFI_ERROR (Status));
+
+      PciAddress = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0);
+
+      if (!IS_PCI_BRIDGE (&Pci)) {
+        //
+        // PCI bridges will be called later
+        // Here just need for PCI device or PCI to cardbus controller
+        // EfiPciBeforeChildBusEnumeration for PCI Device Node
+        //
+        PreprocessController (
+            PciDevice,
+            PciDevice->BusNumber,
+            PciDevice->DeviceNumber,
+            PciDevice->FunctionNumber,
+            EfiPciBeforeChildBusEnumeration
+            );
+      }
+
+      if (FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+        //
+        // For Pci Hotplug controller devcie only
+        //
+        if (gPciHotPlugInit != NULL) {
+          //
+          // Check if it is a Hotplug PCI controller
+          //
+          if (IsRootPciHotPlugController (PciDevice->DevicePath, &HpIndex)) {
+            gPciRootHpcData[HpIndex].Found = TRUE;
+
+            if (!gPciRootHpcData[HpIndex].Initialized) {
+
+              Status = CreateEventForHpc (HpIndex, &Event);
+
+              ASSERT (!EFI_ERROR (Status));
+
+              Status = gPciHotPlugInit->InitializeRootHpc (
+                                          gPciHotPlugInit,
+                                          gPciRootHpcPool[HpIndex].HpcDevicePath,
+                                          PciAddress,
+                                          Event,
+                                          &State
+                                          );
+
+              PreprocessController (
+                PciDevice,
+                PciDevice->BusNumber,
+                PciDevice->DeviceNumber,
+                PciDevice->FunctionNumber,
+                EfiPciBeforeChildBusEnumeration
+              );
+            }
+          }
+        }
+      }
+
+      if (IS_PCI_BRIDGE (&Pci) || IS_CARDBUS_BRIDGE (&Pci)) {
+        //
+        // For PPB
+        //
+        if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+          //
+          // If Hot Plug is not supported,
+          // get the bridge information
+          //
+          Status = PciSearchDevice (
+                    Bridge,
+                    &Pci,
+                    StartBusNumber,
+                    Device,
+                    Func,
+                    &PciDevice
+                    );
+
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+        } else {
+          //
+          // If Hot Plug is supported,
+          // Get the bridge information
+          //
+          BusPadding = FALSE;
+          if (gPciHotPlugInit != NULL) {
+
+            if (IsRootPciHotPlugBus (PciDevice->DevicePath, &HpIndex)) {
+
+              //
+              // If it is initialized, get the padded bus range
+              //
+              Status = gPciHotPlugInit->GetResourcePadding (
+                                          gPciHotPlugInit,
+                                          gPciRootHpcPool[HpIndex].HpbDevicePath,
+                                          PciAddress,
+                                          &State,
+                                          (VOID **) &Descriptors,
+                                          &Attributes
+                                          );
+
+              if (EFI_ERROR (Status)) {
+                return Status;
+              }
+
+              BusRange = 0;
+              Status = PciGetBusRange (
+                        &Descriptors,
+                        NULL,
+                        NULL,
+                        &BusRange
+                        );
+
+              FreePool (Descriptors);
+
+              if (EFI_ERROR (Status)) {
+                return Status;
+              }
+
+              BusPadding = TRUE;
+            }
+          }
+        }
+
+        Status = PciAllocateBusNumber (Bridge, *SubBusNumber, 1, SubBusNumber);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        SecondBus = *SubBusNumber;
+
+        Register  = (UINT16) ((SecondBus << 8) | (UINT16) StartBusNumber);
+        Address   = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, PCI_BRIDGE_PRIMARY_BUS_REGISTER_OFFSET);
+
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint16,
+                                        Address,
+                                        1,
+                                        &Register
+                                        );
+
+
+        //
+        // If it is PPB, resursively search down this bridge
+        //
+        if (IS_PCI_BRIDGE (&Pci)) {
+
+          //
+          // Temporarily initialize SubBusNumber to maximum bus number to ensure the
+          // PCI configuration transaction to go through any PPB
+          //
+          Register  = 0xFF;
+          Address   = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET);
+          Status = PciRootBridgeIo->Pci.Write (
+                                          PciRootBridgeIo,
+                                          EfiPciWidthUint8,
+                                          Address,
+                                          1,
+                                          &Register
+                                          );
+
+          //
+          // Nofify EfiPciBeforeChildBusEnumeration for PCI Brige
+          //
+          PreprocessController (
+            PciDevice,
+            PciDevice->BusNumber,
+            PciDevice->DeviceNumber,
+            PciDevice->FunctionNumber,
+            EfiPciBeforeChildBusEnumeration
+            );
+
+          Status = PciScanBus (
+                    PciDevice,
+                    SecondBus,
+                    SubBusNumber,
+                    PaddedBusRange
+                    );
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+        }
+
+        if (FeaturePcdGet (PcdPciBusHotplugDeviceSupport) && BusPadding) {
+          //
+          // Ensure the device is enabled and initialized
+          //
+          if ((Attributes == EfiPaddingPciRootBridge) &&
+              (State & EFI_HPC_STATE_ENABLED) != 0    &&
+              (State & EFI_HPC_STATE_INITIALIZED) != 0) {
+            *PaddedBusRange = (UINT8) ((UINT8) (BusRange) + *PaddedBusRange);
+          } else {
+            //
+            // Reserve the larger one between the actual occupied bus number and padded bus number
+            //
+            Status = PciAllocateBusNumber (PciDevice, SecondBus, (UINT8) (BusRange), &PaddedSubBus);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+            *SubBusNumber = MAX (PaddedSubBus, *SubBusNumber);
+          }
+        }
+
+        //
+        // Set the current maximum bus number under the PPB
+        //
+        Address = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET);
+
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint8,
+                                        Address,
+                                        1,
+                                        SubBusNumber
+                                        );
+      } else  {
+        //
+        // It is device. Check PCI IOV for Bus reservation
+        // Go through each function, just reserve the MAX ReservedBusNum for one device
+        //
+        if (PcdGetBool (PcdSrIovSupport) && PciDevice->SrIovCapabilityOffset != 0) {
+          if (TempReservedBusNum < PciDevice->ReservedBusNum) {
+
+            Status = PciAllocateBusNumber (PciDevice, *SubBusNumber, (UINT8) (PciDevice->ReservedBusNum - TempReservedBusNum), SubBusNumber);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+            TempReservedBusNum = PciDevice->ReservedBusNum;
+
+            if (Func == 0) {
+              DEBUG ((EFI_D_INFO, "PCI-IOV ScanBus - SubBusNumber - 0x%x\n", *SubBusNumber));
+            } else {
+              DEBUG ((EFI_D_INFO, "PCI-IOV ScanBus - SubBusNumber - 0x%x (Update)\n", *SubBusNumber));
+            }
+          }
+        }
+      }
+
+      if (Func == 0 && !IS_PCI_MULTI_FUNC (&Pci)) {
+
+        //
+        // Skip sub functions, this is not a multi function device
+        //
+
+        Func = PCI_MAX_FUNC;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Process Option Rom on the specified root bridge.
+
+  @param Bridge  Pci root bridge device instance.
+
+  @retval EFI_SUCCESS   Success process.
+  @retval other         Some error occurred when processing Option Rom on the root bridge.
+
+**/
+EFI_STATUS
+PciRootBridgeP2CProcess (
+  IN PCI_IO_DEVICE *Bridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+  EFI_HPC_STATE   State;
+  UINT64          PciAddress;
+  EFI_STATUS      Status;
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (IS_CARDBUS_BRIDGE (&Temp->Pci)) {
+
+      if (gPciHotPlugInit != NULL && Temp->Allocated && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+
+        //
+        // Raise the EFI_IOB_PCI_HPC_INIT status code
+        //
+        REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+          EFI_PROGRESS_CODE,
+          EFI_IO_BUS_PCI | EFI_IOB_PCI_HPC_INIT,
+          Temp->DevicePath
+          );
+
+        PciAddress = EFI_PCI_ADDRESS (Temp->BusNumber, Temp->DeviceNumber, Temp->FunctionNumber, 0);
+        Status = gPciHotPlugInit->InitializeRootHpc (
+                                    gPciHotPlugInit,
+                                    Temp->DevicePath,
+                                    PciAddress,
+                                    NULL,
+                                    &State
+                                    );
+
+        if (!EFI_ERROR (Status)) {
+          Status = PciBridgeEnumerator (Temp);
+
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+        }
+
+        CurrentLink = CurrentLink->ForwardLink;
+        continue;
+
+      }
+    }
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      Status = PciRootBridgeP2CProcess (Temp);
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Process Option Rom on the specified host bridge.
+
+  @param PciResAlloc    Pointer to instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS   Success process.
+  @retval EFI_NOT_FOUND Can not find the root bridge instance.
+  @retval other         Some error occurred when processing Option Rom on the host bridge.
+
+**/
+EFI_STATUS
+PciHostBridgeP2CProcess (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  )
+{
+  EFI_HANDLE    RootBridgeHandle;
+  PCI_IO_DEVICE *RootBridgeDev;
+  EFI_STATUS    Status;
+
+  if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    return EFI_SUCCESS;
+  }
+
+  RootBridgeHandle = NULL;
+
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+    //
+    // Get RootBridg Device by handle
+    //
+    RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    Status = PciRootBridgeP2CProcess (RootBridgeDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to enumerate the entire host bridge
+  in a given platform.
+
+  @param PciResAlloc   A pointer to the PCI Host Resource Allocation protocol.
+
+  @retval EFI_SUCCESS            Successfully enumerated the host bridge.
+  @retval EFI_OUT_OF_RESOURCES   No enough memory available.
+  @retval other                  Some error occurred when enumerating the host bridge.
+
+**/
+EFI_STATUS
+PciHostBridgeEnumerator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc
+  )
+{
+  EFI_HANDLE                        RootBridgeHandle;
+  PCI_IO_DEVICE                     *RootBridgeDev;
+  EFI_STATUS                        Status;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL   *PciRootBridgeIo;
+  UINT16                            MinBus;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptors;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration;
+  UINT8                             StartBusNumber;
+  LIST_ENTRY                        RootBridgeList;
+  LIST_ENTRY                        *Link;
+
+  if (FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    InitializeHotPlugSupport ();
+  }
+
+  InitializeListHead (&RootBridgeList);
+
+  //
+  // Notify the bus allocation phase is about to start
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeBeginBusAllocation);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DEBUG((EFI_D_INFO, "PCI Bus First Scanning\n"));
+  RootBridgeHandle = NULL;
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+    //
+    // if a root bridge instance is found, create root bridge device for it
+    //
+
+    RootBridgeDev = CreateRootBridge (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Enumerate all the buses under this root bridge
+    //
+    Status = PciRootBridgeEnumerator (
+              PciResAlloc,
+              RootBridgeDev
+              );
+
+    if (gPciHotPlugInit != NULL && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+      InsertTailList (&RootBridgeList, &(RootBridgeDev->Link));
+    } else {
+      DestroyRootBridge (RootBridgeDev);
+    }
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Notify the bus allocation phase is finished for the first time
+  //
+  NotifyPhase (PciResAlloc, EfiPciHostBridgeEndBusAllocation);
+
+  if (gPciHotPlugInit != NULL && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    //
+    // Reset all assigned PCI bus number in all PPB
+    //
+    RootBridgeHandle = NULL;
+    Link = GetFirstNode (&RootBridgeList);
+    while ((PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) &&
+      (!IsNull (&RootBridgeList, Link))) {
+      RootBridgeDev = PCI_IO_DEVICE_FROM_LINK (Link);
+      //
+      // Get the Bus information
+      //
+      Status = PciResAlloc->StartBusEnumeration (
+                              PciResAlloc,
+                              RootBridgeHandle,
+                              (VOID **) &Configuration
+                              );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      //
+      // Get the bus number to start with
+      //
+      StartBusNumber  = (UINT8) (Configuration->AddrRangeMin);
+
+      ResetAllPpbBusNumber (
+        RootBridgeDev,
+        StartBusNumber
+      );
+
+      FreePool (Configuration);
+      Link = RemoveEntryList (Link);
+      DestroyRootBridge (RootBridgeDev);
+    }
+
+    //
+    // Wait for all HPC initialized
+    //
+    Status = AllRootHPCInitialized (STALL_1_SECOND * 15);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Some root HPC failed to initialize\n"));
+      return Status;
+    }
+
+    //
+    // Notify the bus allocation phase is about to start for the 2nd time
+    //
+    Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeBeginBusAllocation);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    DEBUG((EFI_D_INFO, "PCI Bus Second Scanning\n"));
+    RootBridgeHandle = NULL;
+    while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+      //
+      // if a root bridge instance is found, create root bridge device for it
+      //
+      RootBridgeDev = CreateRootBridge (RootBridgeHandle);
+
+      if (RootBridgeDev == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      //
+      // Enumerate all the buses under this root bridge
+      //
+      Status = PciRootBridgeEnumerator (
+                PciResAlloc,
+                RootBridgeDev
+                );
+
+      DestroyRootBridge (RootBridgeDev);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+
+    //
+    // Notify the bus allocation phase is to end for the 2nd time
+    //
+    NotifyPhase (PciResAlloc, EfiPciHostBridgeEndBusAllocation);
+  }
+
+  //
+  // Notify the resource allocation phase is to start
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeBeginResourceAllocation);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  RootBridgeHandle = NULL;
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+    //
+    // if a root bridge instance is found, create root bridge device for it
+    //
+    RootBridgeDev = CreateRootBridge (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Status = StartManagingRootBridge (RootBridgeDev);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    PciRootBridgeIo = RootBridgeDev->PciRootBridgeIo;
+    Status          = PciRootBridgeIo->Configuration (PciRootBridgeIo, (VOID **) &Descriptors);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Status = PciGetBusRange (&Descriptors, &MinBus, NULL, NULL);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Determine root bridge attribute by calling interface of Pcihostbridge
+    // protocol
+    //
+    DetermineRootBridgeAttributes (
+      PciResAlloc,
+      RootBridgeDev
+      );
+
+    //
+    // Collect all the resource information under this root bridge
+    // A database that records all the information about pci device subject to this
+    // root bridge will then be created
+    //
+    Status = PciPciDeviceInfoCollector (
+              RootBridgeDev,
+              (UINT8) MinBus
+              );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    InsertRootBridge (RootBridgeDev);
+
+    //
+    // Record the hostbridge handle
+    //
+    AddHostBridgeEnumerator (RootBridgeDev->PciRootBridgeIo->ParentHandle);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.h
new file mode 100644
index 0000000000..6c2ade3f36
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.h
@@ -0,0 +1,165 @@
+/** @file
+  Internal library declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_LIB_H_
+#define _EFI_PCI_LIB_H_
+
+
+typedef struct {
+  EFI_HANDLE            Handle;
+} EFI_DEVICE_HANDLE_EXTENDED_DATA_PAYLOAD;
+
+typedef struct {
+  UINT32                             Bar;
+  UINT16                             DevicePathSize;
+  UINT16                             ReqResSize;
+  UINT16                             AllocResSize;
+  UINT8                              *DevicePath;
+  UINT8                              *ReqRes;
+  UINT8                              *AllocRes;
+} EFI_RESOURCE_ALLOC_FAILURE_ERROR_DATA_PAYLOAD;
+
+
+/**
+  Retrieve the PCI Card device BAR information via PciIo interface.
+
+  @param PciIoDevice        PCI Card device instance.
+
+**/
+VOID
+GetBackPcCardBar (
+  IN  PCI_IO_DEVICE                  *PciIoDevice
+  );
+
+/**
+  Remove rejected pci device from specific root bridge
+  handle.
+
+  @param RootBridgeHandle  Specific parent root bridge handle.
+  @param Bridge            Bridge device instance.
+
+**/
+VOID
+RemoveRejectedPciDevices (
+  IN EFI_HANDLE        RootBridgeHandle,
+  IN PCI_IO_DEVICE     *Bridge
+  );
+
+/**
+  Submits the I/O and memory resource requirements for the specified PCI Host Bridge.
+
+  @param PciResAlloc  Point to protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS           Successfully finished resource allocation.
+  @retval EFI_NOT_FOUND         Cannot get root bridge instance.
+  @retval EFI_OUT_OF_RESOURCES  Platform failed to program the resources if no hot plug supported.
+  @retval other                 Some error occurred when allocating resources for the PCI Host Bridge.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determine whether need support hotplug.
+
+**/
+EFI_STATUS
+PciHostBridgeResourceAllocator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  );
+
+/**
+  Allocate NumberOfBuses buses and return the next available PCI bus number.
+
+  @param  Bridge           Bridge device instance.
+  @param  StartBusNumber   Current available PCI bus number.
+  @param  NumberOfBuses    Number of buses enumerated below the StartBusNumber.
+  @param  NextBusNumber    Next available PCI bus number.
+
+  @retval EFI_SUCCESS           Available bus number resource is enough. Next available PCI bus number
+                                is returned in NextBusNumber.
+  @retval EFI_OUT_OF_RESOURCES  Available bus number resource is not enough for allocation.
+
+**/
+EFI_STATUS
+PciAllocateBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  IN UINT8                              NumberOfBuses,
+  OUT UINT8                             *NextBusNumber
+  );
+
+/**
+  Scan pci bus and assign bus number to the given PCI bus system.
+
+  @param  Bridge           Bridge device instance.
+  @param  StartBusNumber   start point.
+  @param  SubBusNumber     Point to sub bus number.
+  @param  PaddedBusRange   Customized bus number.
+
+  @retval EFI_SUCCESS      Successfully scanned and assigned bus number.
+  @retval other            Some error occurred when scanning pci bus.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determine whether need support hotplug.
+
+**/
+EFI_STATUS
+PciScanBus (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  OUT UINT8                             *SubBusNumber,
+  OUT UINT8                             *PaddedBusRange
+  );
+
+/**
+  Process Option Rom on the specified root bridge.
+
+  @param Bridge  Pci root bridge device instance.
+
+  @retval EFI_SUCCESS   Success process.
+  @retval other         Some error occurred when processing Option Rom on the root bridge.
+
+**/
+EFI_STATUS
+PciRootBridgeP2CProcess (
+  IN PCI_IO_DEVICE *Bridge
+  );
+
+/**
+  Process Option Rom on the specified host bridge.
+
+  @param PciResAlloc    Pointer to instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS   Success process.
+  @retval EFI_NOT_FOUND Can not find the root bridge instance.
+  @retval other         Some error occurred when processing Option Rom on the host bridge.
+
+**/
+EFI_STATUS
+PciHostBridgeP2CProcess (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  );
+
+/**
+  This function is used to enumerate the entire host bridge
+  in a given platform.
+
+  @param PciResAlloc   A pointer to the PCI Host Resource Allocation protocol.
+
+  @retval EFI_SUCCESS            Successfully enumerated the host bridge.
+  @retval EFI_OUT_OF_RESOURCES   No enough memory available.
+  @retval other                  Some error occurred when enumerating the host bridge.
+
+**/
+EFI_STATUS
+PciHostBridgeEnumerator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.c
new file mode 100644
index 0000000000..2bc4f8c5e8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.c
@@ -0,0 +1,783 @@
+/** @file
+  PCI Rom supporting funtions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+/**
+  Load the EFI Image from Option ROM
+
+  @param PciIoDevice   PCI IO device instance.
+  @param FilePath      The file path of the EFI Image
+  @param BufferSize    On input the size of Buffer in bytes. On output with a return
+                       code of EFI_SUCCESS, the amount of data transferred to Buffer.
+                       On output with a return code of EFI_BUFFER_TOO_SMALL,
+                       the size of Buffer required to retrieve the requested file.
+  @param Buffer        The memory buffer to transfer the file to. If Buffer is NULL,
+                       then no the size of the requested file is returned in BufferSize.
+
+  @retval EFI_SUCCESS           The file was loaded.
+  @retval EFI_INVALID_PARAMETER FilePath is not a valid device path, or
+                                BufferSize is NULL.
+  @retval EFI_NOT_FOUND         Not found PCI Option Rom on PCI device.
+  @retval EFI_DEVICE_ERROR      Failed to decompress PCI Option Rom image.
+  @retval EFI_BUFFER_TOO_SMALL  The BufferSize is too small to read the current directory entry.
+                                BufferSize has been updated with the size needed to complete the request.
+**/
+EFI_STATUS
+LocalLoadFile2 (
+  IN PCI_IO_DEVICE            *PciIoDevice,
+  IN EFI_DEVICE_PATH_PROTOCOL *FilePath,
+  IN OUT UINTN                *BufferSize,
+  IN VOID                     *Buffer      OPTIONAL
+  )
+{
+  EFI_STATUS                                Status;
+  MEDIA_RELATIVE_OFFSET_RANGE_DEVICE_PATH   *EfiOpRomImageNode;
+  EFI_PCI_EXPANSION_ROM_HEADER              *EfiRomHeader;
+  PCI_DATA_STRUCTURE                        *Pcir;
+  UINT32                                    ImageSize;
+  UINT8                                     *ImageBuffer;
+  UINT32                                    ImageLength;
+  UINT32                                    DestinationSize;
+  UINT32                                    ScratchSize;
+  VOID                                      *Scratch;
+  EFI_DECOMPRESS_PROTOCOL                   *Decompress;
+  UINT32                                    InitializationSize;
+
+  EfiOpRomImageNode = (MEDIA_RELATIVE_OFFSET_RANGE_DEVICE_PATH *) FilePath;
+  if ((EfiOpRomImageNode == NULL) ||
+      (DevicePathType (FilePath) != MEDIA_DEVICE_PATH) ||
+      (DevicePathSubType (FilePath) != MEDIA_RELATIVE_OFFSET_RANGE_DP) ||
+      (DevicePathNodeLength (FilePath) != sizeof (MEDIA_RELATIVE_OFFSET_RANGE_DEVICE_PATH)) ||
+      (!IsDevicePathEnd (NextDevicePathNode (FilePath))) ||
+      (EfiOpRomImageNode->StartingOffset > EfiOpRomImageNode->EndingOffset) ||
+      (EfiOpRomImageNode->EndingOffset >= PciIoDevice->RomSize) ||
+      (BufferSize == NULL)
+      ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  EfiRomHeader = (EFI_PCI_EXPANSION_ROM_HEADER *) (
+      (UINT8 *) PciIoDevice->PciIo.RomImage + EfiOpRomImageNode->StartingOffset
+      );
+  if (EfiRomHeader->Signature != PCI_EXPANSION_ROM_HEADER_SIGNATURE) {
+    return EFI_NOT_FOUND;
+  }
+
+
+  Pcir = (PCI_DATA_STRUCTURE *) ((UINT8 *) EfiRomHeader + EfiRomHeader->PcirOffset);
+  ASSERT (Pcir->Signature == PCI_DATA_STRUCTURE_SIGNATURE);
+
+  if ((Pcir->CodeType == PCI_CODE_TYPE_EFI_IMAGE) &&
+      (EfiRomHeader->EfiSignature == EFI_PCI_EXPANSION_ROM_HEADER_EFISIGNATURE) &&
+      ((EfiRomHeader->EfiSubsystem == EFI_IMAGE_SUBSYSTEM_EFI_BOOT_SERVICE_DRIVER) ||
+       (EfiRomHeader->EfiSubsystem == EFI_IMAGE_SUBSYSTEM_EFI_RUNTIME_DRIVER)) &&
+      (EfiRomHeader->CompressionType <= EFI_PCI_EXPANSION_ROM_HEADER_COMPRESSED)
+       ) {
+
+    ImageSize = Pcir->ImageLength * 512;
+    InitializationSize = (UINT32) EfiRomHeader->InitializationSize * 512;
+    if (InitializationSize > ImageSize || EfiRomHeader->EfiImageHeaderOffset >=  InitializationSize) {
+      return EFI_NOT_FOUND;
+    }
+
+    ImageBuffer             = (UINT8 *) EfiRomHeader + EfiRomHeader->EfiImageHeaderOffset;
+    ImageLength             = InitializationSize - EfiRomHeader->EfiImageHeaderOffset;
+
+    if (EfiRomHeader->CompressionType != EFI_PCI_EXPANSION_ROM_HEADER_COMPRESSED) {
+      //
+      // Uncompressed: Copy the EFI Image directly to user's buffer
+      //
+      if (Buffer == NULL || *BufferSize < ImageLength) {
+        *BufferSize = ImageLength;
+        return EFI_BUFFER_TOO_SMALL;
+      }
+
+      *BufferSize = ImageLength;
+      CopyMem (Buffer, ImageBuffer, ImageLength);
+      return EFI_SUCCESS;
+
+    } else {
+      //
+      // Compressed: Uncompress before copying
+      //
+      Status = gBS->LocateProtocol (&gEfiDecompressProtocolGuid, NULL, (VOID **) &Decompress);
+      if (EFI_ERROR (Status)) {
+        return EFI_DEVICE_ERROR;
+      }
+      Status = Decompress->GetInfo (
+                             Decompress,
+                             ImageBuffer,
+                             ImageLength,
+                             &DestinationSize,
+                             &ScratchSize
+                             );
+      if (EFI_ERROR (Status)) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      if (Buffer == NULL || *BufferSize < DestinationSize) {
+        *BufferSize = DestinationSize;
+        return EFI_BUFFER_TOO_SMALL;
+      }
+
+      *BufferSize = DestinationSize;
+      Scratch = AllocatePool (ScratchSize);
+      if (Scratch == NULL) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      Status = Decompress->Decompress (
+                             Decompress,
+                             ImageBuffer,
+                             ImageLength,
+                             Buffer,
+                             DestinationSize,
+                             Scratch,
+                             ScratchSize
+                             );
+      FreePool (Scratch);
+
+      if (EFI_ERROR (Status)) {
+        return EFI_DEVICE_ERROR;
+      }
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Initialize a PCI LoadFile2 instance.
+
+  @param PciIoDevice   PCI IO Device.
+
+**/
+VOID
+InitializePciLoadFile2 (
+  IN PCI_IO_DEVICE       *PciIoDevice
+  )
+{
+  PciIoDevice->LoadFile2.LoadFile = LoadFile2;
+}
+
+/**
+  Causes the driver to load a specified file.
+
+  @param This        Indicates a pointer to the calling context.
+  @param FilePath    The device specific path of the file to load.
+  @param BootPolicy  Should always be FALSE.
+  @param BufferSize  On input the size of Buffer in bytes. On output with a return
+                     code of EFI_SUCCESS, the amount of data transferred to Buffer.
+                     On output with a return code of EFI_BUFFER_TOO_SMALL,
+                     the size of Buffer required to retrieve the requested file.
+  @param Buffer      The memory buffer to transfer the file to. If Buffer is NULL,
+                     then no the size of the requested file is returned in BufferSize.
+
+  @retval EFI_SUCCESS           The file was loaded.
+  @retval EFI_UNSUPPORTED       BootPolicy is TRUE.
+  @retval EFI_INVALID_PARAMETER FilePath is not a valid device path, or
+                                BufferSize is NULL.
+  @retval EFI_NOT_FOUND         Not found PCI Option Rom on PCI device.
+  @retval EFI_DEVICE_ERROR      Failed to decompress PCI Option Rom image.
+  @retval EFI_BUFFER_TOO_SMALL  The BufferSize is too small to read the current directory entry.
+                                BufferSize has been updated with the size needed to complete the request.
+
+**/
+EFI_STATUS
+EFIAPI
+LoadFile2 (
+  IN EFI_LOAD_FILE2_PROTOCOL  *This,
+  IN EFI_DEVICE_PATH_PROTOCOL *FilePath,
+  IN BOOLEAN                  BootPolicy,
+  IN OUT UINTN                *BufferSize,
+  IN VOID                     *Buffer      OPTIONAL
+  )
+{
+  PCI_IO_DEVICE                             *PciIoDevice;
+
+  if (BootPolicy) {
+    return EFI_UNSUPPORTED;
+  }
+  PciIoDevice = PCI_IO_DEVICE_FROM_LOAD_FILE2_THIS (This);
+
+  return LocalLoadFile2 (
+           PciIoDevice,
+           FilePath,
+           BufferSize,
+           Buffer
+           );
+}
+
+/**
+  Get Pci device's oprom information.
+
+  @param PciIoDevice    Input Pci device instance.
+                        Output Pci device instance with updated OptionRom size.
+
+  @retval EFI_NOT_FOUND Pci device has not Option Rom.
+  @retval EFI_SUCCESS   Pci device has Option Rom.
+
+**/
+EFI_STATUS
+GetOpRomInfo (
+  IN OUT PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  UINT8                           RomBarIndex;
+  UINT32                          AllOnes;
+  UINT64                          Address;
+  EFI_STATUS                      Status;
+  UINT8                           Bus;
+  UINT8                           Device;
+  UINT8                           Function;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  Bus             = PciIoDevice->BusNumber;
+  Device          = PciIoDevice->DeviceNumber;
+  Function        = PciIoDevice->FunctionNumber;
+
+  PciRootBridgeIo = PciIoDevice->PciRootBridgeIo;
+
+  //
+  // Offset is 0x30 if is not ppb
+  //
+
+  //
+  // 0x30
+  //
+  RomBarIndex = PCI_EXPANSION_ROM_BASE;
+
+  if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+    //
+    // If is ppb, 0x38
+    //
+    RomBarIndex = PCI_BRIDGE_ROMBAR;
+  }
+  //
+  // The bit0 is 0 to prevent the enabling of the Rom address decoder
+  //
+  AllOnes = 0xfffffffe;
+  Address = EFI_PCI_ADDRESS (Bus, Device, Function, RomBarIndex);
+
+  Status = PciRootBridgeIo->Pci.Write (
+                                  PciRootBridgeIo,
+                                  EfiPciWidthUint32,
+                                  Address,
+                                  1,
+                                  &AllOnes
+                                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Read back
+  //
+  Status = PciRootBridgeIo->Pci.Read(
+                                  PciRootBridgeIo,
+                                  EfiPciWidthUint32,
+                                  Address,
+                                  1,
+                                  &AllOnes
+                                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Bits [1, 10] are reserved
+  //
+  AllOnes &= 0xFFFFF800;
+  if ((AllOnes == 0) || (AllOnes == 0xFFFFF800)) {
+    return EFI_NOT_FOUND;
+  }
+
+  PciIoDevice->RomSize = (UINT64) ((~AllOnes) + 1);
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if the RomImage contains EFI Images.
+
+  @param  RomImage  The ROM address of Image for check.
+  @param  RomSize   Size of ROM for check.
+
+  @retval TRUE     ROM contain EFI Image.
+  @retval FALSE    ROM not contain EFI Image.
+
+**/
+BOOLEAN
+ContainEfiImage (
+  IN VOID            *RomImage,
+  IN UINT64          RomSize
+  )
+{
+  PCI_EXPANSION_ROM_HEADER  *RomHeader;
+  PCI_DATA_STRUCTURE        *RomPcir;
+  UINT8                     Indicator;
+
+  Indicator = 0;
+  RomHeader = RomImage;
+  if (RomHeader == NULL) {
+    return FALSE;
+  }
+
+  do {
+    if (RomHeader->Signature != PCI_EXPANSION_ROM_HEADER_SIGNATURE) {
+      RomHeader = (PCI_EXPANSION_ROM_HEADER *) ((UINT8 *) RomHeader + 512);
+      continue;
+    }
+
+    //
+    // The PCI Data Structure must be DWORD aligned. 
+    //
+    if (RomHeader->PcirOffset == 0 ||
+        (RomHeader->PcirOffset & 3) != 0 ||
+        (UINT8 *) RomHeader + RomHeader->PcirOffset + sizeof (PCI_DATA_STRUCTURE) > (UINT8 *) RomImage + RomSize) {
+      break;
+    }
+
+    RomPcir = (PCI_DATA_STRUCTURE *) ((UINT8 *) RomHeader + RomHeader->PcirOffset);
+    if (RomPcir->Signature != PCI_DATA_STRUCTURE_SIGNATURE) {
+      break;
+    }
+
+    if (RomPcir->CodeType == PCI_CODE_TYPE_EFI_IMAGE) {
+      return TRUE;
+    }
+
+    Indicator = RomPcir->Indicator;
+    RomHeader = (PCI_EXPANSION_ROM_HEADER *) ((UINT8 *) RomHeader + RomPcir->ImageLength * 512);
+  } while (((UINT8 *) RomHeader < (UINT8 *) RomImage + RomSize) && ((Indicator & 0x80) == 0x00));
+
+  return FALSE;
+}
+
+/**
+  Load Option Rom image for specified PCI device.
+
+  @param PciDevice Pci device instance.
+  @param RomBase   Base address of Option Rom.
+
+  @retval EFI_OUT_OF_RESOURCES No enough memory to hold image.
+  @retval EFI_SUCESS           Successfully loaded Option Rom.
+
+**/
+EFI_STATUS
+LoadOpRomImage (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT64          RomBase
+  )
+{
+  UINT8                     RomBarIndex;
+  UINT8                     Indicator;
+  UINT16                    OffsetPcir;
+  UINT32                    RomBarOffset;
+  UINT32                    RomBar;
+  EFI_STATUS                RetStatus;
+  BOOLEAN                   FirstCheck;
+  UINT8                     *Image;
+  PCI_EXPANSION_ROM_HEADER  *RomHeader;
+  PCI_DATA_STRUCTURE        *RomPcir;
+  UINT64                    RomSize;
+  UINT64                    RomImageSize;
+  UINT32                    LegacyImageLength;
+  UINT8                     *RomInMemory;
+  UINT8                     CodeType;
+
+  RomSize       = PciDevice->RomSize;
+
+  Indicator     = 0;
+  RomImageSize  = 0;
+  RomInMemory   = NULL;
+  CodeType      = 0xFF;
+
+  //
+  // Get the RomBarIndex
+  //
+
+  //
+  // 0x30
+  //
+  RomBarIndex = PCI_EXPANSION_ROM_BASE;
+  if (IS_PCI_BRIDGE (&(PciDevice->Pci))) {
+    //
+    // if is ppb
+    //
+
+    //
+    // 0x38
+    //
+    RomBarIndex = PCI_BRIDGE_ROMBAR;
+  }
+  //
+  // Allocate memory for Rom header and PCIR
+  //
+  RomHeader = AllocatePool (sizeof (PCI_EXPANSION_ROM_HEADER));
+  if (RomHeader == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  RomPcir = AllocatePool (sizeof (PCI_DATA_STRUCTURE));
+  if (RomPcir == NULL) {
+    FreePool (RomHeader);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  RomBar = (UINT32) RomBase;
+
+  //
+  // Enable RomBar
+  //
+  RomDecode (PciDevice, RomBarIndex, RomBar, TRUE);
+
+  RomBarOffset  = RomBar;
+  RetStatus     = EFI_NOT_FOUND;
+  FirstCheck    = TRUE;
+  LegacyImageLength = 0;
+
+  do {
+    PciDevice->PciRootBridgeIo->Mem.Read (
+                                      PciDevice->PciRootBridgeIo,
+                                      EfiPciWidthUint8,
+                                      RomBarOffset,
+                                      sizeof (PCI_EXPANSION_ROM_HEADER),
+                                      (UINT8 *) RomHeader
+                                      );
+
+    if (RomHeader->Signature != PCI_EXPANSION_ROM_HEADER_SIGNATURE) {
+      RomBarOffset = RomBarOffset + 512;
+      if (FirstCheck) {
+        break;
+      } else {
+        RomImageSize = RomImageSize + 512;
+        continue;
+      }
+    }
+
+    FirstCheck  = FALSE;
+    OffsetPcir  = RomHeader->PcirOffset;
+    //
+    // If the pointer to the PCI Data Structure is invalid, no further images can be located. 
+    // The PCI Data Structure must be DWORD aligned. 
+    //
+    if (OffsetPcir == 0 ||
+        (OffsetPcir & 3) != 0 ||
+        RomImageSize + OffsetPcir + sizeof (PCI_DATA_STRUCTURE) > RomSize) {
+      break;
+    }
+    PciDevice->PciRootBridgeIo->Mem.Read (
+                                      PciDevice->PciRootBridgeIo,
+                                      EfiPciWidthUint8,
+                                      RomBarOffset + OffsetPcir,
+                                      sizeof (PCI_DATA_STRUCTURE),
+                                      (UINT8 *) RomPcir
+                                      );
+    //
+    // If a valid signature is not present in the PCI Data Structure, no further images can be located.
+    //
+    if (RomPcir->Signature != PCI_DATA_STRUCTURE_SIGNATURE) {
+      break;
+    }
+    if (RomImageSize + RomPcir->ImageLength * 512 > RomSize) {
+      break;
+    }
+    if (RomPcir->CodeType == PCI_CODE_TYPE_PCAT_IMAGE) {
+      CodeType = PCI_CODE_TYPE_PCAT_IMAGE;
+      LegacyImageLength = ((UINT32)((EFI_LEGACY_EXPANSION_ROM_HEADER *)RomHeader)->Size512) * 512;
+    }
+    Indicator     = RomPcir->Indicator;
+    RomImageSize  = RomImageSize + RomPcir->ImageLength * 512;
+    RomBarOffset  = RomBarOffset + RomPcir->ImageLength * 512;
+  } while (((Indicator & 0x80) == 0x00) && ((RomBarOffset - RomBar) < RomSize));
+
+  //
+  // Some Legacy Cards do not report the correct ImageLength so used the maximum
+  // of the legacy length and the PCIR Image Length
+  //
+  if (CodeType == PCI_CODE_TYPE_PCAT_IMAGE) {
+    RomImageSize = MAX (RomImageSize, LegacyImageLength);
+  }
+
+  if (RomImageSize > 0) {
+    RetStatus = EFI_SUCCESS;
+    Image     = AllocatePool ((UINT32) RomImageSize);
+    if (Image == NULL) {
+      RomDecode (PciDevice, RomBarIndex, RomBar, FALSE);
+      FreePool (RomHeader);
+      FreePool (RomPcir);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Copy Rom image into memory
+    //
+    PciDevice->PciRootBridgeIo->Mem.Read (
+                                      PciDevice->PciRootBridgeIo,
+                                      EfiPciWidthUint8,
+                                      RomBar,
+                                      (UINT32) RomImageSize,
+                                      Image
+                                      );
+    RomInMemory = Image;
+  }
+
+  RomDecode (PciDevice, RomBarIndex, RomBar, FALSE);
+
+  PciDevice->EmbeddedRom    = TRUE;
+  PciDevice->PciIo.RomSize  = RomImageSize;
+  PciDevice->PciIo.RomImage = RomInMemory;
+
+  //
+  // For OpROM read from PCI device:
+  //   Add the Rom Image to internal database for later PCI light enumeration
+  //
+  PciRomAddImageMapping (
+    NULL,
+    PciDevice->PciRootBridgeIo->SegmentNumber,
+    PciDevice->BusNumber,
+    PciDevice->DeviceNumber,
+    PciDevice->FunctionNumber,
+    (UINT64) (UINTN) PciDevice->PciIo.RomImage,
+    PciDevice->PciIo.RomSize
+    );
+
+  //
+  // Free allocated memory
+  //
+  FreePool (RomHeader);
+  FreePool (RomPcir);
+
+  return RetStatus;
+}
+
+/**
+  Enable/Disable Option Rom decode.
+
+  @param PciDevice    Pci device instance.
+  @param RomBarIndex  The BAR index of the standard PCI Configuration header to use as the
+                      base address for resource range. The legal range for this field is 0..5.
+  @param RomBar       Base address of Option Rom.
+  @param Enable       Flag for enable/disable decode.
+
+**/
+VOID
+RomDecode (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT8           RomBarIndex,
+  IN UINT32          RomBar,
+  IN BOOLEAN         Enable
+  )
+{
+  UINT32              Value32;
+  UINT32              Offset;
+  UINT32              OffsetMax;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  PciIo = &PciDevice->PciIo;
+  if (Enable) {
+    //
+    // Clear all bars
+    //
+    OffsetMax = 0x24;
+    if (IS_PCI_BRIDGE(&PciDevice->Pci)) {
+      OffsetMax = 0x14;
+    }
+
+    for (Offset = 0x10; Offset <= OffsetMax; Offset += sizeof (UINT32)) {
+      PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, Offset, 1, &gAllZero);
+    }
+
+    //
+    // set the Rom base address: now is hardcode
+    // enable its decoder
+    //
+    Value32 = RomBar | 0x1;
+    PciIo->Pci.Write (
+                 PciIo,
+                 (EFI_PCI_IO_PROTOCOL_WIDTH) EfiPciWidthUint32,
+                 RomBarIndex,
+                 1,
+                 &Value32
+                 );
+
+    //
+    // Programe all upstream bridge
+    //
+    ProgrameUpstreamBridgeForRom(PciDevice, RomBar, TRUE);
+
+    //
+    // Setting the memory space bit in the function's command register
+    //
+    PCI_ENABLE_COMMAND_REGISTER(PciDevice, EFI_PCI_COMMAND_MEMORY_SPACE);
+
+  } else {
+
+    //
+    // disable command register decode to memory
+    //
+    PCI_DISABLE_COMMAND_REGISTER(PciDevice, EFI_PCI_COMMAND_MEMORY_SPACE);
+
+    //
+    // Destroy the programmed bar in all the upstream bridge.
+    //
+    ProgrameUpstreamBridgeForRom(PciDevice, RomBar, FALSE);
+
+    //
+    // disable rom decode
+    //
+    Value32 = 0xFFFFFFFE;
+    PciIo->Pci.Write (
+                 PciIo,
+                 (EFI_PCI_IO_PROTOCOL_WIDTH) EfiPciWidthUint32,
+                 RomBarIndex,
+                 1,
+                 &Value32
+                 );
+
+  }
+}
+
+/**
+  Load and start the Option Rom image.
+
+  @param PciDevice       Pci device instance.
+
+  @retval EFI_SUCCESS    Successfully loaded and started PCI Option Rom image.
+  @retval EFI_NOT_FOUND  Failed to process PCI Option Rom image.
+
+**/
+EFI_STATUS
+ProcessOpRomImage (
+  IN  PCI_IO_DEVICE   *PciDevice
+  )
+{
+  UINT8                                    Indicator;
+  UINT32                                   ImageSize;
+  VOID                                     *RomBar;
+  UINT8                                    *RomBarOffset;
+  EFI_HANDLE                               ImageHandle;
+  EFI_STATUS                               Status;
+  EFI_STATUS                               RetStatus;
+  EFI_PCI_EXPANSION_ROM_HEADER             *EfiRomHeader;
+  PCI_DATA_STRUCTURE                       *Pcir;
+  EFI_DEVICE_PATH_PROTOCOL                 *PciOptionRomImageDevicePath;
+  MEDIA_RELATIVE_OFFSET_RANGE_DEVICE_PATH  EfiOpRomImageNode;
+  VOID                                     *Buffer;
+  UINTN                                    BufferSize;
+
+  Indicator = 0;
+
+  //
+  // Get the Address of the Option Rom image
+  //
+  RomBar        = PciDevice->PciIo.RomImage;
+  RomBarOffset  = (UINT8 *) RomBar;
+  RetStatus     = EFI_NOT_FOUND;
+
+  if (RomBar == NULL) {
+    return RetStatus;
+  }
+  ASSERT (((EFI_PCI_EXPANSION_ROM_HEADER *) RomBarOffset)->Signature == PCI_EXPANSION_ROM_HEADER_SIGNATURE);
+
+  do {
+    EfiRomHeader = (EFI_PCI_EXPANSION_ROM_HEADER *) RomBarOffset;
+    if (EfiRomHeader->Signature != PCI_EXPANSION_ROM_HEADER_SIGNATURE) {
+      RomBarOffset += 512;
+      continue;
+    }
+
+    Pcir        = (PCI_DATA_STRUCTURE *) (RomBarOffset + EfiRomHeader->PcirOffset);
+    ASSERT (Pcir->Signature == PCI_DATA_STRUCTURE_SIGNATURE);
+    ImageSize   = (UINT32) (Pcir->ImageLength * 512);
+    Indicator   = Pcir->Indicator;
+
+    //
+    // Skip the image if it is not an EFI PCI Option ROM image
+    //
+    if (Pcir->CodeType != PCI_CODE_TYPE_EFI_IMAGE) {
+      goto NextImage;
+    }
+
+    //
+    // Skip the EFI PCI Option ROM image if its machine type is not supported
+    //
+    if (!EFI_IMAGE_MACHINE_TYPE_SUPPORTED (EfiRomHeader->EfiMachineType)) {
+      goto NextImage;
+    }
+
+    //
+    // Ignore the EFI PCI Option ROM image if it is an EFI application
+    //
+    if (EfiRomHeader->EfiSubsystem == EFI_IMAGE_SUBSYSTEM_EFI_APPLICATION) {
+      goto NextImage;
+    }
+
+    //
+    // Create Pci Option Rom Image device path header
+    //
+    EfiOpRomImageNode.Header.Type     = MEDIA_DEVICE_PATH;
+    EfiOpRomImageNode.Header.SubType  = MEDIA_RELATIVE_OFFSET_RANGE_DP;
+    SetDevicePathNodeLength (&EfiOpRomImageNode.Header, sizeof (EfiOpRomImageNode));
+    EfiOpRomImageNode.StartingOffset  = (UINTN) RomBarOffset - (UINTN) RomBar;
+    EfiOpRomImageNode.EndingOffset    = (UINTN) RomBarOffset + ImageSize - 1 - (UINTN) RomBar;
+
+    PciOptionRomImageDevicePath = AppendDevicePathNode (PciDevice->DevicePath, &EfiOpRomImageNode.Header);
+    ASSERT (PciOptionRomImageDevicePath != NULL);
+
+    //
+    // load image and start image
+    //
+    BufferSize  = 0;
+    Buffer      = NULL;
+    ImageHandle = NULL;
+
+    Status = gBS->LoadImage (
+                    FALSE,
+                    gPciBusDriverBinding.DriverBindingHandle,
+                    PciOptionRomImageDevicePath,
+                    Buffer,
+                    BufferSize,
+                    &ImageHandle
+                    );
+
+    FreePool (PciOptionRomImageDevicePath);
+
+    if (!EFI_ERROR (Status)) {
+      Status = gBS->StartImage (ImageHandle, NULL, NULL);
+      if (!EFI_ERROR (Status)) {
+        AddDriver (PciDevice, ImageHandle);
+        PciRomAddImageMapping (
+          ImageHandle,
+          PciDevice->PciRootBridgeIo->SegmentNumber,
+          PciDevice->BusNumber,
+          PciDevice->DeviceNumber,
+          PciDevice->FunctionNumber,
+          (UINT64) (UINTN) PciDevice->PciIo.RomImage,
+          PciDevice->PciIo.RomSize
+          );
+        RetStatus = EFI_SUCCESS;
+      }
+    }
+
+NextImage:
+    RomBarOffset += ImageSize;
+
+  } while (((Indicator & 0x80) == 0x00) && ((UINTN) (RomBarOffset - (UINT8 *) RomBar) < PciDevice->RomSize));
+
+  return RetStatus;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.h
new file mode 100644
index 0000000000..25f78a417f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.h
@@ -0,0 +1,142 @@
+/** @file
+  PCI Rom supporting funtions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_OPTION_ROM_SUPPORT_H_
+#define _EFI_PCI_OPTION_ROM_SUPPORT_H_
+
+
+/**
+  Initialize a PCI LoadFile2 instance.
+
+  @param PciIoDevice   PCI IO Device.
+
+**/
+VOID
+InitializePciLoadFile2 (
+  IN PCI_IO_DEVICE       *PciIoDevice
+  );
+
+/**
+  Causes the driver to load a specified file.
+
+  @param This        Indicates a pointer to the calling context.
+  @param FilePath    The device specific path of the file to load.
+  @param BootPolicy  Should always be FALSE.
+  @param BufferSize  On input the size of Buffer in bytes. On output with a return
+                     code of EFI_SUCCESS, the amount of data transferred to Buffer.
+                     On output with a return code of EFI_BUFFER_TOO_SMALL,
+                     the size of Buffer required to retrieve the requested file.
+  @param Buffer      The memory buffer to transfer the file to. If Buffer is NULL,
+                     then no the size of the requested file is returned in BufferSize.
+
+  @retval EFI_SUCCESS           The file was loaded.
+  @retval EFI_UNSUPPORTED       BootPolicy is TRUE.
+  @retval EFI_INVALID_PARAMETER FilePath is not a valid device path, or
+                                BufferSize is NULL.
+  @retval EFI_NOT_FOUND         Not found PCI Option Rom on PCI device.
+  @retval EFI_DEVICE_ERROR      Failed to decompress PCI Option Rom image.
+  @retval EFI_BUFFER_TOO_SMALL  The BufferSize is too small to read the current directory entry.
+                                BufferSize has been updated with the size needed to complete the request.
+
+**/
+EFI_STATUS
+EFIAPI
+LoadFile2 (
+  IN EFI_LOAD_FILE2_PROTOCOL  *This,
+  IN EFI_DEVICE_PATH_PROTOCOL *FilePath,
+  IN BOOLEAN                  BootPolicy,
+  IN OUT UINTN                *BufferSize,
+  IN VOID                     *Buffer      OPTIONAL
+  );
+
+/**
+  Check if the RomImage contains EFI Images.
+
+  @param  RomImage  The ROM address of Image for check.
+  @param  RomSize   Size of ROM for check.
+
+  @retval TRUE     ROM contain EFI Image.
+  @retval FALSE    ROM not contain EFI Image.
+
+**/
+BOOLEAN
+ContainEfiImage (
+  IN VOID            *RomImage,
+  IN UINT64          RomSize
+  );
+
+/**
+  Get Pci device's oprom information.
+
+  @param PciIoDevice    Input Pci device instance.
+                        Output Pci device instance with updated OptionRom size.
+
+  @retval EFI_NOT_FOUND Pci device has not Option Rom.
+  @retval EFI_SUCCESS   Pci device has Option Rom.
+
+**/
+EFI_STATUS
+GetOpRomInfo (
+  IN OUT PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  Load Option Rom image for specified PCI device.
+
+  @param PciDevice Pci device instance.
+  @param RomBase   Base address of Option Rom.
+
+  @retval EFI_OUT_OF_RESOURCES No enough memory to hold image.
+  @retval EFI_SUCESS           Successfully loaded Option Rom.
+
+**/
+EFI_STATUS
+LoadOpRomImage (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT64          RomBase
+  );
+
+/**
+  Enable/Disable Option Rom decode.
+
+  @param PciDevice    Pci device instance.
+  @param RomBarIndex  The BAR index of the standard PCI Configuration header to use as the
+                      base address for resource range. The legal range for this field is 0..5.
+  @param RomBar       Base address of Option Rom.
+  @param Enable       Flag for enable/disable decode.
+
+**/
+VOID
+RomDecode (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT8           RomBarIndex,
+  IN UINT32          RomBar,
+  IN BOOLEAN         Enable
+  );
+
+/**
+  Load and start the Option Rom image.
+
+  @param PciDevice       Pci device instance.
+
+  @retval EFI_SUCCESS    Successfully loaded and started PCI Option Rom image.
+  @retval EFI_NOT_FOUND  Failed to process PCI Option Rom image.
+
+**/
+EFI_STATUS
+ProcessOpRomImage (
+  IN PCI_IO_DEVICE   *PciDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.c
new file mode 100644
index 0000000000..ab655e7657
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.c
@@ -0,0 +1,88 @@
+/** @file
+  Power management support fucntions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2012, 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 "PciBus.h"
+
+/**
+  This function is intended to turn off PWE assertion and
+  put the device to D0 state if the device supports
+  PCI Power Management.
+
+  @param PciIoDevice      PCI device instance.
+
+  @retval EFI_UNSUPPORTED PCI Device does not support power management.
+  @retval EFI_SUCCESS     Turned off PWE successfully.
+
+**/
+EFI_STATUS
+ResetPowerManagementFeature (
+  IN PCI_IO_DEVICE *PciIoDevice
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       PowerManagementRegBlock;
+  UINT16      PowerManagementCSR;
+
+  PowerManagementRegBlock = 0;
+
+  Status = LocateCapabilityRegBlock (
+            PciIoDevice,
+            EFI_PCI_CAPABILITY_ID_PMI,
+            &PowerManagementRegBlock,
+            NULL
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Turn off the PWE assertion and put the device into D0 State
+  //
+
+  //
+  // Read PMCSR
+  //
+  Status = PciIoDevice->PciIo.Pci.Read (
+                                    &PciIoDevice->PciIo,
+                                    EfiPciIoWidthUint16,
+                                    PowerManagementRegBlock + 4,
+                                    1,
+                                    &PowerManagementCSR
+                                    );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Clear PME_Status bit
+    //
+    PowerManagementCSR |= BIT15;
+    //
+    // Clear PME_En bit. PowerState = D0.
+    //
+    PowerManagementCSR &= ~(BIT8 | BIT1 | BIT0);
+
+    //
+    // Write PMCSR
+    //
+    Status = PciIoDevice->PciIo.Pci.Write (
+                                      &PciIoDevice->PciIo,
+                                      EfiPciIoWidthUint16,
+                                      PowerManagementRegBlock + 4,
+                                      1,
+                                      &PowerManagementCSR
+                                      );
+  }
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.h
new file mode 100644
index 0000000000..45ba59f286
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.h
@@ -0,0 +1,34 @@
+/** @file
+  Power management support fucntions delaration for PCI Bus module.
+
+Copyright (c) 2006 - 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_POWER_MANAGEMENT_H_
+#define _EFI_PCI_POWER_MANAGEMENT_H_
+
+/**
+  This function is intended to turn off PWE assertion and
+  put the device to D0 state if the device supports
+  PCI Power Management.
+
+  @param PciIoDevice      PCI device instance.
+
+  @retval EFI_UNSUPPORTED PCI Device does not support power management.
+  @retval EFI_SUCCESS     Turned off PWE successfully.
+
+**/
+EFI_STATUS
+ResetPowerManagementFeature (
+  IN PCI_IO_DEVICE *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.c
new file mode 100644
index 0000000000..e12d59f1d0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.c
@@ -0,0 +1,2291 @@
+/** @file
+  PCI resouces support functions implemntation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+//
+// The default policy for the PCI bus driver is NOT to reserve I/O ranges for both ISA aliases and VGA aliases.
+//
+BOOLEAN mReserveIsaAliases = FALSE;
+BOOLEAN mReserveVgaAliases = FALSE;
+BOOLEAN mPolicyDetermined  = FALSE;
+
+/**
+  The function is used to skip VGA range.
+
+  @param Start    Returned start address including VGA range.
+  @param Length   The length of VGA range.
+
+**/
+VOID
+SkipVGAAperture (
+  OUT UINT64   *Start,
+  IN  UINT64   Length
+  )
+{
+  UINT64  Original;
+  UINT64  Mask;
+  UINT64  StartOffset;
+  UINT64  LimitOffset;
+
+  ASSERT (Start != NULL);
+  //
+  // For legacy VGA, bit 10 to bit 15 is not decoded
+  //
+  Mask        = 0x3FF;
+
+  Original    = *Start;
+  StartOffset = Original & Mask;
+  LimitOffset = ((*Start) + Length - 1) & Mask;
+  if (LimitOffset >= VGABASE1) {
+    *Start = *Start - StartOffset + VGALIMIT2 + 1;
+  }
+}
+
+/**
+  This function is used to skip ISA aliasing aperture.
+
+  @param Start    Returned start address including ISA aliasing aperture.
+  @param Length   The length of ISA aliasing aperture.
+
+**/
+VOID
+SkipIsaAliasAperture (
+  OUT UINT64   *Start,
+  IN  UINT64   Length
+  )
+{
+
+  UINT64  Original;
+  UINT64  Mask;
+  UINT64  StartOffset;
+  UINT64  LimitOffset;
+
+  ASSERT (Start != NULL);
+
+  //
+  // For legacy ISA, bit 10 to bit 15 is not decoded
+  //
+  Mask        = 0x3FF;
+
+  Original    = *Start;
+  StartOffset = Original & Mask;
+  LimitOffset = ((*Start) + Length - 1) & Mask;
+
+  if (LimitOffset >= ISABASE) {
+    *Start = *Start - StartOffset + ISALIMIT + 1;
+  }
+}
+
+/**
+  This function inserts a resource node into the resource list.
+  The resource list is sorted in descend order.
+
+  @param Bridge  PCI resource node for bridge.
+  @param ResNode Resource node want to be inserted.
+
+**/
+VOID
+InsertResourceNode (
+  IN OUT PCI_RESOURCE_NODE   *Bridge,
+  IN     PCI_RESOURCE_NODE   *ResNode
+  )
+{
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *Temp;
+  UINT64            ResNodeAlignRest;
+  UINT64            TempAlignRest;
+
+  ASSERT (Bridge  != NULL);
+  ASSERT (ResNode != NULL);
+
+  InsertHeadList (&Bridge->ChildList, &ResNode->Link);
+
+  CurrentLink = Bridge->ChildList.ForwardLink->ForwardLink;
+  while (CurrentLink != &Bridge->ChildList) {
+    Temp = RESOURCE_NODE_FROM_LINK (CurrentLink);
+
+    if (ResNode->Alignment > Temp->Alignment) {
+      break;
+    } else if (ResNode->Alignment == Temp->Alignment) {
+      ResNodeAlignRest  = ResNode->Length & ResNode->Alignment;
+      TempAlignRest     = Temp->Length & Temp->Alignment;
+      if ((ResNodeAlignRest == 0) || (ResNodeAlignRest >= TempAlignRest)) {
+        break;
+      }
+    }
+
+    SwapListEntries (&ResNode->Link, CurrentLink);
+
+    CurrentLink = ResNode->Link.ForwardLink;
+  }
+}
+
+/**
+  This routine is used to merge two different resource trees in need of
+  resoure degradation.
+
+  For example, if an upstream PPB doesn't support,
+  prefetchable memory decoding, the PCI bus driver will choose to call this function
+  to merge prefectchable memory resource list into normal memory list.
+
+  If the TypeMerge is TRUE, Res resource type is changed to the type of destination resource
+  type.
+  If Dst is NULL or Res is NULL, ASSERT ().
+
+  @param Dst        Point to destination resource tree.
+  @param Res        Point to source resource tree.
+  @param TypeMerge  If the TypeMerge is TRUE, Res resource type is changed to the type of
+                    destination resource type.
+
+**/
+VOID
+MergeResourceTree (
+  IN PCI_RESOURCE_NODE   *Dst,
+  IN PCI_RESOURCE_NODE   *Res,
+  IN BOOLEAN             TypeMerge
+  )
+{
+
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *Temp;
+
+  ASSERT (Dst != NULL);
+  ASSERT (Res != NULL);
+
+  while (!IsListEmpty (&Res->ChildList)) {
+    CurrentLink = Res->ChildList.ForwardLink;
+
+    Temp        = RESOURCE_NODE_FROM_LINK (CurrentLink);
+
+    if (TypeMerge) {
+      Temp->ResType = Dst->ResType;
+    }
+
+    RemoveEntryList (CurrentLink);
+    InsertResourceNode (Dst, Temp);
+  }
+}
+
+/**
+  This function is used to calculate the IO16 aperture
+  for a bridge.
+
+  @param Bridge    PCI resource node for bridge.
+
+**/
+VOID
+CalculateApertureIo16 (
+  IN PCI_RESOURCE_NODE    *Bridge
+  )
+{
+  EFI_STATUS              Status;
+  UINT64                  Aperture;
+  LIST_ENTRY              *CurrentLink;
+  PCI_RESOURCE_NODE       *Node;
+  UINT64                  Offset;
+  EFI_PCI_PLATFORM_POLICY PciPolicy;
+  UINT64                  PaddingAperture;
+
+  if (!mPolicyDetermined) {
+    //
+    // Check PciPlatform policy
+    //
+    Status = EFI_NOT_FOUND;
+    PciPolicy = 0;
+    if (gPciPlatformProtocol != NULL) {
+      Status = gPciPlatformProtocol->GetPlatformPolicy (
+                                       gPciPlatformProtocol,
+                                       &PciPolicy
+                                       );
+    }
+
+    if (EFI_ERROR (Status) && gPciOverrideProtocol != NULL) {
+      Status = gPciOverrideProtocol->GetPlatformPolicy (
+                                       gPciOverrideProtocol,
+                                       &PciPolicy
+                                       );
+    }
+
+    if (!EFI_ERROR (Status)) {
+      if ((PciPolicy & EFI_RESERVE_ISA_IO_ALIAS) != 0) {
+        mReserveIsaAliases = TRUE;
+      }
+      if ((PciPolicy & EFI_RESERVE_VGA_IO_ALIAS) != 0) {
+        mReserveVgaAliases = TRUE;
+      }
+    }
+    mPolicyDetermined = TRUE;
+  }
+
+  Aperture        = 0;
+  PaddingAperture = 0;
+
+  if (Bridge == NULL) {
+    return ;
+  }
+
+  //
+  // Assume the bridge is aligned
+  //
+  for ( CurrentLink = GetFirstNode (&Bridge->ChildList)
+      ; !IsNull (&Bridge->ChildList, CurrentLink)
+      ; CurrentLink = GetNextNode (&Bridge->ChildList, CurrentLink)
+      ) {
+
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    if (Node->ResourceUsage == PciResUsagePadding) {
+      ASSERT (PaddingAperture == 0);
+      PaddingAperture = Node->Length;
+      continue;
+    }
+    //
+    // Consider the aperture alignment
+    //
+    Offset = Aperture & (Node->Alignment);
+
+    if (Offset != 0) {
+
+      Aperture = Aperture + (Node->Alignment + 1) - Offset;
+
+    }
+
+    //
+    // IsaEnable and VGAEnable can not be implemented now.
+    // If both of them are enabled, then the IO resource would
+    // become too limited to meet the requirement of most of devices.
+    //
+    if (mReserveIsaAliases || mReserveVgaAliases) {
+      if (!IS_PCI_BRIDGE (&(Node->PciDev->Pci)) && !IS_CARDBUS_BRIDGE (&(Node->PciDev->Pci))) {
+        //
+        // Check if there is need to support ISA/VGA decoding
+        // If so, we need to avoid isa/vga aliasing range
+        //
+        if (mReserveIsaAliases) {
+          SkipIsaAliasAperture (
+            &Aperture,
+            Node->Length
+            );
+          Offset = Aperture & (Node->Alignment);
+          if (Offset != 0) {
+            Aperture = Aperture + (Node->Alignment + 1) - Offset;
+          }
+        } else if (mReserveVgaAliases) {
+          SkipVGAAperture (
+            &Aperture,
+            Node->Length
+            );
+          Offset = Aperture & (Node->Alignment);
+          if (Offset != 0) {
+            Aperture = Aperture + (Node->Alignment + 1) - Offset;
+          }
+        }
+      }
+    }
+
+    Node->Offset = Aperture;
+
+    //
+    // Increment aperture by the length of node
+    //
+    Aperture += Node->Length;
+  }
+
+  //
+  // Adjust the aperture with the bridge's alignment
+  //
+  Offset = Aperture & (Bridge->Alignment);
+
+  if (Offset != 0) {
+    Aperture = Aperture + (Bridge->Alignment + 1) - Offset;
+  }
+
+  Bridge->Length = Aperture;
+  //
+  // At last, adjust the bridge's alignment to the first child's alignment
+  // if the bridge has at least one child
+  //
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  if (CurrentLink != &Bridge->ChildList) {
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    if (Node->Alignment > Bridge->Alignment) {
+      Bridge->Alignment = Node->Alignment;
+    }
+  }
+
+  //
+  // Hotplug controller needs padding resources.
+  // Use the larger one between the padding resource and actual occupied resource.
+  //
+  Bridge->Length = MAX (Bridge->Length, PaddingAperture);
+}
+
+/**
+  This function is used to calculate the resource aperture
+  for a given bridge device.
+
+  @param Bridge      PCI resouce node for given bridge device.
+
+**/
+VOID
+CalculateResourceAperture (
+  IN PCI_RESOURCE_NODE    *Bridge
+  )
+{
+  UINT64            Aperture;
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *Node;
+  UINT64            PaddingAperture;
+  UINT64            Offset;
+
+  Aperture        = 0;
+  PaddingAperture = 0;
+
+  if (Bridge == NULL) {
+    return ;
+  }
+
+  if (Bridge->ResType == PciBarTypeIo16) {
+
+    CalculateApertureIo16 (Bridge);
+    return ;
+  }
+
+  //
+  // Assume the bridge is aligned
+  //
+  for ( CurrentLink = GetFirstNode (&Bridge->ChildList)
+      ; !IsNull (&Bridge->ChildList, CurrentLink)
+      ; CurrentLink = GetNextNode (&Bridge->ChildList, CurrentLink)
+      ) {
+
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    if (Node->ResourceUsage == PciResUsagePadding) {
+      ASSERT (PaddingAperture == 0);
+      PaddingAperture = Node->Length;
+      continue;
+    }
+
+    //
+    // Apply padding resource if available
+    //
+    Offset = Aperture & (Node->Alignment);
+
+    if (Offset != 0) {
+
+      Aperture = Aperture + (Node->Alignment + 1) - Offset;
+
+    }
+
+    //
+    // Recode current aperture as a offset
+    // this offset will be used in future real allocation
+    //
+    Node->Offset = Aperture;
+
+    //
+    // Increment aperture by the length of node
+    //
+    Aperture += Node->Length;
+  }
+
+  //
+  // At last, adjust the aperture with the bridge's
+  // alignment
+  //
+  Offset = Aperture & (Bridge->Alignment);
+  if (Offset != 0) {
+    Aperture = Aperture + (Bridge->Alignment + 1) - Offset;
+  }
+
+  //
+  // If the bridge has already padded the resource and the
+  // amount of padded resource is larger, then keep the
+  // padded resource
+  //
+  if (Bridge->Length < Aperture) {
+    Bridge->Length = Aperture;
+  }
+
+  //
+  // Adjust the bridge's alignment to the first child's alignment
+  // if the bridge has at least one child
+  //
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  if (CurrentLink != &Bridge->ChildList) {
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    if (Node->Alignment > Bridge->Alignment) {
+      Bridge->Alignment = Node->Alignment;
+    }
+  }
+
+  //
+  // Hotplug controller needs padding resources.
+  // Use the larger one between the padding resource and actual occupied resource.
+  //
+  Bridge->Length = MAX (Bridge->Length, PaddingAperture);
+}
+
+/**
+  Get IO/Memory resource infor for given PCI device.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO .
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+GetResourceFromDevice (
+  IN     PCI_IO_DEVICE     *PciDev,
+  IN OUT PCI_RESOURCE_NODE *IoNode,
+  IN OUT PCI_RESOURCE_NODE *Mem32Node,
+  IN OUT PCI_RESOURCE_NODE *PMem32Node,
+  IN OUT PCI_RESOURCE_NODE *Mem64Node,
+  IN OUT PCI_RESOURCE_NODE *PMem64Node
+  )
+{
+
+  UINT8             Index;
+  PCI_RESOURCE_NODE *Node;
+  BOOLEAN           ResourceRequested;
+
+  Node              = NULL;
+  ResourceRequested = FALSE;
+
+  for (Index = 0; Index < PCI_MAX_BAR; Index++) {
+
+    switch ((PciDev->PciBar)[Index].BarType) {
+
+    case PciBarTypeMem32:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypeMem32,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        Mem32Node,
+        Node
+        );
+
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypeMem64:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypeMem64,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        Mem64Node,
+        Node
+        );
+
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypePMem64:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypePMem64,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        PMem64Node,
+        Node
+        );
+
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypePMem32:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypePMem32,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        PMem32Node,
+        Node
+        );
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypeIo16:
+    case PciBarTypeIo32:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypeIo16,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        IoNode,
+        Node
+        );
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypeUnknown:
+      break;
+
+    default:
+      break;
+    }
+  }
+
+  //
+  // Add VF resource
+  //
+  for (Index = 0; Index < PCI_MAX_BAR; Index++) {
+
+    switch ((PciDev->VfPciBar)[Index].BarType) {
+
+    case PciBarTypeMem32:
+
+      Node = CreateVfResourceNode (
+              PciDev,
+              (PciDev->VfPciBar)[Index].Length,
+              (PciDev->VfPciBar)[Index].Alignment,
+              Index,
+              PciBarTypeMem32,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        Mem32Node,
+        Node
+        );
+
+      break;
+
+    case PciBarTypeMem64:
+
+      Node = CreateVfResourceNode (
+              PciDev,
+              (PciDev->VfPciBar)[Index].Length,
+              (PciDev->VfPciBar)[Index].Alignment,
+              Index,
+              PciBarTypeMem64,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        Mem64Node,
+        Node
+        );
+
+      break;
+
+    case PciBarTypePMem64:
+
+      Node = CreateVfResourceNode (
+              PciDev,
+              (PciDev->VfPciBar)[Index].Length,
+              (PciDev->VfPciBar)[Index].Alignment,
+              Index,
+              PciBarTypePMem64,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        PMem64Node,
+        Node
+        );
+
+      break;
+
+    case PciBarTypePMem32:
+
+      Node = CreateVfResourceNode (
+              PciDev,
+              (PciDev->VfPciBar)[Index].Length,
+              (PciDev->VfPciBar)[Index].Alignment,
+              Index,
+              PciBarTypePMem32,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        PMem32Node,
+        Node
+        );
+      break;
+
+    case PciBarTypeIo16:
+    case PciBarTypeIo32:
+      break;
+
+    case PciBarTypeUnknown:
+      break;
+
+    default:
+      break;
+    }
+  }
+  // If there is no resource requested from this device,
+  // then we indicate this device has been allocated naturally.
+  //
+  if (!ResourceRequested) {
+    PciDev->Allocated = TRUE;
+  }
+}
+
+/**
+  This function is used to create a resource node.
+
+  @param PciDev       Pci device instance.
+  @param Length       Length of Io/Memory resource.
+  @param Alignment    Alignment of resource.
+  @param Bar          Bar index.
+  @param ResType      Type of resource: IO/Memory.
+  @param ResUsage     Resource usage.
+
+  @return PCI resource node created for given PCI device.
+          NULL means PCI resource node is not created.
+
+**/
+PCI_RESOURCE_NODE *
+CreateResourceNode (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN UINT64                Length,
+  IN UINT64                Alignment,
+  IN UINT8                 Bar,
+  IN PCI_BAR_TYPE          ResType,
+  IN PCI_RESOURCE_USAGE    ResUsage
+  )
+{
+  PCI_RESOURCE_NODE *Node;
+
+  Node    = NULL;
+
+  Node    = AllocateZeroPool (sizeof (PCI_RESOURCE_NODE));
+  ASSERT (Node != NULL);
+  if (Node == NULL) {
+    return NULL;
+  }
+
+  Node->Signature     = PCI_RESOURCE_SIGNATURE;
+  Node->PciDev        = PciDev;
+  Node->Length        = Length;
+  Node->Alignment     = Alignment;
+  Node->Bar           = Bar;
+  Node->ResType       = ResType;
+  Node->Reserved      = FALSE;
+  Node->ResourceUsage = ResUsage;
+  InitializeListHead (&Node->ChildList);
+
+  return Node;
+}
+
+/**
+  This function is used to create a IOV VF resource node.
+
+  @param PciDev       Pci device instance.
+  @param Length       Length of Io/Memory resource.
+  @param Alignment    Alignment of resource.
+  @param Bar          Bar index.
+  @param ResType      Type of resource: IO/Memory.
+  @param ResUsage     Resource usage.
+
+  @return PCI resource node created for given VF PCI device.
+          NULL means PCI resource node is not created.
+
+**/
+PCI_RESOURCE_NODE *
+CreateVfResourceNode (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN UINT64                Length,
+  IN UINT64                Alignment,
+  IN UINT8                 Bar,
+  IN PCI_BAR_TYPE          ResType,
+  IN PCI_RESOURCE_USAGE    ResUsage
+  )
+{
+  PCI_RESOURCE_NODE *Node;
+
+  Node = CreateResourceNode (PciDev, Length, Alignment, Bar, ResType, ResUsage);
+  if (Node == NULL) {
+    return Node;
+  }
+
+  Node->Virtual = TRUE;
+
+  return Node;
+}
+
+/**
+  This function is used to extract resource request from
+  device node list.
+
+  @param Bridge     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+CreateResourceMap (
+  IN     PCI_IO_DEVICE     *Bridge,
+  IN OUT PCI_RESOURCE_NODE *IoNode,
+  IN OUT PCI_RESOURCE_NODE *Mem32Node,
+  IN OUT PCI_RESOURCE_NODE *PMem32Node,
+  IN OUT PCI_RESOURCE_NODE *Mem64Node,
+  IN OUT PCI_RESOURCE_NODE *PMem64Node
+  )
+{
+  PCI_IO_DEVICE     *Temp;
+  PCI_RESOURCE_NODE *IoBridge;
+  PCI_RESOURCE_NODE *Mem32Bridge;
+  PCI_RESOURCE_NODE *PMem32Bridge;
+  PCI_RESOURCE_NODE *Mem64Bridge;
+  PCI_RESOURCE_NODE *PMem64Bridge;
+  LIST_ENTRY        *CurrentLink;
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    //
+    // Create resource nodes for this device by scanning the
+    // Bar array in the device private data
+    // If the upstream bridge doesn't support this device,
+    // no any resource node will be created for this device
+    //
+    GetResourceFromDevice (
+      Temp,
+      IoNode,
+      Mem32Node,
+      PMem32Node,
+      Mem64Node,
+      PMem64Node
+      );
+
+    if (IS_PCI_BRIDGE (&Temp->Pci)) {
+
+      //
+      // If the device has children, create a bridge resource node for this PPB
+      // Note: For PPB, memory aperture is aligned with 1MB and IO aperture
+      // is aligned with 4KB (smaller alignments may be supported).
+      //
+      IoBridge = CreateResourceNode (
+                   Temp,
+                   0,
+                   Temp->BridgeIoAlignment,
+                   PPB_IO_RANGE,
+                   PciBarTypeIo16,
+                   PciResUsageTypical
+                   );
+
+      Mem32Bridge = CreateResourceNode (
+                      Temp,
+                      0,
+                      0xFFFFF,
+                      PPB_MEM32_RANGE,
+                      PciBarTypeMem32,
+                      PciResUsageTypical
+                      );
+
+      PMem32Bridge = CreateResourceNode (
+                       Temp,
+                       0,
+                       0xFFFFF,
+                       PPB_PMEM32_RANGE,
+                       PciBarTypePMem32,
+                       PciResUsageTypical
+                       );
+
+      Mem64Bridge = CreateResourceNode (
+                      Temp,
+                      0,
+                      0xFFFFF,
+                      PPB_MEM64_RANGE,
+                      PciBarTypeMem64,
+                      PciResUsageTypical
+                      );
+
+      PMem64Bridge = CreateResourceNode (
+                       Temp,
+                       0,
+                       0xFFFFF,
+                       PPB_PMEM64_RANGE,
+                       PciBarTypePMem64,
+                       PciResUsageTypical
+                       );
+
+      //
+      // Recursively create resouce map on this bridge
+      //
+      CreateResourceMap (
+        Temp,
+        IoBridge,
+        Mem32Bridge,
+        PMem32Bridge,
+        Mem64Bridge,
+        PMem64Bridge
+        );
+
+      if (ResourceRequestExisted (IoBridge)) {
+        InsertResourceNode (
+          IoNode,
+          IoBridge
+          );
+      } else {
+        FreePool (IoBridge);
+        IoBridge = NULL;
+      }
+
+      //
+      // If there is node under this resource bridge,
+      // then calculate bridge's aperture of this type
+      // and insert it into the respective resource tree.
+      // If no, delete this resource bridge
+      //
+      if (ResourceRequestExisted (Mem32Bridge)) {
+        InsertResourceNode (
+          Mem32Node,
+          Mem32Bridge
+          );
+      } else {
+        FreePool (Mem32Bridge);
+        Mem32Bridge = NULL;
+      }
+
+      //
+      // If there is node under this resource bridge,
+      // then calculate bridge's aperture of this type
+      // and insert it into the respective resource tree.
+      // If no, delete this resource bridge
+      //
+      if (ResourceRequestExisted (PMem32Bridge)) {
+        InsertResourceNode (
+          PMem32Node,
+          PMem32Bridge
+          );
+      } else {
+        FreePool (PMem32Bridge);
+        PMem32Bridge = NULL;
+      }
+
+      //
+      // If there is node under this resource bridge,
+      // then calculate bridge's aperture of this type
+      // and insert it into the respective resource tree.
+      // If no, delete this resource bridge
+      //
+      if (ResourceRequestExisted (Mem64Bridge)) {
+        InsertResourceNode (
+          Mem64Node,
+          Mem64Bridge
+          );
+      } else {
+        FreePool (Mem64Bridge);
+        Mem64Bridge = NULL;
+      }
+
+      //
+      // If there is node under this resource bridge,
+      // then calculate bridge's aperture of this type
+      // and insert it into the respective resource tree.
+      // If no, delete this resource bridge
+      //
+      if (ResourceRequestExisted (PMem64Bridge)) {
+        InsertResourceNode (
+          PMem64Node,
+          PMem64Bridge
+          );
+      } else {
+        FreePool (PMem64Bridge);
+        PMem64Bridge = NULL;
+      }
+
+    }
+
+    //
+    // If it is P2C, apply hard coded resource padding
+    //
+    if (IS_CARDBUS_BRIDGE (&Temp->Pci)) {
+      ResourcePaddingForCardBusBridge (
+        Temp,
+        IoNode,
+        Mem32Node,
+        PMem32Node,
+        Mem64Node,
+        PMem64Node
+        );
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  //
+  // To do some platform specific resource padding ...
+  //
+  ResourcePaddingPolicy (
+    Bridge,
+    IoNode,
+    Mem32Node,
+    PMem32Node,
+    Mem64Node,
+    PMem64Node
+    );
+
+  //
+  // Degrade resource if necessary
+  //
+  DegradeResource (
+    Bridge,
+    Mem32Node,
+    PMem32Node,
+    Mem64Node,
+    PMem64Node
+    );
+
+  //
+  // Calculate resource aperture for this bridge device
+  //
+  CalculateResourceAperture (Mem32Node);
+  CalculateResourceAperture (PMem32Node);
+  CalculateResourceAperture (Mem64Node);
+  CalculateResourceAperture (PMem64Node);
+  CalculateResourceAperture (IoNode);
+}
+
+/**
+  This function is used to do the resource padding for a specific platform.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ResourcePaddingPolicy (
+  IN PCI_IO_DEVICE     *PciDev,
+  IN PCI_RESOURCE_NODE *IoNode,
+  IN PCI_RESOURCE_NODE *Mem32Node,
+  IN PCI_RESOURCE_NODE *PMem32Node,
+  IN PCI_RESOURCE_NODE *Mem64Node,
+  IN PCI_RESOURCE_NODE *PMem64Node
+  )
+{
+  //
+  // Create padding resource node
+  //
+  if (PciDev->ResourcePaddingDescriptors != NULL) {
+    ApplyResourcePadding (
+      PciDev,
+      IoNode,
+      Mem32Node,
+      PMem32Node,
+      Mem64Node,
+      PMem64Node
+      );
+  }
+}
+
+/**
+  This function is used to degrade resource if the upstream bridge
+  doesn't support certain resource. Degradation path is
+  PMEM64 -> MEM64  -> MEM32
+  PMEM64 -> PMEM32 -> MEM32
+  IO32   -> IO16.
+
+  @param Bridge     Pci device instance.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+DegradeResource (
+  IN PCI_IO_DEVICE     *Bridge,
+  IN PCI_RESOURCE_NODE *Mem32Node,
+  IN PCI_RESOURCE_NODE *PMem32Node,
+  IN PCI_RESOURCE_NODE *Mem64Node,
+  IN PCI_RESOURCE_NODE *PMem64Node
+  )
+{
+  PCI_IO_DEVICE        *Temp;
+  LIST_ENTRY           *ChildDeviceLink;
+  LIST_ENTRY           *ChildNodeLink;
+  LIST_ENTRY           *NextChildNodeLink;
+  PCI_RESOURCE_NODE    *TempNode;
+
+  //
+  // If any child device has both option ROM and 64-bit BAR, degrade its PMEM64/MEM64
+  // requests in case that if a legacy option ROM image can not access 64-bit resources.
+  //
+  ChildDeviceLink = Bridge->ChildList.ForwardLink;
+  while (ChildDeviceLink != NULL && ChildDeviceLink != &Bridge->ChildList) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (ChildDeviceLink);
+    if (Temp->RomSize != 0) {
+      if (!IsListEmpty (&Mem64Node->ChildList)) {      
+        ChildNodeLink = Mem64Node->ChildList.ForwardLink;
+        while (ChildNodeLink != &Mem64Node->ChildList) {
+          TempNode = RESOURCE_NODE_FROM_LINK (ChildNodeLink);
+          NextChildNodeLink = ChildNodeLink->ForwardLink;
+
+          if (TempNode->PciDev == Temp) {
+            RemoveEntryList (ChildNodeLink);
+            InsertResourceNode (Mem32Node, TempNode);
+          }
+          ChildNodeLink = NextChildNodeLink;
+        }        
+      }
+
+      if (!IsListEmpty (&PMem64Node->ChildList)) {      
+        ChildNodeLink = PMem64Node->ChildList.ForwardLink;
+        while (ChildNodeLink != &PMem64Node->ChildList) {
+          TempNode = RESOURCE_NODE_FROM_LINK (ChildNodeLink);
+          NextChildNodeLink = ChildNodeLink->ForwardLink;
+
+          if (TempNode->PciDev == Temp) {
+            RemoveEntryList (ChildNodeLink);
+            InsertResourceNode (PMem32Node, TempNode);
+          }
+          ChildNodeLink = NextChildNodeLink;
+        }        
+      }
+
+    }
+    ChildDeviceLink = ChildDeviceLink->ForwardLink;
+  }
+
+  //
+  // If firmware is in 32-bit mode,
+  // then degrade PMEM64/MEM64 requests
+  //
+  if (sizeof (UINTN) <= 4) {
+    MergeResourceTree (
+      Mem32Node,
+      Mem64Node,
+      TRUE
+      );
+
+    MergeResourceTree (
+      PMem32Node,
+      PMem64Node,
+      TRUE
+      );
+  } else {
+    //
+    // if the bridge does not support MEM64, degrade MEM64 to MEM32
+    //
+    if (!BridgeSupportResourceDecode (Bridge, EFI_BRIDGE_MEM64_DECODE_SUPPORTED)) {
+        MergeResourceTree (
+          Mem32Node,
+          Mem64Node,
+          TRUE
+          );
+    }
+
+    //
+    // if the bridge does not support PMEM64, degrade PMEM64 to PMEM32
+    //
+    if (!BridgeSupportResourceDecode (Bridge, EFI_BRIDGE_PMEM64_DECODE_SUPPORTED)) {
+      MergeResourceTree (
+        PMem32Node,
+        PMem64Node,
+        TRUE
+        );
+    } 
+
+    //
+    // if both PMEM64 and PMEM32 requests from child devices, which can not be satisfied
+    // by a P2P bridge simultaneously, keep PMEM64 and degrade PMEM32 to MEM32.
+    //
+    if (!IsListEmpty (&PMem64Node->ChildList) && Bridge->Parent != NULL) {
+      MergeResourceTree (
+        Mem32Node,
+        PMem32Node,
+        TRUE
+        );
+    }
+  }
+
+  //
+  // If bridge doesn't support Pmem32
+  // degrade it to mem32
+  //
+  if (!BridgeSupportResourceDecode (Bridge, EFI_BRIDGE_PMEM32_DECODE_SUPPORTED)) {
+    MergeResourceTree (
+      Mem32Node,
+      PMem32Node,
+      TRUE
+      );
+  }
+
+  //
+  // if root bridge supports combined Pmem Mem decoding
+  // merge these two type of resource
+  //
+  if (BridgeSupportResourceDecode (Bridge, EFI_BRIDGE_PMEM_MEM_COMBINE_SUPPORTED)) {
+    MergeResourceTree (
+      Mem32Node,
+      PMem32Node,
+      FALSE
+      );
+
+    //
+    // No need to check if to degrade MEM64 after merge, because
+    // if there are PMEM64 still here, 64-bit decode should be supported
+    // by the root bride.
+    //
+    MergeResourceTree (
+      Mem64Node,
+      PMem64Node,
+      FALSE
+      );
+  }
+}
+
+/**
+  Test whether bridge device support decode resource.
+
+  @param Bridge    Bridge device instance.
+  @param Decode    Decode type according to resource type.
+
+  @return TRUE     The bridge device support decode resource.
+  @return FALSE    The bridge device don't support decode resource.
+
+**/
+BOOLEAN
+BridgeSupportResourceDecode (
+  IN PCI_IO_DEVICE *Bridge,
+  IN UINT32        Decode
+  )
+{
+  if (((Bridge->Decodes) & Decode) != 0) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  This function is used to program the resource allocated
+  for each resource node under specified bridge.
+
+  @param Base     Base address of resource to be progammed.
+  @param Bridge   PCI resource node for the bridge device.
+
+  @retval EFI_SUCCESS            Successfully to program all resouces
+                                 on given PCI bridge device.
+  @retval EFI_OUT_OF_RESOURCES   Base is all one.
+
+**/
+EFI_STATUS
+ProgramResource (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Bridge
+  )
+{
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *Node;
+  EFI_STATUS        Status;
+
+  if (Base == gAllOne) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != &Bridge->ChildList) {
+
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+
+    if (!IS_PCI_BRIDGE (&(Node->PciDev->Pci))) {
+
+      if (IS_CARDBUS_BRIDGE (&(Node->PciDev->Pci))) {
+        //
+        // Program the PCI Card Bus device
+        //
+        ProgramP2C (Base, Node);
+      } else {
+        //
+        // Program the PCI device BAR
+        //
+        ProgramBar (Base, Node);
+      }
+    } else {
+      //
+      // Program the PCI devices under this bridge
+      //
+      Status = ProgramResource (Base + Node->Offset, Node);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      ProgramPpbApperture (Base, Node);
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Program Bar register for PCI device.
+
+  @param Base  Base address for PCI device resource to be progammed.
+  @param Node  Point to resoure node structure.
+
+**/
+VOID
+ProgramBar (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT64              Address;
+  UINT32              Address32;
+
+  ASSERT (Node->Bar < PCI_MAX_BAR);
+
+  //
+  // Check VF BAR
+  //
+  if (Node->Virtual) {
+    ProgramVfBar (Base, Node);
+    return;
+  }
+
+  Address = 0;
+  PciIo   = &(Node->PciDev->PciIo);
+
+  Address = Base + Node->Offset;
+
+  //
+  // Indicate pci bus driver has allocated
+  // resource for this device
+  // It might be a temporary solution here since
+  // pci device could have multiple bar
+  //
+  Node->PciDev->Allocated = TRUE;
+
+  switch ((Node->PciDev->PciBar[Node->Bar]).BarType) {
+
+  case PciBarTypeIo16:
+  case PciBarTypeIo32:
+  case PciBarTypeMem32:
+  case PciBarTypePMem32:
+
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+
+    break;
+
+  case PciBarTypeMem64:
+  case PciBarTypePMem64:
+
+    Address32 = (UINT32) (Address & 0x00000000FFFFFFFF);
+
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) RShiftU64 (Address, 32);
+
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (UINT8) ((Node->PciDev->PciBar[Node->Bar]).Offset + 4),
+                 1,
+                 &Address32
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Program IOV VF Bar register for PCI device.
+
+  @param Base  Base address for PCI device resource to be progammed.
+  @param Node  Point to resoure node structure.
+
+**/
+EFI_STATUS
+ProgramVfBar (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT64              Address;
+  UINT32              Address32;
+
+  ASSERT (Node->Bar < PCI_MAX_BAR);
+  ASSERT (Node->Virtual);
+
+  Address = 0;
+  PciIo   = &(Node->PciDev->PciIo);
+
+  Address = Base + Node->Offset;
+
+  //
+  // Indicate pci bus driver has allocated
+  // resource for this device
+  // It might be a temporary solution here since
+  // pci device could have multiple bar
+  //
+  Node->PciDev->Allocated = TRUE;
+
+  switch ((Node->PciDev->VfPciBar[Node->Bar]).BarType) {
+
+  case PciBarTypeMem32:
+  case PciBarTypePMem32:
+
+    PciIo->Pci.Write (
+                PciIo,
+                EfiPciIoWidthUint32,
+                (Node->PciDev->VfPciBar[Node->Bar]).Offset,
+                1,
+                &Address
+                );
+
+    Node->PciDev->VfPciBar[Node->Bar].BaseAddress = Address;
+    break;
+
+  case PciBarTypeMem64:
+  case PciBarTypePMem64:
+
+    Address32 = (UINT32) (Address & 0x00000000FFFFFFFF);
+
+    PciIo->Pci.Write (
+                PciIo,
+                EfiPciIoWidthUint32,
+                (Node->PciDev->VfPciBar[Node->Bar]).Offset,
+                1,
+                &Address32
+                );
+
+    Address32 = (UINT32) RShiftU64 (Address, 32);
+
+    PciIo->Pci.Write (
+                PciIo,
+                EfiPciIoWidthUint32,
+                ((Node->PciDev->VfPciBar[Node->Bar]).Offset + 4),
+                1,
+                &Address32
+                );
+
+    Node->PciDev->VfPciBar[Node->Bar].BaseAddress = Address;
+    break;
+
+  case PciBarTypeIo16:
+  case PciBarTypeIo32:
+    break;
+
+  default:
+    break;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Program PCI-PCI bridge apperture.
+
+  @param Base  Base address for resource.
+  @param Node  Point to resoure node structure.
+
+**/
+VOID
+ProgramPpbApperture (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT64              Address;
+  UINT32              Address32;
+
+  Address = 0;
+  //
+  // If no device resource of this PPB, return anyway
+  // Apperture is set default in the initialization code
+  //
+  if (Node->Length == 0 || Node->ResourceUsage == PciResUsagePadding) {
+    //
+    // For padding resource node, just ignore when programming
+    //
+    return ;
+  }
+
+  PciIo   = &(Node->PciDev->PciIo);
+  Address = Base + Node->Offset;
+
+  //
+  // Indicate the PPB resource has been allocated
+  //
+  Node->PciDev->Allocated = TRUE;
+
+  switch (Node->Bar) {
+
+  case PPB_BAR_0:
+  case PPB_BAR_1:
+    switch ((Node->PciDev->PciBar[Node->Bar]).BarType) {
+
+      case PciBarTypeIo16:
+      case PciBarTypeIo32:
+      case PciBarTypeMem32:
+      case PciBarTypePMem32:
+
+        PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address
+                 );
+
+        Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+        Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+        break;
+
+      case PciBarTypeMem64:
+      case PciBarTypePMem64:
+
+        Address32 = (UINT32) (Address & 0x00000000FFFFFFFF);
+
+        PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address32
+                 );
+
+        Address32 = (UINT32) RShiftU64 (Address, 32);
+
+        PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (UINT8) ((Node->PciDev->PciBar[Node->Bar]).Offset + 4),
+                 1,
+                 &Address32
+                 );
+
+        Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+        Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+        break;
+
+      default:
+        break;
+    }
+    break;
+
+  case PPB_IO_RANGE:
+
+    Address32 = ((UINT32) (Address)) >> 8;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint8,
+                 0x1C,
+                 1,
+                 &Address32
+                 );
+
+    Address32 >>= 8;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x30,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) (Address + Node->Length - 1);
+    Address32 = ((UINT32) (Address32)) >> 8;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint8,
+                 0x1D,
+                 1,
+                 &Address32
+                 );
+
+    Address32 >>= 8;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x32,
+                 1,
+                 &Address32
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    break;
+
+  case PPB_MEM32_RANGE:
+
+    Address32 = ((UINT32) (Address)) >> 16;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x20,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) (Address + Node->Length - 1);
+    Address32 = ((UINT32) (Address32)) >> 16;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x22,
+                 1,
+                 &Address32
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    break;
+
+  case PPB_PMEM32_RANGE:
+  case PPB_PMEM64_RANGE:
+
+    Address32 = ((UINT32) (Address)) >> 16;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x24,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) (Address + Node->Length - 1);
+    Address32 = ((UINT32) (Address32)) >> 16;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x26,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) RShiftU64 (Address, 32);
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 0x28,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) RShiftU64 ((Address + Node->Length - 1), 32);
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 0x2C,
+                 1,
+                 &Address32
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Program parent bridge for Option Rom.
+
+  @param PciDevice      Pci deivce instance.
+  @param OptionRomBase  Base address for Optiona Rom.
+  @param Enable         Enable or disable PCI memory.
+
+**/
+VOID
+ProgrameUpstreamBridgeForRom (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT32          OptionRomBase,
+  IN BOOLEAN         Enable
+  )
+{
+  PCI_IO_DEVICE     *Parent;
+  PCI_RESOURCE_NODE Node;
+  //
+  // For root bridge, just return.
+  //
+  Parent = PciDevice->Parent;
+  ZeroMem (&Node, sizeof (Node));
+  while (Parent != NULL) {
+    if (!IS_PCI_BRIDGE (&Parent->Pci)) {
+      break;
+    }
+
+    Node.PciDev     = Parent;
+    Node.Length     = PciDevice->RomSize;
+    Node.Alignment  = 0;
+    Node.Bar        = PPB_MEM32_RANGE;
+    Node.ResType    = PciBarTypeMem32;
+    Node.Offset     = 0;
+
+    //
+    // Program PPB to only open a single <= 16MB apperture
+    //
+    if (Enable) {
+      ProgramPpbApperture (OptionRomBase, &Node);
+      PCI_ENABLE_COMMAND_REGISTER (Parent, EFI_PCI_COMMAND_MEMORY_SPACE);
+    } else {
+      InitializePpb (Parent);
+      PCI_DISABLE_COMMAND_REGISTER (Parent, EFI_PCI_COMMAND_MEMORY_SPACE);
+    }
+
+    Parent = Parent->Parent;
+  }
+}
+
+/**
+  Test whether resource exists for a bridge.
+
+  @param Bridge  Point to resource node for a bridge.
+
+  @retval TRUE   There is resource on the given bridge.
+  @retval FALSE  There isn't resource on the given bridge.
+
+**/
+BOOLEAN
+ResourceRequestExisted (
+  IN PCI_RESOURCE_NODE    *Bridge
+  )
+{
+  if (Bridge != NULL) {
+    if (!IsListEmpty (&Bridge->ChildList) || Bridge->Length != 0) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Initialize resource pool structure.
+
+  @param ResourcePool Point to resource pool structure. This pool
+                      is reset to all zero when returned.
+  @param ResourceType Type of resource.
+
+**/
+VOID
+InitializeResourcePool (
+  IN OUT PCI_RESOURCE_NODE   *ResourcePool,
+  IN     PCI_BAR_TYPE        ResourceType
+  )
+{
+  ZeroMem (ResourcePool, sizeof (PCI_RESOURCE_NODE));
+  ResourcePool->ResType   = ResourceType;
+  ResourcePool->Signature = PCI_RESOURCE_SIGNATURE;
+  InitializeListHead (&ResourcePool->ChildList);
+}
+
+/**
+  Destory given resource tree.
+
+  @param Bridge  PCI resource root node of resource tree.
+
+**/
+VOID
+DestroyResourceTree (
+  IN PCI_RESOURCE_NODE *Bridge
+  )
+{
+  PCI_RESOURCE_NODE *Temp;
+  LIST_ENTRY        *CurrentLink;
+
+  while (!IsListEmpty (&Bridge->ChildList)) {
+
+    CurrentLink = Bridge->ChildList.ForwardLink;
+
+    Temp        = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    ASSERT (Temp);
+
+    RemoveEntryList (CurrentLink);
+
+    if (IS_PCI_BRIDGE (&(Temp->PciDev->Pci))) {
+      DestroyResourceTree (Temp);
+    }
+
+    FreePool (Temp);
+  }
+}
+
+/**
+  Insert resource padding for P2C.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ResourcePaddingForCardBusBridge (
+  IN PCI_IO_DEVICE        *PciDev,
+  IN PCI_RESOURCE_NODE    *IoNode,
+  IN PCI_RESOURCE_NODE    *Mem32Node,
+  IN PCI_RESOURCE_NODE    *PMem32Node,
+  IN PCI_RESOURCE_NODE    *Mem64Node,
+  IN PCI_RESOURCE_NODE    *PMem64Node
+  )
+{
+  PCI_RESOURCE_NODE *Node;
+
+  Node = NULL;
+
+  //
+  // Memory Base/Limit Register 0
+  // Bar 1 denodes memory range 0
+  //
+  Node = CreateResourceNode (
+           PciDev,
+           0x2000000,
+           0x1ffffff,
+           1,
+           PciBarTypeMem32,
+           PciResUsagePadding
+           );
+
+  InsertResourceNode (
+    Mem32Node,
+    Node
+    );
+
+  //
+  // Memory Base/Limit Register 1
+  // Bar 2 denodes memory range1
+  //
+  Node = CreateResourceNode (
+           PciDev,
+           0x2000000,
+           0x1ffffff,
+           2,
+           PciBarTypePMem32,
+           PciResUsagePadding
+           );
+
+  InsertResourceNode (
+    PMem32Node,
+    Node
+    );
+
+  //
+  // Io Base/Limit
+  // Bar 3 denodes io range 0
+  //
+  Node = CreateResourceNode (
+           PciDev,
+           0x100,
+           0xff,
+           3,
+           PciBarTypeIo16,
+           PciResUsagePadding
+           );
+
+  InsertResourceNode (
+    IoNode,
+    Node
+    );
+
+  //
+  // Io Base/Limit
+  // Bar 4 denodes io range 0
+  //
+  Node = CreateResourceNode (
+           PciDev,
+           0x100,
+           0xff,
+           4,
+           PciBarTypeIo16,
+           PciResUsagePadding
+           );
+
+  InsertResourceNode (
+    IoNode,
+    Node
+    );
+}
+
+/**
+  Program PCI Card device register for given resource node.
+
+  @param Base    Base address of PCI Card device to be programmed.
+  @param Node    Given resource node.
+
+**/
+VOID
+ProgramP2C (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT64              Address;
+  UINT64              TempAddress;
+  UINT16              BridgeControl;
+
+  Address = 0;
+  PciIo   = &(Node->PciDev->PciIo);
+
+  Address = Base + Node->Offset;
+
+  //
+  // Indicate pci bus driver has allocated
+  // resource for this device
+  // It might be a temporary solution here since
+  // pci device could have multiple bar
+  //
+  Node->PciDev->Allocated = TRUE;
+
+  switch (Node->Bar) {
+
+  case P2C_BAR_0:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    break;
+
+  case P2C_MEM_1:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_MEMORY_BASE_0,
+                 1,
+                 &Address
+                 );
+
+    TempAddress = Address + Node->Length - 1;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_MEMORY_LIMIT_0,
+                 1,
+                 &TempAddress
+                 );
+
+    if (Node->ResType == PciBarTypeMem32) {
+      //
+      // Set non-prefetchable bit
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+      BridgeControl &= (UINT16) ~PCI_CARD_PREFETCHABLE_MEMORY_0_ENABLE;
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+    } else {
+      //
+      // Set pre-fetchable bit
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+      BridgeControl |= PCI_CARD_PREFETCHABLE_MEMORY_0_ENABLE;
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+    }
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    Node->PciDev->PciBar[Node->Bar].BarType     = Node->ResType;
+
+    break;
+
+  case P2C_MEM_2:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_MEMORY_BASE_1,
+                 1,
+                 &Address
+                 );
+
+    TempAddress = Address + Node->Length - 1;
+
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_MEMORY_LIMIT_1,
+                 1,
+                 &TempAddress
+                 );
+
+    if (Node->ResType == PciBarTypeMem32) {
+
+      //
+      // Set non-prefetchable bit
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+      BridgeControl &= (UINT16) ~(PCI_CARD_PREFETCHABLE_MEMORY_1_ENABLE);
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+    } else {
+
+      //
+      // Set pre-fetchable bit
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+      BridgeControl |= PCI_CARD_PREFETCHABLE_MEMORY_1_ENABLE;
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+    }
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    Node->PciDev->PciBar[Node->Bar].BarType     = Node->ResType;
+    break;
+
+  case P2C_IO_1:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_IO_BASE_0_LOWER,
+                 1,
+                 &Address
+                 );
+
+    TempAddress = Address + Node->Length - 1;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_IO_LIMIT_0_LOWER,
+                 1,
+                 &TempAddress
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    Node->PciDev->PciBar[Node->Bar].BarType     = Node->ResType;
+
+    break;
+
+  case P2C_IO_2:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_IO_BASE_1_LOWER,
+                 1,
+                 &Address
+                 );
+
+    TempAddress = Address + Node->Length - 1;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_IO_LIMIT_1_LOWER,
+                 1,
+                 &TempAddress
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    Node->PciDev->PciBar[Node->Bar].BarType     = Node->ResType;
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Create padding resource node.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ApplyResourcePadding (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN PCI_RESOURCE_NODE     *IoNode,
+  IN PCI_RESOURCE_NODE     *Mem32Node,
+  IN PCI_RESOURCE_NODE     *PMem32Node,
+  IN PCI_RESOURCE_NODE     *Mem64Node,
+  IN PCI_RESOURCE_NODE     *PMem64Node
+  )
+{
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Ptr;
+  PCI_RESOURCE_NODE                 *Node;
+  UINT8                             DummyBarIndex;
+
+  DummyBarIndex = 0;
+  Ptr           = PciDev->ResourcePaddingDescriptors;
+
+  while (((EFI_ACPI_END_TAG_DESCRIPTOR *) Ptr)->Desc != ACPI_END_TAG_DESCRIPTOR) {
+
+    if (Ptr->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR && Ptr->ResType == ACPI_ADDRESS_SPACE_TYPE_IO) {
+      if (Ptr->AddrLen != 0) {
+
+        Node = CreateResourceNode (
+                PciDev,
+                Ptr->AddrLen,
+                Ptr->AddrRangeMax,
+                DummyBarIndex,
+                PciBarTypeIo16,
+                PciResUsagePadding
+                );
+        InsertResourceNode (
+          IoNode,
+          Node
+          );
+      }
+
+      Ptr++;
+      continue;
+    }
+
+    if (Ptr->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR && Ptr->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+
+      if (Ptr->AddrSpaceGranularity == 32) {
+
+        //
+        // prefechable
+        //
+        if (Ptr->SpecificFlag == 0x6) {
+          if (Ptr->AddrLen != 0) {
+            Node = CreateResourceNode (
+                    PciDev,
+                    Ptr->AddrLen,
+                    Ptr->AddrRangeMax,
+                    DummyBarIndex,
+                    PciBarTypePMem32,
+                    PciResUsagePadding
+                    );
+            InsertResourceNode (
+              PMem32Node,
+              Node
+              );
+          }
+
+          Ptr++;
+          continue;
+        }
+
+        //
+        // Non-prefechable
+        //
+        if (Ptr->SpecificFlag == 0) {
+          if (Ptr->AddrLen != 0) {
+            Node = CreateResourceNode (
+                    PciDev,
+                    Ptr->AddrLen,
+                    Ptr->AddrRangeMax,
+                    DummyBarIndex,
+                    PciBarTypeMem32,
+                    PciResUsagePadding
+                    );
+            InsertResourceNode (
+              Mem32Node,
+              Node
+              );
+          }
+
+          Ptr++;
+          continue;
+        }
+      }
+
+      if (Ptr->AddrSpaceGranularity == 64) {
+
+        //
+        // prefechable
+        //
+        if (Ptr->SpecificFlag == 0x6) {
+          if (Ptr->AddrLen != 0) {
+            Node = CreateResourceNode (
+                    PciDev,
+                    Ptr->AddrLen,
+                    Ptr->AddrRangeMax,
+                    DummyBarIndex,
+                    PciBarTypePMem64,
+                    PciResUsagePadding
+                    );
+            InsertResourceNode (
+              PMem64Node,
+              Node
+              );
+          }
+
+          Ptr++;
+          continue;
+        }
+
+        //
+        // Non-prefechable
+        //
+        if (Ptr->SpecificFlag == 0) {
+          if (Ptr->AddrLen != 0) {
+            Node = CreateResourceNode (
+                    PciDev,
+                    Ptr->AddrLen,
+                    Ptr->AddrRangeMax,
+                    DummyBarIndex,
+                    PciBarTypeMem64,
+                    PciResUsagePadding
+                    );
+            InsertResourceNode (
+              Mem64Node,
+              Node
+              );
+          }
+
+          Ptr++;
+          continue;
+        }
+      }
+    }
+
+    Ptr++;
+  }
+}
+
+/**
+  Get padding resource for PCI-PCI bridge.
+
+  @param  PciIoDevice     PCI-PCI bridge device instance.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determines
+          whether need to pad resource for them.
+**/
+VOID
+GetResourcePaddingPpb (
+  IN  PCI_IO_DEVICE                  *PciIoDevice
+  )
+{
+  if (gPciHotPlugInit != NULL && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    if (PciIoDevice->ResourcePaddingDescriptors == NULL) {
+      GetResourcePaddingForHpb (PciIoDevice);
+    }
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.h
new file mode 100644
index 0000000000..763ddbc4ed
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.h
@@ -0,0 +1,463 @@
+/** @file
+  PCI resouces support functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_RESOURCE_SUPPORT_H_
+#define _EFI_PCI_RESOURCE_SUPPORT_H_
+
+typedef enum {
+  PciResUsageTypical,
+  PciResUsagePadding
+} PCI_RESOURCE_USAGE;
+
+#define PCI_RESOURCE_SIGNATURE  SIGNATURE_32 ('p', 'c', 'r', 'c')
+
+typedef struct {
+  UINT32              Signature;
+  LIST_ENTRY          Link;
+  LIST_ENTRY          ChildList;
+  PCI_IO_DEVICE       *PciDev;
+  UINT64              Alignment;
+  UINT64              Offset;
+  UINT8               Bar;
+  PCI_BAR_TYPE        ResType;
+  UINT64              Length;
+  BOOLEAN             Reserved;
+  PCI_RESOURCE_USAGE  ResourceUsage;
+  BOOLEAN             Virtual;
+} PCI_RESOURCE_NODE;
+
+#define RESOURCE_NODE_FROM_LINK(a) \
+  CR (a, PCI_RESOURCE_NODE, Link, PCI_RESOURCE_SIGNATURE)
+
+/**
+  The function is used to skip VGA range.
+
+  @param Start    Returned start address including VGA range.
+  @param Length   The length of VGA range.
+
+**/
+VOID
+SkipVGAAperture (
+  OUT UINT64   *Start,
+  IN  UINT64   Length
+  );
+
+/**
+  This function is used to skip ISA aliasing aperture.
+
+  @param Start    Returned start address including ISA aliasing aperture.
+  @param Length   The length of ISA aliasing aperture.
+
+**/
+VOID
+SkipIsaAliasAperture (
+  OUT UINT64   *Start,
+  IN  UINT64   Length
+  );
+
+/**
+  This function inserts a resource node into the resource list.
+  The resource list is sorted in descend order.
+
+  @param Bridge  PCI resource node for bridge.
+  @param ResNode Resource node want to be inserted.
+
+**/
+VOID
+InsertResourceNode (
+  IN OUT PCI_RESOURCE_NODE   *Bridge,
+  IN     PCI_RESOURCE_NODE   *ResNode
+  );
+
+/**
+  This routine is used to merge two different resource trees in need of
+  resoure degradation.
+
+  For example, if an upstream PPB doesn't support,
+  prefetchable memory decoding, the PCI bus driver will choose to call this function
+  to merge prefectchable memory resource list into normal memory list.
+
+  If the TypeMerge is TRUE, Res resource type is changed to the type of destination resource
+  type.
+  If Dst is NULL or Res is NULL, ASSERT ().
+
+  @param Dst        Point to destination resource tree.
+  @param Res        Point to source resource tree.
+  @param TypeMerge  If the TypeMerge is TRUE, Res resource type is changed to the type of
+                    destination resource type.
+
+**/
+VOID
+MergeResourceTree (
+  IN PCI_RESOURCE_NODE   *Dst,
+  IN PCI_RESOURCE_NODE   *Res,
+  IN BOOLEAN             TypeMerge
+  );
+
+/**
+  This function is used to calculate the IO16 aperture
+  for a bridge.
+
+  @param Bridge    PCI resource node for bridge.
+
+**/
+VOID
+CalculateApertureIo16 (
+  IN PCI_RESOURCE_NODE    *Bridge
+  );
+
+/**
+  This function is used to calculate the resource aperture
+  for a given bridge device.
+
+  @param Bridge      PCI resouce node for given bridge device.
+
+**/
+VOID
+CalculateResourceAperture (
+  IN PCI_RESOURCE_NODE    *Bridge
+  );
+
+/**
+  Get IO/Memory resource infor for given PCI device.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO .
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+GetResourceFromDevice (
+  IN     PCI_IO_DEVICE     *PciDev,
+  IN OUT PCI_RESOURCE_NODE *IoNode,
+  IN OUT PCI_RESOURCE_NODE *Mem32Node,
+  IN OUT PCI_RESOURCE_NODE *PMem32Node,
+  IN OUT PCI_RESOURCE_NODE *Mem64Node,
+  IN OUT PCI_RESOURCE_NODE *PMem64Node
+  );
+
+/**
+  This function is used to create a resource node.
+
+  @param PciDev       Pci device instance.
+  @param Length       Length of Io/Memory resource.
+  @param Alignment    Alignment of resource.
+  @param Bar          Bar index.
+  @param ResType      Type of resource: IO/Memory.
+  @param ResUsage     Resource usage.
+
+  @return PCI resource node created for given PCI device.
+          NULL means PCI resource node is not created.
+
+**/
+PCI_RESOURCE_NODE *
+CreateResourceNode (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN UINT64                Length,
+  IN UINT64                Alignment,
+  IN UINT8                 Bar,
+  IN PCI_BAR_TYPE          ResType,
+  IN PCI_RESOURCE_USAGE    ResUsage
+  );
+
+/**
+  This function is used to extract resource request from
+  IOV VF device node list.
+
+  @param PciDev       Pci device instance.
+  @param Length       Length of Io/Memory resource.
+  @param Alignment    Alignment of resource.
+  @param Bar          Bar index.
+  @param ResType      Type of resource: IO/Memory.
+  @param ResUsage     Resource usage.
+
+  @return PCI resource node created for given PCI device.
+          NULL means PCI resource node is not created.
+
+**/
+PCI_RESOURCE_NODE *
+CreateVfResourceNode (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN UINT64                Length,
+  IN UINT64                Alignment,
+  IN UINT8                 Bar,
+  IN PCI_BAR_TYPE          ResType,
+  IN PCI_RESOURCE_USAGE    ResUsage
+  );
+
+/**
+  This function is used to extract resource request from
+  device node list.
+
+  @param Bridge     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+CreateResourceMap (
+  IN     PCI_IO_DEVICE     *Bridge,
+  IN OUT PCI_RESOURCE_NODE *IoNode,
+  IN OUT PCI_RESOURCE_NODE *Mem32Node,
+  IN OUT PCI_RESOURCE_NODE *PMem32Node,
+  IN OUT PCI_RESOURCE_NODE *Mem64Node,
+  IN OUT PCI_RESOURCE_NODE *PMem64Node
+  );
+
+/**
+  This function is used to do the resource padding for a specific platform.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ResourcePaddingPolicy (
+  IN PCI_IO_DEVICE     *PciDev,
+  IN PCI_RESOURCE_NODE *IoNode,
+  IN PCI_RESOURCE_NODE *Mem32Node,
+  IN PCI_RESOURCE_NODE *PMem32Node,
+  IN PCI_RESOURCE_NODE *Mem64Node,
+  IN PCI_RESOURCE_NODE *PMem64Node
+  );
+
+/**
+  This function is used to degrade resource if the upstream bridge
+  doesn't support certain resource. Degradation path is
+  PMEM64 -> MEM64  -> MEM32
+  PMEM64 -> PMEM32 -> MEM32
+  IO32   -> IO16.
+
+  @param Bridge     Pci device instance.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+DegradeResource (
+  IN PCI_IO_DEVICE     *Bridge,
+  IN PCI_RESOURCE_NODE *Mem32Node,
+  IN PCI_RESOURCE_NODE *PMem32Node,
+  IN PCI_RESOURCE_NODE *Mem64Node,
+  IN PCI_RESOURCE_NODE *PMem64Node
+  );
+
+/**
+  Test whether bridge device support decode resource.
+
+  @param Bridge    Bridge device instance.
+  @param Decode    Decode type according to resource type.
+
+  @return TRUE     The bridge device support decode resource.
+  @return FALSE    The bridge device don't support decode resource.
+
+**/
+BOOLEAN
+BridgeSupportResourceDecode (
+  IN PCI_IO_DEVICE *Bridge,
+  IN UINT32        Decode
+  );
+
+/**
+  This function is used to program the resource allocated
+  for each resource node under specified bridge.
+
+  @param Base     Base address of resource to be progammed.
+  @param Bridge   PCI resource node for the bridge device.
+
+  @retval EFI_SUCCESS            Successfully to program all resouces
+                                 on given PCI bridge device.
+  @retval EFI_OUT_OF_RESOURCES   Base is all one.
+
+**/
+EFI_STATUS
+ProgramResource (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Bridge
+  );
+
+/**
+  Program Bar register for PCI device.
+
+  @param Base  Base address for PCI device resource to be progammed.
+  @param Node  Point to resoure node structure.
+
+**/
+VOID
+ProgramBar (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  );
+
+/**
+  Program IOV VF Bar register for PCI device.
+
+  @param Base  Base address for PCI device resource to be progammed.
+  @param Node  Point to resoure node structure.
+
+**/
+EFI_STATUS
+ProgramVfBar (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  );
+
+/**
+  Program PCI-PCI bridge apperture.
+
+  @param Base  Base address for resource.
+  @param Node  Point to resoure node structure.
+
+**/
+VOID
+ProgramPpbApperture (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  );
+
+/**
+  Program parent bridge for Option Rom.
+
+  @param PciDevice      Pci deivce instance.
+  @param OptionRomBase  Base address for Optiona Rom.
+  @param Enable         Enable or disable PCI memory.
+
+**/
+VOID
+ProgrameUpstreamBridgeForRom (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT32          OptionRomBase,
+  IN BOOLEAN         Enable
+  );
+
+/**
+  Test whether resource exists for a bridge.
+
+  @param Bridge  Point to resource node for a bridge.
+
+  @retval TRUE   There is resource on the given bridge.
+  @retval FALSE  There isn't resource on the given bridge.
+
+**/
+BOOLEAN
+ResourceRequestExisted (
+  IN PCI_RESOURCE_NODE    *Bridge
+  );
+
+/**
+  Initialize resource pool structure.
+
+  @param ResourcePool Point to resource pool structure. This pool
+                      is reset to all zero when returned.
+  @param ResourceType Type of resource.
+
+**/
+VOID
+InitializeResourcePool (
+  IN OUT PCI_RESOURCE_NODE   *ResourcePool,
+  IN     PCI_BAR_TYPE        ResourceType
+  );
+
+/**
+  Destory given resource tree.
+
+  @param Bridge  PCI resource root node of resource tree.
+
+**/
+VOID
+DestroyResourceTree (
+  IN PCI_RESOURCE_NODE *Bridge
+  );
+
+/**
+  Insert resource padding for P2C.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ResourcePaddingForCardBusBridge (
+  IN PCI_IO_DEVICE        *PciDev,
+  IN PCI_RESOURCE_NODE    *IoNode,
+  IN PCI_RESOURCE_NODE    *Mem32Node,
+  IN PCI_RESOURCE_NODE    *PMem32Node,
+  IN PCI_RESOURCE_NODE    *Mem64Node,
+  IN PCI_RESOURCE_NODE    *PMem64Node
+  );
+
+/**
+  Program PCI Card device register for given resource node.
+
+  @param Base    Base address of PCI Card device to be programmed.
+  @param Node    Given resource node.
+
+**/
+VOID
+ProgramP2C (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  );
+
+/**
+  Create padding resource node.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ApplyResourcePadding (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN PCI_RESOURCE_NODE     *IoNode,
+  IN PCI_RESOURCE_NODE     *Mem32Node,
+  IN PCI_RESOURCE_NODE     *PMem32Node,
+  IN PCI_RESOURCE_NODE     *Mem64Node,
+  IN PCI_RESOURCE_NODE     *PMem64Node
+  );
+
+/**
+  Get padding resource for PCI-PCI bridge.
+
+  @param  PciIoDevice     PCI-PCI bridge device instance.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determines
+          whether need to pad resource for them.
+**/
+VOID
+GetResourcePaddingPpb (
+  IN  PCI_IO_DEVICE                  *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.c
new file mode 100644
index 0000000000..f48c3a0c59
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.c
@@ -0,0 +1,126 @@
+/** @file
+  Set up ROM Table for PCI Bus module.
+
+Copyright (c) 2006 - 2009, 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 "PciBus.h"
+
+//
+// PCI ROM image information
+//
+typedef struct {
+  EFI_HANDLE  ImageHandle;
+  UINTN       Seg;
+  UINT8       Bus;
+  UINT8       Dev;
+  UINT8       Func;
+  UINT64      RomAddress;
+  UINT64      RomLength;
+} EFI_PCI_ROM_IMAGE_MAPPING;
+
+UINTN                      mNumberOfPciRomImages     = 0;
+UINTN                      mMaxNumberOfPciRomImages  = 0;
+EFI_PCI_ROM_IMAGE_MAPPING  *mRomImageTable           = NULL;
+
+/**
+  Add the Rom Image to internal database for later PCI light enumeration.
+
+  @param ImageHandle    Option Rom image handle.
+  @param Seg            Segment of PCI space.
+  @param Bus            Bus NO of PCI space.
+  @param Dev            Dev NO of PCI space.
+  @param Func           Func NO of PCI space.
+  @param RomAddress     Base address of OptionRom.
+  @param RomLength      Length of rom image.
+
+**/
+VOID
+PciRomAddImageMapping (
+  IN  EFI_HANDLE  ImageHandle,
+  IN  UINTN       Seg,
+  IN  UINT8       Bus,
+  IN  UINT8       Dev,
+  IN  UINT8       Func,
+  IN  UINT64      RomAddress,
+  IN  UINT64      RomLength
+  )
+{
+  EFI_PCI_ROM_IMAGE_MAPPING *TempMapping;
+
+  if (mNumberOfPciRomImages >= mMaxNumberOfPciRomImages) {
+
+    mMaxNumberOfPciRomImages += 0x20;
+
+    TempMapping = NULL;
+    TempMapping = AllocatePool (mMaxNumberOfPciRomImages * sizeof (EFI_PCI_ROM_IMAGE_MAPPING));
+    if (TempMapping == NULL) {
+      return ;
+    }
+
+    CopyMem (TempMapping, mRomImageTable, mNumberOfPciRomImages * sizeof (EFI_PCI_ROM_IMAGE_MAPPING));
+
+    if (mRomImageTable != NULL) {
+      FreePool (mRomImageTable);
+    }
+
+    mRomImageTable = TempMapping;
+  }
+
+  mRomImageTable[mNumberOfPciRomImages].ImageHandle = ImageHandle;
+  mRomImageTable[mNumberOfPciRomImages].Seg         = Seg;
+  mRomImageTable[mNumberOfPciRomImages].Bus         = Bus;
+  mRomImageTable[mNumberOfPciRomImages].Dev         = Dev;
+  mRomImageTable[mNumberOfPciRomImages].Func        = Func;
+  mRomImageTable[mNumberOfPciRomImages].RomAddress  = RomAddress;
+  mRomImageTable[mNumberOfPciRomImages].RomLength   = RomLength;
+  mNumberOfPciRomImages++;
+}
+
+/**
+  Get Option rom driver's mapping for PCI device.
+
+  @param PciIoDevice Device instance.
+
+  @retval TRUE   Found Image mapping.
+  @retval FALSE  Cannot found image mapping.
+
+**/
+BOOLEAN
+PciRomGetImageMapping (
+  IN  PCI_IO_DEVICE                       *PciIoDevice
+  )
+{
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+  UINTN                           Index;
+  BOOLEAN                         Found;
+
+  PciRootBridgeIo = PciIoDevice->PciRootBridgeIo;
+  Found           = FALSE;
+
+  for (Index = 0; Index < mNumberOfPciRomImages; Index++) {
+    if (mRomImageTable[Index].Seg  == PciRootBridgeIo->SegmentNumber &&
+        mRomImageTable[Index].Bus  == PciIoDevice->BusNumber         &&
+        mRomImageTable[Index].Dev  == PciIoDevice->DeviceNumber      &&
+        mRomImageTable[Index].Func == PciIoDevice->FunctionNumber    ) {
+        Found = TRUE;
+
+      if (mRomImageTable[Index].ImageHandle != NULL) {
+        AddDriver (PciIoDevice, mRomImageTable[Index].ImageHandle);
+      } else {
+        PciIoDevice->PciIo.RomImage = (VOID *) (UINTN) mRomImageTable[Index].RomAddress;
+        PciIoDevice->PciIo.RomSize  = (UINTN) mRomImageTable[Index].RomLength;
+      }
+    }
+  }
+
+  return Found;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.h
new file mode 100644
index 0000000000..d443f83336
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.h
@@ -0,0 +1,55 @@
+/** @file
+  Set up ROM Table for PCI Bus module.
+
+Copyright (c) 2006 - 2009, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PCI_ROM_TABLE_H_
+#define _EFI_PCI_ROM_TABLE_H_
+
+/**
+  Add the Rom Image to internal database for later PCI light enumeration.
+
+  @param ImageHandle    Option Rom image handle.
+  @param Seg            Segment of PCI space.
+  @param Bus            Bus NO of PCI space.
+  @param Dev            Dev NO of PCI space.
+  @param Func           Func NO of PCI space.
+  @param RomAddress     Base address of OptionRom.
+  @param RomLength      Length of rom image.
+
+**/
+VOID
+PciRomAddImageMapping (
+  IN  EFI_HANDLE  ImageHandle,
+  IN  UINTN       Seg,
+  IN  UINT8       Bus,
+  IN  UINT8       Dev,
+  IN  UINT8       Func,
+  IN  UINT64      RomAddress,
+  IN  UINT64      RomLength
+  );
+
+/**
+  Get Option rom driver's mapping for PCI device.
+
+  @param PciIoDevice Device instance.
+
+  @retval TRUE   Found Image mapping.
+  @retval FALSE  Cannot found image mapping.
+
+**/
+BOOLEAN
+PciRomGetImageMapping (
+  IN  PCI_IO_DEVICE                       *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.c b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.c
new file mode 100644
index 0000000000..c866e88016
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.c
@@ -0,0 +1,1471 @@
+/** @file
+
+  Provides the basic interfaces to abstract a PCI Host Bridge Resource Allocation.
+
+Copyright (c) 1999 - 2016, 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 "PciHostBridge.h"
+#include "PciRootBridge.h"
+#include "PciHostResource.h"
+
+
+EFI_METRONOME_ARCH_PROTOCOL *mMetronome;
+EFI_CPU_IO2_PROTOCOL        *mCpuIo;
+
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR16 *mAcpiAddressSpaceTypeStr[] = {
+  L"Mem", L"I/O", L"Bus"
+};
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR16 *mPciResourceTypeStr[] = {
+  L"I/O", L"Mem", L"PMem", L"Mem64", L"PMem64", L"Bus"
+};
+
+/**
+  Ensure the compatibility of an IO space descriptor with the IO aperture.
+
+  The IO space descriptor can come from the GCD IO space map, or it can
+  represent a gap between two neighboring IO space descriptors. In the latter
+  case, the GcdIoType field is expected to be EfiGcdIoTypeNonExistent.
+
+  If the IO space descriptor already has type EfiGcdIoTypeIo, then no action is
+  taken -- it is by definition compatible with the aperture.
+
+  Otherwise, the intersection of the IO space descriptor is calculated with the
+  aperture. If the intersection is the empty set (no overlap), no action is
+  taken; the IO space descriptor is compatible with the aperture.
+
+  Otherwise, the type of the descriptor is investigated again. If the type is
+  EfiGcdIoTypeNonExistent (representing a gap, or a genuine descriptor with
+  such a type), then an attempt is made to add the intersection as IO space to
+  the GCD IO space map. This ensures continuity for the aperture, and the
+  descriptor is deemed compatible with the aperture.
+
+  Otherwise, the IO space descriptor is incompatible with the IO aperture.
+
+  @param[in] Base        Base address of the aperture.
+  @param[in] Length      Length of the aperture.
+  @param[in] Descriptor  The descriptor to ensure compatibility with the
+                         aperture for.
+
+  @retval EFI_SUCCESS            The descriptor is compatible. The GCD IO space
+                                 map may have been updated, for continuity
+                                 within the aperture.
+  @retval EFI_INVALID_PARAMETER  The descriptor is incompatible.
+  @return                        Error codes from gDS->AddIoSpace().
+**/
+EFI_STATUS
+IntersectIoDescriptor (
+  IN  UINT64                            Base,
+  IN  UINT64                            Length,
+  IN  CONST EFI_GCD_IO_SPACE_DESCRIPTOR *Descriptor
+  )
+{
+  UINT64                                IntersectionBase;
+  UINT64                                IntersectionEnd;
+  EFI_STATUS                            Status;
+
+  if (Descriptor->GcdIoType == EfiGcdIoTypeIo) {
+    return EFI_SUCCESS;
+  }
+
+  IntersectionBase = MAX (Base, Descriptor->BaseAddress);
+  IntersectionEnd = MIN (Base + Length,
+                      Descriptor->BaseAddress + Descriptor->Length);
+  if (IntersectionBase >= IntersectionEnd) {
+    //
+    // The descriptor and the aperture don't overlap.
+    //
+    return EFI_SUCCESS;
+  }
+
+  if (Descriptor->GcdIoType == EfiGcdIoTypeNonExistent) {
+    Status = gDS->AddIoSpace (EfiGcdIoTypeIo, IntersectionBase,
+                    IntersectionEnd - IntersectionBase);
+
+    DEBUG ((EFI_ERROR (Status) ? EFI_D_ERROR : EFI_D_VERBOSE,
+      "%a: %a: add [%Lx, %Lx): %r\n", gEfiCallerBaseName, __FUNCTION__,
+      IntersectionBase, IntersectionEnd, Status));
+    return Status;
+  }
+
+  DEBUG ((EFI_D_ERROR, "%a: %a: desc [%Lx, %Lx) type %u conflicts with "
+    "aperture [%Lx, %Lx)\n", gEfiCallerBaseName, __FUNCTION__,
+    Descriptor->BaseAddress, Descriptor->BaseAddress + Descriptor->Length,
+    (UINT32)Descriptor->GcdIoType, Base, Base + Length));
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Add IO space to GCD.
+  The routine checks the GCD database and only adds those which are
+  not added in the specified range to GCD.
+
+  @param Base   Base address of the IO space.
+  @param Length Length of the IO space.
+
+  @retval EFI_SUCCES The IO space was added successfully.
+**/
+EFI_STATUS
+AddIoSpace (
+  IN  UINT64                        Base,
+  IN  UINT64                        Length
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             Index;
+  UINTN                             NumberOfDescriptors;
+  EFI_GCD_IO_SPACE_DESCRIPTOR       *IoSpaceMap;
+
+  Status = gDS->GetIoSpaceMap (&NumberOfDescriptors, &IoSpaceMap);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "%a: %a: GetIoSpaceMap(): %r\n",
+      gEfiCallerBaseName, __FUNCTION__, Status));
+    return Status;
+  }
+
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    Status = IntersectIoDescriptor (Base, Length, &IoSpaceMap[Index]);
+    if (EFI_ERROR (Status)) {
+      goto FreeIoSpaceMap;
+    }
+  }
+
+  DEBUG_CODE (
+    //
+    // Make sure there are adjacent descriptors covering [Base, Base + Length).
+    // It is possible that they have not been merged; merging can be prevented
+    // by allocation.
+    //
+    UINT64                      CheckBase;
+    EFI_STATUS                  CheckStatus;
+    EFI_GCD_IO_SPACE_DESCRIPTOR Descriptor;
+
+    for (CheckBase = Base;
+         CheckBase < Base + Length;
+         CheckBase = Descriptor.BaseAddress + Descriptor.Length) {
+      CheckStatus = gDS->GetIoSpaceDescriptor (CheckBase, &Descriptor);
+      ASSERT_EFI_ERROR (CheckStatus);
+      ASSERT (Descriptor.GcdIoType == EfiGcdIoTypeIo);
+    }
+    );
+
+FreeIoSpaceMap:
+  FreePool (IoSpaceMap);
+
+  return Status;
+}
+
+/**
+  Ensure the compatibility of a memory space descriptor with the MMIO aperture.
+
+  The memory space descriptor can come from the GCD memory space map, or it can
+  represent a gap between two neighboring memory space descriptors. In the
+  latter case, the GcdMemoryType field is expected to be
+  EfiGcdMemoryTypeNonExistent.
+
+  If the memory space descriptor already has type
+  EfiGcdMemoryTypeMemoryMappedIo, and its capabilities are a superset of the
+  required capabilities, then no action is taken -- it is by definition
+  compatible with the aperture.
+
+  Otherwise, the intersection of the memory space descriptor is calculated with
+  the aperture. If the intersection is the empty set (no overlap), no action is
+  taken; the memory space descriptor is compatible with the aperture.
+
+  Otherwise, the type of the descriptor is investigated again. If the type is
+  EfiGcdMemoryTypeNonExistent (representing a gap, or a genuine descriptor with
+  such a type), then an attempt is made to add the intersection as MMIO space
+  to the GCD memory space map, with the specified capabilities. This ensures
+  continuity for the aperture, and the descriptor is deemed compatible with the
+  aperture.
+
+  Otherwise, the memory space descriptor is incompatible with the MMIO
+  aperture.
+
+  @param[in] Base         Base address of the aperture.
+  @param[in] Length       Length of the aperture.
+  @param[in] Capabilities Capabilities required by the aperture.
+  @param[in] Descriptor   The descriptor to ensure compatibility with the
+                          aperture for.
+
+  @retval EFI_SUCCESS            The descriptor is compatible. The GCD memory
+                                 space map may have been updated, for
+                                 continuity within the aperture.
+  @retval EFI_INVALID_PARAMETER  The descriptor is incompatible.
+  @return                        Error codes from gDS->AddMemorySpace().
+**/
+EFI_STATUS
+IntersectMemoryDescriptor (
+  IN  UINT64                                Base,
+  IN  UINT64                                Length,
+  IN  UINT64                                Capabilities,
+  IN  CONST EFI_GCD_MEMORY_SPACE_DESCRIPTOR *Descriptor
+  )
+{
+  UINT64                                    IntersectionBase;
+  UINT64                                    IntersectionEnd;
+  EFI_STATUS                                Status;
+
+  if (Descriptor->GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo &&
+      (Descriptor->Capabilities & Capabilities) == Capabilities) {
+    return EFI_SUCCESS;
+  }
+
+  IntersectionBase = MAX (Base, Descriptor->BaseAddress);
+  IntersectionEnd = MIN (Base + Length,
+                      Descriptor->BaseAddress + Descriptor->Length);
+  if (IntersectionBase >= IntersectionEnd) {
+    //
+    // The descriptor and the aperture don't overlap.
+    //
+    return EFI_SUCCESS;
+  }
+
+  if (Descriptor->GcdMemoryType == EfiGcdMemoryTypeNonExistent) {
+    Status = gDS->AddMemorySpace (EfiGcdMemoryTypeMemoryMappedIo,
+                    IntersectionBase, IntersectionEnd - IntersectionBase,
+                    Capabilities);
+
+    DEBUG ((EFI_ERROR (Status) ? EFI_D_ERROR : EFI_D_VERBOSE,
+      "%a: %a: add [%Lx, %Lx): %r\n", gEfiCallerBaseName, __FUNCTION__,
+      IntersectionBase, IntersectionEnd, Status));
+    return Status;
+  }
+
+  DEBUG ((EFI_D_ERROR, "%a: %a: desc [%Lx, %Lx) type %u cap %Lx conflicts "
+    "with aperture [%Lx, %Lx) cap %Lx\n", gEfiCallerBaseName, __FUNCTION__,
+    Descriptor->BaseAddress, Descriptor->BaseAddress + Descriptor->Length,
+    (UINT32)Descriptor->GcdMemoryType, Descriptor->Capabilities,
+    Base, Base + Length, Capabilities));
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Add MMIO space to GCD.
+  The routine checks the GCD database and only adds those which are
+  not added in the specified range to GCD.
+
+  @param Base         Base address of the MMIO space.
+  @param Length       Length of the MMIO space.
+  @param Capabilities Capabilities of the MMIO space.
+
+  @retval EFI_SUCCES The MMIO space was added successfully.
+**/
+EFI_STATUS
+AddMemoryMappedIoSpace (
+  IN  UINT64                            Base,
+  IN  UINT64                            Length,
+  IN  UINT64                            Capabilities
+  )
+{
+  EFI_STATUS                            Status;
+  UINTN                                 Index;
+  UINTN                                 NumberOfDescriptors;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR       *MemorySpaceMap;
+
+  Status = gDS->GetMemorySpaceMap (&NumberOfDescriptors, &MemorySpaceMap);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "%a: %a: GetMemorySpaceMap(): %r\n",
+      gEfiCallerBaseName, __FUNCTION__, Status));
+    return Status;
+  }
+
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    Status = IntersectMemoryDescriptor (Base, Length, Capabilities,
+               &MemorySpaceMap[Index]);
+    if (EFI_ERROR (Status)) {
+      goto FreeMemorySpaceMap;
+    }
+  }
+
+  DEBUG_CODE (
+    //
+    // Make sure there are adjacent descriptors covering [Base, Base + Length).
+    // It is possible that they have not been merged; merging can be prevented
+    // by allocation and different capabilities.
+    //
+    UINT64                          CheckBase;
+    EFI_STATUS                      CheckStatus;
+    EFI_GCD_MEMORY_SPACE_DESCRIPTOR Descriptor;
+
+    for (CheckBase = Base;
+         CheckBase < Base + Length;
+         CheckBase = Descriptor.BaseAddress + Descriptor.Length) {
+      CheckStatus = gDS->GetMemorySpaceDescriptor (CheckBase, &Descriptor);
+      ASSERT_EFI_ERROR (CheckStatus);
+      ASSERT (Descriptor.GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo);
+      ASSERT ((Descriptor.Capabilities & Capabilities) == Capabilities);
+    }
+    );
+
+FreeMemorySpaceMap:
+  FreePool (MemorySpaceMap);
+
+  return Status;
+}
+
+/**
+
+  Entry point of this driver.
+
+  @param ImageHandle  Image handle of this driver.
+  @param SystemTable  Pointer to standard EFI system table.
+
+  @retval EFI_SUCCESS       Succeed.
+  @retval EFI_DEVICE_ERROR  Fail to install PCI_ROOT_BRIDGE_IO protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializePciHostBridge (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS                  Status;
+  PCI_HOST_BRIDGE_INSTANCE    *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE    *RootBridge;
+  PCI_ROOT_BRIDGE             *RootBridges;
+  UINTN                       RootBridgeCount;
+  UINTN                       Index;
+  PCI_ROOT_BRIDGE_APERTURE    *MemApertures[4];
+  UINTN                       MemApertureIndex;
+  BOOLEAN                     ResourceAssigned;
+  LIST_ENTRY                  *Link;
+
+  RootBridges = PciHostBridgeGetRootBridges (&RootBridgeCount);
+  if ((RootBridges == NULL) || (RootBridgeCount == 0)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = gBS->LocateProtocol (&gEfiMetronomeArchProtocolGuid, NULL, (VOID **) &mMetronome);
+  ASSERT_EFI_ERROR (Status);
+  Status = gBS->LocateProtocol (&gEfiCpuIo2ProtocolGuid, NULL, (VOID **) &mCpuIo);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Most systems in the world including complex servers have only one Host Bridge.
+  //
+  HostBridge = AllocateZeroPool (sizeof (PCI_HOST_BRIDGE_INSTANCE));
+  ASSERT (HostBridge != NULL);
+
+  HostBridge->Signature        = PCI_HOST_BRIDGE_SIGNATURE;
+  HostBridge->CanRestarted     = TRUE;
+  InitializeListHead (&HostBridge->RootBridges);
+  ResourceAssigned             = FALSE;
+
+  //
+  // Create Root Bridge Device Handle in this Host Bridge
+  //
+  for (Index = 0; Index < RootBridgeCount; Index++) {
+    //
+    // Create Root Bridge Handle Instance
+    //
+    RootBridge = CreateRootBridge (&RootBridges[Index]);
+    ASSERT (RootBridge != NULL);
+    if (RootBridge == NULL) {
+      continue;
+    }
+
+    //
+    // Make sure all root bridges share the same ResourceAssigned value.
+    //
+    if (Index == 0) {
+      ResourceAssigned = RootBridges[Index].ResourceAssigned;
+    } else {
+      ASSERT (ResourceAssigned == RootBridges[Index].ResourceAssigned);
+    }
+
+    if (RootBridges[Index].Io.Base <= RootBridges[Index].Io.Limit) {
+      Status = AddIoSpace (
+                 RootBridges[Index].Io.Base,
+                 RootBridges[Index].Io.Limit - RootBridges[Index].Io.Base + 1
+                 );
+      ASSERT_EFI_ERROR (Status);
+      if (ResourceAssigned) {
+        Status = gDS->AllocateIoSpace (
+                        EfiGcdAllocateAddress,
+                        EfiGcdIoTypeIo,
+                        0,
+                        RootBridges[Index].Io.Limit - RootBridges[Index].Io.Base + 1,
+                        &RootBridges[Index].Io.Base,
+                        gImageHandle,
+                        NULL
+                        );
+        ASSERT_EFI_ERROR (Status);
+      }
+    }
+
+    //
+    // Add all the Mem/PMem aperture to GCD
+    // Mem/PMem shouldn't overlap with each other
+    // Root bridge which needs to combine MEM and PMEM should only report
+    // the MEM aperture in Mem
+    //
+    MemApertures[0] = &RootBridges[Index].Mem;
+    MemApertures[1] = &RootBridges[Index].MemAbove4G;
+    MemApertures[2] = &RootBridges[Index].PMem;
+    MemApertures[3] = &RootBridges[Index].PMemAbove4G;
+
+    for (MemApertureIndex = 0; MemApertureIndex < sizeof (MemApertures) / sizeof (MemApertures[0]); MemApertureIndex++) {
+      if (MemApertures[MemApertureIndex]->Base <= MemApertures[MemApertureIndex]->Limit) {
+        Status = AddMemoryMappedIoSpace (
+                   MemApertures[MemApertureIndex]->Base,
+                   MemApertures[MemApertureIndex]->Limit - MemApertures[MemApertureIndex]->Base + 1,
+                   EFI_MEMORY_UC
+                   );
+        ASSERT_EFI_ERROR (Status);
+        Status = gDS->SetMemorySpaceAttributes (
+                        MemApertures[MemApertureIndex]->Base,
+                        MemApertures[MemApertureIndex]->Limit - MemApertures[MemApertureIndex]->Base + 1,
+                        EFI_MEMORY_UC
+                        );
+        if (EFI_ERROR (Status)) {
+          DEBUG ((DEBUG_WARN, "PciHostBridge driver failed to set EFI_MEMORY_UC to MMIO aperture - %r.\n", Status));
+        }
+        if (ResourceAssigned) {
+          Status = gDS->AllocateMemorySpace (
+                          EfiGcdAllocateAddress,
+                          EfiGcdMemoryTypeMemoryMappedIo,
+                          0,
+                          MemApertures[MemApertureIndex]->Limit - MemApertures[MemApertureIndex]->Base + 1,
+                          &MemApertures[MemApertureIndex]->Base,
+                          gImageHandle,
+                          NULL
+                          );
+          ASSERT_EFI_ERROR (Status);
+        }
+      }
+    }
+    //
+    // Insert Root Bridge Handle Instance
+    //
+    InsertTailList (&HostBridge->RootBridges, &RootBridge->Link);
+  }
+
+  //
+  // When resources were assigned, it's not needed to expose
+  // PciHostBridgeResourceAllocation protocol.
+  //
+  if (!ResourceAssigned) {
+    HostBridge->ResAlloc.NotifyPhase = NotifyPhase;
+    HostBridge->ResAlloc.GetNextRootBridge = GetNextRootBridge;
+    HostBridge->ResAlloc.GetAllocAttributes = GetAttributes;
+    HostBridge->ResAlloc.StartBusEnumeration = StartBusEnumeration;
+    HostBridge->ResAlloc.SetBusNumbers = SetBusNumbers;
+    HostBridge->ResAlloc.SubmitResources = SubmitResources;
+    HostBridge->ResAlloc.GetProposedResources = GetProposedResources;
+    HostBridge->ResAlloc.PreprocessController = PreprocessController;
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &HostBridge->Handle,
+                    &gEfiPciHostBridgeResourceAllocationProtocolGuid, &HostBridge->ResAlloc,
+                    NULL
+                    );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    RootBridge->RootBridgeIo.ParentHandle = HostBridge->Handle;
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &RootBridge->Handle,
+                    &gEfiDevicePathProtocolGuid, RootBridge->DevicePath,
+                    &gEfiPciRootBridgeIoProtocolGuid, &RootBridge->RootBridgeIo,
+                    NULL
+                    );
+    ASSERT_EFI_ERROR (Status);
+  }
+  PciHostBridgeFreeRootBridges (RootBridges, RootBridgeCount);
+  return Status;
+}
+
+/**
+  This routine constructs the resource descriptors for all root bridges and call PciHostBridgeResourceConflict().
+
+  @param HostBridge The Host Bridge Instance where the resource adjustment happens.
+**/
+VOID
+ResourceConflict (
+  IN  PCI_HOST_BRIDGE_INSTANCE *HostBridge
+  )
+{
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Resources;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+  PCI_ROOT_BRIDGE_INSTANCE          *RootBridge;
+  LIST_ENTRY                        *Link;
+  UINTN                             RootBridgeCount;
+  PCI_RESOURCE_TYPE                 Index;
+  PCI_RES_NODE                      *ResAllocNode;
+
+  RootBridgeCount = 0;
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridgeCount++;
+  }
+
+  Resources = AllocatePool (
+                RootBridgeCount * (TypeMax * sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR)) +
+                sizeof (EFI_ACPI_END_TAG_DESCRIPTOR)
+                );
+  ASSERT (Resources != NULL);
+
+  for (Link = GetFirstNode (&HostBridge->RootBridges), Descriptor = Resources
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    for (Index = TypeIo; Index < TypeMax; Index++) {
+      ResAllocNode = &RootBridge->ResAllocNode[Index];
+
+      Descriptor->Desc = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Descriptor->Len = sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3;
+      Descriptor->AddrRangeMin = ResAllocNode->Base;
+      Descriptor->AddrRangeMax = ResAllocNode->Alignment;
+      Descriptor->AddrLen = ResAllocNode->Length;
+      switch (ResAllocNode->Type) {
+
+      case TypeIo:
+        Descriptor->ResType = ACPI_ADDRESS_SPACE_TYPE_IO;
+        break;
+
+      case TypePMem32:
+        Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+      case TypeMem32:
+        Descriptor->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+        Descriptor->AddrSpaceGranularity = 32;
+        break;
+
+      case TypePMem64:
+        Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+      case TypeMem64:
+        Descriptor->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+        Descriptor->AddrSpaceGranularity = 64;
+        break;
+
+      case TypeBus:
+        Descriptor->ResType = ACPI_ADDRESS_SPACE_TYPE_BUS;
+        break;
+
+      default:
+        break;
+      }
+
+      Descriptor++;
+    }
+    //
+    // Terminate the root bridge resources.
+    //
+    End = (EFI_ACPI_END_TAG_DESCRIPTOR *) Descriptor;
+    End->Desc = ACPI_END_TAG_DESCRIPTOR;
+    End->Checksum = 0x0;
+
+    Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) (End + 1);
+  }
+  //
+  // Terminate the host bridge resources.
+  //
+  End = (EFI_ACPI_END_TAG_DESCRIPTOR *) Descriptor;
+  End->Desc = ACPI_END_TAG_DESCRIPTOR;
+  End->Checksum = 0x0;
+
+  DEBUG ((DEBUG_ERROR, "Call PciHostBridgeResourceConflict().\n"));
+  PciHostBridgeResourceConflict (HostBridge->Handle, Resources);
+  FreePool (Resources);
+}
+
+/**
+  Allocate Length of MMIO or IO resource with alignment BitsOfAlignment
+  from GCD range [BaseAddress, Limit).
+
+  @param Mmio            TRUE for MMIO and FALSE for IO.
+  @param Length          Length of the resource to allocate.
+  @param BitsOfAlignment Alignment of the resource to allocate.
+  @param BaseAddress     The starting address the allocation is from.
+  @param Limit           The ending address the allocation is to.
+
+  @retval  The base address of the allocated resource or MAX_UINT64 if allocation
+           fails.
+**/
+UINT64
+AllocateResource (
+  BOOLEAN Mmio,
+  UINT64  Length,
+  UINTN   BitsOfAlignment,
+  UINT64  BaseAddress,
+  UINT64  Limit
+  )
+{
+  EFI_STATUS Status;
+
+  if (BaseAddress < Limit) {
+    //
+    // Have to make sure Aligment is handled since we are doing direct address allocation
+    //
+    BaseAddress = ALIGN_VALUE (BaseAddress, LShiftU64 (1, BitsOfAlignment));
+
+    while (BaseAddress + Length <= Limit + 1) {
+      if (Mmio) {
+        Status = gDS->AllocateMemorySpace (
+                        EfiGcdAllocateAddress,
+                        EfiGcdMemoryTypeMemoryMappedIo,
+                        BitsOfAlignment,
+                        Length,
+                        &BaseAddress,
+                        gImageHandle,
+                        NULL
+                        );
+      } else {
+        Status = gDS->AllocateIoSpace (
+                        EfiGcdAllocateAddress,
+                        EfiGcdIoTypeIo,
+                        BitsOfAlignment,
+                        Length,
+                        &BaseAddress,
+                        gImageHandle,
+                        NULL
+                        );
+      }
+
+      if (!EFI_ERROR (Status)) {
+        return BaseAddress;
+      }
+      BaseAddress += LShiftU64 (1, BitsOfAlignment);
+    }
+  }
+  return MAX_UINT64;
+}
+
+/**
+
+  Enter a certain phase of the PCI enumeration process.
+
+  @param This   The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL instance.
+  @param Phase  The phase during enumeration.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Wrong phase parameter passed in.
+  @retval EFI_NOT_READY          Resources have not been submitted yet.
+
+**/
+EFI_STATUS
+EFIAPI
+NotifyPhase (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PHASE    Phase
+  )
+{
+  PCI_HOST_BRIDGE_INSTANCE              *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE              *RootBridge;
+  LIST_ENTRY                            *Link;
+  EFI_PHYSICAL_ADDRESS                  BaseAddress;
+  UINTN                                 BitsOfAlignment;
+  UINT64                                Alignment;
+  EFI_STATUS                            Status;
+  EFI_STATUS                            ReturnStatus;
+  PCI_RESOURCE_TYPE                     Index;
+  PCI_RESOURCE_TYPE                     Index1;
+  PCI_RESOURCE_TYPE                     Index2;
+  BOOLEAN                               ResNodeHandled[TypeMax];
+  UINT64                                MaxAlignment;
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+
+  switch (Phase) {
+  case EfiPciHostBridgeBeginEnumeration:
+    if (!HostBridge->CanRestarted) {
+      return EFI_NOT_READY;
+    }
+    //
+    // Reset Root Bridge
+    //
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+          ; !IsNull (&HostBridge->RootBridges, Link)
+          ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+          ) {
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      for (Index = TypeIo; Index < TypeMax; Index++) {
+        RootBridge->ResAllocNode[Index].Type   = Index;
+        RootBridge->ResAllocNode[Index].Base   = 0;
+        RootBridge->ResAllocNode[Index].Length = 0;
+        RootBridge->ResAllocNode[Index].Status = ResNone;
+
+        RootBridge->ResourceSubmitted = FALSE;
+      }
+    }
+
+    HostBridge->CanRestarted = TRUE;
+    break;
+
+  case EfiPciHostBridgeBeginBusAllocation:
+    //
+    // No specific action is required here, can perform any chipset specific programing
+    //
+    HostBridge->CanRestarted = FALSE;
+    break;
+
+  case EfiPciHostBridgeEndBusAllocation:
+    //
+    // No specific action is required here, can perform any chipset specific programing
+    //
+    break;
+
+  case EfiPciHostBridgeBeginResourceAllocation:
+    //
+    // No specific action is required here, can perform any chipset specific programing
+    //
+    break;
+
+  case EfiPciHostBridgeAllocateResources:
+    ReturnStatus = EFI_SUCCESS;
+
+    //
+    // Make sure the resource for all root bridges has been submitted.
+    //
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+         ; !IsNull (&HostBridge->RootBridges, Link)
+         ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+         ) {
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      if (!RootBridge->ResourceSubmitted) {
+        return EFI_NOT_READY;
+      }
+    }
+
+    DEBUG ((EFI_D_INFO, "PciHostBridge: NotifyPhase (AllocateResources)\n"));
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+         ; !IsNull (&HostBridge->RootBridges, Link)
+         ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+         ) {
+      for (Index = TypeIo; Index < TypeBus; Index++) {
+        ResNodeHandled[Index] = FALSE;
+      }
+
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      DEBUG ((EFI_D_INFO, " RootBridge: %s\n", RootBridge->DevicePathStr));
+
+      for (Index1 = TypeIo; Index1 < TypeBus; Index1++) {
+        if (RootBridge->ResAllocNode[Index1].Status == ResNone) {
+          ResNodeHandled[Index1] = TRUE;
+        } else {
+          //
+          // Allocate the resource node with max alignment at first
+          //
+          MaxAlignment = 0;
+          Index = TypeMax;
+          for (Index2 = TypeIo; Index2 < TypeBus; Index2++) {
+            if (ResNodeHandled[Index2]) {
+              continue;
+            }
+            if (MaxAlignment <= RootBridge->ResAllocNode[Index2].Alignment) {
+              MaxAlignment = RootBridge->ResAllocNode[Index2].Alignment;
+              Index = Index2;
+            }
+          }
+
+          ASSERT (Index < TypeMax);
+          ResNodeHandled[Index] = TRUE;
+          Alignment = RootBridge->ResAllocNode[Index].Alignment;
+          BitsOfAlignment = LowBitSet64 (Alignment + 1);
+          BaseAddress = MAX_UINT64;
+
+          switch (Index) {
+          case TypeIo:
+            BaseAddress = AllocateResource (
+                            FALSE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (15, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->Io.Base, Alignment + 1),
+                            RootBridge->Io.Limit
+                            );
+            break;
+
+          case TypeMem64:
+            BaseAddress = AllocateResource (
+                            TRUE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (63, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->MemAbove4G.Base, Alignment + 1),
+                            RootBridge->MemAbove4G.Limit
+                            );
+            if (BaseAddress != MAX_UINT64) {
+              break;
+            }
+            //
+            // If memory above 4GB is not available, try memory below 4GB
+            //
+
+          case TypeMem32:
+            BaseAddress = AllocateResource (
+                            TRUE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (31, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->Mem.Base, Alignment + 1),
+                            RootBridge->Mem.Limit
+                            );
+            break;
+
+          case TypePMem64:
+            BaseAddress = AllocateResource (
+                            TRUE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (63, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->PMemAbove4G.Base, Alignment + 1),
+                            RootBridge->PMemAbove4G.Limit
+                            );
+            if (BaseAddress != MAX_UINT64) {
+              break;
+            }
+            //
+            // If memory above 4GB is not available, try memory below 4GB
+            //
+          case TypePMem32:
+            BaseAddress = AllocateResource (
+                            TRUE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (31, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->PMem.Base, Alignment + 1),
+                            RootBridge->PMem.Limit
+                            );
+            break;
+
+          default:
+            ASSERT (FALSE);
+            break;
+          }
+
+          DEBUG ((DEBUG_INFO, "  %s: Base/Length/Alignment = %lx/%lx/%lx - ",
+                  mPciResourceTypeStr[Index], BaseAddress, RootBridge->ResAllocNode[Index].Length, Alignment));
+          if (BaseAddress != MAX_UINT64) {
+            RootBridge->ResAllocNode[Index].Base = BaseAddress;
+            RootBridge->ResAllocNode[Index].Status = ResAllocated;
+            DEBUG ((DEBUG_INFO, "Success\n"));
+          } else {
+            ReturnStatus = EFI_OUT_OF_RESOURCES;
+            DEBUG ((DEBUG_ERROR, "Out Of Resource!\n"));
+          }
+        }
+      }
+    }
+
+    if (ReturnStatus == EFI_OUT_OF_RESOURCES) {
+      ResourceConflict (HostBridge);
+    }
+
+    //
+    // Set resource to zero for nodes where allocation fails
+    //
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+          ; !IsNull (&HostBridge->RootBridges, Link)
+          ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+          ) {
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      for (Index = TypeIo; Index < TypeBus; Index++) {
+        if (RootBridge->ResAllocNode[Index].Status != ResAllocated) {
+          RootBridge->ResAllocNode[Index].Length = 0;
+        }
+      }
+    }
+    return ReturnStatus;
+
+  case EfiPciHostBridgeSetResources:
+    //
+    // HostBridgeInstance->CanRestarted = FALSE;
+    //
+    break;
+
+  case EfiPciHostBridgeFreeResources:
+    //
+    // HostBridgeInstance->CanRestarted = FALSE;
+    //
+    ReturnStatus = EFI_SUCCESS;
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+         ; !IsNull (&HostBridge->RootBridges, Link)
+         ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+         ) {
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      for (Index = TypeIo; Index < TypeBus; Index++) {
+        if (RootBridge->ResAllocNode[Index].Status == ResAllocated) {
+          switch (Index) {
+          case TypeIo:
+            Status = gDS->FreeIoSpace (RootBridge->ResAllocNode[Index].Base, RootBridge->ResAllocNode[Index].Length);
+            if (EFI_ERROR (Status)) {
+              ReturnStatus = Status;
+            }
+            break;
+
+          case TypeMem32:
+          case TypePMem32:
+          case TypeMem64:
+          case TypePMem64:
+            Status = gDS->FreeMemorySpace (RootBridge->ResAllocNode[Index].Base, RootBridge->ResAllocNode[Index].Length);
+            if (EFI_ERROR (Status)) {
+              ReturnStatus = Status;
+            }
+            break;
+
+          default:
+            ASSERT (FALSE);
+            break;
+          }
+
+          RootBridge->ResAllocNode[Index].Type = Index;
+          RootBridge->ResAllocNode[Index].Base = 0;
+          RootBridge->ResAllocNode[Index].Length = 0;
+          RootBridge->ResAllocNode[Index].Status = ResNone;
+        }
+      }
+
+      RootBridge->ResourceSubmitted = FALSE;
+    }
+
+    HostBridge->CanRestarted = TRUE;
+    return ReturnStatus;
+
+  case EfiPciHostBridgeEndResourceAllocation:
+    //
+    // The resource allocation phase is completed.  No specific action is required
+    // here. This notification can be used to perform any chipset specific programming.
+    //
+    break;
+
+  case EfiPciHostBridgeEndEnumeration:
+    //
+    // The Host Bridge Enumeration is completed. No specific action is required here.
+    // This notification can be used to perform any chipset specific programming.
+    //
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Return the device handle of the next PCI root bridge that is associated with
+  this Host Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  Returns the device handle of the next PCI Root Bridge.
+                           On input, it holds the RootBridgeHandle returned by the most
+                           recent call to GetNextRootBridge().The handle for the first
+                           PCI Root Bridge is returned if RootBridgeHandle is NULL on input.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_NOT_FOUND          Next PCI root bridge not found.
+  @retval EFI_INVALID_PARAMETER  Wrong parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+GetNextRootBridge (
+  IN     EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN OUT EFI_HANDLE                                       *RootBridgeHandle
+  )
+{
+  BOOLEAN                   ReturnNext;
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+
+  if (RootBridgeHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  ReturnNext = (BOOLEAN) (*RootBridgeHandle == NULL);
+
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+      ; !IsNull (&HostBridge->RootBridges, Link)
+      ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+      ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (ReturnNext) {
+      *RootBridgeHandle = RootBridge->Handle;
+      return EFI_SUCCESS;
+    }
+
+    ReturnNext = (BOOLEAN) (*RootBridgeHandle == RootBridge->Handle);
+  }
+
+  if (ReturnNext) {
+    ASSERT (IsNull (&HostBridge->RootBridges, Link));
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_INVALID_PARAMETER;
+  }
+}
+
+/**
+
+  Returns the attributes of a PCI Root Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The device handle of the PCI Root Bridge
+                           that the caller is interested in.
+  @param Attributes        The pointer to attributes of the PCI Root Bridge.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Attributes parameter passed in is NULL or
+                                 RootBridgeHandle is not an EFI_HANDLE
+                                 that was returned on a previous call to
+                                 GetNextRootBridge().
+
+**/
+EFI_STATUS
+EFIAPI
+GetAttributes (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT UINT64                                           *Attributes
+  )
+{
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+
+  if (Attributes == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+      ; !IsNull (&HostBridge->RootBridges, Link)
+      ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+      ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      *Attributes = RootBridge->AllocationAttributes;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  This is the request from the PCI enumerator to set up
+  the specified PCI Root Bridge for bus enumeration process.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge to be set up.
+  @param Configuration     Pointer to the pointer to the PCI bus resource descriptor.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_OUT_OF_RESOURCES   Not enough pool to be allocated.
+  @retval EFI_INVALID_PARAMETER  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+StartBusEnumeration (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT VOID                                             **Configuration
+  )
+{
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      *Configuration = AllocatePool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+      if (*Configuration == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) *Configuration;
+      Descriptor->Desc                  = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Descriptor->Len                   = sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3;
+      Descriptor->ResType               = ACPI_ADDRESS_SPACE_TYPE_BUS;
+      Descriptor->GenFlag               = 0;
+      Descriptor->SpecificFlag          = 0;
+      Descriptor->AddrSpaceGranularity  = 0;
+      Descriptor->AddrRangeMin          = RootBridge->Bus.Base;
+      Descriptor->AddrRangeMax          = 0;
+      Descriptor->AddrTranslationOffset = 0;
+      Descriptor->AddrLen               = RootBridge->Bus.Limit - RootBridge->Bus.Base + 1;
+
+      End = (EFI_ACPI_END_TAG_DESCRIPTOR *) (Descriptor + 1);
+      End->Desc = ACPI_END_TAG_DESCRIPTOR;
+      End->Checksum = 0x0;
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  This function programs the PCI Root Bridge hardware so that
+  it decodes the specified PCI bus range.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge whose bus range is to be programmed.
+  @param Configuration     The pointer to the PCI bus resource descriptor.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Wrong parameters passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+SetBusNumbers (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_HANDLE                                       RootBridgeHandle,
+  IN VOID                                             *Configuration
+  )
+{
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration;
+  End = (EFI_ACPI_END_TAG_DESCRIPTOR *) (Descriptor + 1);
+
+  //
+  // Check the Configuration is valid
+  //
+  if ((Descriptor->Desc != ACPI_ADDRESS_SPACE_DESCRIPTOR) ||
+      (Descriptor->ResType != ACPI_ADDRESS_SPACE_TYPE_BUS) ||
+      (End->Desc != ACPI_END_TAG_DESCRIPTOR)
+     ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+
+      if (Descriptor->AddrLen == 0) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      if ((Descriptor->AddrRangeMin < RootBridge->Bus.Base) ||
+          (Descriptor->AddrRangeMin + Descriptor->AddrLen - 1 > RootBridge->Bus.Limit)
+         ) {
+        return EFI_INVALID_PARAMETER;
+      }
+      //
+      // Update the Bus Range
+      //
+      RootBridge->ResAllocNode[TypeBus].Base    = Descriptor->AddrRangeMin;
+      RootBridge->ResAllocNode[TypeBus].Length  = Descriptor->AddrLen;
+      RootBridge->ResAllocNode[TypeBus].Status  = ResAllocated;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  Submits the I/O and memory resource requirements for the specified PCI Root Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge whose I/O and memory resource requirements.
+                           are being submitted.
+  @param Configuration     The pointer to the PCI I/O and PCI memory resource descriptor.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Wrong parameters passed in.
+**/
+EFI_STATUS
+EFIAPI
+SubmitResources (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_HANDLE                                       RootBridgeHandle,
+  IN VOID                                             *Configuration
+  )
+{
+  LIST_ENTRY                        *Link;
+  PCI_HOST_BRIDGE_INSTANCE          *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE          *RootBridge;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  PCI_RESOURCE_TYPE                 Type;
+
+  //
+  // Check the input parameter: Configuration
+  //
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      DEBUG ((EFI_D_INFO, "PciHostBridge: SubmitResources for %s\n", RootBridge->DevicePathStr));
+      //
+      // Check the resource descriptors.
+      // If the Configuration includes one or more invalid resource descriptors, all the resource
+      // descriptors are ignored and the function returns EFI_INVALID_PARAMETER.
+      //
+      for (Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration; Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR; Descriptor++) {
+        if (Descriptor->ResType > ACPI_ADDRESS_SPACE_TYPE_BUS) {
+          return EFI_INVALID_PARAMETER;
+        }
+
+        DEBUG ((EFI_D_INFO, " %s: Granularity/SpecificFlag = %ld / %02x%s\n",
+                mAcpiAddressSpaceTypeStr[Descriptor->ResType], Descriptor->AddrSpaceGranularity, Descriptor->SpecificFlag,
+                (Descriptor->SpecificFlag & EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) != 0 ? L" (Prefetchable)" : L""
+                ));
+        DEBUG ((EFI_D_INFO, "      Length/Alignment = 0x%lx / 0x%lx\n", Descriptor->AddrLen, Descriptor->AddrRangeMax));
+        switch (Descriptor->ResType) {
+        case ACPI_ADDRESS_SPACE_TYPE_MEM:
+          if (Descriptor->AddrSpaceGranularity != 32 && Descriptor->AddrSpaceGranularity != 64) {
+            return EFI_INVALID_PARAMETER;
+          }
+          if (Descriptor->AddrSpaceGranularity == 32 && Descriptor->AddrLen >= SIZE_4GB) {
+            return EFI_INVALID_PARAMETER;
+          }
+          //
+          // If the PCI root bridge does not support separate windows for nonprefetchable and
+          // prefetchable memory, then the PCI bus driver needs to include requests for
+          // prefetchable memory in the nonprefetchable memory pool.
+          //
+          if (((RootBridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_COMBINE_MEM_PMEM) != 0) &&
+              ((Descriptor->SpecificFlag & EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) != 0)
+             ) {
+            return EFI_INVALID_PARAMETER;
+          }
+        case ACPI_ADDRESS_SPACE_TYPE_IO:
+          //
+          // Check aligment, it should be of the form 2^n-1
+          //
+          if (GetPowerOfTwo64 (Descriptor->AddrRangeMax + 1) != (Descriptor->AddrRangeMax + 1)) {
+            return EFI_INVALID_PARAMETER;
+          }
+          break;
+        default:
+          ASSERT (FALSE);
+          break;
+        }
+      }
+      if (Descriptor->Desc != ACPI_END_TAG_DESCRIPTOR) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      for (Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration; Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR; Descriptor++) {
+        if (Descriptor->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+          if (Descriptor->AddrSpaceGranularity == 32) {
+            if ((Descriptor->SpecificFlag & EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) != 0) {
+              Type = TypePMem32;
+            } else {
+              Type = TypeMem32;
+            }
+          } else {
+            ASSERT (Descriptor->AddrSpaceGranularity == 64);
+            if ((Descriptor->SpecificFlag & EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) != 0) {
+              Type = TypePMem64;
+            } else {
+              Type = TypeMem64;
+            }
+          }
+        } else {
+          ASSERT (Descriptor->ResType == ACPI_ADDRESS_SPACE_TYPE_IO);
+          Type = TypeIo;
+        }
+        RootBridge->ResAllocNode[Type].Length    = Descriptor->AddrLen;
+        RootBridge->ResAllocNode[Type].Alignment = Descriptor->AddrRangeMax;
+        RootBridge->ResAllocNode[Type].Status    = ResSubmitted;
+      }
+      RootBridge->ResourceSubmitted = TRUE;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  This function returns the proposed resource settings for the specified
+  PCI Root Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge handle.
+  @param Configuration     The pointer to the pointer to the PCI I/O
+                           and memory resource descriptor.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_OUT_OF_RESOURCES   Not enough pool to be allocated.
+  @retval EFI_INVALID_PARAMETER  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+GetProposedResources (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT VOID                                             **Configuration
+  )
+{
+  LIST_ENTRY                        *Link;
+  PCI_HOST_BRIDGE_INSTANCE          *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE          *RootBridge;
+  UINTN                             Index;
+  UINTN                             Number;
+  VOID                              *Buffer;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+  UINT64                            ResStatus;
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+      ; !IsNull (&HostBridge->RootBridges, Link)
+      ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+      ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      for (Index = 0, Number = 0; Index < TypeBus; Index++) {
+        if (RootBridge->ResAllocNode[Index].Status != ResNone) {
+          Number++;
+        }
+      }
+
+      Buffer = AllocateZeroPool (Number * sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+      if (Buffer == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Buffer;
+      for (Index = 0; Index < TypeBus; Index++) {
+        ResStatus = RootBridge->ResAllocNode[Index].Status;
+        if (ResStatus != ResNone) {
+          Descriptor->Desc                  = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+          Descriptor->Len                   = sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3;;
+          Descriptor->GenFlag               = 0;
+          Descriptor->AddrRangeMin          = RootBridge->ResAllocNode[Index].Base;
+          Descriptor->AddrRangeMax          = 0;
+          Descriptor->AddrTranslationOffset = (ResStatus == ResAllocated) ? EFI_RESOURCE_SATISFIED : PCI_RESOURCE_LESS;
+          Descriptor->AddrLen               = RootBridge->ResAllocNode[Index].Length;
+
+          switch (Index) {
+
+          case TypeIo:
+            Descriptor->ResType              = ACPI_ADDRESS_SPACE_TYPE_IO;
+            break;
+
+          case TypePMem32:
+            Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+          case TypeMem32:
+            Descriptor->ResType              = ACPI_ADDRESS_SPACE_TYPE_MEM;
+            Descriptor->AddrSpaceGranularity = 32;
+            break;
+
+          case TypePMem64:
+            Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+          case TypeMem64:
+            Descriptor->ResType              = ACPI_ADDRESS_SPACE_TYPE_MEM;
+            Descriptor->AddrSpaceGranularity = 64;
+            break;
+          }
+
+          Descriptor++;
+        }
+      }
+      End = (EFI_ACPI_END_TAG_DESCRIPTOR *) Descriptor;
+      End->Desc      = ACPI_END_TAG_DESCRIPTOR;
+      End->Checksum  = 0;
+
+      *Configuration = Buffer;
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  This function is called for all the PCI controllers that the PCI
+  bus driver finds. Can be used to Preprogram the controller.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge handle.
+  @param PciAddress        Address of the controller on the PCI bus.
+  @param Phase             The Phase during resource allocation.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+PreprocessController (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL          *This,
+  IN  EFI_HANDLE                                                RootBridgeHandle,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS               PciAddress,
+  IN  EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE              Phase
+  )
+{
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+
+  if ((UINT32) Phase > EfiPciBeforeResourceCollection) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.h b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.h
new file mode 100644
index 0000000000..9a8ca21f48
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.h
@@ -0,0 +1,252 @@
+/** @file
+
+  The Header file of the Pci Host Bridge Driver.
+
+Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PCI_HOST_BRIDGE_H_
+#define _PCI_HOST_BRIDGE_H_
+
+
+#include <PiDxe.h>
+#include <IndustryStandard/Acpi.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PciHostBridgeLib.h>
+#include <Protocol/PciHostBridgeResourceAllocation.h>
+
+#include "PciRootBridge.h"
+
+#define PCI_HOST_BRIDGE_SIGNATURE SIGNATURE_32 ('p', 'h', 'b', 'g')
+typedef struct {
+  UINTN                                             Signature;
+  EFI_HANDLE                                        Handle;
+  LIST_ENTRY                                        RootBridges;
+  BOOLEAN                                           CanRestarted;
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  ResAlloc;
+} PCI_HOST_BRIDGE_INSTANCE;
+
+#define PCI_HOST_BRIDGE_FROM_THIS(a) CR (a, PCI_HOST_BRIDGE_INSTANCE, ResAlloc, PCI_HOST_BRIDGE_SIGNATURE)
+
+//
+// Driver Entry Point
+//
+/**
+
+  Entry point of this driver.
+
+  @param ImageHandle  -  Image handle of this driver.
+  @param SystemTable  -  Pointer to standard EFI system table.
+
+  @retval EFI_SUCCESS       -  Succeed.
+  @retval EFI_DEVICE_ERROR  -  Fail to install PCI_ROOT_BRIDGE_IO protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializePciHostBridge (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  );
+
+//
+//  HostBridge Resource Allocation interface
+//
+/**
+
+  Enter a certain phase of the PCI enumeration process.
+
+  @param This   The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL instance.
+  @param Phase  The phase during enumeration.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Wrong phase parameter passed in.
+  @retval EFI_NOT_READY          Resources have not been submitted yet.
+
+**/
+EFI_STATUS
+EFIAPI
+NotifyPhase (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL   *This,
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PHASE      Phase
+  );
+
+/**
+
+  Return the device handle of the next PCI root bridge that is associated with
+  this Host Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  Returns the device handle of the next PCI Root Bridge.
+                           On input, it holds the RootBridgeHandle returned by the most
+                           recent call to GetNextRootBridge().The handle for the first
+                           PCI Root Bridge is returned if RootBridgeHandle is NULL on input.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_NOT_FOUND          Next PCI root bridge not found.
+  @retval EFI_INVALID_PARAMETER  Wrong parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+GetNextRootBridge (
+  IN     EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN OUT EFI_HANDLE                                       *RootBridgeHandle
+  );
+
+/**
+
+  Returns the attributes of a PCI Root Bridge.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance
+  @param RootBridgeHandle  -  The device handle of the PCI Root Bridge
+                              that the caller is interested in
+  @param Attributes        -  The pointer to attributes of the PCI Root Bridge
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  Attributes parameter passed in is NULL or
+                            @retval RootBridgeHandle is not an EFI_HANDLE
+                            @retval that was returned on a previous call to
+                            @retval GetNextRootBridge().
+
+**/
+EFI_STATUS
+EFIAPI
+GetAttributes (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT UINT64                                           *Attributes
+  );
+
+/**
+
+  This is the request from the PCI enumerator to set up
+  the specified PCI Root Bridge for bus enumeration process.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  -  The PCI Root Bridge to be set up.
+  @param Configuration     -  Pointer to the pointer to the PCI bus resource descriptor.
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_OUT_OF_RESOURCES   -  Not enough pool to be allocated.
+  @retval EFI_INVALID_PARAMETER  -  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+StartBusEnumeration (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT VOID                                             **Configuration
+  );
+
+/**
+
+  This function programs the PCI Root Bridge hardware so that
+  it decodes the specified PCI bus range.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  -  The PCI Root Bridge whose bus range is to be programmed.
+  @param Configuration     -  The pointer to the PCI bus resource descriptor.
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  Wrong parameters passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+SetBusNumbers (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_HANDLE                                       RootBridgeHandle,
+  IN VOID                                             *Configuration
+  );
+
+/**
+
+  Submits the I/O and memory resource requirements for the specified PCI Root Bridge.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance
+  @param RootBridgeHandle  -  The PCI Root Bridge whose I/O and memory resource requirements
+                              are being submitted
+  @param Configuration     -  The pointer to the PCI I/O and PCI memory resource descriptor
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  Wrong parameters passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+SubmitResources (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_HANDLE                                       RootBridgeHandle,
+  IN VOID                                             *Configuration
+  );
+
+/**
+
+  This function returns the proposed resource settings for the specified
+  PCI Root Bridge.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  -  The PCI Root Bridge handle.
+  @param Configuration     -  The pointer to the pointer to the PCI I/O
+                              and memory resource descriptor.
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_OUT_OF_RESOURCES   -  Not enough pool to be allocated.
+  @retval EFI_INVALID_PARAMETER  -  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+GetProposedResources (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT VOID                                             **Configuration
+  );
+
+/**
+
+  This function is called for all the PCI controllers that the PCI
+  bus driver finds. Can be used to Preprogram the controller.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  -  The PCI Root Bridge handle.
+  @param PciAddress        -  Address of the controller on the PCI bus.
+  @param Phase             -  The Phase during resource allocation.
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+PreprocessController (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL          *This,
+  IN EFI_HANDLE                                                RootBridgeHandle,
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS               PciAddress,
+  IN EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE              Phase
+  );
+
+/**
+  This routine constructs the resource descriptors for all root bridges and call PciHostBridgeResourceConflict().
+
+  @param HostBridge The Host Bridge Instance where the resource adjustment happens.
+**/
+VOID
+ResourceConflict (
+  IN  PCI_HOST_BRIDGE_INSTANCE *HostBridge
+  );
+
+extern EFI_METRONOME_ARCH_PROTOCOL *mMetronome;
+extern EFI_CPU_IO2_PROTOCOL        *mCpuIo;
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridgeDxe.inf b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridgeDxe.inf
new file mode 100644
index 0000000000..d8b0439699
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridgeDxe.inf
@@ -0,0 +1,56 @@
+## @file
+#   Generic PCI Host Bridge driver.
+#
+#  Copyright (c) 2009 - 2016, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PciHostBridgeDxe
+  FILE_GUID                      = 128FB770-5E79-4176-9E51-9BB268A17DD1
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializePciHostBridge
+
+[Sources]
+  PciHostBridge.h
+  PciRootBridge.h
+  PciHostBridge.c
+  PciRootBridgeIo.c
+  PciHostResource.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  DebugLib
+  DxeServicesTableLib
+  DevicePathLib
+  BaseMemoryLib
+  BaseLib
+  PciSegmentLib
+  PciHostBridgeLib
+
+[Protocols]
+  gEfiMetronomeArchProtocolGuid                   ## CONSUMES
+  gEfiCpuIo2ProtocolGuid                          ## CONSUMES
+  gEfiDevicePathProtocolGuid                      ## BY_START
+  gEfiPciRootBridgeIoProtocolGuid                 ## BY_START
+  gEfiPciHostBridgeResourceAllocationProtocolGuid ## BY_START
+
+[Depex]
+  gEfiCpuIo2ProtocolGuid AND
+  gEfiMetronomeArchProtocolGuid AND
+  gEfiCpuArchProtocolGuid
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostResource.h b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostResource.h
new file mode 100644
index 0000000000..f35222b634
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostResource.h
@@ -0,0 +1,47 @@
+/** @file
+
+  The Header file of the Pci Host Bridge Driver.
+
+Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+#ifndef _PCI_HOST_RESOURCE_H_
+#define _PCI_HOST_RESOURCE_H_
+
+#include <PiDxe.h>
+
+#define PCI_RESOURCE_LESS         0xFFFFFFFFFFFFFFFE
+
+typedef enum {
+  TypeIo    = 0,
+  TypeMem32,
+  TypePMem32,
+  TypeMem64,
+  TypePMem64,
+  TypeBus,
+  TypeMax
+} PCI_RESOURCE_TYPE;
+
+typedef enum {
+  ResNone,
+  ResSubmitted,
+  ResAllocated,
+  ResStatusMax
+} RES_STATUS;
+
+typedef struct {
+  PCI_RESOURCE_TYPE Type;
+  UINT64            Base;
+  UINT64            Length;
+  UINT64            Alignment;
+  RES_STATUS        Status;
+} PCI_RES_NODE;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridge.h b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridge.h
new file mode 100644
index 0000000000..13185b41ac
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridge.h
@@ -0,0 +1,578 @@
+/** @file
+
+  The PCI Root Bridge header file.
+
+Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PCI_ROOT_BRIDGE_H_
+#define _PCI_ROOT_BRIDGE_H_
+
+#include <PiDxe.h>
+#include <IndustryStandard/Acpi.h>
+#include <IndustryStandard/Pci.h>
+
+//
+// Driver Consumed Protocol Prototypes
+//
+#include <Protocol/Metronome.h>
+#include <Protocol/CpuIo2.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PciRootBridgeIo.h>
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/PciSegmentLib.h>
+#include "PciHostResource.h"
+
+
+typedef enum {
+  IoOperation,
+  MemOperation,
+  PciOperation
+} OPERATION_TYPE;
+
+#define MAP_INFO_SIGNATURE  SIGNATURE_32 ('_', 'm', 'a', 'p')
+typedef struct {
+  UINT32                                    Signature;
+  LIST_ENTRY                                Link;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_OPERATION Operation;
+  UINTN                                     NumberOfBytes;
+  UINTN                                     NumberOfPages;
+  EFI_PHYSICAL_ADDRESS                      HostAddress;
+  EFI_PHYSICAL_ADDRESS                      MappedHostAddress;
+} MAP_INFO;
+#define MAP_INFO_FROM_LINK(a) CR (a, MAP_INFO, Link, MAP_INFO_SIGNATURE)
+
+#define PCI_ROOT_BRIDGE_SIGNATURE SIGNATURE_32 ('_', 'p', 'r', 'b')
+
+typedef struct {
+  UINT32                            Signature;
+  LIST_ENTRY                        Link;
+  EFI_HANDLE                        Handle;
+  UINT64                            AllocationAttributes;
+  UINT64                            Attributes;
+  UINT64                            Supports;
+  PCI_RES_NODE                      ResAllocNode[TypeMax];
+  PCI_ROOT_BRIDGE_APERTURE          Bus;
+  PCI_ROOT_BRIDGE_APERTURE          Io;
+  PCI_ROOT_BRIDGE_APERTURE          Mem;
+  PCI_ROOT_BRIDGE_APERTURE          PMem;
+  PCI_ROOT_BRIDGE_APERTURE          MemAbove4G;
+  PCI_ROOT_BRIDGE_APERTURE          PMemAbove4G;
+  BOOLEAN                           DmaAbove4G;
+  BOOLEAN                           NoExtendedConfigSpace;
+  VOID                              *ConfigBuffer;
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath;
+  CHAR16                            *DevicePathStr;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL   RootBridgeIo;
+
+  BOOLEAN                           ResourceSubmitted;
+  LIST_ENTRY                        Maps;
+} PCI_ROOT_BRIDGE_INSTANCE;
+
+#define ROOT_BRIDGE_FROM_THIS(a) CR (a, PCI_ROOT_BRIDGE_INSTANCE, RootBridgeIo, PCI_ROOT_BRIDGE_SIGNATURE)
+
+#define ROOT_BRIDGE_FROM_LINK(a) CR (a, PCI_ROOT_BRIDGE_INSTANCE, Link, PCI_ROOT_BRIDGE_SIGNATURE)
+
+/**
+  Construct the Pci Root Bridge instance.
+
+  @param Bridge            The root bridge instance.
+
+  @return The pointer to PCI_ROOT_BRIDGE_INSTANCE just created
+          or NULL if creation fails.
+**/
+PCI_ROOT_BRIDGE_INSTANCE *
+CreateRootBridge (
+  IN PCI_ROOT_BRIDGE       *Bridge
+  );
+
+//
+// Protocol Member Function Prototypes
+//
+/**
+
+  Poll an address in memory mapped space until an exit condition is met
+  or a timeout occurs.
+
+  @param This     -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width    -  Width of the memory operation.
+  @param Address  -  The base address of the memory operation.
+  @param Mask     -  Mask used for polling criteria.
+  @param Value    -  Comparison value used for polling exit criteria.
+  @param Delay    -  Number of 100ns units to poll.
+  @param Result   -  Pointer to the last value read from memory location.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_TIMEOUT            -  Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES   -  Fail due to lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPollMem (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT64                                 Address,
+  IN  UINT64                                 Mask,
+  IN  UINT64                                 Value,
+  IN  UINT64                                 Delay,
+  OUT UINT64                                 *Result
+  )
+;
+
+/**
+
+  Poll an address in I/O space until an exit condition is met
+  or a timeout occurs.
+
+  @param This     -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width    -  Width of I/O operation.
+  @param Address  -  The base address of the I/O operation.
+  @param Mask     -  Mask used for polling criteria.
+  @param Value    -  Comparison value used for polling exit criteria.
+  @param Delay    -  Number of 100ns units to poll.
+  @param Result   -  Pointer to the last value read from memory location.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_TIMEOUT            -  Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES   -  Fail due to lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPollIo (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT64                                 Address,
+  IN  UINT64                                 Mask,
+  IN  UINT64                                 Value,
+  IN  UINT64                                 Delay,
+  OUT UINT64                                 *Result
+  )
+;
+
+/**
+
+  Allow read from memory mapped I/O space.
+
+  @param This     -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width    -  The width of memory operation.
+  @param Address  -  Base address of the memory operation.
+  @param Count    -  Number of memory opeartion to perform.
+  @param Buffer   -  The destination buffer to store data.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_OUT_OF_RESOURCES   -  Fail due to lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMemRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *Buffer
+  )
+;
+
+/**
+
+  Allow write to memory mapped I/O space.
+
+  @param This     -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width    -  The width of memory operation.
+  @param Address  -  Base address of the memory operation.
+  @param Count    -  Number of memory opeartion to perform.
+  @param Buffer   -  The source buffer to write data from.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_OUT_OF_RESOURCES   -  Fail due to lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMemWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *Buffer
+  )
+;
+
+/**
+
+  Enable a PCI driver to read PCI controller registers in the
+  PCI root bridge I/O space.
+
+  @param This         -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width        -  Signifies the width of the memory operation.
+  @param UserAddress  -  The base address of the I/O operation.
+  @param Count        -  The number of I/O operations to perform.
+  @param UserBuffer   -  The destination buffer to store the results.
+
+  @retval EFI_SUCCESS            -  The data was read from the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoIoRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 UserAddress,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *UserBuffer
+  )
+;
+
+/**
+
+  Enable a PCI driver to write to PCI controller registers in the
+  PCI root bridge I/O space.
+
+  @param This         -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width        -  Signifies the width of the memory operation.
+  @param UserAddress  -  The base address of the I/O operation.
+  @param Count        -  The number of I/O operations to perform.
+  @param UserBuffer   -  The source buffer to write data from.
+
+  @retval EFI_SUCCESS            -  The data was written to the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoIoWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 UserAddress,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *UserBuffer
+  )
+;
+
+/**
+
+  Copy one region of PCI root bridge memory space to be copied to
+  another region of PCI root bridge memory space.
+
+  @param This         -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width        -  Signifies the width of the memory operation.
+  @param DestAddress  -  Destination address of the memory operation.
+  @param SrcAddress   -  Source address of the memory operation.
+  @param Count        -  Number of memory operations to perform.
+
+  @retval EFI_SUCCESS            -  The data was copied successfully.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoCopyMem (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL          *This,
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH    Width,
+  IN UINT64                                   DestAddress,
+  IN UINT64                                   SrcAddress,
+  IN UINTN                                    Count
+  )
+;
+
+/**
+
+  Allows read from PCI configuration space.
+
+  @param This     -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width    -  Signifies the width of the memory operation.
+  @param Address  -  The address within the PCI configuration space
+                     for the PCI controller.
+  @param Count    -  The number of PCI configuration operations
+                     to perform.
+  @param Buffer   -  The destination buffer to store the results.
+
+  @retval EFI_SUCCESS            -  The data was read from the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *Buffer
+  )
+;
+
+/**
+
+  Allows write to PCI configuration space.
+
+  @param This     -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width    -  Signifies the width of the memory operation.
+  @param Address  -  The address within the PCI configuration space
+                     for the PCI controller.
+  @param Count    -  The number of PCI configuration operations
+                     to perform.
+  @param Buffer   -  The source buffer to get the results.
+
+  @retval EFI_SUCCESS            -  The data was written to the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *Buffer
+  )
+;
+
+/**
+  Provides the PCI controller-specific address needed to access
+  system memory for DMA.
+
+  @param This           A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Operation      Indicate if the bus master is going to read or write
+                        to system memory.
+  @param HostAddress    The system memory address to map on 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 system memory's HostAddress.
+  @param Mapping        The value to pass to Unmap() when the bus master DMA
+                        operation is complete.
+
+  @retval EFI_SUCCESS            Success.
+  @retval EFI_INVALID_PARAMETER  Invalid parameters found.
+  @retval EFI_UNSUPPORTED        The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_DEVICE_ERROR       The System hardware could not map the requested address.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMap (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL            *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_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.
+
+  The Unmap() function completes the Map() operation and releases any
+  corresponding resources.
+  If the operation was an EfiPciOperationBusMasterWrite or
+  EfiPciOperationBusMasterWrite64, the data is committed to the target system
+  memory.
+  Any resources used for the mapping are freed.
+
+  @param[in] This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in] Mapping   The mapping value returned from Map().
+
+  @retval EFI_SUCCESS            The range was unmapped.
+  @retval EFI_INVALID_PARAMETER  Mapping is not a value that was returned by Map().
+  @retval EFI_DEVICE_ERROR       The data was not committed to the target system memory.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoUnmap (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  VOID                             *Mapping
+  )
+;
+
+/**
+  Allocates pages that are suitable for an EfiPciOperationBusMasterCommonBuffer
+  or EfiPciOperationBusMasterCommonBuffer64 mapping.
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Type        This parameter is not used and must be ignored.
+  @param MemoryType  The type of memory to allocate, EfiBootServicesData or
+                     EfiRuntimeServicesData.
+  @param Pages       The number of pages to allocate.
+  @param HostAddress A pointer to store the base system memory address of the
+                     allocated range.
+  @param Attributes  The requested bit mask of attributes for the allocated
+                     range. Only the attributes
+                     EFI_PCI_ATTRIBUTE_MEMORY_WRITE_COMBINE,
+                     EFI_PCI_ATTRIBUTE_MEMORY_CACHED, and
+                     EFI_PCI_ATTRIBUTE_DUAL_ADDRESS_CYCLE may be used with this
+                     function.
+
+  @retval EFI_SUCCESS            The requested memory pages were allocated.
+  @retval EFI_INVALID_PARAMETER  MemoryType is invalid.
+  @retval EFI_INVALID_PARAMETER  HostAddress is NULL.
+  @retval EFI_UNSUPPORTED        Attributes is unsupported. The only legal
+                                 attribute bits are MEMORY_WRITE_COMBINE,
+                                 MEMORY_CACHED, and DUAL_ADDRESS_CYCLE.
+  @retval EFI_OUT_OF_RESOURCES   The memory pages could not be allocated.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoAllocateBuffer (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  EFI_ALLOCATE_TYPE                Type,
+  IN  EFI_MEMORY_TYPE                  MemoryType,
+  IN  UINTN                            Pages,
+  OUT VOID                             **HostAddress,
+  IN  UINT64                           Attributes
+  )
+;
+
+/**
+
+  Free memory allocated in AllocateBuffer.
+
+  @param This         -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+                         instance.
+  @param Pages        -  Number of pages to free.
+  @param HostAddress  -  The base system memory address of the
+                         allocated range.
+
+  @retval EFI_SUCCESS            -  Requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoFreeBuffer (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  UINTN                            Pages,
+  OUT VOID                             *HostAddress
+  )
+;
+
+/**
+
+  Flushes all PCI posted write transactions from a PCI host
+  bridge to system memory.
+
+  @param This  - Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+
+  @retval EFI_SUCCESS       -  PCI posted write transactions were flushed
+                       @retval from PCI host bridge to system memory.
+  @retval EFI_DEVICE_ERROR  -  Fail due to hardware error.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoFlush (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This
+  )
+;
+
+/**
+  Gets the attributes that a PCI root bridge supports setting with
+  SetAttributes(), and the attributes that a PCI root bridge is currently
+  using.
+
+  The GetAttributes() function returns the mask of attributes that this PCI
+  root bridge supports and the mask of attributes that the PCI root bridge is
+  currently using.
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Supported   A pointer to the mask of attributes that this PCI root
+                     bridge supports setting with SetAttributes().
+  @param Attributes  A pointer to the mask of attributes that this PCI root
+                     bridge is currently using.
+
+  @retval  EFI_SUCCESS           If Supports is not NULL, then the attributes
+                                 that the PCI root bridge supports is returned
+                                 in Supports. If Attributes is not NULL, then
+                                 the attributes that the PCI root bridge is
+                                 currently using is returned in Attributes.
+  @retval  EFI_INVALID_PARAMETER Both Supports and Attributes are NULL.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoGetAttributes (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  OUT UINT64                           *Supported,
+  OUT UINT64                           *Attributes
+  )
+;
+
+/**
+
+  Sets the attributes for a resource range on a PCI root bridge.
+
+  @param This            -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Attributes      -  The mask of attributes to set.
+  @param ResourceBase    -  Pointer to the base address of the resource range
+                            to be modified by the attributes specified by Attributes.
+  @param ResourceLength  -  Pointer to the length of the resource range to be modified.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_OUT_OF_RESOURCES   -  Not enough resources to set the attributes upon.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoSetAttributes (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN     UINT64                           Attributes,
+  IN OUT UINT64                           *ResourceBase,
+  IN OUT UINT64                           *ResourceLength
+  )
+;
+
+/**
+
+  Retrieves the current resource settings of this PCI root bridge
+  in the form of a set of ACPI 2.0 resource descriptor.
+
+  @param This       -  Pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Resources  -  Pointer to the ACPI 2.0 resource descriptor that
+                       describe the current configuration of this PCI root
+                       bridge.
+
+  @retval EFI_SUCCESS      -  Success.
+  @retval EFI_UNSUPPORTED  -  Current configuration of the PCI root bridge
+                      @retval could not be retrieved.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoConfiguration (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  OUT VOID                             **Resources
+  )
+;
+
+
+extern EFI_METRONOME_ARCH_PROTOCOL *mMetronome;
+extern EFI_CPU_IO2_PROTOCOL         *mCpuIo;
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridgeIo.c b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridgeIo.c
new file mode 100644
index 0000000000..b2d76d67af
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridgeIo.c
@@ -0,0 +1,1594 @@
+/** @file
+
+  PCI Root Bridge Io Protocol code.
+
+Copyright (c) 1999 - 2016, 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 "PciHostBridge.h"
+#include "PciRootBridge.h"
+#include "PciHostResource.h"
+
+#define NO_MAPPING  (VOID *) (UINTN) -1
+
+//
+// Lookup table for increment values based on transfer widths
+//
+UINT8 mInStride[] = {
+  1, // EfiPciWidthUint8
+  2, // EfiPciWidthUint16
+  4, // EfiPciWidthUint32
+  8, // EfiPciWidthUint64
+  0, // EfiPciWidthFifoUint8
+  0, // EfiPciWidthFifoUint16
+  0, // EfiPciWidthFifoUint32
+  0, // EfiPciWidthFifoUint64
+  1, // EfiPciWidthFillUint8
+  2, // EfiPciWidthFillUint16
+  4, // EfiPciWidthFillUint32
+  8  // EfiPciWidthFillUint64
+};
+
+//
+// Lookup table for increment values based on transfer widths
+//
+UINT8 mOutStride[] = {
+  1, // EfiPciWidthUint8
+  2, // EfiPciWidthUint16
+  4, // EfiPciWidthUint32
+  8, // EfiPciWidthUint64
+  1, // EfiPciWidthFifoUint8
+  2, // EfiPciWidthFifoUint16
+  4, // EfiPciWidthFifoUint32
+  8, // EfiPciWidthFifoUint64
+  0, // EfiPciWidthFillUint8
+  0, // EfiPciWidthFillUint16
+  0, // EfiPciWidthFillUint32
+  0  // EfiPciWidthFillUint64
+};
+
+/**
+  Construct the Pci Root Bridge instance.
+
+  @param Bridge            The root bridge instance.
+
+  @return The pointer to PCI_ROOT_BRIDGE_INSTANCE just created
+          or NULL if creation fails.
+**/
+PCI_ROOT_BRIDGE_INSTANCE *
+CreateRootBridge (
+  IN PCI_ROOT_BRIDGE       *Bridge
+  )
+{
+  PCI_ROOT_BRIDGE_INSTANCE *RootBridge;
+  PCI_RESOURCE_TYPE        Index;
+  CHAR16                   *DevicePathStr;
+  PCI_ROOT_BRIDGE_APERTURE *Aperture;
+
+  DevicePathStr = NULL;
+
+  DEBUG ((EFI_D_INFO, "RootBridge: "));
+  DEBUG ((EFI_D_INFO, "%s\n", DevicePathStr = ConvertDevicePathToText (Bridge->DevicePath, FALSE, FALSE)));
+  DEBUG ((EFI_D_INFO, "  Support/Attr: %lx / %lx\n", Bridge->Supports, Bridge->Attributes));
+  DEBUG ((EFI_D_INFO, "    DmaAbove4G: %s\n", Bridge->DmaAbove4G ? L"Yes" : L"No"));
+  DEBUG ((EFI_D_INFO, "NoExtConfSpace: %s\n", Bridge->NoExtendedConfigSpace ? L"Yes" : L"No"));
+  DEBUG ((EFI_D_INFO, "     AllocAttr: %lx (%s%s)\n", Bridge->AllocationAttributes,
+          (Bridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_COMBINE_MEM_PMEM) != 0 ? L"CombineMemPMem " : L"",
+          (Bridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_MEM64_DECODE) != 0 ? L"Mem64Decode" : L""
+          ));
+  DEBUG ((EFI_D_INFO, "           Bus: %lx - %lx\n", Bridge->Bus.Base, Bridge->Bus.Limit));
+  DEBUG ((EFI_D_INFO, "            Io: %lx - %lx\n", Bridge->Io.Base, Bridge->Io.Limit));
+  DEBUG ((EFI_D_INFO, "           Mem: %lx - %lx\n", Bridge->Mem.Base, Bridge->Mem.Limit));
+  DEBUG ((EFI_D_INFO, "    MemAbove4G: %lx - %lx\n", Bridge->MemAbove4G.Base, Bridge->MemAbove4G.Limit));
+  DEBUG ((EFI_D_INFO, "          PMem: %lx - %lx\n", Bridge->PMem.Base, Bridge->PMem.Limit));
+  DEBUG ((EFI_D_INFO, "   PMemAbove4G: %lx - %lx\n", Bridge->PMemAbove4G.Base, Bridge->PMemAbove4G.Limit));
+
+  //
+  // Make sure Mem and MemAbove4G apertures are valid
+  //
+  if (Bridge->Mem.Base <= Bridge->Mem.Limit) {
+    ASSERT (Bridge->Mem.Limit < SIZE_4GB);
+    if (Bridge->Mem.Limit >= SIZE_4GB) {
+      return NULL;
+    }
+  }
+  if (Bridge->MemAbove4G.Base <= Bridge->MemAbove4G.Limit) {
+    ASSERT (Bridge->MemAbove4G.Base >= SIZE_4GB);
+    if (Bridge->MemAbove4G.Base < SIZE_4GB) {
+      return NULL;
+    }
+  }
+  if (Bridge->PMem.Base <= Bridge->PMem.Limit) {
+    ASSERT (Bridge->PMem.Limit < SIZE_4GB);
+    if (Bridge->PMem.Limit >= SIZE_4GB) {
+      return NULL;
+    }
+  }
+  if (Bridge->PMemAbove4G.Base <= Bridge->PMemAbove4G.Limit) {
+    ASSERT (Bridge->PMemAbove4G.Base >= SIZE_4GB);
+    if (Bridge->PMemAbove4G.Base < SIZE_4GB) {
+      return NULL;
+    }
+  }
+
+  //
+  // Ignore AllocationAttributes when resources were already assigned.
+  //
+  if (!Bridge->ResourceAssigned) {
+    if ((Bridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_COMBINE_MEM_PMEM) != 0) {
+      //
+      // If this bit is set, then the PCI Root Bridge does not
+      // support separate windows for Non-prefetchable and Prefetchable
+      // memory.
+      //
+      ASSERT (Bridge->PMem.Base > Bridge->PMem.Limit);
+      ASSERT (Bridge->PMemAbove4G.Base > Bridge->PMemAbove4G.Limit);
+      if ((Bridge->PMem.Base <= Bridge->PMem.Limit) ||
+          (Bridge->PMemAbove4G.Base <= Bridge->PMemAbove4G.Limit)
+          ) {
+        return NULL;
+      }
+    }
+
+    if ((Bridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_MEM64_DECODE) == 0) {
+      //
+      // If this bit is not set, then the PCI Root Bridge does not support
+      // 64 bit memory windows.
+      //
+      ASSERT (Bridge->MemAbove4G.Base > Bridge->MemAbove4G.Limit);
+      ASSERT (Bridge->PMemAbove4G.Base > Bridge->PMemAbove4G.Limit);
+      if ((Bridge->MemAbove4G.Base <= Bridge->MemAbove4G.Limit) ||
+          (Bridge->PMemAbove4G.Base <= Bridge->PMemAbove4G.Limit)
+          ) {
+        return NULL;
+      }
+    }
+  }
+
+  RootBridge = AllocateZeroPool (sizeof (PCI_ROOT_BRIDGE_INSTANCE));
+  ASSERT (RootBridge != NULL);
+
+  RootBridge->Signature = PCI_ROOT_BRIDGE_SIGNATURE;
+  RootBridge->Supports = Bridge->Supports;
+  RootBridge->Attributes = Bridge->Attributes;
+  RootBridge->DmaAbove4G = Bridge->DmaAbove4G;
+  RootBridge->NoExtendedConfigSpace = Bridge->NoExtendedConfigSpace;
+  RootBridge->AllocationAttributes = Bridge->AllocationAttributes;
+  RootBridge->DevicePath = DuplicateDevicePath (Bridge->DevicePath);
+  RootBridge->DevicePathStr = DevicePathStr;
+  RootBridge->ConfigBuffer = AllocatePool (
+    TypeMax * sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR)
+    );
+  ASSERT (RootBridge->ConfigBuffer != NULL);
+  InitializeListHead (&RootBridge->Maps);
+
+  CopyMem (&RootBridge->Bus, &Bridge->Bus, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->Io, &Bridge->Io, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->Mem, &Bridge->Mem, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->MemAbove4G, &Bridge->MemAbove4G, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->PMem, &Bridge->PMem, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->PMemAbove4G, &Bridge->PMemAbove4G, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+
+  for (Index = TypeIo; Index < TypeMax; Index++) {
+    switch (Index) {
+    case TypeBus:
+      Aperture = &RootBridge->Bus;
+      break;
+    case TypeIo:
+      Aperture = &RootBridge->Io;
+      break;
+    case TypeMem32:
+      Aperture = &RootBridge->Mem;
+      break;
+    case TypeMem64:
+      Aperture = &RootBridge->MemAbove4G;
+      break;
+    case TypePMem32:
+      Aperture = &RootBridge->PMem;
+      break;
+    case TypePMem64:
+      Aperture = &RootBridge->PMemAbove4G;
+      break;
+    default:
+      ASSERT (FALSE);
+      Aperture = NULL;
+      break;
+    }
+    RootBridge->ResAllocNode[Index].Type     = Index;
+    if (Bridge->ResourceAssigned && (Aperture->Limit >= Aperture->Base)) {
+      RootBridge->ResAllocNode[Index].Base   = Aperture->Base;
+      RootBridge->ResAllocNode[Index].Length = Aperture->Limit - Aperture->Base + 1;
+      RootBridge->ResAllocNode[Index].Status = ResAllocated;
+    } else {
+      RootBridge->ResAllocNode[Index].Base   = 0;
+      RootBridge->ResAllocNode[Index].Length = 0;
+      RootBridge->ResAllocNode[Index].Status = ResNone;
+    }
+  }
+
+  RootBridge->RootBridgeIo.SegmentNumber  = Bridge->Segment;
+  RootBridge->RootBridgeIo.PollMem        = RootBridgeIoPollMem;
+  RootBridge->RootBridgeIo.PollIo         = RootBridgeIoPollIo;
+  RootBridge->RootBridgeIo.Mem.Read       = RootBridgeIoMemRead;
+  RootBridge->RootBridgeIo.Mem.Write      = RootBridgeIoMemWrite;
+  RootBridge->RootBridgeIo.Io.Read        = RootBridgeIoIoRead;
+  RootBridge->RootBridgeIo.Io.Write       = RootBridgeIoIoWrite;
+  RootBridge->RootBridgeIo.CopyMem        = RootBridgeIoCopyMem;
+  RootBridge->RootBridgeIo.Pci.Read       = RootBridgeIoPciRead;
+  RootBridge->RootBridgeIo.Pci.Write      = RootBridgeIoPciWrite;
+  RootBridge->RootBridgeIo.Map            = RootBridgeIoMap;
+  RootBridge->RootBridgeIo.Unmap          = RootBridgeIoUnmap;
+  RootBridge->RootBridgeIo.AllocateBuffer = RootBridgeIoAllocateBuffer;
+  RootBridge->RootBridgeIo.FreeBuffer     = RootBridgeIoFreeBuffer;
+  RootBridge->RootBridgeIo.Flush          = RootBridgeIoFlush;
+  RootBridge->RootBridgeIo.GetAttributes  = RootBridgeIoGetAttributes;
+  RootBridge->RootBridgeIo.SetAttributes  = RootBridgeIoSetAttributes;
+  RootBridge->RootBridgeIo.Configuration  = RootBridgeIoConfiguration;
+
+  return RootBridge;
+}
+
+/**
+  Check parameters for IO,MMIO,PCI read/write services of PCI Root Bridge IO.
+
+  The I/O operations are carried out exactly as requested. The caller is
+  responsible for satisfying any alignment and I/O width restrictions that a PI
+  System on a platform might require. For example on some platforms, width
+  requests of EfiCpuIoWidthUint64 do not work. Misaligned buffers, on the other
+  hand, will be handled by the driver.
+
+  @param[in] This           A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+
+  @param[in] OperationType  I/O operation type: IO/MMIO/PCI.
+
+  @param[in] Width          Signifies the width of the I/O or Memory operation.
+
+  @param[in] Address        The base address of the I/O operation.
+
+  @param[in] Count          The number of I/O operations to perform. The number
+                            of bytes moved is Width size * Count, starting at
+                            Address.
+
+  @param[in] Buffer         For read operations, the destination buffer to
+                            store the results. For write operations, the source
+                            buffer from which to write data.
+
+  @retval EFI_SUCCESS            The parameters for this request pass the
+                                 checks.
+
+  @retval EFI_INVALID_PARAMETER  Width is invalid for this PI system.
+
+  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
+
+  @retval EFI_UNSUPPORTED        The Buffer is not aligned for the given Width.
+
+  @retval EFI_UNSUPPORTED        The address range specified by Address, Width,
+                                 and Count is not valid for this PI system.
+**/
+EFI_STATUS
+RootBridgeIoCheckParameter (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN OPERATION_TYPE                         OperationType,
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN UINT64                                 Address,
+  IN UINTN                                  Count,
+  IN VOID                                   *Buffer
+  )
+{
+  PCI_ROOT_BRIDGE_INSTANCE                     *RootBridge;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS  *PciRbAddr;
+  UINT64                                       Base;
+  UINT64                                       Limit;
+  UINT32                                       Size;
+
+  //
+  // Check to see if Buffer is NULL
+  //
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check to see if Width is in the valid range
+  //
+  if ((UINT32) Width >= EfiPciWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // For FIFO type, the target address won't increase during the access,
+  // so treat Count as 1
+  //
+  if (Width >= EfiPciWidthFifoUint8 && Width <= EfiPciWidthFifoUint64) {
+    Count = 1;
+  }
+
+  Width = (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) (Width & 0x03);
+  Size  = 1 << Width;
+
+  //
+  // Check to see if Address is aligned
+  //
+  if ((Address & (Size - 1)) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+
+  //
+  // Check to see if any address associated with this transfer exceeds the
+  // maximum allowed address.  The maximum address implied by the parameters
+  // passed in is Address + Size * Count.  If the following condition is met,
+  // then the transfer is not supported.
+  //
+  //    Address + Size * Count > Limit + 1
+  //
+  // Since Limit can be the maximum integer value supported by the CPU and
+  // Count can also be the maximum integer value supported by the CPU, this
+  // range check must be adjusted to avoid all oveflow conditions.
+  //
+  if (OperationType == IoOperation) {
+    //
+    // Allow Legacy IO access
+    //
+    if (Address + MultU64x32 (Count, Size) <= 0x1000) {
+      if ((RootBridge->Attributes & (
+           EFI_PCI_ATTRIBUTE_ISA_IO | EFI_PCI_ATTRIBUTE_VGA_PALETTE_IO | EFI_PCI_ATTRIBUTE_VGA_IO |
+           EFI_PCI_ATTRIBUTE_IDE_PRIMARY_IO | EFI_PCI_ATTRIBUTE_IDE_SECONDARY_IO |
+           EFI_PCI_ATTRIBUTE_ISA_IO_16 | EFI_PCI_ATTRIBUTE_VGA_PALETTE_IO_16 | EFI_PCI_ATTRIBUTE_VGA_IO_16)) != 0) {
+        return EFI_SUCCESS;
+      }
+    }
+    Base = RootBridge->Io.Base;
+    Limit = RootBridge->Io.Limit;
+  } else if (OperationType == MemOperation) {
+    //
+    // Allow Legacy MMIO access
+    //
+    if ((Address >= 0xA0000) && (Address + MultU64x32 (Count, Size)) <= 0xC0000) {
+      if ((RootBridge->Attributes & EFI_PCI_ATTRIBUTE_VGA_MEMORY) != 0) {
+        return EFI_SUCCESS;
+      }
+    }
+    //
+    // By comparing the Address against Limit we know which range to be used
+    // for checking
+    //
+    if (Address + MultU64x32 (Count, Size) <= RootBridge->Mem.Limit + 1) {
+      Base = RootBridge->Mem.Base;
+      Limit = RootBridge->Mem.Limit;
+    } else {
+      Base = RootBridge->MemAbove4G.Base;
+      Limit = RootBridge->MemAbove4G.Limit;
+    }
+  } else {
+    PciRbAddr = (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS*) &Address;
+    if (PciRbAddr->Bus < RootBridge->Bus.Base ||
+        PciRbAddr->Bus > RootBridge->Bus.Limit) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (PciRbAddr->Device > PCI_MAX_DEVICE ||
+        PciRbAddr->Function > PCI_MAX_FUNC) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (PciRbAddr->ExtendedRegister != 0) {
+      Address = PciRbAddr->ExtendedRegister;
+    } else {
+      Address = PciRbAddr->Register;
+    }
+    Base = 0;
+    Limit = RootBridge->NoExtendedConfigSpace ? 0xFF : 0xFFF;
+  }
+
+  if (Address < Base) {
+      return EFI_INVALID_PARAMETER;
+  }
+
+  if (Address + MultU64x32 (Count, Size) > Limit + 1) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Polls an address in memory mapped I/O space until an exit condition is met,
+  or a timeout occurs.
+
+  This function provides a standard way to poll a PCI memory location. A PCI
+  memory read operation is performed at the PCI memory address specified by
+  Address for the width specified by Width. The result of this PCI memory read
+  operation is stored in Result. This PCI memory read operation is repeated
+  until either a timeout of Delay 100 ns units has expired, or (Result & Mask)
+  is equal to Value.
+
+  @param[in]   This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width     Signifies the width of the memory operations.
+  @param[in]   Address   The base address of the memory operations. The caller
+                         is responsible for aligning Address if required.
+  @param[in]   Mask      Mask used for the polling criteria. Bytes above Width
+                         in Mask are ignored. The bits in the bytes below Width
+                         which are zero in Mask are ignored when polling the
+                         memory address.
+  @param[in]   Value     The comparison value used for the polling exit
+                         criteria.
+  @param[in]   Delay     The number of 100 ns units to poll. Note that timer
+                         available may be of poorer granularity.
+  @param[out]  Result    Pointer to the last value read from the memory
+                         location.
+
+  @retval EFI_SUCCESS            The last data returned from the access matched
+                                 the poll exit criteria.
+  @retval EFI_INVALID_PARAMETER  Width is invalid.
+  @retval EFI_INVALID_PARAMETER  Result is NULL.
+  @retval EFI_TIMEOUT            Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a
+                                 lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPollMem (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT64                                 Address,
+  IN  UINT64                                 Mask,
+  IN  UINT64                                 Value,
+  IN  UINT64                                 Delay,
+  OUT UINT64                                 *Result
+  )
+{
+  EFI_STATUS  Status;
+  UINT64      NumberOfTicks;
+  UINT32      Remainder;
+
+  if (Result == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((UINT32)Width > EfiPciWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // No matter what, always do a single poll.
+  //
+  Status = This->Mem.Read (This, Width, Address, 1, Result);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((*Result & Mask) == Value) {
+    return EFI_SUCCESS;
+  }
+
+  if (Delay == 0) {
+    return EFI_SUCCESS;
+
+  } else {
+
+    //
+    // Determine the proper # of metronome ticks to wait for polling the
+    // location.  The nuber of ticks is Roundup (Delay /
+    // mMetronome->TickPeriod)+1
+    // The "+1" to account for the possibility of the first tick being short
+    // because we started in the middle of a tick.
+    //
+    // BugBug: overriding mMetronome->TickPeriod with UINT32 until Metronome
+    // protocol definition is updated.
+    //
+    NumberOfTicks = DivU64x32Remainder (Delay, (UINT32) mMetronome->TickPeriod,
+                      &Remainder);
+    if (Remainder != 0) {
+      NumberOfTicks += 1;
+    }
+    NumberOfTicks += 1;
+
+    while (NumberOfTicks != 0) {
+
+      mMetronome->WaitForTick (mMetronome, 1);
+
+      Status = This->Mem.Read (This, Width, Address, 1, Result);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      if ((*Result & Mask) == Value) {
+        return EFI_SUCCESS;
+      }
+
+      NumberOfTicks -= 1;
+    }
+  }
+  return EFI_TIMEOUT;
+}
+
+/**
+  Reads from the I/O space of a PCI Root Bridge. Returns when either the
+  polling exit criteria is satisfied or after a defined duration.
+
+  This function provides a standard way to poll a PCI I/O location. A PCI I/O
+  read operation is performed at the PCI I/O address specified by Address for
+  the width specified by Width.
+  The result of this PCI I/O read operation is stored in Result. This PCI I/O
+  read operation is repeated until either a timeout of Delay 100 ns units has
+  expired, or (Result & Mask) is equal to Value.
+
+  @param[in] This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in] Width     Signifies the width of the I/O operations.
+  @param[in] Address   The base address of the I/O operations. The caller is
+                       responsible for aligning Address if required.
+  @param[in] Mask      Mask used for the polling criteria. Bytes above Width in
+                       Mask are ignored. The bits in the bytes below Width
+                       which are zero in Mask are ignored when polling the I/O
+                       address.
+  @param[in] Value     The comparison value used for the polling exit criteria.
+  @param[in] Delay     The number of 100 ns units to poll. Note that timer
+                       available may be of poorer granularity.
+  @param[out] Result   Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS            The last data returned from the access matched
+                                 the poll exit criteria.
+  @retval EFI_INVALID_PARAMETER  Width is invalid.
+  @retval EFI_INVALID_PARAMETER  Result is NULL.
+  @retval EFI_TIMEOUT            Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a
+                                 lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPollIo (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT64                                 Address,
+  IN  UINT64                                 Mask,
+  IN  UINT64                                 Value,
+  IN  UINT64                                 Delay,
+  OUT UINT64                                 *Result
+  )
+{
+  EFI_STATUS  Status;
+  UINT64      NumberOfTicks;
+  UINT32      Remainder;
+
+  //
+  // No matter what, always do a single poll.
+  //
+
+  if (Result == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((UINT32)Width > EfiPciWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = This->Io.Read (This, Width, Address, 1, Result);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  if ((*Result & Mask) == Value) {
+    return EFI_SUCCESS;
+  }
+
+  if (Delay == 0) {
+    return EFI_SUCCESS;
+
+  } else {
+
+    //
+    // Determine the proper # of metronome ticks to wait for polling the
+    // location.  The number of ticks is Roundup (Delay /
+    // mMetronome->TickPeriod)+1
+    // The "+1" to account for the possibility of the first tick being short
+    // because we started in the middle of a tick.
+    //
+    NumberOfTicks = DivU64x32Remainder (Delay, (UINT32)mMetronome->TickPeriod,
+                      &Remainder);
+    if (Remainder != 0) {
+      NumberOfTicks += 1;
+    }
+    NumberOfTicks += 1;
+
+    while (NumberOfTicks != 0) {
+
+      mMetronome->WaitForTick (mMetronome, 1);
+
+      Status = This->Io.Read (This, Width, Address, 1, Result);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      if ((*Result & Mask) == Value) {
+        return EFI_SUCCESS;
+      }
+
+      NumberOfTicks -= 1;
+    }
+  }
+  return EFI_TIMEOUT;
+}
+
+/**
+  Enables a PCI driver to access PCI controller registers in the PCI root
+  bridge memory space.
+
+  The Mem.Read(), and Mem.Write() functions enable a driver to access PCI
+  controller registers in the PCI root bridge memory space.
+  The memory operations are carried out exactly as requested. The caller is
+  responsible for satisfying any alignment and memory width restrictions that a
+  PCI Root Bridge on a platform might require.
+
+  @param[in]   This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width     Signifies the width of the memory operation.
+  @param[in]   Address   The base address of the memory operation. The caller
+                         is responsible for aligning the Address if required.
+  @param[in]   Count     The number of memory operations to perform. Bytes
+                         moved is Width size * Count, starting at Address.
+  @param[out]  Buffer    For read operations, the destination buffer to store
+                         the results. For write operations, the source buffer
+                         to write data from.
+
+  @retval EFI_SUCCESS            The data was read from or written to the PCI
+                                 root bridge.
+  @retval EFI_INVALID_PARAMETER  Width is invalid for this PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a
+                                 lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMemRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  OUT    VOID                                   *Buffer
+  )
+{
+  EFI_STATUS                             Status;
+
+  Status = RootBridgeIoCheckParameter (This, MemOperation, Width, Address,
+                                       Count, Buffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return mCpuIo->Mem.Read (mCpuIo, (EFI_CPU_IO_PROTOCOL_WIDTH) Width, Address, Count, Buffer);
+}
+
+/**
+  Enables a PCI driver to access PCI controller registers in the PCI root
+  bridge memory space.
+
+  The Mem.Read(), and Mem.Write() functions enable a driver to access PCI
+  controller registers in the PCI root bridge memory space.
+  The memory operations are carried out exactly as requested. The caller is
+  responsible for satisfying any alignment and memory width restrictions that a
+  PCI Root Bridge on a platform might require.
+
+  @param[in]   This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width     Signifies the width of the memory operation.
+  @param[in]   Address   The base address of the memory operation. The caller
+                         is responsible for aligning the Address if required.
+  @param[in]   Count     The number of memory operations to perform. Bytes
+                         moved is Width size * Count, starting at Address.
+  @param[in]   Buffer    For read operations, the destination buffer to store
+                         the results. For write operations, the source buffer
+                         to write data from.
+
+  @retval EFI_SUCCESS            The data was read from or written to the PCI
+                                 root bridge.
+  @retval EFI_INVALID_PARAMETER  Width is invalid for this PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a
+                                 lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMemWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN     VOID                                   *Buffer
+  )
+{
+  EFI_STATUS                             Status;
+
+  Status = RootBridgeIoCheckParameter (This, MemOperation, Width, Address,
+                                       Count, Buffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return mCpuIo->Mem.Write (mCpuIo, (EFI_CPU_IO_PROTOCOL_WIDTH) Width, Address, Count, Buffer);
+}
+
+/**
+  Enables a PCI driver to access PCI controller registers in the PCI root
+  bridge I/O space.
+
+  @param[in]   This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width       Signifies the width of the memory operations.
+  @param[in]   Address     The base address of the I/O operation. The caller is
+                           responsible for aligning the Address if required.
+  @param[in]   Count       The number of I/O operations to perform. Bytes moved
+                           is Width size * Count, starting at Address.
+  @param[out]  Buffer      For read operations, the destination buffer to store
+                           the results. For write operations, the source buffer
+                           to write data from.
+
+  @retval EFI_SUCCESS              The data was read from or written to the PCI
+                                   root bridge.
+  @retval EFI_INVALID_PARAMETER    Width is invalid for this PCI root bridge.
+  @retval EFI_INVALID_PARAMETER    Buffer is NULL.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a
+                                   lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoIoRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  OUT    VOID                                   *Buffer
+  )
+{
+  EFI_STATUS                                    Status;
+  Status = RootBridgeIoCheckParameter (
+             This, IoOperation, Width,
+             Address, Count, Buffer
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return mCpuIo->Io.Read (mCpuIo, (EFI_CPU_IO_PROTOCOL_WIDTH) Width, Address, Count, Buffer);
+}
+
+/**
+  Enables a PCI driver to access PCI controller registers in the PCI root
+  bridge I/O space.
+
+  @param[in]   This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width       Signifies the width of the memory operations.
+  @param[in]   Address     The base address of the I/O operation. The caller is
+                           responsible for aligning the Address if required.
+  @param[in]   Count       The number of I/O operations to perform. Bytes moved
+                           is Width size * Count, starting at Address.
+  @param[in]   Buffer      For read operations, the destination buffer to store
+                           the results. For write operations, the source buffer
+                           to write data from.
+
+  @retval EFI_SUCCESS              The data was read from or written to the PCI
+                                   root bridge.
+  @retval EFI_INVALID_PARAMETER    Width is invalid for this PCI root bridge.
+  @retval EFI_INVALID_PARAMETER    Buffer is NULL.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a
+                                   lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoIoWrite (
+  IN       EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL         *This,
+  IN       EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH   Width,
+  IN       UINT64                                  Address,
+  IN       UINTN                                   Count,
+  IN       VOID                                    *Buffer
+  )
+{
+  EFI_STATUS                                    Status;
+  Status = RootBridgeIoCheckParameter (
+             This, IoOperation, Width,
+             Address, Count, Buffer
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return mCpuIo->Io.Write (mCpuIo, (EFI_CPU_IO_PROTOCOL_WIDTH) Width, Address, Count, Buffer);
+}
+
+/**
+  Enables a PCI driver to copy one region of PCI root bridge memory space to
+  another region of PCI root bridge memory space.
+
+  The CopyMem() function enables a PCI driver to copy one region of PCI root
+  bridge memory space to another region of PCI root bridge memory space. This
+  is especially useful for video scroll operation on a memory mapped video
+  buffer.
+  The memory operations are carried out exactly as requested. The caller is
+  responsible for satisfying any alignment and memory width restrictions that a
+  PCI root bridge on a platform might require.
+
+  @param[in] This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+                         instance.
+  @param[in] Width       Signifies the width of the memory operations.
+  @param[in] DestAddress The destination address of the memory operation. The
+                         caller is responsible for aligning the DestAddress if
+                         required.
+  @param[in] SrcAddress  The source address of the memory operation. The caller
+                         is responsible for aligning the SrcAddress if
+                         required.
+  @param[in] Count       The number of memory operations to perform. Bytes
+                         moved is Width size * Count, starting at DestAddress
+                         and SrcAddress.
+
+  @retval  EFI_SUCCESS             The data was copied from one memory region
+                                   to another memory region.
+  @retval  EFI_INVALID_PARAMETER   Width is invalid for this PCI root bridge.
+  @retval  EFI_OUT_OF_RESOURCES    The request could not be completed due to a
+                                   lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoCopyMem (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL              *This,
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH        Width,
+  IN UINT64                                       DestAddress,
+  IN UINT64                                       SrcAddress,
+  IN UINTN                                        Count
+  )
+{
+  EFI_STATUS  Status;
+  BOOLEAN     Forward;
+  UINTN       Stride;
+  UINTN       Index;
+  UINT64      Result;
+
+  if ((UINT32) Width > EfiPciWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DestAddress == SrcAddress) {
+    return EFI_SUCCESS;
+  }
+
+  Stride = (UINTN) (1 << Width);
+
+  Forward = TRUE;
+  if ((DestAddress > SrcAddress) &&
+      (DestAddress < (SrcAddress + Count * Stride))) {
+    Forward = FALSE;
+    SrcAddress = SrcAddress + (Count - 1) * Stride;
+    DestAddress = DestAddress + (Count - 1) * Stride;
+  }
+
+  for (Index = 0; Index < Count; Index++) {
+    Status = RootBridgeIoMemRead (
+               This,
+               Width,
+               SrcAddress,
+               1,
+               &Result
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    Status = RootBridgeIoMemWrite (
+               This,
+               Width,
+               DestAddress,
+               1,
+               &Result
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    if (Forward) {
+      SrcAddress += Stride;
+      DestAddress += Stride;
+    } else {
+      SrcAddress -= Stride;
+      DestAddress -= Stride;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  PCI configuration space access.
+
+  @param This     A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Read     TRUE indicating it's a read operation.
+  @param Width    Signifies the width of the memory operation.
+  @param Address  The address within the PCI configuration space
+                  for the PCI controller.
+  @param Count    The number of PCI configuration operations
+                  to perform.
+  @param Buffer   The destination buffer to store the results.
+
+  @retval EFI_SUCCESS            The data was read/written from/to the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  Invalid parameters found.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciAccess (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL       *This,
+  IN     BOOLEAN                               Read,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH Width,
+  IN     UINT64                                Address,
+  IN     UINTN                                 Count,
+  IN OUT VOID                                  *Buffer
+  )
+{
+  EFI_STATUS                                   Status;
+  PCI_ROOT_BRIDGE_INSTANCE                     *RootBridge;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS  PciAddress;
+  UINT8                                        *Uint8Buffer;
+  UINT8                                        InStride;
+  UINT8                                        OutStride;
+  UINTN                                        Size;
+
+  Status = RootBridgeIoCheckParameter (This, PciOperation, Width, Address, Count, Buffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Read Pci configuration space
+  //
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+  CopyMem (&PciAddress, &Address, sizeof (PciAddress));
+
+  if (PciAddress.ExtendedRegister == 0) {
+    PciAddress.ExtendedRegister = PciAddress.Register;
+  }
+
+  Address = PCI_SEGMENT_LIB_ADDRESS (
+              RootBridge->RootBridgeIo.SegmentNumber,
+              PciAddress.Bus,
+              PciAddress.Device,
+              PciAddress.Function,
+              PciAddress.ExtendedRegister
+              );
+
+  //
+  // Select loop based on the width of the transfer
+  //
+  InStride  = mInStride[Width];
+  OutStride = mOutStride[Width];
+  Size      = (UINTN) (1 << (Width & 0x03));
+  for (Uint8Buffer = Buffer; Count > 0; Address += InStride, Uint8Buffer += OutStride, Count--) {
+    if (Read) {
+      PciSegmentReadBuffer (Address, Size, Uint8Buffer);
+    } else {
+      PciSegmentWriteBuffer (Address, Size, Uint8Buffer);
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Allows read from PCI configuration space.
+
+  @param This     A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width    Signifies the width of the memory operation.
+  @param Address  The address within the PCI configuration space
+                  for the PCI controller.
+  @param Count    The number of PCI configuration operations
+                  to perform.
+  @param Buffer   The destination buffer to store the results.
+
+  @retval EFI_SUCCESS           The data was read from the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER Invalid parameters found.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL       *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH Width,
+  IN     UINT64                                Address,
+  IN     UINTN                                 Count,
+  IN OUT VOID                                  *Buffer
+  )
+{
+  return RootBridgeIoPciAccess (This, TRUE, Width, Address, Count, Buffer);
+}
+
+/**
+  Allows write to PCI configuration space.
+
+  @param This     A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width    Signifies the width of the memory operation.
+  @param Address  The address within the PCI configuration space
+                  for the PCI controller.
+  @param Count    The number of PCI configuration operations
+                  to perform.
+  @param Buffer   The source buffer to get the results.
+
+  @retval EFI_SUCCESS            The data was written to the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  Invalid parameters found.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL       *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH Width,
+  IN     UINT64                                Address,
+  IN     UINTN                                 Count,
+  IN OUT VOID                                  *Buffer
+  )
+{
+  return RootBridgeIoPciAccess (This, FALSE, Width, Address, Count, Buffer);
+}
+
+/**
+  Provides the PCI controller-specific address needed to access
+  system memory for DMA.
+
+  @param This           A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Operation      Indicate if the bus master is going to read or write
+                        to system memory.
+  @param HostAddress    The system memory address to map on 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 system memory's HostAddress.
+  @param Mapping        The value to pass to Unmap() when the bus master DMA
+                        operation is complete.
+
+  @retval EFI_SUCCESS            Success.
+  @retval EFI_INVALID_PARAMETER  Invalid parameters found.
+  @retval EFI_UNSUPPORTED        The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_DEVICE_ERROR       The System hardware could not map the requested address.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMap (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL            *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_OPERATION  Operation,
+  IN     VOID                                       *HostAddress,
+  IN OUT UINTN                                      *NumberOfBytes,
+  OUT    EFI_PHYSICAL_ADDRESS                       *DeviceAddress,
+  OUT    VOID                                       **Mapping
+  )
+{
+  EFI_STATUS                                        Status;
+  PCI_ROOT_BRIDGE_INSTANCE                          *RootBridge;
+  EFI_PHYSICAL_ADDRESS                              PhysicalAddress;
+  MAP_INFO                                          *MapInfo;
+
+  if (HostAddress == NULL || NumberOfBytes == NULL || DeviceAddress == NULL ||
+      Mapping == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Make sure that Operation is valid
+  //
+  if ((UINT32) Operation >= EfiPciOperationMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+
+  PhysicalAddress = (EFI_PHYSICAL_ADDRESS) (UINTN) HostAddress;
+  if (!RootBridge->DmaAbove4G && ((PhysicalAddress + *NumberOfBytes) > SIZE_4GB)) {
+    //
+    // If the root bridge can not handle performing DMA above 4GB but
+    // any part of the DMA transfer being mapped is above 4GB, then
+    // map the DMA transfer to a buffer below 4GB.
+    //
+
+    if (Operation == EfiPciOperationBusMasterCommonBuffer ||
+        Operation == EfiPciOperationBusMasterCommonBuffer64) {
+      //
+      // Common Buffer operations can not be remapped.  If the common buffer
+      // if above 4GB, then it is not possible to generate a mapping, so return
+      // an error.
+      //
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // Allocate a MAP_INFO structure to remember the mapping when Unmap() is
+    // called later.
+    //
+    MapInfo = AllocatePool (sizeof (MAP_INFO));
+    if (MapInfo == NULL) {
+      *NumberOfBytes = 0;
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Initialize the MAP_INFO structure
+    //
+    MapInfo->Signature         = MAP_INFO_SIGNATURE;
+    MapInfo->Operation         = Operation;
+    MapInfo->NumberOfBytes     = *NumberOfBytes;
+    MapInfo->NumberOfPages     = EFI_SIZE_TO_PAGES (MapInfo->NumberOfBytes);
+    MapInfo->HostAddress       = PhysicalAddress;
+    MapInfo->MappedHostAddress = SIZE_4GB - 1;
+
+    //
+    // Allocate a buffer below 4GB to map the transfer to.
+    //
+    Status = gBS->AllocatePages (
+                    AllocateMaxAddress,
+                    EfiBootServicesData,
+                    MapInfo->NumberOfPages,
+                    &MapInfo->MappedHostAddress
+                    );
+    if (EFI_ERROR (Status)) {
+      FreePool (MapInfo);
+      *NumberOfBytes = 0;
+      return Status;
+    }
+
+    //
+    // If this is a read operation from the Bus Master's point of view,
+    // then copy the contents of the real buffer into the mapped buffer
+    // so the Bus Master can read the contents of the real buffer.
+    //
+    if (Operation == EfiPciOperationBusMasterRead ||
+        Operation == EfiPciOperationBusMasterRead64) {
+      CopyMem (
+        (VOID *) (UINTN) MapInfo->MappedHostAddress,
+        (VOID *) (UINTN) MapInfo->HostAddress,
+        MapInfo->NumberOfBytes
+        );
+    }
+
+    InsertTailList (&RootBridge->Maps, &MapInfo->Link);
+
+    //
+    // The DeviceAddress is the address of the maped buffer below 4GB
+    //
+    *DeviceAddress = MapInfo->MappedHostAddress;
+    //
+    // Return a pointer to the MAP_INFO structure in Mapping
+    //
+    *Mapping       = MapInfo;
+  } else {
+    //
+    // If the root bridge CAN handle performing DMA above 4GB or
+    // the transfer is below 4GB, so the DeviceAddress is simply the
+    // HostAddress
+    //
+    *DeviceAddress = PhysicalAddress;
+    *Mapping       = NO_MAPPING;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  The Unmap() function completes the Map() operation and releases any
+  corresponding resources.
+  If the operation was an EfiPciOperationBusMasterWrite or
+  EfiPciOperationBusMasterWrite64, the data is committed to the target system
+  memory.
+  Any resources used for the mapping are freed.
+
+  @param[in] This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in] Mapping   The mapping value returned from Map().
+
+  @retval EFI_SUCCESS            The range was unmapped.
+  @retval EFI_INVALID_PARAMETER  Mapping is not a value that was returned by Map().
+  @retval EFI_DEVICE_ERROR       The data was not committed to the target system memory.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoUnmap (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN VOID                             *Mapping
+  )
+{
+  MAP_INFO                 *MapInfo;
+  LIST_ENTRY               *Link;
+  PCI_ROOT_BRIDGE_INSTANCE *RootBridge;
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+  //
+  // See if the Map() operation associated with this Unmap() required a mapping
+  // buffer. If a mapping buffer was not required, then this function simply
+  // returns EFI_SUCCESS.
+  //
+  if (Mapping == NO_MAPPING) {
+    return EFI_SUCCESS;
+  }
+
+  MapInfo = NO_MAPPING;
+  for (Link = GetFirstNode (&RootBridge->Maps)
+       ; !IsNull (&RootBridge->Maps, Link)
+       ; Link = GetNextNode (&RootBridge->Maps, Link)
+       ) {
+    MapInfo = MAP_INFO_FROM_LINK (Link);
+    if (MapInfo == Mapping) {
+      break;
+    }
+  }
+  //
+  // Mapping is not a valid value returned by Map()
+  //
+  if (MapInfo != Mapping) {
+    return EFI_INVALID_PARAMETER;
+  }
+  RemoveEntryList (&MapInfo->Link);
+
+  //
+  // If this is a write operation from the Bus Master's point of view,
+  // then copy the contents of the mapped buffer into the real buffer
+  // so the processor can read the contents of the real buffer.
+  //
+  if (MapInfo->Operation == EfiPciOperationBusMasterWrite ||
+      MapInfo->Operation == EfiPciOperationBusMasterWrite64) {
+    CopyMem (
+      (VOID *) (UINTN) MapInfo->HostAddress,
+      (VOID *) (UINTN) MapInfo->MappedHostAddress,
+      MapInfo->NumberOfBytes
+      );
+  }
+
+  //
+  // Free the mapped buffer and the MAP_INFO structure.
+  //
+  gBS->FreePages (MapInfo->MappedHostAddress, MapInfo->NumberOfPages);
+  FreePool (Mapping);
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocates pages that are suitable for an EfiPciOperationBusMasterCommonBuffer
+  or EfiPciOperationBusMasterCommonBuffer64 mapping.
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Type        This parameter is not used and must be ignored.
+  @param MemoryType  The type of memory to allocate, EfiBootServicesData or
+                     EfiRuntimeServicesData.
+  @param Pages       The number of pages to allocate.
+  @param HostAddress A pointer to store the base system memory address of the
+                     allocated range.
+  @param Attributes  The requested bit mask of attributes for the allocated
+                     range. Only the attributes
+                     EFI_PCI_ATTRIBUTE_MEMORY_WRITE_COMBINE,
+                     EFI_PCI_ATTRIBUTE_MEMORY_CACHED, and
+                     EFI_PCI_ATTRIBUTE_DUAL_ADDRESS_CYCLE may be used with this
+                     function.
+
+  @retval EFI_SUCCESS            The requested memory pages were allocated.
+  @retval EFI_INVALID_PARAMETER  MemoryType is invalid.
+  @retval EFI_INVALID_PARAMETER  HostAddress is NULL.
+  @retval EFI_UNSUPPORTED        Attributes is unsupported. The only legal
+                                 attribute bits are MEMORY_WRITE_COMBINE,
+                                 MEMORY_CACHED, and DUAL_ADDRESS_CYCLE.
+  @retval EFI_OUT_OF_RESOURCES   The memory pages could not be allocated.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoAllocateBuffer (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  EFI_ALLOCATE_TYPE                Type,
+  IN  EFI_MEMORY_TYPE                  MemoryType,
+  IN  UINTN                            Pages,
+  OUT VOID                             **HostAddress,
+  IN  UINT64                           Attributes
+  )
+{
+  EFI_STATUS                Status;
+  EFI_PHYSICAL_ADDRESS      PhysicalAddress;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+  EFI_ALLOCATE_TYPE         AllocateType;
+
+  //
+  // Validate Attributes
+  //
+  if ((Attributes & EFI_PCI_ATTRIBUTE_INVALID_FOR_ALLOCATE_BUFFER) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check for invalid inputs
+  //
+  if (HostAddress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // The only valid memory types are EfiBootServicesData and
+  // EfiRuntimeServicesData
+  //
+  if (MemoryType != EfiBootServicesData &&
+      MemoryType != EfiRuntimeServicesData) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+
+  AllocateType = AllocateAnyPages;
+  if (!RootBridge->DmaAbove4G) {
+    //
+    // Limit allocations to memory below 4GB
+    //
+    AllocateType    = AllocateMaxAddress;
+    PhysicalAddress = (EFI_PHYSICAL_ADDRESS) (SIZE_4GB - 1);
+  }
+  Status = gBS->AllocatePages (
+                  AllocateType,
+                  MemoryType,
+                  Pages,
+                  &PhysicalAddress
+                  );
+  if (!EFI_ERROR (Status)) {
+    *HostAddress = (VOID *) (UINTN) PhysicalAddress;
+  }
+
+  return Status;
+}
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  The FreeBuffer() function frees memory that was allocated with
+  AllocateBuffer().
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Pages       The number of pages to free.
+  @param HostAddress The base system memory address of the allocated range.
+
+  @retval EFI_SUCCESS            The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER  The memory range specified by HostAddress and
+                                 Pages was not allocated with AllocateBuffer().
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoFreeBuffer (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  UINTN                            Pages,
+  OUT VOID                             *HostAddress
+  )
+{
+  return gBS->FreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) HostAddress, Pages);
+}
+
+/**
+  Flushes all PCI posted write transactions from a PCI host bridge to system
+  memory.
+
+  The Flush() function flushes any PCI posted write transactions from a PCI
+  host bridge to system memory. Posted write transactions are generated by PCI
+  bus masters when they perform write transactions to target addresses in
+  system memory.
+  This function does not flush posted write transactions from any PCI bridges.
+  A PCI controller specific action must be taken to guarantee that the posted
+  write transactions have been flushed from the PCI controller and from all the
+  PCI bridges into the PCI host bridge. This is typically done with a PCI read
+  transaction from the PCI controller prior to calling Flush().
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+
+  @retval EFI_SUCCESS        The PCI posted write transactions were flushed
+                             from the PCI host bridge to system memory.
+  @retval EFI_DEVICE_ERROR   The PCI posted write transactions were not flushed
+                             from the PCI host bridge due to a hardware error.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoFlush (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL           *This
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets the attributes that a PCI root bridge supports setting with
+  SetAttributes(), and the attributes that a PCI root bridge is currently
+  using.
+
+  The GetAttributes() function returns the mask of attributes that this PCI
+  root bridge supports and the mask of attributes that the PCI root bridge is
+  currently using.
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Supported   A pointer to the mask of attributes that this PCI root
+                     bridge supports setting with SetAttributes().
+  @param Attributes  A pointer to the mask of attributes that this PCI root
+                     bridge is currently using.
+
+  @retval  EFI_SUCCESS           If Supports is not NULL, then the attributes
+                                 that the PCI root bridge supports is returned
+                                 in Supports. If Attributes is not NULL, then
+                                 the attributes that the PCI root bridge is
+                                 currently using is returned in Attributes.
+  @retval  EFI_INVALID_PARAMETER Both Supports and Attributes are NULL.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoGetAttributes (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  OUT UINT64                           *Supported,
+  OUT UINT64                           *Attributes
+  )
+{
+  PCI_ROOT_BRIDGE_INSTANCE *RootBridge;
+
+  if (Attributes == NULL && Supported == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+  //
+  // Set the return value for Supported and Attributes
+  //
+  if (Supported != NULL) {
+    *Supported  = RootBridge->Supports;
+  }
+
+  if (Attributes != NULL) {
+    *Attributes = RootBridge->Attributes;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets attributes for a resource range on a PCI root bridge.
+
+  The SetAttributes() function sets the attributes specified in Attributes for
+  the PCI root bridge on the resource range specified by ResourceBase and
+  ResourceLength. Since the granularity of setting these attributes may vary
+  from resource type to resource type, and from platform to platform, the
+  actual resource range and the one passed in by the caller may differ. As a
+  result, this function may set the attributes specified by Attributes on a
+  larger resource range than the caller requested. The actual range is returned
+  in ResourceBase and ResourceLength. The caller is responsible for verifying
+  that the actual range for which the attributes were set is acceptable.
+
+  @param This            A pointer to the
+                         EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Attributes      The mask of attributes to set. If the
+                         attribute bit MEMORY_WRITE_COMBINE,
+                         MEMORY_CACHED, or MEMORY_DISABLE is set,
+                         then the resource range is specified by
+                         ResourceBase and ResourceLength. If
+                         MEMORY_WRITE_COMBINE, MEMORY_CACHED, and
+                         MEMORY_DISABLE are not set, then
+                         ResourceBase and ResourceLength are ignored,
+                         and may be NULL.
+  @param ResourceBase    A pointer to the base address of the
+                         resource range to be modified by the
+                         attributes specified by Attributes.
+  @param ResourceLength  A pointer to the length of the resource
+                                   range to be modified by the attributes
+                                   specified by Attributes.
+
+  @retval  EFI_SUCCESS           The current configuration of this PCI root bridge
+                                 was returned in Resources.
+  @retval  EFI_UNSUPPORTED       The current configuration of this PCI root bridge
+                                 could not be retrieved.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoSetAttributes (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN     UINT64                           Attributes,
+  IN OUT UINT64                           *ResourceBase,
+  IN OUT UINT64                           *ResourceLength
+  )
+{
+  PCI_ROOT_BRIDGE_INSTANCE            *RootBridge;
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+
+  if ((Attributes & (~RootBridge->Supports)) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  RootBridge->Attributes = Attributes;
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the current resource settings of this PCI root bridge in the form
+  of a set of ACPI 2.0 resource descriptors.
+
+  There are only two resource descriptor types from the ACPI Specification that
+  may be used to describe the current resources allocated to a PCI root bridge.
+  These are the QWORD Address Space Descriptor (ACPI 2.0 Section 6.4.3.5.1),
+  and the End Tag (ACPI 2.0 Section 6.4.2.8). The QWORD Address Space
+  Descriptor can describe memory, I/O, and bus number ranges for dynamic or
+  fixed resources. The configuration of a PCI root bridge is described with one
+  or more QWORD Address Space Descriptors followed by an End Tag.
+
+  @param[in]   This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[out]  Resources   A pointer to the ACPI 2.0 resource descriptors that
+                           describe the current configuration of this PCI root
+                           bridge. The storage for the ACPI 2.0 resource
+                           descriptors is allocated by this function. The
+                           caller must treat the return buffer as read-only
+                           data, and the buffer must not be freed by the
+                           caller.
+
+  @retval  EFI_SUCCESS     The current configuration of this PCI root bridge
+                           was returned in Resources.
+  @retval  EFI_UNSUPPORTED The current configuration of this PCI root bridge
+                           could not be retrieved.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoConfiguration (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL     *This,
+  OUT VOID                                **Resources
+  )
+{
+  PCI_RESOURCE_TYPE                 Index;
+  PCI_ROOT_BRIDGE_INSTANCE          *RootBridge;
+  PCI_RES_NODE                      *ResAllocNode;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+
+  //
+  // Get this instance of the Root Bridge.
+  //
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+  ZeroMem (
+    RootBridge->ConfigBuffer,
+    TypeMax * sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR)
+    );
+  Descriptor = RootBridge->ConfigBuffer;
+  for (Index = TypeIo; Index < TypeMax; Index++) {
+
+    ResAllocNode = &RootBridge->ResAllocNode[Index];
+
+    if (ResAllocNode->Status != ResAllocated) {
+      continue;
+    }
+
+    Descriptor->Desc = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+    Descriptor->Len  = sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3;
+    Descriptor->AddrRangeMin  = ResAllocNode->Base;
+    Descriptor->AddrRangeMax  = ResAllocNode->Base + ResAllocNode->Length - 1;
+    Descriptor->AddrLen       = ResAllocNode->Length;
+    switch (ResAllocNode->Type) {
+
+    case TypeIo:
+      Descriptor->ResType       = ACPI_ADDRESS_SPACE_TYPE_IO;
+      break;
+
+    case TypePMem32:
+      Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+    case TypeMem32:
+      Descriptor->ResType               = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      Descriptor->AddrSpaceGranularity  = 32;
+      break;
+
+    case TypePMem64:
+      Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+    case TypeMem64:
+      Descriptor->ResType               = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      Descriptor->AddrSpaceGranularity  = 64;
+      break;
+
+    case TypeBus:
+      Descriptor->ResType       = ACPI_ADDRESS_SPACE_TYPE_BUS;
+      break;
+
+    default:
+      break;
+    }
+
+    Descriptor++;
+  }
+  //
+  // Terminate the entries.
+  //
+  End = (EFI_ACPI_END_TAG_DESCRIPTOR *) Descriptor;
+  End->Desc     = ACPI_END_TAG_DESCRIPTOR;
+  End->Checksum = 0x0;
+
+  *Resources = RootBridge->ConfigBuffer;
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/ComponentName.c
new file mode 100644
index 0000000000..994dc847c9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/ComponentName.c
@@ -0,0 +1,288 @@
+/** @file
+  UEFI Component Name and Name2 protocol for Isa serial driver.
+
+Copyright (c) 2006 - 2015, 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 "Serial.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gPciSioSerialComponentName = {
+  SerialComponentNameGetDriverName,
+  SerialComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gPciSioSerialComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) SerialComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) SerialComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mSerialDriverNameTable[] = {
+  {
+    "eng;en",
+    L"PCI SIO Serial Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR16 mSioSerialPortName[] = L"SIO Serial Port #%d";
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR16 mPciSerialPortName[] = L"PCI Serial Port #%d";
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mSerialDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gPciSioSerialComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                Status;
+  EFI_SERIAL_IO_PROTOCOL    *SerialIo;
+  SERIAL_DEV                *SerialDevice;
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+  EFI_GUID                  *IoProtocolGuid;
+
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  IoProtocolGuid = &gEfiSioProtocolGuid;
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gSerialControllerDriver.DriverBindingHandle,
+             IoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    IoProtocolGuid = &gEfiPciIoProtocolGuid;
+    Status = EfiTestManagedDevice (
+               ControllerHandle,
+               gSerialControllerDriver.DriverBindingHandle,
+               IoProtocolGuid
+               );
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ControllerNameTable = NULL;
+  if (ChildHandle != NULL) {
+    Status = EfiTestChildHandle (
+               ControllerHandle,
+               ChildHandle,
+               IoProtocolGuid
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the Serial I/O Protocol from the child handle
+    //
+    Status = gBS->OpenProtocol (
+                    ChildHandle,
+                    &gEfiSerialIoProtocolGuid,
+                    (VOID **) &SerialIo,
+                    gSerialControllerDriver.DriverBindingHandle,
+                    ChildHandle,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the Serial Controller's Device structure
+    //
+    SerialDevice = SERIAL_DEV_FROM_THIS (SerialIo);
+    ControllerNameTable = SerialDevice->ControllerNameTable;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gPciSioSerialComponentName)
+           );
+}
+
+/**
+  Add the ISO639-2 and RFC4646 component name both for the Serial IO device
+
+  @param SerialDevice     A pointer to the SERIAL_DEV instance.
+  @param Instance         Instance ID for the serial device.
+**/
+VOID
+AddName (
+  IN  SERIAL_DEV                               *SerialDevice,
+  IN  UINT32                                   Instance
+  )
+{
+  CHAR16                                       SerialPortName[SERIAL_PORT_NAME_LEN];
+  UnicodeSPrint (
+    SerialPortName,
+    sizeof (SerialPortName),
+    (SerialDevice->PciDeviceInfo != NULL) ? PCI_SERIAL_PORT_NAME : SIO_SERIAL_PORT_NAME,
+    Instance
+    );
+  AddUnicodeString2 (
+    "eng",
+    gPciSioSerialComponentName.SupportedLanguages,
+    &SerialDevice->ControllerNameTable,
+    SerialPortName,
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gPciSioSerialComponentName2.SupportedLanguages,
+    &SerialDevice->ControllerNameTable,
+    SerialPortName,
+    FALSE
+    );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.inf b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.inf
new file mode 100644
index 0000000000..03fddfe75e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.inf
@@ -0,0 +1,81 @@
+## @file
+# Serial driver for standard UARTS on a SIO chip or PCI/PCIE card.
+#
+# Produces the Serial I/O protocol for standard UARTS using Super I/O or PCI I/O.
+#
+# Copyright (c) 2007 - 2015, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions of the BSD License
+# which accompanies this distribution. The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PciSioSerialDxe
+  MODULE_UNI_FILE                = PciSioSerialDxe.uni
+  FILE_GUID                      = E2775B47-D453-4EE3-ADA7-391A1B05AC17
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializePciSioSerial
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gSerialControllerDriver
+#  COMPONENT_NAME                =  gPciSioSerialComponentName
+#  COMPONENT_NAME2               =  gPciSioSerialComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  SerialIo.c
+  Serial.h
+  Serial.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  ReportStatusCodeLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  DevicePathLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  IoLib
+
+[Guids]
+  gEfiUartDevicePathGuid                        ## SOMETIMES_CONSUMES   ## GUID
+
+[Protocols]
+  gEfiSioProtocolGuid                           ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiPciIoProtocolGuid                         ## TO_START
+  gEfiSerialIoProtocolGuid                      ## BY_START
+  gEfiDevicePathProtocolGuid                    ## BY_START
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialUseHalfHandshake|FALSE   ## CONSUMES
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultBaudRate|115200    ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultDataBits|8         ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultParity|1           ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultStopBits|1         ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialClockRate|1843200 ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciSerialParameters     ## CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PciSioSerialDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.uni b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.uni
new file mode 100644
index 0000000000..617f583b35
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxeExtra.uni
new file mode 100644
index 0000000000..935bdba93e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.c b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.c
new file mode 100644
index 0000000000..aeafee247c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.c
@@ -0,0 +1,1248 @@
+/** @file
+  Serial driver for PCI or SIO UARTS.
+
+Copyright (c) 2006 - 2015, 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 "Serial.h"
+
+//
+// ISA Serial Driver Global Variables
+//
+
+EFI_DRIVER_BINDING_PROTOCOL gSerialControllerDriver = {
+  SerialControllerDriverSupported,
+  SerialControllerDriverStart,
+  SerialControllerDriverStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+CONTROLLER_DEVICE_PATH mControllerDevicePathTemplate = {
+  {
+    HARDWARE_DEVICE_PATH,
+    HW_CONTROLLER_DP,
+    {
+      (UINT8) (sizeof (CONTROLLER_DEVICE_PATH)),
+      (UINT8) ((sizeof (CONTROLLER_DEVICE_PATH)) >> 8)
+    }
+  },
+  0
+};
+
+SERIAL_DEV  gSerialDevTemplate = {
+  SERIAL_DEV_SIGNATURE,
+  NULL,
+  {
+    SERIAL_IO_INTERFACE_REVISION,
+    SerialReset,
+    SerialSetAttributes,
+    SerialSetControl,
+    SerialGetControl,
+    SerialWrite,
+    SerialRead,
+    NULL
+  },                                       // SerialIo
+  {
+    SERIAL_PORT_SUPPORT_CONTROL_MASK,
+    SERIAL_PORT_DEFAULT_TIMEOUT,
+    0,
+    16,
+    0,
+    0,
+    0
+  },                                       // SerialMode
+  NULL,                                    // DevicePath
+  NULL,                                    // ParentDevicePath
+  {
+    {
+      MESSAGING_DEVICE_PATH,
+      MSG_UART_DP,
+      {
+        (UINT8) (sizeof (UART_DEVICE_PATH)),
+        (UINT8) ((sizeof (UART_DEVICE_PATH)) >> 8)
+      }
+    },
+    0, 0, 0, 0, 0
+  },                                       // UartDevicePath
+  0,                                       // BaseAddress
+  FALSE,                                   // MmioAccess
+  1,                                       // RegisterStride
+  0,                                       // ClockRate
+  16,                                      // ReceiveFifoDepth
+  { 0, 0 },                                // Receive;
+  16,                                      // TransmitFifoDepth
+  { 0, 0 },                                // Transmit;
+  FALSE,                                   // SoftwareLoopbackEnable;
+  FALSE,                                   // HardwareFlowControl;
+  NULL,                                    // *ControllerNameTable;
+  FALSE,                                   // ContainsControllerNode;
+  0,                                       // Instance;
+  NULL                                     // *PciDeviceInfo;
+};
+
+/**
+  Check the device path node whether it's the Flow Control node or not.
+
+  @param[in] FlowControl    The device path node to be checked.
+  
+  @retval TRUE              It's the Flow Control node.
+  @retval FALSE             It's not.
+
+**/
+BOOLEAN
+IsUartFlowControlDevicePathNode (
+  IN UART_FLOW_CONTROL_DEVICE_PATH *FlowControl
+  )
+{
+  return (BOOLEAN) (
+           (DevicePathType (FlowControl) == MESSAGING_DEVICE_PATH) &&
+           (DevicePathSubType (FlowControl) == MSG_VENDOR_DP) &&
+           (CompareGuid (&FlowControl->Guid, &gEfiUartDevicePathGuid))
+           );
+}
+
+/**
+  The user Entry Point for module PciSioSerial. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializePciSioSerial (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gSerialControllerDriver,
+             ImageHandle,
+             &gPciSioSerialComponentName,
+             &gPciSioSerialComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Initialize UART default setting in gSerialDevTempate
+  //
+  gSerialDevTemplate.SerialMode.BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
+  gSerialDevTemplate.SerialMode.DataBits = PcdGet8 (PcdUartDefaultDataBits);
+  gSerialDevTemplate.SerialMode.Parity   = PcdGet8 (PcdUartDefaultParity);
+  gSerialDevTemplate.SerialMode.StopBits = PcdGet8 (PcdUartDefaultStopBits);
+  gSerialDevTemplate.UartDevicePath.BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
+  gSerialDevTemplate.UartDevicePath.DataBits = PcdGet8 (PcdUartDefaultDataBits);
+  gSerialDevTemplate.UartDevicePath.Parity   = PcdGet8 (PcdUartDefaultParity);
+  gSerialDevTemplate.UartDevicePath.StopBits = PcdGet8 (PcdUartDefaultStopBits);
+  gSerialDevTemplate.ClockRate = PcdGet32 (PcdSerialClockRate);
+
+  return Status;
+}
+
+/**
+  Return whether the controller is a SIO serial controller.
+
+  @param  Controller   The controller handle.
+
+  @retval EFI_SUCCESS  The controller is a SIO serial controller.
+  @retval others       The controller is not a SIO serial controller.
+**/
+EFI_STATUS
+IsSioSerialController (
+  EFI_HANDLE               Controller
+  )
+{
+  EFI_STATUS               Status;
+  EFI_SIO_PROTOCOL         *Sio;
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;
+  ACPI_HID_DEVICE_PATH     *Acpi;
+
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSioProtocolGuid,
+                  (VOID **) &Sio,
+                  gSerialControllerDriver.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Close the I/O Abstraction(s) used to perform the supported test
+    //
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiSioProtocolGuid,
+           gSerialControllerDriver.DriverBindingHandle,
+           Controller
+           );
+
+    Status = gBS->OpenProtocol (
+      Controller,
+      &gEfiDevicePathProtocolGuid,
+      (VOID **) &DevicePath,
+      gSerialControllerDriver.DriverBindingHandle,
+      Controller,
+      EFI_OPEN_PROTOCOL_BY_DRIVER
+      );
+    ASSERT (Status != EFI_ALREADY_STARTED);
+
+    if (!EFI_ERROR (Status)) {
+      do {
+        Acpi = (ACPI_HID_DEVICE_PATH *) DevicePath;
+        DevicePath = NextDevicePathNode (DevicePath);
+      } while (!IsDevicePathEnd (DevicePath));
+
+      if (DevicePathType (Acpi) != ACPI_DEVICE_PATH ||
+          (DevicePathSubType (Acpi) != ACPI_DP && DevicePathSubType (Acpi) != ACPI_EXTENDED_DP) ||
+          Acpi->HID != EISA_PNP_ID (0x501)
+          ) {
+        Status = EFI_UNSUPPORTED;
+      }
+    }
+
+    //
+    // Close protocol, don't use device path protocol in the Support() function
+    //
+    gBS->CloseProtocol (
+      Controller,
+      &gEfiDevicePathProtocolGuid,
+      gSerialControllerDriver.DriverBindingHandle,
+      Controller
+      );
+  }
+  return Status;
+}
+
+/**
+  Return whether the controller is a PCI serial controller.
+
+  @param  Controller   The controller handle.
+
+  @retval EFI_SUCCESS  The controller is a PCI serial controller.
+  @retval others       The controller is not a PCI serial controller.
+**/
+EFI_STATUS
+IsPciSerialController (
+  EFI_HANDLE               Controller
+  )
+{
+  EFI_STATUS               Status;
+  EFI_PCI_IO_PROTOCOL      *PciIo;
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;
+  PCI_TYPE00               Pci;
+  PCI_SERIAL_PARAMETER     *PciSerialParameter;
+
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+    Controller,
+    &gEfiPciIoProtocolGuid,
+    (VOID **) &PciIo,
+    gSerialControllerDriver.DriverBindingHandle,
+    Controller,
+    EFI_OPEN_PROTOCOL_BY_DRIVER
+    );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    Status = PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0, sizeof (Pci), &Pci);
+    if (!EFI_ERROR (Status)) {
+      if (!IS_PCI_16550_SERIAL (&Pci)) {
+        for (PciSerialParameter = (PCI_SERIAL_PARAMETER *) PcdGetPtr (PcdPciSerialParameters)
+             ; PciSerialParameter->VendorId != 0xFFFF
+             ; PciSerialParameter++
+             ) {
+          if ((Pci.Hdr.VendorId == PciSerialParameter->VendorId) &&
+              (Pci.Hdr.DeviceId == PciSerialParameter->DeviceId)
+              ) {
+            break;
+          }
+        }
+        if (PciSerialParameter->VendorId == 0xFFFF) {
+          Status = EFI_UNSUPPORTED;
+        } else {
+          Status = EFI_SUCCESS;
+        }
+      }
+    }
+
+    //
+    // Close the I/O Abstraction(s) used to perform the supported test
+    //
+    gBS->CloseProtocol (
+      Controller,
+      &gEfiPciIoProtocolGuid,
+      gSerialControllerDriver.DriverBindingHandle,
+      Controller
+      );
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+    Controller,
+    &gEfiDevicePathProtocolGuid,
+    (VOID **) &DevicePath,
+    gSerialControllerDriver.DriverBindingHandle,
+    Controller,
+    EFI_OPEN_PROTOCOL_BY_DRIVER
+    );
+  ASSERT (Status != EFI_ALREADY_STARTED);
+
+  //
+  // Close protocol, don't use device path protocol in the Support() function
+  //
+  gBS->CloseProtocol (
+    Controller,
+    &gEfiDevicePathProtocolGuid,
+    gSerialControllerDriver.DriverBindingHandle,
+    Controller
+    );
+
+  return Status;
+}
+
+/**
+  Check to see if this driver supports the given controller
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller           The handle of the controller to test.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path.
+
+  @return EFI_SUCCESS          This driver can support the given controller
+
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+
+{
+  EFI_STATUS                                Status;
+  UART_DEVICE_PATH                          *Uart;
+  UART_FLOW_CONTROL_DEVICE_PATH             *FlowControl;
+
+  //
+  // Test RemainingDevicePath
+  //
+  if ((RemainingDevicePath != NULL) && !IsDevicePathEnd (RemainingDevicePath)) {
+    Status = EFI_UNSUPPORTED;
+
+    Uart = SkipControllerDevicePathNode (RemainingDevicePath, NULL, NULL);
+    if (DevicePathType (Uart) != MESSAGING_DEVICE_PATH ||
+        DevicePathSubType (Uart) != MSG_UART_DP ||
+        DevicePathNodeLength (Uart) != sizeof (UART_DEVICE_PATH)
+        ) {
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // Do a rough check because Clock Rate is unknown until DriverBindingStart()
+    //
+    if (!VerifyUartParameters (0, Uart->BaudRate, Uart->DataBits, Uart->Parity, Uart->StopBits, NULL, NULL)) {
+      return EFI_UNSUPPORTED;
+    }
+
+    FlowControl = (UART_FLOW_CONTROL_DEVICE_PATH *) NextDevicePathNode (Uart);
+    if (IsUartFlowControlDevicePathNode (FlowControl)) {
+      //
+      // If the second node is Flow Control Node,
+      //   return error when it request other than hardware flow control.
+      //
+      if ((ReadUnaligned32 (&FlowControl->FlowControlMap) & ~UART_FLOW_CONTROL_HARDWARE) != 0) {
+        return EFI_UNSUPPORTED;
+      }
+    }
+  }
+
+  Status = IsSioSerialController (Controller);
+  if (EFI_ERROR (Status)) {
+    Status = IsPciSerialController (Controller);
+  }
+  return Status;  
+}
+
+/**
+  Create the child serial device instance.
+
+  @param Controller           The parent controller handle.
+  @param Uart                 Pointer to the UART device path node in RemainingDevicePath,
+                              or NULL if RemainingDevicePath is NULL.
+  @param ParentDevicePath     Pointer to the parent device path.
+  @param CreateControllerNode TRUE to create the controller node.
+  @param Instance             Instance number of the serial device.
+                              The value will be set to the controller node
+                              if CreateControllerNode is TRUE.
+  @param ParentIo             A union type pointer to either Sio or PciIo.
+  @param PciSerialParameter   The PCI serial parameter to be used by current serial device.
+                              NULL for SIO serial device.
+  @param PciDeviceInfo        The PCI device info for the current serial device.
+                              NULL for SIO serial device.
+
+  @retval EFI_SUCCESS         The serial device was created successfully.
+  @retval others              The serial device wasn't created.
+**/
+EFI_STATUS
+CreateSerialDevice (
+  IN EFI_HANDLE                     Controller,
+  IN UART_DEVICE_PATH               *Uart,
+  IN EFI_DEVICE_PATH_PROTOCOL       *ParentDevicePath,
+  IN BOOLEAN                        CreateControllerNode,
+  IN UINT32                         Instance,
+  IN PARENT_IO_PROTOCOL_PTR         ParentIo,
+  IN PCI_SERIAL_PARAMETER           *PciSerialParameter, OPTIONAL
+  IN PCI_DEVICE_INFO                *PciDeviceInfo       OPTIONAL
+  )
+{
+  EFI_STATUS                                 Status;
+  SERIAL_DEV                                 *SerialDevice;
+  UINT8                                      BarIndex;
+  UINT64                                     Offset;
+  UART_FLOW_CONTROL_DEVICE_PATH              *FlowControl;
+  UINT32                                     FlowControlMap;
+  ACPI_RESOURCE_HEADER_PTR                   Resources;
+  EFI_ACPI_IO_PORT_DESCRIPTOR                *Io;
+  EFI_ACPI_FIXED_LOCATION_IO_PORT_DESCRIPTOR *FixedIo;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR          *AddressSpace;
+  EFI_DEVICE_PATH_PROTOCOL                   *TempDevicePath;
+
+  BarIndex = 0;
+  Offset = 0;
+  FlowControl = NULL;
+  FlowControlMap = 0;
+
+  //
+  // Initialize the serial device instance
+  //
+  SerialDevice = AllocateCopyPool (sizeof (SERIAL_DEV), &gSerialDevTemplate);
+  ASSERT (SerialDevice != NULL);
+
+  SerialDevice->SerialIo.Mode    = &(SerialDevice->SerialMode);
+  SerialDevice->ParentDevicePath = ParentDevicePath;
+  SerialDevice->PciDeviceInfo    = PciDeviceInfo;
+  SerialDevice->Instance         = Instance;
+
+  if (Uart != NULL) {
+    CopyMem (&SerialDevice->UartDevicePath, Uart, sizeof (UART_DEVICE_PATH));
+    FlowControl = (UART_FLOW_CONTROL_DEVICE_PATH *) NextDevicePathNode (Uart);
+    if (IsUartFlowControlDevicePathNode (FlowControl)) {
+      FlowControlMap = ReadUnaligned32 (&FlowControl->FlowControlMap);
+    } else {
+      FlowControl = NULL;
+    }
+  }
+
+  //
+  // For PCI serial device, use the information from PCD
+  //
+  if (PciSerialParameter != NULL) {
+    BarIndex = (PciSerialParameter->BarIndex == PCI_BAR_ALL) ? 0 : PciSerialParameter->BarIndex;
+    Offset = PciSerialParameter->Offset;
+    if (PciSerialParameter->RegisterStride != 0) {
+      SerialDevice->RegisterStride = PciSerialParameter->RegisterStride;
+    }
+    if (PciSerialParameter->ClockRate != 0) {
+      SerialDevice->ClockRate = PciSerialParameter->ClockRate;
+    }
+    if (PciSerialParameter->ReceiveFifoDepth != 0) {
+      SerialDevice->ReceiveFifoDepth = PciSerialParameter->ReceiveFifoDepth;
+    }
+    if (PciSerialParameter->TransmitFifoDepth != 0) {
+      SerialDevice->TransmitFifoDepth = PciSerialParameter->TransmitFifoDepth;
+    }
+  }
+
+  //
+  // Pass NULL ActualBaudRate to VerifyUartParameters to disallow baudrate degrade.
+  // DriverBindingStart() shouldn't create a handle with different UART device path.
+  //
+  if (!VerifyUartParameters (SerialDevice->ClockRate, SerialDevice->UartDevicePath.BaudRate, SerialDevice->UartDevicePath.DataBits,
+                            SerialDevice->UartDevicePath.Parity, SerialDevice->UartDevicePath.StopBits, NULL, NULL
+                            )) {
+    Status = EFI_INVALID_PARAMETER;
+    goto CreateError;
+  }
+
+  if (PciSerialParameter == NULL) {
+    Status = ParentIo.Sio->GetResources (ParentIo.Sio, &Resources);
+  } else {
+    Status = ParentIo.PciIo->GetBarAttributes (ParentIo.PciIo, BarIndex, NULL, (VOID **) &Resources);
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Get the base address information from ACPI resource descriptor.
+    // ACPI_IO_PORT_DESCRIPTOR and ACPI_FIXED_LOCATION_IO_PORT_DESCRIPTOR are returned from Sio;
+    // ACPI_ADDRESS_SPACE_DESCRIPTOR is returned from PciIo.
+    //
+    while ((Resources.SmallHeader->Byte != ACPI_END_TAG_DESCRIPTOR) && (SerialDevice->BaseAddress == 0)) {
+      switch (Resources.SmallHeader->Byte) {
+      case ACPI_IO_PORT_DESCRIPTOR:
+        Io = (EFI_ACPI_IO_PORT_DESCRIPTOR *) Resources.SmallHeader;
+        if (Io->Length != 0) {
+          SerialDevice->BaseAddress = Io->BaseAddressMin;
+        }
+        break;
+
+      case ACPI_FIXED_LOCATION_IO_PORT_DESCRIPTOR:
+        FixedIo = (EFI_ACPI_FIXED_LOCATION_IO_PORT_DESCRIPTOR *) Resources.SmallHeader;
+        if (FixedIo->Length != 0) {
+          SerialDevice->BaseAddress = FixedIo->BaseAddress;
+        }
+        break;
+
+      case ACPI_ADDRESS_SPACE_DESCRIPTOR:
+        AddressSpace = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Resources.SmallHeader;
+        if (AddressSpace->AddrLen != 0) {
+          if (AddressSpace->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+            SerialDevice->MmioAccess = TRUE;
+          }
+          SerialDevice->BaseAddress = AddressSpace->AddrRangeMin + Offset;
+        }
+        break;
+      }
+
+      if (Resources.SmallHeader->Bits.Type == 0) {
+        Resources.SmallHeader = (ACPI_SMALL_RESOURCE_HEADER *) ((UINT8 *) Resources.SmallHeader
+                                                                + Resources.SmallHeader->Bits.Length
+                                                                + sizeof (*Resources.SmallHeader));
+      } else {
+        Resources.LargeHeader = (ACPI_LARGE_RESOURCE_HEADER *) ((UINT8 *) Resources.LargeHeader
+                                                                + Resources.LargeHeader->Length
+                                                                + sizeof (*Resources.LargeHeader));
+      }
+    }
+  }
+
+  if (SerialDevice->BaseAddress == 0) {
+    Status = EFI_INVALID_PARAMETER;
+    goto CreateError;
+  }
+
+  SerialDevice->HardwareFlowControl = (BOOLEAN) (FlowControlMap == UART_FLOW_CONTROL_HARDWARE);
+
+  //
+  // Report status code the serial present
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_P_PC_PRESENCE_DETECT | EFI_PERIPHERAL_SERIAL_PORT,
+    SerialDevice->ParentDevicePath
+    );
+
+  if (!SerialPresent (SerialDevice)) {
+    Status = EFI_DEVICE_ERROR;
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE,
+      EFI_P_EC_NOT_DETECTED | EFI_PERIPHERAL_SERIAL_PORT,
+      SerialDevice->ParentDevicePath
+      );
+    goto CreateError;
+  }
+
+  //
+  // 1. Append Controller device path node.
+  //
+  if (CreateControllerNode) {
+    mControllerDevicePathTemplate.ControllerNumber = SerialDevice->Instance;
+    SerialDevice->DevicePath = AppendDevicePathNode (
+                                 SerialDevice->ParentDevicePath,
+                                 (EFI_DEVICE_PATH_PROTOCOL *) &mControllerDevicePathTemplate
+                                 );
+    SerialDevice->ContainsControllerNode = TRUE;
+  }
+
+  //
+  // 2. Append UART device path node.
+  //    The Uart setings are zero here.
+  //    SetAttribute() will update them to match the default setings.
+  //
+  TempDevicePath = SerialDevice->DevicePath;
+  if (TempDevicePath != NULL) {
+    SerialDevice->DevicePath = AppendDevicePathNode (
+                                 TempDevicePath,
+                                 (EFI_DEVICE_PATH_PROTOCOL *) &SerialDevice->UartDevicePath
+                                 );
+    FreePool (TempDevicePath);
+  } else {
+    SerialDevice->DevicePath = AppendDevicePathNode (
+                                 SerialDevice->ParentDevicePath,
+                                 (EFI_DEVICE_PATH_PROTOCOL *) &SerialDevice->UartDevicePath
+                                 );
+  }
+  //
+  // 3. Append the Flow Control device path node.
+  //    Only produce the Flow Control node when remaining device path has it
+  //
+  if (FlowControl != NULL) {
+    TempDevicePath = SerialDevice->DevicePath;
+    if (TempDevicePath != NULL) {
+      SerialDevice->DevicePath = AppendDevicePathNode (
+                                   TempDevicePath,
+                                   (EFI_DEVICE_PATH_PROTOCOL *) FlowControl
+                                   );
+      FreePool (TempDevicePath);
+    }
+  }
+  ASSERT (SerialDevice->DevicePath != NULL);
+
+  //
+  // Fill in Serial I/O Mode structure based on either the RemainingDevicePath or defaults.
+  //
+  SerialDevice->SerialMode.BaudRate = SerialDevice->UartDevicePath.BaudRate;
+  SerialDevice->SerialMode.DataBits = SerialDevice->UartDevicePath.DataBits;
+  SerialDevice->SerialMode.Parity   = SerialDevice->UartDevicePath.Parity;
+  SerialDevice->SerialMode.StopBits = SerialDevice->UartDevicePath.StopBits;
+
+  //
+  // Issue a reset to initialize the COM port
+  //
+  Status = SerialDevice->SerialIo.Reset (&SerialDevice->SerialIo);
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE,
+      EFI_P_EC_CONTROLLER_ERROR | EFI_PERIPHERAL_SERIAL_PORT,
+      SerialDevice->DevicePath
+      );
+    goto CreateError;
+  }
+
+  AddName (SerialDevice, Instance);
+  //
+  // Install protocol interfaces for the serial device.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &SerialDevice->Handle,
+                  &gEfiDevicePathProtocolGuid, SerialDevice->DevicePath,
+                  &gEfiSerialIoProtocolGuid, &SerialDevice->SerialIo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto CreateError;
+  }
+  //
+  // Open For Child Device
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  PciSerialParameter != NULL ? &gEfiPciIoProtocolGuid : &gEfiSioProtocolGuid,
+                  (VOID **) &ParentIo,
+                  gSerialControllerDriver.DriverBindingHandle,
+                  SerialDevice->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           &SerialDevice->Handle,
+           &gEfiDevicePathProtocolGuid, SerialDevice->DevicePath,
+           &gEfiSerialIoProtocolGuid, &SerialDevice->SerialIo,
+           NULL
+           );
+  }
+
+CreateError:
+  if (EFI_ERROR (Status)) {
+    if (SerialDevice->DevicePath != NULL) {
+      FreePool (SerialDevice->DevicePath);
+    }
+    if (SerialDevice->ControllerNameTable != NULL) {
+      FreeUnicodeStringTable (SerialDevice->ControllerNameTable);
+    }
+    FreePool (SerialDevice);
+  }
+  return Status;
+}
+
+/**
+  Returns an array of pointers containing all the child serial device pointers.
+
+  @param Controller      The parent controller handle.
+  @param IoProtocolGuid  The protocol GUID, either equals to gEfiSioProtocolGuid
+                         or equals to gEfiPciIoProtocolGuid.
+  @param Count           Count of the serial devices.
+
+  @return  An array of pointers containing all the child serial device pointers.
+**/
+SERIAL_DEV **
+GetChildSerialDevices (
+  IN EFI_HANDLE                       Controller,
+  IN EFI_GUID                         *IoProtocolGuid,
+  OUT UINTN                           *Count
+  )
+{
+  EFI_STATUS                                 Status;
+  UINTN                                      Index;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY        *OpenInfoBuffer;
+  UINTN                                      EntryCount;
+  SERIAL_DEV                                 **SerialDevices;
+  EFI_SERIAL_IO_PROTOCOL                     *SerialIo;
+  BOOLEAN                                    OpenByDriver;
+
+  *Count = 0;
+  //
+  // If the SerialIo instance specified by RemainingDevicePath is already created,
+  // update the attributes/control.
+  //
+  Status = gBS->OpenProtocolInformation (
+    Controller,
+    IoProtocolGuid,
+    &OpenInfoBuffer,
+    &EntryCount
+    );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  SerialDevices = AllocatePool (EntryCount * sizeof (SERIAL_DEV *));
+  ASSERT (SerialDevices != NULL);
+
+  *Count = 0;
+  OpenByDriver = FALSE;
+  for (Index = 0; Index < EntryCount; Index++) {
+    if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+      Status = gBS->OpenProtocol (
+        OpenInfoBuffer[Index].ControllerHandle,
+        &gEfiSerialIoProtocolGuid,
+        (VOID **) &SerialIo,
+        gSerialControllerDriver.DriverBindingHandle,
+        Controller,
+        EFI_OPEN_PROTOCOL_GET_PROTOCOL
+        );
+      if (!EFI_ERROR (Status)) {
+        SerialDevices[(*Count)++] = SERIAL_DEV_FROM_THIS (SerialIo);
+      }
+    }
+
+
+    if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) != 0) {
+      ASSERT (OpenInfoBuffer[Index].AgentHandle == gSerialControllerDriver.DriverBindingHandle);
+      OpenByDriver = TRUE;
+    }
+  }
+  if (OpenInfoBuffer != NULL) {
+    FreePool (OpenInfoBuffer);
+  }
+
+  ASSERT ((*Count == 0) || (OpenByDriver));
+
+  return SerialDevices;
+}
+
+/**
+  Start to management the controller passed in
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller           The handle of the controller to test.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path.
+
+  @return EFI_SUCCESS   Driver is started successfully
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                                 Status;
+  UINTN                                      Index;
+  EFI_DEVICE_PATH_PROTOCOL                   *ParentDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL                   *Node;
+  EFI_SERIAL_IO_PROTOCOL                     *SerialIo;
+  UINT32                                     ControllerNumber;
+  UART_DEVICE_PATH                           *Uart;
+  UART_FLOW_CONTROL_DEVICE_PATH              *FlowControl;
+  UINT32                                     Control;
+  PARENT_IO_PROTOCOL_PTR                     ParentIo;
+  ACPI_HID_DEVICE_PATH                       *Acpi;
+  EFI_GUID                                   *IoProtocolGuid;
+  PCI_SERIAL_PARAMETER                       *PciSerialParameter;
+  PCI_SERIAL_PARAMETER                       DefaultPciSerialParameter;
+  PCI_TYPE00                                 Pci;
+  UINT32                                     PciSerialCount;
+  SERIAL_DEV                                 **SerialDevices;
+  UINTN                                      SerialDeviceCount;
+  PCI_DEVICE_INFO                            *PciDeviceInfo;
+  UINT64                                     Supports;
+  BOOLEAN                                    ContainsControllerNode;
+
+  //
+  // Get the Parent Device Path
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    return Status;
+  }
+  //
+  // Report status code enable the serial
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_P_PC_ENABLE | EFI_PERIPHERAL_SERIAL_PORT,
+    ParentDevicePath
+    );
+
+  //
+  // Grab the IO abstraction we need to get any work done
+  //
+  IoProtocolGuid = &gEfiSioProtocolGuid;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  IoProtocolGuid,
+                  (VOID **) &ParentIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    IoProtocolGuid = &gEfiPciIoProtocolGuid;
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    IoProtocolGuid,
+                    (VOID **) &ParentIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+  }
+  ASSERT (!EFI_ERROR (Status) || Status == EFI_ALREADY_STARTED);
+
+  //
+  // Do nothing for END device path node
+  //
+  if ((RemainingDevicePath != NULL) && IsDevicePathEnd (RemainingDevicePath)) {
+    return EFI_SUCCESS;
+  }
+
+  ControllerNumber = 0;
+  ContainsControllerNode = FALSE;
+  SerialDevices = GetChildSerialDevices (Controller, IoProtocolGuid, &SerialDeviceCount);
+  //
+  // If the SerialIo instance specified by RemainingDevicePath is already created,
+  // update the attributes/control.
+  //
+  if ((SerialDeviceCount != 0) && (RemainingDevicePath != NULL)) {
+    Uart = (UART_DEVICE_PATH *) SkipControllerDevicePathNode (RemainingDevicePath, &ContainsControllerNode, &ControllerNumber);
+    for (Index = 0; Index < SerialDeviceCount; Index++) {
+      ASSERT ((SerialDevices != NULL) && (SerialDevices[Index] != NULL));
+      if ((!SerialDevices[Index]->ContainsControllerNode && !ContainsControllerNode) ||
+          (SerialDevices[Index]->ContainsControllerNode && ContainsControllerNode && SerialDevices[Index]->Instance == ControllerNumber)
+          ) {
+        SerialIo = &SerialDevices[Index]->SerialIo;
+        Status = EFI_INVALID_PARAMETER;
+        //
+        // Pass NULL ActualBaudRate to VerifyUartParameters to disallow baudrate degrade.
+        // DriverBindingStart() shouldn't create a handle with different UART device path.
+        //
+        if (VerifyUartParameters (SerialDevices[Index]->ClockRate, Uart->BaudRate, Uart->DataBits,
+                                  (EFI_PARITY_TYPE) Uart->Parity, (EFI_STOP_BITS_TYPE) Uart->StopBits, NULL, NULL)) {
+          Status = SerialIo->SetAttributes (
+                               SerialIo,
+                               Uart->BaudRate,
+                               SerialIo->Mode->ReceiveFifoDepth,
+                               SerialIo->Mode->Timeout,
+                               (EFI_PARITY_TYPE) Uart->Parity,
+                               Uart->DataBits,
+                               (EFI_STOP_BITS_TYPE) Uart->StopBits
+                               );
+        }
+        FlowControl = (UART_FLOW_CONTROL_DEVICE_PATH *) NextDevicePathNode (Uart);
+        if (!EFI_ERROR (Status) && IsUartFlowControlDevicePathNode (FlowControl)) {
+          Status = SerialIo->GetControl (SerialIo, &Control);
+          if (!EFI_ERROR (Status)) {
+            if (ReadUnaligned32 (&FlowControl->FlowControlMap) == UART_FLOW_CONTROL_HARDWARE) {
+              Control |= EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE;
+            } else {
+              Control &= ~EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE;
+            }
+            //
+            // Clear the bits that are not allowed to pass to SetControl
+            //
+            Control &= (EFI_SERIAL_REQUEST_TO_SEND | EFI_SERIAL_DATA_TERMINAL_READY |
+                        EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE | EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE |
+                        EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE);
+            Status = SerialIo->SetControl (SerialIo, Control);
+          }
+        }
+        break;
+      }
+    }
+    if (Index != SerialDeviceCount) {
+      //
+      // Directly return if the SerialIo instance specified by RemainingDevicePath is found and updated.
+      // Otherwise continue to create the instance specified by RemainingDevicePath.
+      //
+      if (SerialDevices != NULL) {
+        FreePool (SerialDevices);
+      }
+      return Status;
+    }
+  }
+
+  if (RemainingDevicePath != NULL) {
+    Uart = (UART_DEVICE_PATH *) SkipControllerDevicePathNode (RemainingDevicePath, &ContainsControllerNode, &ControllerNumber);
+  } else {
+    Uart = NULL;
+  }
+
+  PciDeviceInfo = NULL;
+  if (IoProtocolGuid == &gEfiSioProtocolGuid) {
+    Status = EFI_NOT_FOUND;
+    if (RemainingDevicePath == NULL || !ContainsControllerNode) {
+      Node = ParentDevicePath;
+      do {
+        Acpi = (ACPI_HID_DEVICE_PATH *) Node;
+        Node = NextDevicePathNode (Node);
+      } while (!IsDevicePathEnd (Node));
+      Status = CreateSerialDevice (Controller, Uart, ParentDevicePath, FALSE, Acpi->UID, ParentIo, NULL, NULL);
+      DEBUG ((EFI_D_INFO, "PciSioSerial: Create SIO child serial device - %r\n", Status));
+    }
+  } else {
+    Status = ParentIo.PciIo->Pci.Read (ParentIo.PciIo, EfiPciIoWidthUint8, 0, sizeof (Pci), &Pci);
+    if (!EFI_ERROR (Status)) {
+      //
+      // PcdPciSerialParameters takes the higher priority.
+      //
+      PciSerialCount = 0;
+      for (PciSerialParameter = PcdGetPtr (PcdPciSerialParameters); PciSerialParameter->VendorId != 0xFFFF; PciSerialParameter++) {
+        if ((PciSerialParameter->VendorId == Pci.Hdr.VendorId) &&
+            (PciSerialParameter->DeviceId == Pci.Hdr.DeviceId)
+            ) {
+          PciSerialCount++;
+        }
+      }
+
+      if (SerialDeviceCount == 0) {
+        //
+        // Enable the IO & MEM decoding when creating the first child.
+        // Restore the PCI attributes when all children is destroyed (PciDeviceInfo->ChildCount == 0).
+        //
+        PciDeviceInfo = AllocatePool (sizeof (PCI_DEVICE_INFO));
+        ASSERT (PciDeviceInfo != NULL);
+        PciDeviceInfo->ChildCount = 0;
+        PciDeviceInfo->PciIo = ParentIo.PciIo;
+        Status = ParentIo.PciIo->Attributes (
+          ParentIo.PciIo,
+          EfiPciIoAttributeOperationGet,
+          0,
+          &PciDeviceInfo->PciAttributes
+          );
+
+        if (!EFI_ERROR (Status)) {
+          Status = ParentIo.PciIo->Attributes (
+            ParentIo.PciIo,
+            EfiPciIoAttributeOperationSupported,
+            0,
+            &Supports
+            );
+          if (!EFI_ERROR (Status)) {
+            Supports &= (UINT64)(EFI_PCI_IO_ATTRIBUTE_IO | EFI_PCI_IO_ATTRIBUTE_MEMORY);
+            Status = ParentIo.PciIo->Attributes (
+              ParentIo.PciIo,
+              EfiPciIoAttributeOperationEnable,
+              Supports,
+              NULL
+              );
+          }
+        }
+      } else {
+        //
+        // Re-use the PciDeviceInfo stored in existing children.
+        //
+        ASSERT ((SerialDevices != NULL) && (SerialDevices[0] != NULL));
+        PciDeviceInfo = SerialDevices[0]->PciDeviceInfo;
+        ASSERT (PciDeviceInfo != NULL);
+      }
+
+      Status = EFI_NOT_FOUND;
+      if (PciSerialCount <= 1) {
+        //
+        // PCI serial device contains only one UART
+        //
+        if (RemainingDevicePath == NULL || !ContainsControllerNode) {
+          //
+          // This PCI serial device is matched by class code in Supported()
+          //
+          if (PciSerialCount == 0) {
+            DefaultPciSerialParameter.VendorId = Pci.Hdr.VendorId;
+            DefaultPciSerialParameter.DeviceId = Pci.Hdr.DeviceId;
+            DefaultPciSerialParameter.BarIndex = 0;
+            DefaultPciSerialParameter.Offset = 0;
+            DefaultPciSerialParameter.RegisterStride = 0;
+            DefaultPciSerialParameter.ClockRate = 0;
+            PciSerialParameter = &DefaultPciSerialParameter;
+          } else if (PciSerialCount == 1) {
+            PciSerialParameter = PcdGetPtr (PcdPciSerialParameters);
+          }
+
+          Status = CreateSerialDevice (Controller, Uart, ParentDevicePath, FALSE, 0, ParentIo, PciSerialParameter, PciDeviceInfo);
+          DEBUG ((EFI_D_INFO, "PciSioSerial: Create PCI child serial device (single) - %r\n", Status));
+          if (!EFI_ERROR (Status)) {
+            PciDeviceInfo->ChildCount++;
+          }
+        }
+      } else {
+        //
+        // PCI serial device contains multiple UARTs
+        //
+        if (RemainingDevicePath == NULL || ContainsControllerNode) {
+          PciSerialCount = 0;
+          for (PciSerialParameter = PcdGetPtr (PcdPciSerialParameters); PciSerialParameter->VendorId != 0xFFFF; PciSerialParameter++) {
+            if ((PciSerialParameter->VendorId == Pci.Hdr.VendorId) &&
+                (PciSerialParameter->DeviceId == Pci.Hdr.DeviceId) &&
+                ((RemainingDevicePath == NULL) || (ControllerNumber == PciSerialCount))
+                ) {
+              //
+              // Create controller node when PCI serial device contains multiple UARTs
+              //
+              Status = CreateSerialDevice (Controller, Uart, ParentDevicePath, TRUE, PciSerialCount, ParentIo, PciSerialParameter, PciDeviceInfo);
+              PciSerialCount++;
+              DEBUG ((EFI_D_INFO, "PciSioSerial: Create PCI child serial device (multiple) - %r\n", Status));
+              if (!EFI_ERROR (Status)) {
+                PciDeviceInfo->ChildCount++;
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  if (SerialDevices != NULL) {
+    FreePool (SerialDevices);
+  }
+
+  //
+  // For multiple PCI serial devices, set Status to SUCCESS if one child is created successfully
+  //
+  if ((PciDeviceInfo != NULL) && (PciDeviceInfo->ChildCount != 0)) {
+    Status = EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status) && (SerialDeviceCount == 0)) {
+    if (PciDeviceInfo != NULL) {
+      Status = ParentIo.PciIo->Attributes (
+        ParentIo.PciIo,
+        EfiPciIoAttributeOperationSet,
+        PciDeviceInfo->PciAttributes,
+        NULL
+        );
+      ASSERT_EFI_ERROR (Status);
+      FreePool (PciDeviceInfo);
+    }
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiDevicePathProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    gBS->CloseProtocol (
+           Controller,
+           IoProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+
+  return Status;
+}
+
+/**
+  Disconnect this driver with the controller, uninstall related protocol instance
+
+  @param  This                  A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The handle of the controller to test.
+  @param  NumberOfChildren      Number of child device.
+  @param  ChildHandleBuffer     A pointer to the remaining portion of a device path.
+
+  @retval EFI_SUCCESS           Operation successfully
+  @retval EFI_DEVICE_ERROR      Cannot stop the driver successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+
+{
+  EFI_STATUS                          Status;
+  UINTN                               Index;
+  BOOLEAN                             AllChildrenStopped;
+  EFI_SERIAL_IO_PROTOCOL              *SerialIo;
+  SERIAL_DEV                          *SerialDevice;
+  VOID                                *IoProtocol;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  PCI_DEVICE_INFO                     *PciDeviceInfo;
+
+  PciDeviceInfo = NULL;
+
+  Status = gBS->HandleProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath
+                  );
+
+  //
+  // Report the status code disable the serial
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_P_PC_DISABLE | EFI_PERIPHERAL_SERIAL_PORT,
+    DevicePath
+    );
+
+  if (NumberOfChildren == 0) {
+    //
+    // Close the bus driver
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiPciIoProtocolGuid,
+                    &IoProtocol,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                    );
+    gBS->CloseProtocol (
+           Controller,
+           !EFI_ERROR (Status) ? &gEfiPciIoProtocolGuid : &gEfiSioProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiDevicePathProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = gBS->OpenProtocol (
+                    ChildHandleBuffer[Index],
+                    &gEfiSerialIoProtocolGuid,
+                    (VOID **) &SerialIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (!EFI_ERROR (Status)) {
+
+      SerialDevice = SERIAL_DEV_FROM_THIS (SerialIo);
+      ASSERT ((PciDeviceInfo == NULL) || (PciDeviceInfo == SerialDevice->PciDeviceInfo));
+      PciDeviceInfo = SerialDevice->PciDeviceInfo;
+
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      PciDeviceInfo != NULL ? &gEfiPciIoProtocolGuid : &gEfiSioProtocolGuid,
+                      This->DriverBindingHandle,
+                      ChildHandleBuffer[Index]
+                      );
+
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      ChildHandleBuffer[Index],
+                      &gEfiDevicePathProtocolGuid, SerialDevice->DevicePath,
+                      &gEfiSerialIoProtocolGuid,   &SerialDevice->SerialIo,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        gBS->OpenProtocol (
+               Controller,
+               PciDeviceInfo != NULL ? &gEfiPciIoProtocolGuid : &gEfiSioProtocolGuid,
+               &IoProtocol,
+               This->DriverBindingHandle,
+               ChildHandleBuffer[Index],
+               EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+               );
+      } else {
+        FreePool (SerialDevice->DevicePath);
+        FreeUnicodeStringTable (SerialDevice->ControllerNameTable);
+        FreePool (SerialDevice);
+
+        if (PciDeviceInfo != NULL) {
+          ASSERT (PciDeviceInfo->ChildCount != 0);
+          PciDeviceInfo->ChildCount--;
+        }
+      }
+    }
+
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  } else {
+    //
+    // If all children are destroyed, restore the PCI attributes.
+    //
+    if ((PciDeviceInfo != NULL) && (PciDeviceInfo->ChildCount == 0)) {
+      ASSERT (PciDeviceInfo->PciIo != NULL);
+      Status = PciDeviceInfo->PciIo->Attributes (
+        PciDeviceInfo->PciIo,
+        EfiPciIoAttributeOperationSet,
+        PciDeviceInfo->PciAttributes,
+        NULL
+        );
+      ASSERT_EFI_ERROR (Status);
+      FreePool (PciDeviceInfo);
+    }
+    return EFI_SUCCESS;
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.h b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.h
new file mode 100644
index 0000000000..f147e69044
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.h
@@ -0,0 +1,789 @@
+/** @file
+  Header file for PciSioSerial Driver
+
+Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _SERIAL_H_
+#define _SERIAL_H_
+
+
+#include <Uefi.h>
+
+#include <IndustryStandard/Pci.h>
+
+#include <Protocol/SuperIo.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/SerialIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/PcdLib.h>
+#include <Library/IoLib.h>
+#include <Library/PrintLib.h>
+
+//
+// Driver Binding Externs
+//
+extern EFI_DRIVER_BINDING_PROTOCOL  gSerialControllerDriver;
+extern EFI_COMPONENT_NAME_PROTOCOL  gPciSioSerialComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gPciSioSerialComponentName2;
+
+#define SIO_SERIAL_PORT_NAME L"SIO Serial Port #%d"
+#define PCI_SERIAL_PORT_NAME L"PCI Serial Port #%d"
+#define SERIAL_PORT_NAME_LEN (sizeof (SIO_SERIAL_PORT_NAME) / sizeof (CHAR16) + MAXIMUM_VALUE_CHARACTERS)
+
+//
+// Internal Data Structures
+//
+#define TIMEOUT_STALL_INTERVAL  10
+
+#pragma pack(1)
+///
+/// PcdPciSerialParameters contains zero or more instances of the below structure.
+/// If a PCI device contains multiple UARTs, PcdPciSerialParameters needs to contain
+/// two instances of the below structure, with the VendorId and DeviceId equals to the
+/// device ID and vendor ID of the device. If the PCI device uses the first two BARs
+/// to support multiple UARTs, BarIndex of first instance equals to 0 and BarIndex of
+/// second one equals to 1; if the PCI device uses the first BAR to support multiple
+/// UARTs, BarIndex of both instance equals to 0 and Offset of first instance equals
+/// to 0 while Offset of second one equals to some value bigger or equal to 8.
+/// For certain UART whose register needs to be accessed in DWORD aligned address,
+/// RegisterStride equals to 4.
+///
+typedef struct {
+  UINT16  VendorId;          ///< Vendor ID to match the PCI device.  The value 0xFFFF terminates the list of entries.
+  UINT16  DeviceId;          ///< Device ID to match the PCI device
+  UINT32  ClockRate;         ///< UART clock rate.  Set to 0 for default clock rate of 1843200 Hz
+  UINT64  Offset;            ///< The byte offset into to the BAR
+  UINT8   BarIndex;          ///< Which BAR to get the UART base address
+  UINT8   RegisterStride;    ///< UART register stride in bytes.  Set to 0 for default register stride of 1 byte.
+  UINT16  ReceiveFifoDepth;  ///< UART receive FIFO depth in bytes. Set to 0 for a default FIFO depth of 16 bytes.
+  UINT16  TransmitFifoDepth; ///< UART transmit FIFO depth in bytes. Set to 0 for a default FIFO depth of 16 bytes.
+  UINT8   Reserved[2];
+} PCI_SERIAL_PARAMETER;
+#pragma pack()
+
+#define SERIAL_MAX_FIFO_SIZE 17       ///< Actual FIFO size is 16. FIFO based on circular wastes one unit.
+typedef struct {
+  UINT16  Head;                       ///< Head pointer of the FIFO. Empty when (Head == Tail).
+  UINT16  Tail;                       ///< Tail pointer of the FIFO. Full when ((Tail + 1) % SERIAL_MAX_FIFO_SIZE == Head).
+  UINT8   Data[SERIAL_MAX_FIFO_SIZE]; ///< Store the FIFO data.
+} SERIAL_DEV_FIFO;
+
+typedef union {
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  EFI_SIO_PROTOCOL          *Sio;
+} PARENT_IO_PROTOCOL_PTR;
+
+typedef struct {
+  EFI_PCI_IO_PROTOCOL      *PciIo;        // Pointer to parent PciIo instance.
+  UINTN                    ChildCount;    // Count of child SerialIo instance.
+  UINT64                   PciAttributes; // Original PCI attributes.
+} PCI_DEVICE_INFO;
+
+typedef struct {
+  UINT32                   Signature;
+  EFI_HANDLE               Handle;
+  EFI_SERIAL_IO_PROTOCOL   SerialIo;
+  EFI_SERIAL_IO_MODE       SerialMode;
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;
+
+  EFI_DEVICE_PATH_PROTOCOL *ParentDevicePath;
+  UART_DEVICE_PATH         UartDevicePath;
+
+  EFI_PHYSICAL_ADDRESS     BaseAddress;       ///< UART base address
+  BOOLEAN                  MmioAccess;        ///< TRUE for MMIO, FALSE for IO
+  UINT8                    RegisterStride;    ///< UART Register Stride
+  UINT32                   ClockRate;         ///< UART clock rate
+
+  UINT16                   ReceiveFifoDepth;  ///< UART receive FIFO depth in bytes.
+  SERIAL_DEV_FIFO          Receive;           ///< The FIFO used to store received data
+
+  UINT16                   TransmitFifoDepth; ///< UART transmit FIFO depth in bytes.
+  SERIAL_DEV_FIFO          Transmit;          ///< The FIFO used to store to-transmit data
+
+  BOOLEAN                  SoftwareLoopbackEnable;
+  BOOLEAN                  HardwareFlowControl;
+  EFI_UNICODE_STRING_TABLE *ControllerNameTable;
+  BOOLEAN                  ContainsControllerNode; ///< TRUE if the device produced contains Controller node
+  UINT32                   Instance;
+  PCI_DEVICE_INFO          *PciDeviceInfo;
+} SERIAL_DEV;
+
+#define SERIAL_DEV_SIGNATURE    SIGNATURE_32 ('s', 'e', 'r', 'd')
+#define SERIAL_DEV_FROM_THIS(a) CR (a, SERIAL_DEV, SerialIo, SERIAL_DEV_SIGNATURE)
+
+//
+// Serial Driver Defaults
+//
+#define SERIAL_PORT_DEFAULT_TIMEOUT             1000000
+#define SERIAL_PORT_SUPPORT_CONTROL_MASK        (EFI_SERIAL_CLEAR_TO_SEND                | \
+                                                 EFI_SERIAL_DATA_SET_READY               | \
+                                                 EFI_SERIAL_RING_INDICATE                | \
+                                                 EFI_SERIAL_CARRIER_DETECT               | \
+                                                 EFI_SERIAL_REQUEST_TO_SEND              | \
+                                                 EFI_SERIAL_DATA_TERMINAL_READY          | \
+                                                 EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE     | \
+                                                 EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE     | \
+                                                 EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE | \
+                                                 EFI_SERIAL_OUTPUT_BUFFER_EMPTY          | \
+                                                 EFI_SERIAL_INPUT_BUFFER_EMPTY)
+
+#define SERIAL_PORT_MIN_TIMEOUT             1         // 1 uS
+#define SERIAL_PORT_MAX_TIMEOUT             100000000 // 100 seconds
+//
+// UART Registers
+//
+#define SERIAL_REGISTER_THR 0  ///< WO   Transmit Holding Register
+#define SERIAL_REGISTER_RBR 0  ///< RO   Receive Buffer Register
+#define SERIAL_REGISTER_DLL 0  ///< R/W  Divisor Latch LSB
+#define SERIAL_REGISTER_DLM 1  ///< R/W  Divisor Latch MSB
+#define SERIAL_REGISTER_IER 1  ///< R/W  Interrupt Enable Register
+#define SERIAL_REGISTER_IIR 2  ///< RO   Interrupt Identification Register
+#define SERIAL_REGISTER_FCR 2  ///< WO   FIFO Cotrol Register
+#define SERIAL_REGISTER_LCR 3  ///< R/W  Line Control Register
+#define SERIAL_REGISTER_MCR 4  ///< R/W  Modem Control Register
+#define SERIAL_REGISTER_LSR 5  ///< R/W  Line Status Register
+#define SERIAL_REGISTER_MSR 6  ///< R/W  Modem Status Register
+#define SERIAL_REGISTER_SCR 7  ///< R/W  Scratch Pad Register
+#pragma pack(1)
+
+///
+/// Interrupt Enable Register
+///
+typedef union {
+  struct {
+    UINT8 Ravie : 1;   ///< Receiver Data Available Interrupt Enable
+    UINT8 Theie : 1;   ///< Transmistter Holding Register Empty Interrupt Enable
+    UINT8 Rie : 1;     ///< Receiver Interrupt Enable
+    UINT8 Mie : 1;     ///< Modem Interrupt Enable
+    UINT8 Reserved : 4;
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_IER;
+
+///
+/// FIFO Control Register
+///
+typedef union {
+  struct {
+    UINT8 TrFIFOE : 1;   ///< Transmit and Receive FIFO Enable
+    UINT8 ResetRF : 1;   ///< Reset Reciever FIFO
+    UINT8 ResetTF : 1;   ///< Reset Transmistter FIFO
+    UINT8 Dms : 1;       ///< DMA Mode Select
+    UINT8 Reserved : 1;
+    UINT8 TrFIFO64 : 1;  ///< Enable 64 byte FIFO
+    UINT8 Rtb : 2;       ///< Receive Trigger Bits
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_FCR;
+
+///
+/// Line Control Register
+///
+typedef union {
+  struct {
+    UINT8 SerialDB : 2;   ///< Number of Serial Data Bits
+    UINT8 StopB : 1;      ///< Number of Stop Bits
+    UINT8 ParEn : 1;      ///< Parity Enable
+    UINT8 EvenPar : 1;    ///< Even Parity Select
+    UINT8 SticPar : 1;    ///< Sticky Parity
+    UINT8 BrCon : 1;      ///< Break Control
+    UINT8 DLab : 1;       ///< Divisor Latch Access Bit
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_LCR;
+
+///
+/// Modem Control Register
+///
+typedef union {
+  struct {
+    UINT8 DtrC : 1;  ///< Data Terminal Ready Control
+    UINT8 Rts : 1;   ///< Request To Send Control
+    UINT8 Out1 : 1;  ///< Output1
+    UINT8 Out2 : 1;  ///< Output2, used to disable interrupt
+    UINT8 Lme : 1;   ///< Loopback Mode Enable
+    UINT8 Reserved : 3;
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_MCR;
+
+///
+/// Line Status Register
+///
+typedef union {
+  struct {
+    UINT8 Dr : 1;     ///< Receiver Data Ready Status
+    UINT8 Oe : 1;     ///< Overrun Error Status
+    UINT8 Pe : 1;     ///< Parity Error Status
+    UINT8 Fe : 1;     ///< Framing Error Status
+    UINT8 Bi : 1;     ///< Break Interrupt Status
+    UINT8 Thre : 1;   ///< Transmistter Holding Register Status
+    UINT8 Temt : 1;   ///< Transmitter Empty Status
+    UINT8 FIFOe : 1;  ///< FIFO Error Status
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_LSR;
+
+///
+/// Modem Status Register
+///
+typedef union {
+  struct {
+    UINT8 DeltaCTS : 1;         ///< Delta Clear To Send Status
+    UINT8 DeltaDSR : 1;         ///< Delta Data Set Ready Status
+    UINT8 TrailingEdgeRI : 1;   ///< Trailing Edge of Ring Indicator Status
+    UINT8 DeltaDCD : 1;         ///< Delta Data Carrier Detect Status
+    UINT8 Cts : 1;              ///< Clear To Send Status
+    UINT8 Dsr : 1;              ///< Data Set Ready Status
+    UINT8 Ri : 1;               ///< Ring Indicator Status
+    UINT8 Dcd : 1;              ///< Data Carrier Detect Status
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_MSR;
+
+#pragma pack()
+//
+// Define serial register I/O macros
+//
+#define READ_RBR(S)     SerialReadRegister (S, SERIAL_REGISTER_RBR)
+#define READ_DLL(S)     SerialReadRegister (S, SERIAL_REGISTER_DLL)
+#define READ_DLM(S)     SerialReadRegister (S, SERIAL_REGISTER_DLM)
+#define READ_IER(S)     SerialReadRegister (S, SERIAL_REGISTER_IER)
+#define READ_IIR(S)     SerialReadRegister (S, SERIAL_REGISTER_IIR)
+#define READ_LCR(S)     SerialReadRegister (S, SERIAL_REGISTER_LCR)
+#define READ_MCR(S)     SerialReadRegister (S, SERIAL_REGISTER_MCR)
+#define READ_LSR(S)     SerialReadRegister (S, SERIAL_REGISTER_LSR)
+#define READ_MSR(S)     SerialReadRegister (S, SERIAL_REGISTER_MSR)
+#define READ_SCR(S)     SerialReadRegister (S, SERIAL_REGISTER_SCR)
+
+#define WRITE_THR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_THR, D)
+#define WRITE_DLL(S, D) SerialWriteRegister (S, SERIAL_REGISTER_DLL, D)
+#define WRITE_DLM(S, D) SerialWriteRegister (S, SERIAL_REGISTER_DLM, D)
+#define WRITE_IER(S, D) SerialWriteRegister (S, SERIAL_REGISTER_IER, D)
+#define WRITE_FCR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_FCR, D)
+#define WRITE_LCR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_LCR, D)
+#define WRITE_MCR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_MCR, D)
+#define WRITE_LSR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_LSR, D)
+#define WRITE_MSR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_MSR, D)
+#define WRITE_SCR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_SCR, D)
+
+//
+// Prototypes
+// Driver model protocol interface
+//
+/**
+  Check to see if this driver supports the given controller
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller           The handle of the controller to test.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path.
+
+  @return EFI_SUCCESS          This driver can support the given controller
+
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Start to management the controller passed in
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller           The handle of the controller to test.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path.
+
+  @return EFI_SUCCESS          Driver is started successfully
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Disconnect this driver with the controller, uninstall related protocol instance
+
+  @param  This                  A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The handle of the controller to test.
+  @param  NumberOfChildren      Number of child device.
+  @param  ChildHandleBuffer     A pointer to the remaining portion of a device path.
+
+  @retval EFI_SUCCESS           Operation successfully
+  @retval EFI_DEVICE_ERROR      Cannot stop the driver successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  );
+
+//
+// Serial I/O Protocol Interface
+//
+/**
+  Reset serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+
+  @retval EFI_SUCCESS        Reset successfully
+  @retval EFI_DEVICE_ERROR   Failed to reset
+
+**/
+EFI_STATUS
+EFIAPI
+SerialReset (
+  IN EFI_SERIAL_IO_PROTOCOL         *This
+  );
+
+/**
+  Set new attributes to a serial device.
+
+  @param This                     Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param  BaudRate                 The baudrate of the serial device
+  @param  ReceiveFifoDepth         The depth of receive FIFO buffer
+  @param  Timeout                  The request timeout for a single char
+  @param  Parity                   The type of parity used in serial device
+  @param  DataBits                 Number of databits used in serial device
+  @param  StopBits                 Number of stopbits used in serial device
+
+  @retval  EFI_SUCCESS              The new attributes were set
+  @retval  EFI_INVALID_PARAMETERS   One or more attributes have an unsupported value
+  @retval  EFI_UNSUPPORTED          Data Bits can not set to 5 or 6
+  @retval  EFI_DEVICE_ERROR         The serial device is not functioning correctly (no return)
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetAttributes (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  IN UINT64                         BaudRate,
+  IN UINT32                         ReceiveFifoDepth,
+  IN UINT32                         Timeout,
+  IN EFI_PARITY_TYPE                Parity,
+  IN UINT8                          DataBits,
+  IN EFI_STOP_BITS_TYPE             StopBits
+  );
+
+/**
+  Set Control Bits.
+
+  @param This              Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param Control           Control bits that can be settable
+
+  @retval EFI_SUCCESS       New Control bits were set successfully
+  @retval EFI_UNSUPPORTED   The Control bits wanted to set are not supported
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetControl (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  IN UINT32                         Control
+  );
+
+/**
+  Get ControlBits.
+
+  @param This          Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param Control       Control signals of the serial device
+
+  @retval EFI_SUCCESS   Get Control signals successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SerialGetControl (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  OUT UINT32                        *Control
+  );
+
+/**
+  Write the specified number of bytes to serial device.
+
+  @param This                Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param  BufferSize         On input the size of Buffer, on output the amount of
+                             data actually written
+  @param  Buffer             The buffer of data to write
+
+  @retval EFI_SUCCESS        The data were written successfully
+  @retval EFI_DEVICE_ERROR   The device reported an error
+  @retval EFI_TIMEOUT        The write operation was stopped due to timeout
+
+**/
+EFI_STATUS
+EFIAPI
+SerialWrite (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  IN OUT UINTN                      *BufferSize,
+  IN VOID                           *Buffer
+  );
+
+/**
+  Read the specified number of bytes from serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param BufferSize         On input the size of Buffer, on output the amount of
+                            data returned in buffer
+  @param Buffer             The buffer to return the data into
+
+  @retval EFI_SUCCESS        The data were read successfully
+  @retval EFI_DEVICE_ERROR   The device reported an error
+  @retval EFI_TIMEOUT        The read operation was stopped due to timeout
+
+**/
+EFI_STATUS
+EFIAPI
+SerialRead (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  IN OUT UINTN                      *BufferSize,
+  OUT VOID                          *Buffer
+  );
+
+//
+// Internal Functions
+//
+/**
+  Use scratchpad register to test if this serial port is present.
+
+  @param SerialDevice   Pointer to serial device structure
+
+  @return if this serial port is present
+**/
+BOOLEAN
+SerialPresent (
+  IN SERIAL_DEV                     *SerialDevice
+  );
+
+/**
+  Detect whether specific FIFO is full or not.
+
+  @param Fifo    A pointer to the Data Structure SERIAL_DEV_FIFO
+
+  @return whether specific FIFO is full or not
+
+**/
+BOOLEAN
+SerialFifoFull (
+  IN SERIAL_DEV_FIFO                *Fifo
+  );
+
+/**
+  Detect whether specific FIFO is empty or not.
+ 
+  @param  Fifo    A pointer to the Data Structure SERIAL_DEV_FIFO
+
+  @return whether specific FIFO is empty or not
+
+**/
+BOOLEAN
+SerialFifoEmpty (
+  IN SERIAL_DEV_FIFO                *Fifo
+  );
+
+/**
+  Add data to specific FIFO.
+
+  @param Fifo                  A pointer to the Data Structure SERIAL_DEV_FIFO
+  @param Data                  the data added to FIFO
+
+  @retval EFI_SUCCESS           Add data to specific FIFO successfully
+  @retval EFI_OUT_OF_RESOURCE   Failed to add data because FIFO is already full
+
+**/
+EFI_STATUS
+SerialFifoAdd (
+  IN SERIAL_DEV_FIFO                *Fifo,
+  IN UINT8                          Data
+  );
+
+/**
+  Remove data from specific FIFO.
+
+  @param Fifo                  A pointer to the Data Structure SERIAL_DEV_FIFO
+  @param Data                  the data removed from FIFO
+
+  @retval EFI_SUCCESS           Remove data from specific FIFO successfully
+  @retval EFI_OUT_OF_RESOURCE   Failed to remove data because FIFO is empty
+
+**/
+EFI_STATUS
+SerialFifoRemove (
+  IN  SERIAL_DEV_FIFO               *Fifo,
+  OUT UINT8                         *Data
+  );
+
+/**
+  Reads and writes all avaliable data.
+
+  @param SerialDevice           The device to flush
+
+  @retval EFI_SUCCESS           Data was read/written successfully.
+  @retval EFI_OUT_OF_RESOURCE   Failed because software receive FIFO is full.  Note, when
+                                this happens, pending writes are not done.
+
+**/
+EFI_STATUS
+SerialReceiveTransmit (
+  IN SERIAL_DEV                     *SerialDevice
+  );
+
+/**
+  Read serial port.
+
+  @param SerialDev     Pointer to serial device
+  @param Offset        Offset in register group
+
+  @return Data read from serial port
+**/
+UINT8
+SerialReadRegister (
+  IN SERIAL_DEV                            *SerialDev,
+  IN UINT32                                Offset
+  );
+
+/**
+  Write serial port.
+
+  @param  SerialDev     Pointer to serial device
+  @param  Offset        Offset in register group
+  @param  Data          data which is to be written to some serial port register
+**/
+VOID
+SerialWriteRegister (
+  IN SERIAL_DEV                            *SerialDev,
+  IN UINT32                                Offset,
+  IN UINT8                                 Data
+  );
+
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Add the component name for the serial io device
+
+  @param SerialDevice     A pointer to the SERIAL_DEV instance.
+  @param Uid              Unique ID for the serial device.
+**/
+VOID
+AddName (
+  IN  SERIAL_DEV                               *SerialDevice,
+  IN  UINT32                                   Uid
+  );
+
+/**
+  Checks whether the UART parameters are valid and computes the Divisor.
+
+  @param  ClockRate      The clock rate of the serial device used to verify
+                         the BaudRate. Do not verify the BaudRate if it's 0.
+  @param  BaudRate       The requested baudrate of the serial device.
+  @param  DataBits       Number of databits used in serial device.
+  @param  Parity         The type of parity used in serial device.
+  @param  StopBits       Number of stopbits used in serial device.
+  @param  Divisor        Return the divisor if ClockRate is not 0.
+  @param  ActualBaudRate Return the actual supported baudrate without
+                         exceeding BaudRate. NULL means baudrate degradation
+                         is not allowed.
+                         If the requested BaudRate is not supported, the routine
+                         returns TRUE and the Actual Baud Rate when ActualBaudRate
+                         is not NULL, returns FALSE when ActualBaudRate is NULL.
+
+  @retval TRUE   The UART parameters are valid.
+  @retval FALSE  The UART parameters are not valid.
+**/
+BOOLEAN
+VerifyUartParameters (
+  IN     UINT32                  ClockRate,
+  IN     UINT64                  BaudRate,
+  IN     UINT8                   DataBits,
+  IN     EFI_PARITY_TYPE         Parity,
+  IN     EFI_STOP_BITS_TYPE      StopBits,
+     OUT UINT64                  *Divisor,
+     OUT UINT64                  *ActualBaudRate
+  );
+
+/**
+  Skip the optional Controller device path node and return the
+  pointer to the next device path node.
+
+  @param DevicePath             Pointer to the device path.
+  @param ContainsControllerNode Returns TRUE if the Controller device path exists.
+  @param ControllerNumber       Returns the Controller Number if Controller device path exists.
+
+  @return     Pointer to the next device path node.
+**/
+UART_DEVICE_PATH *
+SkipControllerDevicePathNode (
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath,
+  BOOLEAN                           *ContainsControllerNode,
+  UINT32                            *ControllerNumber
+  );
+
+/**
+  Check the device path node whether it's the Flow Control node or not.
+
+  @param[in] FlowControl    The device path node to be checked.
+  
+  @retval TRUE              It's the Flow Control node.
+  @retval FALSE             It's not.
+
+**/
+BOOLEAN
+IsUartFlowControlDevicePathNode (
+  IN UART_FLOW_CONTROL_DEVICE_PATH *FlowControl
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/SerialIo.c b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/SerialIo.c
new file mode 100644
index 0000000000..f1870f3a1b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/SerialIo.c
@@ -0,0 +1,1320 @@
+/** @file
+  SerialIo implementation for PCI or SIO UARTs.
+
+Copyright (c) 2006 - 2015, 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 "Serial.h"
+
+/**
+  Skip the optional Controller device path node and return the
+  pointer to the next device path node.
+
+  @param DevicePath             Pointer to the device path.
+  @param ContainsControllerNode Returns TRUE if the Controller device path exists.
+  @param ControllerNumber       Returns the Controller Number if Controller device path exists.
+
+  @return     Pointer to the next device path node.
+**/
+UART_DEVICE_PATH *
+SkipControllerDevicePathNode (
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath,
+  BOOLEAN                           *ContainsControllerNode,
+  UINT32                            *ControllerNumber
+  )
+{
+  if ((DevicePathType (DevicePath) == HARDWARE_DEVICE_PATH) &&
+      (DevicePathSubType (DevicePath) == HW_CONTROLLER_DP)
+      ) {
+    if (ContainsControllerNode != NULL) {
+      *ContainsControllerNode = TRUE;
+    }
+    if (ControllerNumber != NULL) {
+      *ControllerNumber = ((CONTROLLER_DEVICE_PATH *) DevicePath)->ControllerNumber;
+    }
+    DevicePath = NextDevicePathNode (DevicePath);
+  } else {
+    if (ContainsControllerNode != NULL) {
+      *ContainsControllerNode = FALSE;
+    }
+  }
+  return (UART_DEVICE_PATH *) DevicePath;
+}
+
+/**
+  Checks whether the UART parameters are valid and computes the Divisor.
+
+  @param  ClockRate      The clock rate of the serial device used to verify
+                         the BaudRate. Do not verify the BaudRate if it's 0.
+  @param  BaudRate       The requested baudrate of the serial device.
+  @param  DataBits       Number of databits used in serial device.
+  @param  Parity         The type of parity used in serial device.
+  @param  StopBits       Number of stopbits used in serial device.
+  @param  Divisor        Return the divisor if ClockRate is not 0.
+  @param  ActualBaudRate Return the actual supported baudrate without
+                         exceeding BaudRate. NULL means baudrate degradation
+                         is not allowed.
+                         If the requested BaudRate is not supported, the routine
+                         returns TRUE and the Actual Baud Rate when ActualBaudRate
+                         is not NULL, returns FALSE when ActualBaudRate is NULL.
+
+  @retval TRUE   The UART parameters are valid.
+  @retval FALSE  The UART parameters are not valid.
+**/
+BOOLEAN
+VerifyUartParameters (
+  IN     UINT32                  ClockRate,
+  IN     UINT64                  BaudRate,
+  IN     UINT8                   DataBits,
+  IN     EFI_PARITY_TYPE         Parity,
+  IN     EFI_STOP_BITS_TYPE      StopBits,
+     OUT UINT64                  *Divisor,
+     OUT UINT64                  *ActualBaudRate
+  )
+{
+  UINT64                     Remainder;
+  UINT32                     ComputedBaudRate;
+  UINT64                     ComputedDivisor;
+  UINT64                     Percent;
+
+  if ((DataBits < 5) || (DataBits > 8) ||
+      (Parity < NoParity) || (Parity > SpaceParity) ||
+      (StopBits < OneStopBit) || (StopBits > TwoStopBits) ||
+      ((DataBits == 5) && (StopBits == TwoStopBits)) ||
+      ((DataBits >= 6) && (DataBits <= 8) && (StopBits == OneFiveStopBits))
+      ) {
+    return FALSE;
+  } 
+
+  //
+  // Do not verify the baud rate if clock rate is unknown (0).
+  //
+  if (ClockRate == 0) {
+    return TRUE;
+  }
+
+  //
+  // Compute divisor use to program the baud rate using a round determination
+  // Divisor = ClockRate / 16 / BaudRate = ClockRate / (16 * BaudRate)
+  //         = ClockRate / (BaudRate << 4)
+  //
+  ComputedDivisor = DivU64x64Remainder (ClockRate, LShiftU64 (BaudRate, 4), &Remainder);
+  //
+  // Round Divisor up by 1 if the Remainder is more than half (16 * BaudRate)
+  // BaudRate * 16 / 2 = BaudRate * 8 = (BaudRate << 3)
+  //
+  if (Remainder >= LShiftU64 (BaudRate, 3)) {
+    ComputedDivisor++;
+  }
+  //
+  // If the computed divisor is larger than the maximum value that can be programmed
+  // into the UART, then the requested baud rate can not be supported.
+  //
+  if (ComputedDivisor > MAX_UINT16) {
+    return FALSE;
+  }
+
+  //
+  // If the computed divisor is 0, then use a computed divisor of 1, which will select
+  // the maximum supported baud rate.
+  //
+  if (ComputedDivisor == 0) {
+    ComputedDivisor = 1;
+  }
+
+  //
+  // Actual baud rate that the serial port will be programmed for
+  // should be with in 4% of requested one.
+  //
+  ComputedBaudRate = ClockRate / ((UINT16) ComputedDivisor << 4);
+  if (ComputedBaudRate == 0) {
+    return FALSE;
+  }
+
+  Percent = DivU64x32 (MultU64x32 (BaudRate, 100), ComputedBaudRate);
+  DEBUG ((EFI_D_INFO, "ClockRate = %d\n",  ClockRate));
+  DEBUG ((EFI_D_INFO, "Divisor   = %ld\n", ComputedDivisor));
+  DEBUG ((EFI_D_INFO, "BaudRate/Actual (%ld/%d) = %d%%\n", BaudRate, ComputedBaudRate, Percent));
+
+  //
+  // If the requested BaudRate is not supported:
+  //  Returns TRUE and the Actual Baud Rate when ActualBaudRate is not NULL;
+  //  Returns FALSE when ActualBaudRate is NULL.
+  //
+  if ((Percent >= 96) && (Percent <= 104)) {
+    if (ActualBaudRate != NULL) {
+      *ActualBaudRate = BaudRate;
+    }
+    if (Divisor != NULL) {
+      *Divisor = ComputedDivisor;
+    }
+    return TRUE;
+  }
+  if (ComputedBaudRate < BaudRate) {
+    if (ActualBaudRate != NULL) {
+      *ActualBaudRate = ComputedBaudRate;
+    }
+    if (Divisor != NULL) {
+      *Divisor = ComputedDivisor;
+    }
+    return TRUE;
+  }
+
+  //
+  // ActualBaudRate is higher than requested baud rate and more than 4% 
+  // higher than the requested value.  Increment Divisor if it is less 
+  // than MAX_UINT16 and computed baud rate with new divisor.
+  //
+  if (ComputedDivisor == MAX_UINT16) {
+    return FALSE;
+  }
+  ComputedDivisor++;
+  ComputedBaudRate = ClockRate / ((UINT16) ComputedDivisor << 4);
+  if (ComputedBaudRate == 0) {
+    return FALSE;
+  }
+
+  DEBUG ((EFI_D_INFO, "ClockRate = %d\n",  ClockRate));
+  DEBUG ((EFI_D_INFO, "Divisor   = %ld\n", ComputedDivisor));
+  DEBUG ((EFI_D_INFO, "BaudRate/Actual (%ld/%d) = %d%%\n", BaudRate, ComputedBaudRate, Percent));
+
+  if (ActualBaudRate != NULL) {
+    *ActualBaudRate = ComputedBaudRate;
+  }
+  if (Divisor != NULL) {
+    *Divisor = ComputedDivisor;
+  }
+  return TRUE;
+}
+
+/**
+  Detect whether specific FIFO is full or not.
+
+  @param Fifo    A pointer to the Data Structure SERIAL_DEV_FIFO
+
+  @return whether specific FIFO is full or not
+**/
+BOOLEAN
+SerialFifoFull (
+  IN SERIAL_DEV_FIFO *Fifo
+  )
+{
+  return (BOOLEAN) (((Fifo->Tail + 1) % SERIAL_MAX_FIFO_SIZE) == Fifo->Head);
+}
+
+/**
+  Detect whether specific FIFO is empty or not.
+ 
+  @param  Fifo    A pointer to the Data Structure SERIAL_DEV_FIFO
+
+  @return whether specific FIFO is empty or not
+**/
+BOOLEAN
+SerialFifoEmpty (
+  IN SERIAL_DEV_FIFO *Fifo
+  )
+
+{
+  return (BOOLEAN) (Fifo->Head == Fifo->Tail);
+}
+
+/**
+  Add data to specific FIFO.
+
+  @param Fifo                  A pointer to the Data Structure SERIAL_DEV_FIFO
+  @param Data                  the data added to FIFO
+
+  @retval EFI_SUCCESS           Add data to specific FIFO successfully
+  @retval EFI_OUT_OF_RESOURCE   Failed to add data because FIFO is already full
+**/
+EFI_STATUS
+SerialFifoAdd (
+  IN OUT SERIAL_DEV_FIFO *Fifo,
+  IN     UINT8           Data
+  )
+{
+  //
+  // if FIFO full can not add data
+  //
+  if (SerialFifoFull (Fifo)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // FIFO is not full can add data
+  //
+  Fifo->Data[Fifo->Tail] = Data;
+  Fifo->Tail = (Fifo->Tail + 1) % SERIAL_MAX_FIFO_SIZE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Remove data from specific FIFO.
+
+  @param Fifo                  A pointer to the Data Structure SERIAL_DEV_FIFO
+  @param Data                  the data removed from FIFO
+
+  @retval EFI_SUCCESS           Remove data from specific FIFO successfully
+  @retval EFI_OUT_OF_RESOURCE   Failed to remove data because FIFO is empty
+
+**/
+EFI_STATUS
+SerialFifoRemove (
+  IN OUT SERIAL_DEV_FIFO *Fifo,
+  OUT    UINT8           *Data
+  )
+{
+  //
+  // if FIFO is empty, no data can remove
+  //
+  if (SerialFifoEmpty (Fifo)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // FIFO is not empty, can remove data
+  //
+  *Data = Fifo->Data[Fifo->Head];
+  Fifo->Head = (Fifo->Head + 1) % SERIAL_MAX_FIFO_SIZE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads and writes all avaliable data.
+
+  @param SerialDevice           The device to transmit.
+
+  @retval EFI_SUCCESS           Data was read/written successfully.
+  @retval EFI_OUT_OF_RESOURCE   Failed because software receive FIFO is full.  Note, when
+                                this happens, pending writes are not done.
+
+**/
+EFI_STATUS
+SerialReceiveTransmit (
+  IN SERIAL_DEV *SerialDevice
+  )
+
+{
+  SERIAL_PORT_LSR Lsr;
+  UINT8           Data;
+  BOOLEAN         ReceiveFifoFull;
+  SERIAL_PORT_MSR Msr;
+  SERIAL_PORT_MCR Mcr;
+  UINTN           TimeOut;
+
+  Data = 0;
+
+  //
+  // Begin the read or write
+  //
+  if (SerialDevice->SoftwareLoopbackEnable) {
+    do {
+      ReceiveFifoFull = SerialFifoFull (&SerialDevice->Receive);
+      if (!SerialFifoEmpty (&SerialDevice->Transmit)) {
+        SerialFifoRemove (&SerialDevice->Transmit, &Data);
+        if (ReceiveFifoFull) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        SerialFifoAdd (&SerialDevice->Receive, Data);
+      }
+    } while (!SerialFifoEmpty (&SerialDevice->Transmit));
+  } else {
+    ReceiveFifoFull = SerialFifoFull (&SerialDevice->Receive);
+    //
+    // For full handshake flow control, tell the peer to send data
+    // if receive buffer is available.
+    //
+    if (SerialDevice->HardwareFlowControl &&
+        !FeaturePcdGet(PcdSerialUseHalfHandshake)&&
+        !ReceiveFifoFull
+        ) {
+      Mcr.Data     = READ_MCR (SerialDevice);
+      Mcr.Bits.Rts = 1;
+      WRITE_MCR (SerialDevice, Mcr.Data);
+    }
+    do {
+      Lsr.Data = READ_LSR (SerialDevice);
+
+      //
+      // Flush incomming data to prevent a an overrun during a long write
+      //
+      if ((Lsr.Bits.Dr == 1) && !ReceiveFifoFull) {
+        ReceiveFifoFull = SerialFifoFull (&SerialDevice->Receive);
+        if (!ReceiveFifoFull) {
+          if (Lsr.Bits.FIFOe == 1 || Lsr.Bits.Oe == 1 || Lsr.Bits.Pe == 1 || Lsr.Bits.Fe == 1 || Lsr.Bits.Bi == 1) {
+            REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+              EFI_ERROR_CODE,
+              EFI_P_EC_INPUT_ERROR | EFI_PERIPHERAL_SERIAL_PORT,
+              SerialDevice->DevicePath
+              );
+            if (Lsr.Bits.FIFOe == 1 || Lsr.Bits.Pe == 1|| Lsr.Bits.Fe == 1 || Lsr.Bits.Bi == 1) {
+              Data = READ_RBR (SerialDevice);
+              continue;
+            }
+          }
+
+          Data = READ_RBR (SerialDevice);
+
+          SerialFifoAdd (&SerialDevice->Receive, Data);
+          
+          //
+          // For full handshake flow control, if receive buffer full
+          // tell the peer to stop sending data.
+          //
+          if (SerialDevice->HardwareFlowControl &&
+              !FeaturePcdGet(PcdSerialUseHalfHandshake)   &&
+              SerialFifoFull (&SerialDevice->Receive)
+              ) {
+            Mcr.Data     = READ_MCR (SerialDevice);
+            Mcr.Bits.Rts = 0;
+            WRITE_MCR (SerialDevice, Mcr.Data);
+          }
+
+
+          continue;
+        } else {
+          REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+            EFI_PROGRESS_CODE,
+            EFI_P_SERIAL_PORT_PC_CLEAR_BUFFER | EFI_PERIPHERAL_SERIAL_PORT,
+            SerialDevice->DevicePath
+            );
+        }
+      }
+      //
+      // Do the write
+      //
+      if (Lsr.Bits.Thre == 1 && !SerialFifoEmpty (&SerialDevice->Transmit)) {
+        //
+        // Make sure the transmit data will not be missed
+        //
+        if (SerialDevice->HardwareFlowControl) {
+          //
+          // For half handshake flow control assert RTS before sending.
+          //
+          if (FeaturePcdGet(PcdSerialUseHalfHandshake)) {
+            Mcr.Data     = READ_MCR (SerialDevice);
+            Mcr.Bits.Rts= 0;
+            WRITE_MCR (SerialDevice, Mcr.Data);
+          }
+          //
+          // Wait for CTS
+          //
+          TimeOut   = 0;
+          Msr.Data  = READ_MSR (SerialDevice);
+          while ((Msr.Bits.Dcd == 1) && ((Msr.Bits.Cts == 0) ^ FeaturePcdGet(PcdSerialUseHalfHandshake))) {
+            gBS->Stall (TIMEOUT_STALL_INTERVAL);
+            TimeOut++;
+            if (TimeOut > 5) {
+              break;
+            }
+
+            Msr.Data = READ_MSR (SerialDevice);
+          }
+
+          if ((Msr.Bits.Dcd == 0) || ((Msr.Bits.Cts == 1) ^ FeaturePcdGet(PcdSerialUseHalfHandshake))) {
+            SerialFifoRemove (&SerialDevice->Transmit, &Data);
+            WRITE_THR (SerialDevice, Data);
+          }
+
+          //
+          // For half handshake flow control, tell DCE we are done.
+          //
+          if (FeaturePcdGet(PcdSerialUseHalfHandshake)) {
+            Mcr.Data = READ_MCR (SerialDevice);
+            Mcr.Bits.Rts = 1;
+            WRITE_MCR (SerialDevice, Mcr.Data);
+          }
+        } else {
+          SerialFifoRemove (&SerialDevice->Transmit, &Data);
+          WRITE_THR (SerialDevice, Data);
+        }
+      }
+    } while (Lsr.Bits.Thre == 1 && !SerialFifoEmpty (&SerialDevice->Transmit));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Flush the serial hardware transmit FIFO and shift register.
+
+  @param SerialDevice  The device to flush.
+**/
+VOID
+SerialFlushTransmitFifo (
+  SERIAL_DEV  *SerialDevice
+  )
+{
+  SERIAL_PORT_LSR  Lsr;
+
+  //
+  // Wait for the serial port to be ready, to make sure both the transmit FIFO
+  // and shift register empty.
+  //
+  do {
+    Lsr.Data = READ_LSR (SerialDevice);
+  } while (Lsr.Bits.Temt == 0);
+}
+
+//
+// Interface Functions
+//
+/**
+  Reset serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+
+  @retval EFI_SUCCESS        Reset successfully
+  @retval EFI_DEVICE_ERROR   Failed to reset
+
+**/
+EFI_STATUS
+EFIAPI
+SerialReset (
+  IN EFI_SERIAL_IO_PROTOCOL  *This
+  )
+{
+  EFI_STATUS      Status;
+  SERIAL_DEV      *SerialDevice;
+  SERIAL_PORT_LCR Lcr;
+  SERIAL_PORT_IER Ier;
+  SERIAL_PORT_MCR Mcr;
+  SERIAL_PORT_FCR Fcr;
+  EFI_TPL         Tpl;
+  UINT32          Control;
+
+  SerialDevice = SERIAL_DEV_FROM_THIS (This);
+
+  //
+  // Report the status code reset the serial
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_P_PC_RESET | EFI_PERIPHERAL_SERIAL_PORT,
+    SerialDevice->DevicePath
+    );
+
+  Tpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  SerialFlushTransmitFifo (SerialDevice);
+
+  //
+  // Make sure DLAB is 0.
+  //
+  Lcr.Data      = READ_LCR (SerialDevice);
+  Lcr.Bits.DLab = 0;
+  WRITE_LCR (SerialDevice, Lcr.Data);
+
+  //
+  // Turn off all interrupts
+  //
+  Ier.Data        = READ_IER (SerialDevice);
+  Ier.Bits.Ravie  = 0;
+  Ier.Bits.Theie  = 0;
+  Ier.Bits.Rie    = 0;
+  Ier.Bits.Mie    = 0;
+  WRITE_IER (SerialDevice, Ier.Data);
+
+  //
+  // Reset the FIFO
+  //
+  Fcr.Data = 0;
+  Fcr.Bits.TrFIFOE = 0;
+  WRITE_FCR (SerialDevice, Fcr.Data);
+
+  //
+  // Turn off loopback and disable device interrupt.
+  //
+  Mcr.Data      = READ_MCR (SerialDevice);
+  Mcr.Bits.Out1 = 0;
+  Mcr.Bits.Out2 = 0;
+  Mcr.Bits.Lme  = 0;
+  WRITE_MCR (SerialDevice, Mcr.Data);
+
+  //
+  // Clear the scratch pad register
+  //
+  WRITE_SCR (SerialDevice, 0);
+
+  //
+  // Enable FIFO
+  //
+  Fcr.Bits.TrFIFOE  = 1;
+  if (SerialDevice->ReceiveFifoDepth > 16) {
+    Fcr.Bits.TrFIFO64 = 1;
+  }
+  Fcr.Bits.ResetRF  = 1;
+  Fcr.Bits.ResetTF  = 1;
+  WRITE_FCR (SerialDevice, Fcr.Data);
+
+  //
+  // Go set the current attributes
+  //
+  Status = This->SetAttributes (
+                   This,
+                   This->Mode->BaudRate,
+                   This->Mode->ReceiveFifoDepth,
+                   This->Mode->Timeout,
+                   (EFI_PARITY_TYPE) This->Mode->Parity,
+                   (UINT8) This->Mode->DataBits,
+                   (EFI_STOP_BITS_TYPE) This->Mode->StopBits
+                   );
+
+  if (EFI_ERROR (Status)) {
+    gBS->RestoreTPL (Tpl);
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Go set the current control bits
+  //
+  Control = 0;
+  if (SerialDevice->HardwareFlowControl) {
+    Control |= EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE;
+  }
+  if (SerialDevice->SoftwareLoopbackEnable) {
+    Control |= EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE;
+  }
+  Status = This->SetControl (
+                   This,
+                   Control
+                   );
+
+  if (EFI_ERROR (Status)) {
+    gBS->RestoreTPL (Tpl);
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Reset the software FIFO
+  //
+  SerialDevice->Receive.Head = SerialDevice->Receive.Tail = 0;
+  SerialDevice->Transmit.Head = SerialDevice->Transmit.Tail = 0;
+  gBS->RestoreTPL (Tpl);
+
+  //
+  // Device reset is complete
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Set new attributes to a serial device.
+
+  @param This                     Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param  BaudRate                 The baudrate of the serial device
+  @param  ReceiveFifoDepth         The depth of receive FIFO buffer
+  @param  Timeout                  The request timeout for a single char
+  @param  Parity                   The type of parity used in serial device
+  @param  DataBits                 Number of databits used in serial device
+  @param  StopBits                 Number of stopbits used in serial device
+
+  @retval  EFI_SUCCESS              The new attributes were set
+  @retval  EFI_INVALID_PARAMETERS   One or more attributes have an unsupported value
+  @retval  EFI_UNSUPPORTED          Data Bits can not set to 5 or 6
+  @retval  EFI_DEVICE_ERROR         The serial device is not functioning correctly (no return)
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetAttributes (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  IN UINT64                  BaudRate,
+  IN UINT32                  ReceiveFifoDepth,
+  IN UINT32                  Timeout,
+  IN EFI_PARITY_TYPE         Parity,
+  IN UINT8                   DataBits,
+  IN EFI_STOP_BITS_TYPE      StopBits
+  )
+{
+  EFI_STATUS                Status;
+  SERIAL_DEV                *SerialDevice;
+  UINT64                    Divisor;
+  SERIAL_PORT_LCR           Lcr;
+  UART_DEVICE_PATH          *Uart;
+  EFI_TPL                   Tpl;
+
+  SerialDevice = SERIAL_DEV_FROM_THIS (This);
+
+  //
+  // Check for default settings and fill in actual values.
+  //
+  if (BaudRate == 0) {
+    BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
+  }
+
+  if (ReceiveFifoDepth == 0) {
+    ReceiveFifoDepth = SerialDevice->ReceiveFifoDepth;
+  }
+
+  if (Timeout == 0) {
+    Timeout = SERIAL_PORT_DEFAULT_TIMEOUT;
+  }
+
+  if (Parity == DefaultParity) {
+    Parity = (EFI_PARITY_TYPE) PcdGet8 (PcdUartDefaultParity);
+  }
+
+  if (DataBits == 0) {
+    DataBits = PcdGet8 (PcdUartDefaultDataBits);
+  }
+
+  if (StopBits == DefaultStopBits) {
+    StopBits = (EFI_STOP_BITS_TYPE) PcdGet8 (PcdUartDefaultStopBits);
+  }
+
+  if (!VerifyUartParameters (SerialDevice->ClockRate, BaudRate, DataBits, Parity, StopBits, &Divisor, &BaudRate)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((ReceiveFifoDepth == 0) || (ReceiveFifoDepth > SerialDevice->ReceiveFifoDepth)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Timeout < SERIAL_PORT_MIN_TIMEOUT) || (Timeout > SERIAL_PORT_MAX_TIMEOUT)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Tpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  SerialFlushTransmitFifo (SerialDevice);
+
+  //
+  // Put serial port on Divisor Latch Mode
+  //
+  Lcr.Data      = READ_LCR (SerialDevice);
+  Lcr.Bits.DLab = 1;
+  WRITE_LCR (SerialDevice, Lcr.Data);
+
+  //
+  // Write the divisor to the serial port
+  //
+  WRITE_DLL (SerialDevice, (UINT8) Divisor);
+  WRITE_DLM (SerialDevice, (UINT8) ((UINT16) Divisor >> 8));
+
+  //
+  // Put serial port back in normal mode and set remaining attributes.
+  //
+  Lcr.Bits.DLab = 0;
+
+  switch (Parity) {
+  case NoParity:
+    Lcr.Bits.ParEn    = 0;
+    Lcr.Bits.EvenPar  = 0;
+    Lcr.Bits.SticPar  = 0;
+    break;
+
+  case EvenParity:
+    Lcr.Bits.ParEn    = 1;
+    Lcr.Bits.EvenPar  = 1;
+    Lcr.Bits.SticPar  = 0;
+    break;
+
+  case OddParity:
+    Lcr.Bits.ParEn    = 1;
+    Lcr.Bits.EvenPar  = 0;
+    Lcr.Bits.SticPar  = 0;
+    break;
+
+  case SpaceParity:
+    Lcr.Bits.ParEn    = 1;
+    Lcr.Bits.EvenPar  = 1;
+    Lcr.Bits.SticPar  = 1;
+    break;
+
+  case MarkParity:
+    Lcr.Bits.ParEn    = 1;
+    Lcr.Bits.EvenPar  = 0;
+    Lcr.Bits.SticPar  = 1;
+    break;
+
+  default:
+    break;
+  }
+
+  switch (StopBits) {
+  case OneStopBit:
+    Lcr.Bits.StopB = 0;
+    break;
+
+  case OneFiveStopBits:
+  case TwoStopBits:
+    Lcr.Bits.StopB = 1;
+    break;
+
+  default:
+    break;
+  }
+  //
+  // DataBits
+  //
+  Lcr.Bits.SerialDB = (UINT8) ((DataBits - 5) & 0x03);
+  WRITE_LCR (SerialDevice, Lcr.Data);
+
+  //
+  // Set the Serial I/O mode
+  //
+  This->Mode->BaudRate          = BaudRate;
+  This->Mode->ReceiveFifoDepth  = ReceiveFifoDepth;
+  This->Mode->Timeout           = Timeout;
+  This->Mode->Parity            = Parity;
+  This->Mode->DataBits          = DataBits;
+  This->Mode->StopBits          = StopBits;
+
+  //
+  // See if Device Path Node has actually changed
+  //
+  if (SerialDevice->UartDevicePath.BaudRate == BaudRate &&
+      SerialDevice->UartDevicePath.DataBits == DataBits &&
+      SerialDevice->UartDevicePath.Parity == Parity &&
+      SerialDevice->UartDevicePath.StopBits == StopBits
+      ) {
+    gBS->RestoreTPL (Tpl);
+    return EFI_SUCCESS;
+  }
+  //
+  // Update the device path
+  //
+  SerialDevice->UartDevicePath.BaudRate = BaudRate;
+  SerialDevice->UartDevicePath.DataBits = DataBits;
+  SerialDevice->UartDevicePath.Parity   = (UINT8) Parity;
+  SerialDevice->UartDevicePath.StopBits = (UINT8) StopBits;
+
+  Status = EFI_SUCCESS;
+  if (SerialDevice->Handle != NULL) {
+
+    //
+    // Skip the optional Controller device path node
+    //
+    Uart = SkipControllerDevicePathNode (
+             (EFI_DEVICE_PATH_PROTOCOL *) (
+               (UINT8 *) SerialDevice->DevicePath + GetDevicePathSize (SerialDevice->ParentDevicePath) - END_DEVICE_PATH_LENGTH
+               ),
+             NULL,
+             NULL
+             );
+    CopyMem (Uart, &SerialDevice->UartDevicePath, sizeof (UART_DEVICE_PATH));
+    Status = gBS->ReinstallProtocolInterface (
+                    SerialDevice->Handle,
+                    &gEfiDevicePathProtocolGuid,
+                    SerialDevice->DevicePath,
+                    SerialDevice->DevicePath
+                    );
+  }
+
+  gBS->RestoreTPL (Tpl);
+
+  return Status;
+}
+
+/**
+  Set Control Bits.
+
+  @param This              Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param Control           Control bits that can be settable
+
+  @retval EFI_SUCCESS       New Control bits were set successfully
+  @retval EFI_UNSUPPORTED   The Control bits wanted to set are not supported
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetControl (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  IN UINT32                  Control
+  )
+{
+  SERIAL_DEV                    *SerialDevice;
+  SERIAL_PORT_MCR               Mcr;
+  EFI_TPL                       Tpl;
+  UART_FLOW_CONTROL_DEVICE_PATH *FlowControl;
+  EFI_STATUS                    Status;
+
+  //
+  // The control bits that can be set are :
+  //     EFI_SERIAL_DATA_TERMINAL_READY: 0x0001  // WO
+  //     EFI_SERIAL_REQUEST_TO_SEND: 0x0002  // WO
+  //     EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE: 0x1000  // RW
+  //     EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE: 0x2000  // RW
+  //     EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE: 0x4000 // RW
+  //
+  SerialDevice = SERIAL_DEV_FROM_THIS (This);
+
+  //
+  // first determine the parameter is invalid
+  //
+  if ((Control & (~(EFI_SERIAL_REQUEST_TO_SEND | EFI_SERIAL_DATA_TERMINAL_READY |
+                    EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE | EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE |
+                    EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE))) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Tpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  Mcr.Data = READ_MCR (SerialDevice);
+  Mcr.Bits.DtrC = 0;
+  Mcr.Bits.Rts = 0;
+  Mcr.Bits.Lme = 0;
+  SerialDevice->SoftwareLoopbackEnable = FALSE;
+  SerialDevice->HardwareFlowControl = FALSE;
+
+  if ((Control & EFI_SERIAL_DATA_TERMINAL_READY) == EFI_SERIAL_DATA_TERMINAL_READY) {
+    Mcr.Bits.DtrC = 1;
+  }
+
+  if ((Control & EFI_SERIAL_REQUEST_TO_SEND) == EFI_SERIAL_REQUEST_TO_SEND) {
+    Mcr.Bits.Rts = 1;
+  }
+
+  if ((Control & EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE) == EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE) {
+    Mcr.Bits.Lme = 1;
+  }
+
+  if ((Control & EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE) == EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE) {
+    SerialDevice->HardwareFlowControl = TRUE;
+  }
+
+  WRITE_MCR (SerialDevice, Mcr.Data);
+
+  if ((Control & EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE) == EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE) {
+    SerialDevice->SoftwareLoopbackEnable = TRUE;
+  }
+
+  Status = EFI_SUCCESS;
+  if (SerialDevice->Handle != NULL) {
+    FlowControl = (UART_FLOW_CONTROL_DEVICE_PATH *) (
+                    (UINTN) SerialDevice->DevicePath
+                    + GetDevicePathSize (SerialDevice->ParentDevicePath)
+                    - END_DEVICE_PATH_LENGTH
+                    + sizeof (UART_DEVICE_PATH)
+                    );
+    if (IsUartFlowControlDevicePathNode (FlowControl) &&
+        ((BOOLEAN) (ReadUnaligned32 (&FlowControl->FlowControlMap) == UART_FLOW_CONTROL_HARDWARE) != SerialDevice->HardwareFlowControl)) {
+      //
+      // Flow Control setting is changed, need to reinstall device path protocol
+      //
+      WriteUnaligned32 (&FlowControl->FlowControlMap, SerialDevice->HardwareFlowControl ? UART_FLOW_CONTROL_HARDWARE : 0);
+      Status = gBS->ReinstallProtocolInterface (
+                      SerialDevice->Handle,
+                      &gEfiDevicePathProtocolGuid,
+                      SerialDevice->DevicePath,
+                      SerialDevice->DevicePath
+                      );
+    }
+  }
+
+  gBS->RestoreTPL (Tpl);
+
+  return Status;
+}
+
+/**
+  Get ControlBits.
+
+  @param This          Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param Control       Control signals of the serial device
+
+  @retval EFI_SUCCESS   Get Control signals successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SerialGetControl (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  OUT UINT32                 *Control
+  )
+{
+  SERIAL_DEV      *SerialDevice;
+  SERIAL_PORT_MSR Msr;
+  SERIAL_PORT_MCR Mcr;
+  EFI_TPL         Tpl;
+
+  Tpl           = gBS->RaiseTPL (TPL_NOTIFY);
+
+  SerialDevice  = SERIAL_DEV_FROM_THIS (This);
+
+  *Control      = 0;
+
+  //
+  // Read the Modem Status Register
+  //
+  Msr.Data = READ_MSR (SerialDevice);
+
+  if (Msr.Bits.Cts == 1) {
+    *Control |= EFI_SERIAL_CLEAR_TO_SEND;
+  }
+
+  if (Msr.Bits.Dsr == 1) {
+    *Control |= EFI_SERIAL_DATA_SET_READY;
+  }
+
+  if (Msr.Bits.Ri == 1) {
+    *Control |= EFI_SERIAL_RING_INDICATE;
+  }
+
+  if (Msr.Bits.Dcd == 1) {
+    *Control |= EFI_SERIAL_CARRIER_DETECT;
+  }
+  //
+  // Read the Modem Control Register
+  //
+  Mcr.Data = READ_MCR (SerialDevice);
+
+  if (Mcr.Bits.DtrC == 1) {
+    *Control |= EFI_SERIAL_DATA_TERMINAL_READY;
+  }
+
+  if (Mcr.Bits.Rts == 1) {
+    *Control |= EFI_SERIAL_REQUEST_TO_SEND;
+  }
+
+  if (Mcr.Bits.Lme == 1) {
+    *Control |= EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE;
+  }
+
+  if (SerialDevice->HardwareFlowControl) {
+    *Control |= EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE;
+  }
+  //
+  // Update FIFO status
+  //
+  SerialReceiveTransmit (SerialDevice);
+
+  //
+  // See if the Transmit FIFO is empty
+  //
+  if (SerialFifoEmpty (&SerialDevice->Transmit)) {
+    *Control |= EFI_SERIAL_OUTPUT_BUFFER_EMPTY;
+  }
+
+  //
+  // See if the Receive FIFO is empty.
+  //
+  if (SerialFifoEmpty (&SerialDevice->Receive)) {
+    *Control |= EFI_SERIAL_INPUT_BUFFER_EMPTY;
+  }
+
+  if (SerialDevice->SoftwareLoopbackEnable) {
+    *Control |= EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE;
+  }
+
+  gBS->RestoreTPL (Tpl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Write the specified number of bytes to serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param  BufferSize         On input the size of Buffer, on output the amount of
+                       data actually written
+  @param  Buffer             The buffer of data to write
+
+  @retval EFI_SUCCESS        The data were written successfully
+  @retval EFI_DEVICE_ERROR   The device reported an error
+  @retval EFI_TIMEOUT        The write operation was stopped due to timeout
+
+**/
+EFI_STATUS
+EFIAPI
+SerialWrite (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  IN OUT UINTN               *BufferSize,
+  IN VOID                    *Buffer
+  )
+{
+  SERIAL_DEV  *SerialDevice;
+  UINT8       *CharBuffer;
+  UINT32      Index;
+  UINTN       Elapsed;
+  UINTN       ActualWrite;
+  EFI_TPL     Tpl;
+  UINTN       Timeout;
+  UINTN       BitsPerCharacter;
+
+  SerialDevice  = SERIAL_DEV_FROM_THIS (This);
+  Elapsed       = 0;
+  ActualWrite   = 0;
+
+  if (*BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (Buffer == NULL) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE,
+      EFI_P_EC_OUTPUT_ERROR | EFI_PERIPHERAL_SERIAL_PORT,
+      SerialDevice->DevicePath
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  Tpl         = gBS->RaiseTPL (TPL_NOTIFY);
+
+  CharBuffer  = (UINT8 *) Buffer;
+
+  //
+  // Compute the number of bits in a single character.  This is a start bit,
+  // followed by the number of data bits, followed by the number of stop bits.
+  // The number of stop bits is specified by an enumeration that includes
+  // support for 1.5 stop bits.  Treat 1.5 stop bits as 2 stop bits.
+  //
+  BitsPerCharacter =
+    1 +
+    This->Mode->DataBits +
+    ((This->Mode->StopBits == TwoStopBits) ? 2 : This->Mode->StopBits);
+
+  //
+  // Compute the timeout in microseconds to wait for a single byte to be
+  // transmitted.  The Mode structure contans a Timeout field that is the
+  // maximum time to transmit or receive a character.  However, many UARTs
+  // have a FIFO for transmits, so the time required to add one new character
+  // to the transmit FIFO may be the time required to flush a full FIFO.  If
+  // the Timeout in the Mode structure is smaller than the time required to
+  // flush a full FIFO at the current baud rate, then use a timeout value that
+  // is required to flush a full transmit FIFO.
+  //
+  Timeout = MAX (
+              This->Mode->Timeout,
+              (UINTN)DivU64x64Remainder (
+                BitsPerCharacter * (SerialDevice->TransmitFifoDepth + 1) * 1000000,
+                This->Mode->BaudRate,
+                NULL
+                )
+              );
+  
+  for (Index = 0; Index < *BufferSize; Index++) {
+    SerialFifoAdd (&SerialDevice->Transmit, CharBuffer[Index]);
+
+    while (SerialReceiveTransmit (SerialDevice) != EFI_SUCCESS || !SerialFifoEmpty (&SerialDevice->Transmit)) {
+      //
+      //  Unsuccessful write so check if timeout has expired, if not,
+      //  stall for a bit, increment time elapsed, and try again
+      //
+      if (Elapsed >= Timeout) {
+        *BufferSize = ActualWrite;
+        gBS->RestoreTPL (Tpl);
+        return EFI_TIMEOUT;
+      }
+
+      gBS->Stall (TIMEOUT_STALL_INTERVAL);
+
+      Elapsed += TIMEOUT_STALL_INTERVAL;
+    }
+
+    ActualWrite++;
+    //
+    //  Successful write so reset timeout
+    //
+    Elapsed = 0;
+  }
+
+  gBS->RestoreTPL (Tpl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read the specified number of bytes from serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param BufferSize         On input the size of Buffer, on output the amount of
+                            data returned in buffer
+  @param Buffer             The buffer to return the data into
+
+  @retval EFI_SUCCESS        The data were read successfully
+  @retval EFI_DEVICE_ERROR   The device reported an error
+  @retval EFI_TIMEOUT        The read operation was stopped due to timeout
+
+**/
+EFI_STATUS
+EFIAPI
+SerialRead (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  IN OUT UINTN               *BufferSize,
+  OUT VOID                   *Buffer
+  )
+{
+  SERIAL_DEV  *SerialDevice;
+  UINT32      Index;
+  UINT8       *CharBuffer;
+  UINTN       Elapsed;
+  EFI_STATUS  Status;
+  EFI_TPL     Tpl;
+
+  SerialDevice  = SERIAL_DEV_FROM_THIS (This);
+  Elapsed       = 0;
+
+  if (*BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Tpl     = gBS->RaiseTPL (TPL_NOTIFY);
+
+  Status  = SerialReceiveTransmit (SerialDevice);
+
+  if (EFI_ERROR (Status)) {
+    *BufferSize = 0;
+
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE,
+      EFI_P_EC_INPUT_ERROR | EFI_PERIPHERAL_SERIAL_PORT,
+      SerialDevice->DevicePath
+      );
+
+    gBS->RestoreTPL (Tpl);
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  CharBuffer = (UINT8 *) Buffer;
+  for (Index = 0; Index < *BufferSize; Index++) {
+    while (SerialFifoRemove (&SerialDevice->Receive, &(CharBuffer[Index])) != EFI_SUCCESS) {
+      //
+      //  Unsuccessful read so check if timeout has expired, if not,
+      //  stall for a bit, increment time elapsed, and try again
+      //  Need this time out to get conspliter to work.
+      //
+      if (Elapsed >= This->Mode->Timeout) {
+        *BufferSize = Index;
+        gBS->RestoreTPL (Tpl);
+        return EFI_TIMEOUT;
+      }
+
+      gBS->Stall (TIMEOUT_STALL_INTERVAL);
+      Elapsed += TIMEOUT_STALL_INTERVAL;
+
+      Status = SerialReceiveTransmit (SerialDevice);
+      if (Status == EFI_DEVICE_ERROR) {
+        *BufferSize = Index;
+        gBS->RestoreTPL (Tpl);
+        return EFI_DEVICE_ERROR;
+      }
+    }
+    //
+    //  Successful read so reset timeout
+    //
+    Elapsed = 0;
+  }
+
+  SerialReceiveTransmit (SerialDevice);
+
+  gBS->RestoreTPL (Tpl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Use scratchpad register to test if this serial port is present.
+
+  @param SerialDevice   Pointer to serial device structure
+
+  @return if this serial port is present
+**/
+BOOLEAN
+SerialPresent (
+  IN SERIAL_DEV *SerialDevice
+  )
+
+{
+  UINT8   Temp;
+  BOOLEAN Status;
+
+  Status = TRUE;
+
+  //
+  // Save SCR reg
+  //
+  Temp = READ_SCR (SerialDevice);
+  WRITE_SCR (SerialDevice, 0xAA);
+
+  if (READ_SCR (SerialDevice) != 0xAA) {
+    Status = FALSE;
+  }
+
+  WRITE_SCR (SerialDevice, 0x55);
+
+  if (READ_SCR (SerialDevice) != 0x55) {
+    Status = FALSE;
+  }
+  //
+  // Restore SCR
+  //
+  WRITE_SCR (SerialDevice, Temp);
+  return Status;
+}
+
+/**
+  Read serial port.
+
+  @param SerialDev     Pointer to serial device
+  @param Offset        Offset in register group
+
+  @return Data read from serial port
+
+**/
+UINT8
+SerialReadRegister (
+  IN SERIAL_DEV                            *SerialDev,
+  IN UINT32                                Offset
+  )
+{
+  UINT8                                    Data;
+  EFI_STATUS                               Status;
+
+  if (SerialDev->PciDeviceInfo == NULL) {
+    return IoRead8 ((UINTN) SerialDev->BaseAddress + Offset * SerialDev->RegisterStride);
+  } else {
+    if (SerialDev->MmioAccess) {
+      Status = SerialDev->PciDeviceInfo->PciIo->Mem.Read (SerialDev->PciDeviceInfo->PciIo, EfiPciIoWidthUint8, EFI_PCI_IO_PASS_THROUGH_BAR,
+                                                          SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, 1, &Data);
+    } else {
+      Status = SerialDev->PciDeviceInfo->PciIo->Io.Read (SerialDev->PciDeviceInfo->PciIo, EfiPciIoWidthUint8, EFI_PCI_IO_PASS_THROUGH_BAR,
+                                                         SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, 1, &Data);
+    }
+    ASSERT_EFI_ERROR (Status);
+    return Data;
+  }
+}
+
+/**
+  Write serial port.
+
+  @param  SerialDev     Pointer to serial device
+  @param  Offset        Offset in register group
+  @param  Data          data which is to be written to some serial port register
+**/
+VOID
+SerialWriteRegister (
+  IN SERIAL_DEV                            *SerialDev,
+  IN UINT32                                Offset,
+  IN UINT8                                 Data
+  )
+{
+  EFI_STATUS                               Status;
+
+  if (SerialDev->PciDeviceInfo == NULL) {
+    IoWrite8 ((UINTN) SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, Data);
+  } else {
+    if (SerialDev->MmioAccess) {
+      Status = SerialDev->PciDeviceInfo->PciIo->Mem.Write (SerialDev->PciDeviceInfo->PciIo, EfiPciIoWidthUint8, EFI_PCI_IO_PASS_THROUGH_BAR,
+                                                           SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, 1, &Data);
+    } else {
+      Status = SerialDev->PciDeviceInfo->PciIo->Io.Write (SerialDev->PciDeviceInfo->PciIo, EfiPciIoWidthUint8, EFI_PCI_IO_PASS_THROUGH_BAR,
+                                                          SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, 1, &Data);
+    }
+    ASSERT_EFI_ERROR (Status);
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/ComponentName.c
new file mode 100644
index 0000000000..1e2db1a876
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/ComponentName.c
@@ -0,0 +1,225 @@
+/** @file
+  UfsHcDxe driver produces EFI_UFS_HOST_CONTROLLER_PROTOCOL. The upper layer module
+  uses it to query the MMIO base address of the UFS host controller.
+
+  Copyright (c) 2014, 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 "UfsPciHcDxe.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL gUfsHcComponentName = {
+  UfsHcComponentNameGetDriverName,
+  UfsHcComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUfsHcComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UfsHcComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UfsHcComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUfsHcDriverNameTable[] = {
+  {
+    "eng;en",
+    L"Universal Flash Storage (UFS) Pci Host Controller Driver"
+  },
+  { 
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUfsHcControllerNameTable[] = {
+  {
+    "eng;en",
+    L"Universal Flash Storage (UFS) Pci Host Controller"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL       *This,
+  IN  CHAR8                             *Language,
+  OUT CHAR16                            **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUfsHcDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUfsHcComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+
+  if (Language == NULL || ControllerName == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing Controller Handle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gUfsHcDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUfsHcControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUfsHcComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.c b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.c
new file mode 100644
index 0000000000..23ebbf108f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.c
@@ -0,0 +1,818 @@
+/** @file
+  UfsHcDxe driver is used to provide platform-dependent info, mainly UFS host controller
+  MMIO base, to upper layer UFS drivers.
+
+  Copyright (c) 2014 - 2015, 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 "UfsPciHcDxe.h"
+
+//
+// NVM Express Driver Binding Protocol Instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gUfsHcDriverBinding = {
+  UfsHcDriverBindingSupported,
+  UfsHcDriverBindingStart,
+  UfsHcDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// Template for Ufs host controller private data.
+//
+UFS_HOST_CONTROLLER_PRIVATE_DATA gUfsHcTemplate = {
+  UFS_HC_PRIVATE_DATA_SIGNATURE,  // Signature
+  NULL,                           // Handle
+  {                               // UfsHcProtocol
+    UfsHcGetMmioBar,
+    UfsHcAllocateBuffer,
+    UfsHcFreeBuffer,
+    UfsHcMap,
+    UfsHcUnmap,
+    UfsHcFlush,
+    UfsHcMmioRead,
+    UfsHcMmioWrite
+  },
+  NULL,                           // PciIo
+  0,                              // BarIndex
+  0                               // PciAttributes
+};
+
+/**
+  Get the MMIO base of the UFS host controller.
+
+  @param[in]   This             A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param[out]  MmioBar          The MMIO base address of UFS host controller.
+
+  @retval EFI_SUCCESS           The operation succeeds.
+  @retval others                The operation fails.
+**/
+EFI_STATUS
+EFIAPI
+UfsHcGetMmioBar (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+     OUT UINTN                              *MmioBar
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+  UINT8                             BarIndex;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *BarDesc;
+
+  if ((This == NULL) || (MmioBar == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BarDesc  = NULL;
+  Private  = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo    = Private->PciIo;
+  BarIndex = Private->BarIndex;
+
+  Status = PciIo->GetBarAttributes (
+                    PciIo,
+                    BarIndex,
+                    NULL,
+                    (VOID**) &BarDesc
+                    );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  *MmioBar = (UINTN)BarDesc->AddrRangeMin;
+
+  FreePool (BarDesc);
+
+  return Status;
+}
+
+/**                                                                 
+  Provides the UFS controller-specific addresses needed to access system memory.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @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 UFS 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 UFS controller to use to
+                                access the hosts HostAddress.                                        
+  @param  Mapping               A resulting value to pass to Unmap().
+                                  
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.                                
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMap (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_OPERATION  Operation,
+  IN     VOID                                 *HostAddress,
+  IN OUT UINTN                                *NumberOfBytes,
+     OUT EFI_PHYSICAL_ADDRESS                 *DeviceAddress,
+     OUT VOID                                 **Mapping
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  if ((This == NULL) || (HostAddress == NULL) || (NumberOfBytes == NULL) || (DeviceAddress == NULL) || (Mapping == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->Map (PciIo, (EFI_PCI_IO_PROTOCOL_OPERATION)Operation, HostAddress, NumberOfBytes, DeviceAddress, Mapping);
+  return Status;
+}
+
+/**                                                                 
+  Completes the Map() operation and releases any corresponding resources.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.                                      
+  @param  Mapping               The mapping value returned from Map().
+                                  
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcUnmap (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+  IN  VOID                               *Mapping
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  if ((This == NULL) || (Mapping == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->Unmap (PciIo, Mapping);
+  return Status;
+}
+
+/**                                                                 
+  Allocates pages that are suitable for an EfiUfsHcOperationBusMasterCommonBuffer
+  mapping.                                                                       
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.                               
+  @param  Pages                 The number of pages to allocate.                                
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.                                        
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.                     
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.  
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcAllocateBuffer (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+  IN     EFI_ALLOCATE_TYPE                  Type,
+  IN     EFI_MEMORY_TYPE                    MemoryType,
+  IN     UINTN                              Pages,
+     OUT VOID                               **HostAddress,
+  IN     UINT64                             Attributes
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  if ((This == NULL) || (HostAddress == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->AllocateBuffer (PciIo, Type, MemoryType, Pages, HostAddress, Attributes);
+  return Status;
+}
+
+/**                                                                 
+  Frees memory that was allocated with AllocateBuffer().
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+  @param  Pages                 The number of pages to free.                                
+  @param  HostAddress           The base system memory address of the allocated range.                                    
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+                                     
+**/
+EFI_STATUS
+EFIAPI
+UfsHcFreeBuffer (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+  IN  UINTN                              Pages,
+  IN  VOID                               *HostAddress
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  if ((This == NULL) || (HostAddress == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->FreeBuffer (PciIo, Pages, HostAddress);
+  return Status;
+}
+
+/**                                                                 
+  Flushes all posted write transactions from the UFS bus to attached UFS device.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+                                  
+  @retval EFI_SUCCESS           The posted write transactions were flushed from the UFS bus
+                                to attached UFS device.                                      
+  @retval EFI_DEVICE_ERROR      The posted write transactions were not flushed from the UFS
+                                bus to attached UFS device due to a hardware error.                           
+                                     
+**/
+EFI_STATUS
+EFIAPI
+UfsHcFlush (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->Flush (PciIo);
+  return Status;
+}
+
+/**                                                                 
+  Enable a UFS bus driver to access UFS MMIO registers in the UFS Host Controller memory space.
+
+  @param  This                  A pointer to the EDKII_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the UFS Host Controller MMIO space to start the
+                                memory operation.
+  @param  Count                 The number of memory operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results.
+                                For write operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the UFS host controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the UFS Host Controller memory space.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMmioRead (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL        *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                    Offset,
+  IN     UINTN                                     Count,
+  IN OUT VOID                                      *Buffer
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+  UINT8                             BarIndex;
+
+  Private  = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo    = Private->PciIo;
+  BarIndex = Private->BarIndex;
+
+  Status   = PciIo->Mem.Read (PciIo, (EFI_PCI_IO_PROTOCOL_WIDTH)Width, BarIndex, Offset, Count, Buffer);
+
+  return Status;
+}
+
+/**                                                                 
+  Enable a UFS bus driver to access UFS MMIO registers in the UFS Host Controller memory space.
+
+  @param  This                  A pointer to the EDKII_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the UFS Host Controller MMIO space to start the
+                                memory operation.
+  @param  Count                 The number of memory operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results.
+                                For write operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the UFS host controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the UFS Host Controller memory space.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMmioWrite (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL        *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                    Offset,
+  IN     UINTN                                     Count,
+  IN OUT VOID                                      *Buffer
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+  UINT8                             BarIndex;
+
+  Private  = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo    = Private->PciIo;
+  BarIndex = Private->BarIndex;
+
+  Status   = PciIo->Mem.Write (PciIo, (EFI_PCI_IO_PROTOCOL_WIDTH)Width, BarIndex, Offset, Count, Buffer);
+
+  return Status;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  BOOLEAN                   UfsHcFound;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  PCI_TYPE00                PciData;
+
+  PciIo            = NULL;
+  ParentDevicePath = NULL;
+  UfsHcFound       = FALSE;
+
+  //
+  // UfsHcDxe is a device driver, and should ingore the
+  // "RemainingDevicePath" according to EFI spec
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID *) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+  //
+  // Close the protocol because we don't use it here
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiDevicePathProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Now test the EfiPciIoProtocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Now further check the PCI header: Base class (offset 0x0B) and
+  // Sub Class (offset 0x0A). This controller should be an UFS controller
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        0,
+                        sizeof (PciData),
+                        &PciData
+                        );
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiPciIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Since we already got the PciData, we can close protocol to avoid to carry it on for multiple exit points.
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiPciIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Examine UFS Host Controller PCI Configuration table fields
+  //
+  if (PciData.Hdr.ClassCode[2] == PCI_CLASS_MASS_STORAGE) {
+    if (PciData.Hdr.ClassCode[1] == 0x09 ) { //UFS Controller Subclass
+      UfsHcFound = TRUE;
+    }
+  }
+
+  if (!UfsHcFound) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return Status;
+}
+
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  UINT64                            Supports;
+  UINT8                             BarIndex;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *BarDesc;
+
+  PciIo    = NULL;
+  Private  = NULL;
+  Supports = 0;
+  BarDesc  = NULL;
+
+  //
+  // Now test and open the EfiPciIoProtocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  //
+  // Status == 0 - A normal execution flow, SUCCESS and the program proceeds.
+  // Status == ALREADY_STARTED - A non-zero Status code returned. It indicates
+  //           that the protocol has been opened and should be treated as a
+  //           normal condition and the program proceeds. The Protocol will not
+  //           opened 'again' by this call.
+  // Status != ALREADY_STARTED - Error status, terminate program execution
+  //
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+
+  Private = AllocateCopyPool (sizeof (UFS_HOST_CONTROLLER_PRIVATE_DATA), &gUfsHcTemplate);
+  if (Private == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  Private->PciIo = PciIo;
+
+  for (BarIndex = 0; BarIndex < PCI_MAX_BAR; BarIndex++) {
+    Status = PciIo->GetBarAttributes (
+                      PciIo,
+                      BarIndex,
+                      NULL,
+                      (VOID**) &BarDesc
+                      );
+    if (Status == EFI_UNSUPPORTED) {
+      continue;
+    } else if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    if (BarDesc->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+      Private->BarIndex = BarIndex;
+      FreePool (BarDesc);
+      break;
+    }
+
+    FreePool (BarDesc);
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &Private->PciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status    = PciIo->Attributes (
+                         PciIo,
+                         EfiPciIoAttributeOperationEnable,
+                         Supports,
+                         NULL
+                         );
+  } else {
+    goto Done;
+  }
+
+  ///
+  /// Install UFS_HOST_CONTROLLER protocol
+  ///
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEdkiiUfsHostControllerProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  (VOID*)&(Private->UfsHc)
+                  );
+
+Done:
+  if (EFI_ERROR (Status)) {
+    if ((Private != NULL) && (Private->PciAttributes != 0)) {
+      //
+      // Restore original PCI attributes
+      //
+      Status = PciIo->Attributes (
+                        PciIo,
+                        EfiPciIoAttributeOperationSet,
+                        Private->PciAttributes,
+                        NULL
+                        );
+      ASSERT_EFI_ERROR (Status);
+    }
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiPciIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    if (Private != NULL) {
+      FreePool (Private);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                          Status;
+  UFS_HOST_CONTROLLER_PRIVATE_DATA    *Private;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL  *UfsHc;
+
+  ///
+  /// Get private data
+  ///
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEdkiiUfsHostControllerProtocolGuid,
+                  (VOID **) &UfsHc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (UfsHc);
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEdkiiUfsHostControllerProtocolGuid,
+                  &(Private->UfsHc)
+                  );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Restore original PCI attributes
+    //
+    Status = Private->PciIo->Attributes (
+                               Private->PciIo,
+                               EfiPciIoAttributeOperationSet,
+                               Private->PciAttributes,
+                               NULL
+                               );
+    ASSERT_EFI_ERROR (Status);
+
+    //
+    // Close protocols opened by UFS host controller driver
+    //
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiPciIoProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+
+    FreePool (Private);
+  }
+
+  return Status;
+}
+
+/**
+  The entry point for UFS host controller driver, used to install this driver on the ImageHandle.
+
+  @param[in]  ImageHandle   The firmware allocated handle for this driver image.
+  @param[in]  SystemTable   Pointer to the EFI system table.
+
+  @retval EFI_SUCCESS   Driver loaded.
+  @retval other         Driver not loaded.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverEntry (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS           Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUfsHcDriverBinding,
+             ImageHandle,
+             &gUfsHcComponentName,
+             &gUfsHcComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.h b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.h
new file mode 100644
index 0000000000..89f19feb7a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.h
@@ -0,0 +1,511 @@
+/** @file
+  UfsHcDxe driver is used to provide platform-dependent info, mainly UFS host controller
+  MMIO base, to upper layer UFS drivers.
+
+  Copyright (c) 2014 - 2015, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_UFS_HOST_CONTROLLER_H_
+#define _EFI_UFS_HOST_CONTROLLER_H_
+
+#include <Uefi.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/Acpi.h>
+
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/UfsHostController.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+
+extern EFI_DRIVER_BINDING_PROTOCOL                gUfsHcDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL                gUfsHcComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL               gUfsHcComponentName2;
+
+//
+// Unique signature for private data structure.
+//
+#define UFS_HC_PRIVATE_DATA_SIGNATURE             SIGNATURE_32 ('U','F','S','H')
+
+typedef struct _UFS_HOST_CONTROLLER_PRIVATE_DATA  UFS_HOST_CONTROLLER_PRIVATE_DATA;
+
+//
+// Nvme private data structure.
+//
+struct _UFS_HOST_CONTROLLER_PRIVATE_DATA {
+  UINT32                             Signature;
+  EFI_HANDLE                         Handle;
+
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL UfsHc;
+  EFI_PCI_IO_PROTOCOL                *PciIo;  
+  UINT8                              BarIndex;
+  UINT64                             PciAttributes;
+};
+
+#define UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC(a) \
+  CR (a, \
+      UFS_HOST_CONTROLLER_PRIVATE_DATA, \
+      UfsHc, \
+      UFS_HC_PRIVATE_DATA_SIGNATURE \
+      )
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Get the MMIO base of the UFS host controller.
+
+  @param[in]   This             A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param[out]  MmioBar          The MMIO base address of UFS host controller.
+
+  @retval EFI_SUCCESS           The operation succeeds.
+  @retval others                The operation fails.
+**/
+EFI_STATUS
+EFIAPI
+UfsHcGetMmioBar (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+     OUT UINTN                              *MmioBar
+  );
+
+/**                                                                 
+  Provides the UFS controller-specific addresses needed to access system memory.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @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 UFS 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 UFS controller to use to
+                                access the hosts HostAddress.                                        
+  @param  Mapping               A resulting value to pass to Unmap().
+                                  
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.                                
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMap (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_OPERATION  Operation,
+  IN     VOID                                 *HostAddress,
+  IN OUT UINTN                                *NumberOfBytes,
+     OUT EFI_PHYSICAL_ADDRESS                 *DeviceAddress,
+     OUT VOID                                 **Mapping
+  );
+
+/**                                                                 
+  Completes the Map() operation and releases any corresponding resources.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.                                      
+  @param  Mapping               The mapping value returned from Map().
+                                  
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcUnmap (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This,
+  IN  VOID                                 *Mapping
+  );
+
+/**                                                                 
+  Allocates pages that are suitable for an EfiUfsHcOperationBusMasterCommonBuffer
+  mapping.                                                                       
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.                               
+  @param  Pages                 The number of pages to allocate.                                
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.                                        
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.                     
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.  
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcAllocateBuffer (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+  IN     EFI_ALLOCATE_TYPE                  Type,
+  IN     EFI_MEMORY_TYPE                    MemoryType,
+  IN     UINTN                              Pages,
+     OUT VOID                               **HostAddress,
+  IN     UINT64                             Attributes
+  );
+
+/**                                                                 
+  Frees memory that was allocated with AllocateBuffer().
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+  @param  Pages                 The number of pages to free.                                
+  @param  HostAddress           The base system memory address of the allocated range.                                    
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+                                     
+**/
+EFI_STATUS
+EFIAPI
+UfsHcFreeBuffer (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL    *This,
+  IN  UINTN                                 Pages,
+  IN  VOID                                  *HostAddress
+  );
+
+/**                                                                 
+  Flushes all posted write transactions from the UFS bus to attached UFS device.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+                                  
+  @retval EFI_SUCCESS           The posted write transactions were flushed from the UFS bus
+                                to attached UFS device.                                      
+  @retval EFI_DEVICE_ERROR      The posted write transactions were not flushed from the UFS
+                                bus to attached UFS device due to a hardware error.                           
+                                     
+**/
+EFI_STATUS
+EFIAPI
+UfsHcFlush (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This
+  );
+
+/**                                                                 
+  Enable a UFS bus driver to access UFS MMIO registers in the UFS Host Controller memory space.
+
+  @param  This                  A pointer to the EDKII_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the UFS Host Controller MMIO space to start the
+                                memory operation.
+  @param  Count                 The number of memory operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results.
+                                For write operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the UFS host controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the UFS Host Controller memory space.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMmioRead (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL        *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                    Offset,
+  IN     UINTN                                     Count,
+  IN OUT VOID                                      *Buffer
+  );
+
+/**                                                                 
+  Enable a UFS bus driver to access UFS MMIO registers in the UFS Host Controller memory space.
+
+  @param  This                  A pointer to the EDKII_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the UFS Host Controller MMIO space to start the
+                                memory operation.
+  @param  Count                 The number of memory operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results.
+                                For write operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the UFS host controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the UFS Host Controller memory space.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMmioWrite (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL        *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                    Offset,
+  IN     UINTN                                     Count,
+  IN OUT VOID                                      *Buffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.inf b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.inf
new file mode 100644
index 0000000000..c1ce9ea851
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.inf
@@ -0,0 +1,56 @@
+## @file
+#  Component Description File For Universal Flash Storage Pci Host Controller Module.
+#
+#  Copyright (c) 2014 - 2015, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php.
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UfsPciHcDxe
+  MODULE_UNI_FILE                = UfsPciHcDxe.uni
+  FILE_GUID                      = AF43E178-C2E9-4712-A7CD-08BFDAC7482C
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 0.9
+  ENTRY_POINT                    = UfsHcDriverEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUfsHcDriverBinding
+#  COMPONENT_NAME                =  gUfsHcComponentName
+#  COMPONENT_NAME2               =  gUfsHcComponentName2
+
+[Sources]
+  ComponentName.c
+  UfsPciHcDxe.c
+  UfsPciHcDxe.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  DevicePathLib
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  UefiLib
+
+[Protocols]
+  gEfiPciIoProtocolGuid                       ## TO_START
+  gEfiDevicePathProtocolGuid                  ## TO_START
+  gEdkiiUfsHostControllerProtocolGuid         ## BY_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UfsPciHcDxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.uni b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.uni
new file mode 100644
index 0000000000..5b3a137d80
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxeExtra.uni
new file mode 100644
index 0000000000..fb359fd615
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.c b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.c
new file mode 100644
index 0000000000..36325641e7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.c
@@ -0,0 +1,152 @@
+/** @file
+  UfsPciHcPei driver is used to provide platform-dependent info, mainly UFS host controller
+  MMIO base, to upper layer UFS drivers.
+
+  Copyright (c) 2014, 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 "UfsPciHcPei.h"
+
+EDKII_UFS_HOST_CONTROLLER_PPI  mUfsHostControllerPpi = { GetUfsHcMmioBar };
+
+EFI_PEI_PPI_DESCRIPTOR   mPpiList = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEdkiiPeiUfsHostControllerPpiGuid,
+  &mUfsHostControllerPpi
+};
+
+/**
+  Get the MMIO base address of UFS host controller.
+
+  @param[in]  This               The protocol instance pointer.
+  @param[in]  ControllerId       The ID of the UFS host controller.
+  @param[out] MmioBar            Pointer to the UFS host controller MMIO base address.
+
+  @retval EFI_SUCCESS            The operation succeeds.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+GetUfsHcMmioBar (
+  IN     EDKII_UFS_HOST_CONTROLLER_PPI *This,
+  IN     UINT8                         ControllerId,
+     OUT UINTN                         *MmioBar
+  )
+{
+  UFS_HC_PEI_PRIVATE_DATA  *Private;
+
+  if ((This == NULL) || (MmioBar == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HC_PEI_PRIVATE_DATA_FROM_THIS (This);
+
+  if (ControllerId >= Private->TotalUfsHcs) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  *MmioBar = (UINTN)Private->UfsHcPciAddr[ControllerId];
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The user code starts with this function.
+  
+  @param  FileHandle             Handle of the file being invoked.
+  @param  PeiServices            Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            The driver is successfully initialized.
+  @retval Others                 Can't initialize the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeUfsHcPeim (
+  IN EFI_PEI_FILE_HANDLE       FileHandle,
+  IN CONST EFI_PEI_SERVICES    **PeiServices
+  )
+{
+  EFI_BOOT_MODE            BootMode;
+  EFI_STATUS               Status;
+  UINT16                   Bus;
+  UINT16                   Device;
+  UINT16                   Function;
+  UINT32                   Size;
+  UINT8                    SubClass;
+  UINT8                    BaseClass;
+  UFS_HC_PEI_PRIVATE_DATA  *Private;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  Status = PeiServicesGetBootMode (&BootMode);
+  ///
+  /// We do not export this in S3 boot path, because it is only for recovery.
+  ///
+  if (BootMode == BOOT_ON_S3_RESUME) {
+    return EFI_SUCCESS;
+  }
+
+  Private = (UFS_HC_PEI_PRIVATE_DATA *) AllocateZeroPool (sizeof (UFS_HC_PEI_PRIVATE_DATA));
+  if (Private == NULL) {
+    DEBUG ((EFI_D_ERROR, "Failed to allocate memory for UFS_HC_PEI_PRIVATE_DATA! \n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Private->Signature            = UFS_HC_PEI_SIGNATURE;
+  Private->UfsHostControllerPpi = mUfsHostControllerPpi;
+  Private->PpiList              = mPpiList;
+  Private->PpiList.Ppi          = &Private->UfsHostControllerPpi;
+
+  for (Bus = 0; Bus < 256; Bus++) {
+    for (Device = 0; Device < 32; Device++) {
+      for (Function = 0; Function < 8; Function++) {
+        SubClass  = PciRead8 (PCI_LIB_ADDRESS (Bus, Device, Function, 0x0A));
+        BaseClass = PciRead8 (PCI_LIB_ADDRESS (Bus, Device, Function, 0x0B));
+
+        if ((SubClass == 0x09) && (BaseClass == PCI_CLASS_MASS_STORAGE)) {
+          //
+          // Get the Ufs Pci host controller's MMIO region size.
+          //
+          PciAnd16 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_COMMAND_OFFSET), (UINT16)~(EFI_PCI_COMMAND_BUS_MASTER | EFI_PCI_COMMAND_MEMORY_SPACE));
+          PciWrite32 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_BASE_ADDRESSREG_OFFSET), 0xFFFFFFFF);
+          Size = PciRead32 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_BASE_ADDRESSREG_OFFSET));
+          //
+          // Assign resource to the Ufs Pci host controller's MMIO BAR.
+          // Enable the Ufs Pci host controller by setting BME and MSE bits of PCI_CMD register.
+          //
+          PciWrite32 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_BASE_ADDRESSREG_OFFSET), (UINT32)(PcdGet32 (PcdUfsPciHostControllerMmioBase) + Size * Private->TotalUfsHcs));
+          PciOr16 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_COMMAND_OFFSET), (EFI_PCI_COMMAND_BUS_MASTER | EFI_PCI_COMMAND_MEMORY_SPACE));
+          //
+          // Record the allocated Mmio base address.
+          //
+          Private->UfsHcPciAddr[Private->TotalUfsHcs] = PcdGet32 (PcdUfsPciHostControllerMmioBase) + Size * Private->TotalUfsHcs;
+          Private->TotalUfsHcs++;
+          ASSERT (Private->TotalUfsHcs < MAX_UFS_HCS);
+        }
+      }
+    }
+  }
+
+  ///
+  /// Install Ufs Host Controller PPI
+  ///
+  Status = PeiServicesInstallPpi (&Private->PpiList);
+
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.h b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.h
new file mode 100644
index 0000000000..2405dd76d8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.h
@@ -0,0 +1,62 @@
+/** @file
+  Copyright (c) 2014, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _UFS_PCI_HOST_CONTROLLER_PEI_H_
+#define _UFS_PCI_HOST_CONTROLLER_PEI_H_
+
+#include <PiPei.h>
+
+#include <Ppi/MasterBootMode.h>
+#include <Ppi/UfsHostController.h>
+
+#include <IndustryStandard/Pci.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PciLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+#define UFS_HC_PEI_SIGNATURE    SIGNATURE_32 ('U', 'F', 'S', 'P')
+#define MAX_UFS_HCS             8
+
+typedef struct {
+  UINTN                         Signature;
+  EDKII_UFS_HOST_CONTROLLER_PPI UfsHostControllerPpi;
+  EFI_PEI_PPI_DESCRIPTOR        PpiList;
+  UINTN                         TotalUfsHcs;
+  UINTN                         UfsHcPciAddr[MAX_UFS_HCS];
+} UFS_HC_PEI_PRIVATE_DATA;
+
+#define UFS_HC_PEI_PRIVATE_DATA_FROM_THIS(a)  CR (a, UFS_HC_PEI_PRIVATE_DATA, UfsHostControllerPpi, UFS_HC_PEI_SIGNATURE)
+
+/**
+  Get the MMIO base address of UFS host controller.
+
+  @param[in]  This               The protocol instance pointer.
+  @param[in]  ControllerId       The ID of the UFS host controller.
+  @param[out] MmioBar            Pointer to the UFS host controller MMIO base address.
+
+  @retval EFI_SUCCESS            The operation succeeds.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+GetUfsHcMmioBar (
+  IN     EDKII_UFS_HOST_CONTROLLER_PPI *This,
+  IN     UINT8                         ControllerId,
+     OUT UINTN                         *MmioBar
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.inf b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.inf
new file mode 100644
index 0000000000..b16935628c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.inf
@@ -0,0 +1,56 @@
+## @file
+#  Component Description File For Universal Flash Storage Pci Host Controller Pei Module.
+#
+#  Copyright (c) 2014 - 2015, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php.
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UfsPciHcPei
+  MODULE_UNI_FILE                = UfsPciHcPei.uni
+  FILE_GUID                      = 905DC1AD-C44D-4965-98AC-B6B4444BFD65
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 0.9
+
+  ENTRY_POINT                    = InitializeUfsHcPeim
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  UfsPciHcPei.c
+  UfsPciHcPei.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PciLib
+  DebugLib
+  PeiServicesLib
+  MemoryAllocationLib
+  PeimEntryPoint
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUfsPciHostControllerMmioBase   ## CONSUMES
+
+[Ppis]
+  gEdkiiPeiUfsHostControllerPpiGuid         ## PRODUCES
+
+[Depex]
+  gEfiPeiMasterBootModePpiGuid AND gEfiPeiMemoryDiscoveredPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UfsPciHcPeiExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.uni b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.uni
new file mode 100644
index 0000000000..7b7ea3af7b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPeiExtra.uni
new file mode 100644
index 0000000000..4487eb968f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.c
new file mode 100644
index 0000000000..ad2ba42b55
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.c
@@ -0,0 +1,231 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for UHCI driver.
+
+Copyright (c) 2004 - 2011, 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 "Uhci.h"
+
+
+//
+// EFI Component Name Protocol
+//
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUhciComponentName = {
+  UhciComponentNameGetDriverName,
+  UhciComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUhciComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UhciComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UhciComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUhciDriverNameTable[] = {
+  { "eng;en", L"Usb Uhci Driver" },
+  { NULL, NULL }
+};
+
+
+//
+// EFI Component Name Functions
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUhciDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUhciComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS           Status;
+  USB_HC_DEV           *UhciDev;
+  EFI_USB2_HC_PROTOCOL *Usb2Hc;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gUhciDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  gUhciDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UhciDev = UHC_FROM_USB2_HC_PROTO (Usb2Hc);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           UhciDev->CtrlNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUhciComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.h
new file mode 100644
index 0000000000..fd19a21ec6
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.h
@@ -0,0 +1,145 @@
+/** @file
+
+  This file contains the delarations for componet name routines.
+
+Copyright (c) 2008 - 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _COMPONENT_NAME_H_
+#define _COMPONENT_NAME_H_
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.c
new file mode 100644
index 0000000000..179e36c3a2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.c
@@ -0,0 +1,1889 @@
+/** @file
+
+  The UHCI driver model and HC protocol routines.
+
+Copyright (c) 2004 - 2015, 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 "Uhci.h"
+
+
+EFI_DRIVER_BINDING_PROTOCOL gUhciDriverBinding = {
+  UhciDriverBindingSupported,
+  UhciDriverBindingStart,
+  UhciDriverBindingStop,
+  0x20,
+  NULL,
+  NULL
+};
+
+/**
+  Provides software reset for the USB host controller according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  Attributes             A bit mask of the reset operation to perform.  See
+                                 below for a list of the supported bit mask values.
+
+  @return EFI_SUCCESS            The reset operation succeeded.
+  @return EFI_INVALID_PARAMETER  Attributes is not valid.
+  @return EFI_UNSUPPORTED        This type of reset is not currently supported.
+  @return EFI_DEVICE_ERROR       Other errors.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2Reset (
+  IN EFI_USB2_HC_PROTOCOL   *This,
+  IN UINT16                 Attributes
+  )
+{
+  USB_HC_DEV          *Uhc;
+  EFI_TPL             OldTpl;
+
+  if ((Attributes == EFI_USB_HC_RESET_GLOBAL_WITH_DEBUG) ||
+      (Attributes == EFI_USB_HC_RESET_HOST_WITH_DEBUG)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if (Uhc->DevicePath != NULL) {
+    //
+    // Report Status Code to indicate reset happens
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_IO_BUS_USB | EFI_IOB_PC_RESET),
+      Uhc->DevicePath
+      );
+  }
+
+  OldTpl  = gBS->RaiseTPL (UHCI_TPL);
+
+  switch (Attributes) {
+  case EFI_USB_HC_RESET_GLOBAL:
+    //
+    // Stop schedule and set the Global Reset bit in the command register
+    //
+    UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+    UhciSetRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_GRESET);
+
+    gBS->Stall (UHC_ROOT_PORT_RESET_STALL);
+
+    //
+    // Clear the Global Reset bit to zero.
+    //
+    UhciClearRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_GRESET);
+
+    gBS->Stall (UHC_ROOT_PORT_RECOVERY_STALL);
+    break;
+
+  case EFI_USB_HC_RESET_HOST_CONTROLLER:
+    //
+    // Stop schedule and set Host Controller Reset bit to 1
+    //
+    UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+    UhciSetRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_HCRESET);
+
+    gBS->Stall (UHC_ROOT_PORT_RECOVERY_STALL);
+    break;
+
+  default:
+    goto ON_INVAILD_PARAMETER;
+  }
+
+  //
+  // Delete all old transactions on the USB bus, then
+  // reinitialize the frame list
+  //
+  UhciFreeAllAsyncReq (Uhc);
+  UhciDestoryFrameList (Uhc);
+  UhciInitFrameList (Uhc);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+
+ON_INVAILD_PARAMETER:
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_INVALID_PARAMETER;
+}
+
+
+/**
+  Retrieves current state of the USB host controller according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Variable to receive current device state.
+
+  @return EFI_SUCCESS            The state is returned.
+  @return EFI_INVALID_PARAMETER  State is not valid.
+  @return EFI_DEVICE_ERROR       Other errors.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2GetState (
+  IN   EFI_USB2_HC_PROTOCOL   *This,
+  OUT  EFI_USB_HC_STATE       *State
+  )
+{
+  USB_HC_DEV          *Uhc;
+  UINT16              UsbSts;
+  UINT16              UsbCmd;
+
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Uhc     = UHC_FROM_USB2_HC_PROTO (This);
+
+  UsbCmd  = UhciReadReg (Uhc->PciIo, USBCMD_OFFSET);
+  UsbSts  = UhciReadReg (Uhc->PciIo, USBSTS_OFFSET);
+
+  if ((UsbCmd & USBCMD_EGSM) !=0 ) {
+    *State = EfiUsbHcStateSuspend;
+
+  } else if ((UsbSts & USBSTS_HCH) != 0) {
+    *State = EfiUsbHcStateHalt;
+
+  } else {
+    *State = EfiUsbHcStateOperational;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Sets the USB host controller to a specific state according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Indicates the state of the host controller that will
+                                 be set.
+
+  @return EFI_SUCCESS            Host controller was successfully placed in the state.
+  @return EFI_INVALID_PARAMETER  State is invalid.
+  @return EFI_DEVICE_ERROR       Failed to set the state.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2SetState (
+  IN EFI_USB2_HC_PROTOCOL    *This,
+  IN EFI_USB_HC_STATE        State
+  )
+{
+  EFI_USB_HC_STATE    CurState;
+  USB_HC_DEV          *Uhc;
+  EFI_TPL             OldTpl;
+  EFI_STATUS          Status;
+  UINT16              UsbCmd;
+
+  Uhc     = UHC_FROM_USB2_HC_PROTO (This);
+  Status  = Uhci2GetState (This, &CurState);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (CurState == State) {
+    return EFI_SUCCESS;
+  }
+
+  Status  = EFI_SUCCESS;
+  OldTpl  = gBS->RaiseTPL (UHCI_TPL);
+
+  switch (State) {
+  case EfiUsbHcStateHalt:
+    Status = UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateOperational:
+    UsbCmd = UhciReadReg (Uhc->PciIo, USBCMD_OFFSET);
+
+    if (CurState == EfiUsbHcStateHalt) {
+      //
+      // Set Run/Stop bit to 1, also set the bandwidht reclamation
+      // point to 64 bytes
+      //
+      UsbCmd |= USBCMD_RS | USBCMD_MAXP;
+      UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, UsbCmd);
+
+    } else if (CurState == EfiUsbHcStateSuspend) {
+      //
+      // If FGR(Force Global Resume) bit is 0, set it
+      //
+      if ((UsbCmd & USBCMD_FGR) == 0) {
+        UsbCmd |= USBCMD_FGR;
+        UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, UsbCmd);
+      }
+
+      //
+      // wait 20ms to let resume complete (20ms is specified by UHCI spec)
+      //
+      gBS->Stall (UHC_FORCE_GLOBAL_RESUME_STALL);
+
+      //
+      // Write FGR bit to 0 and EGSM(Enter Global Suspend Mode) bit to 0
+      //
+      UsbCmd &= ~USBCMD_FGR;
+      UsbCmd &= ~USBCMD_EGSM;
+      UsbCmd |= USBCMD_RS;
+      UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, UsbCmd);
+    }
+
+    break;
+
+  case EfiUsbHcStateSuspend:
+    Status = Uhci2SetState (This, EfiUsbHcStateHalt);
+
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto ON_EXIT;
+    }
+
+    //
+    // Set Enter Global Suspend Mode bit to 1.
+    //
+    UsbCmd = UhciReadReg (Uhc->PciIo, USBCMD_OFFSET);
+    UsbCmd |= USBCMD_EGSM;
+    UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, UsbCmd);
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Retrieves capabilities of USB host controller according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  MaxSpeed               A pointer to the max speed USB host controller
+                                 supports.
+  @param  PortNumber             A pointer to the number of root hub ports.
+  @param  Is64BitCapable         A pointer to an integer to show whether USB host
+                                 controller supports 64-bit memory addressing.
+
+  @return EFI_SUCCESS            capabilities were retrieved successfully.
+  @return EFI_INVALID_PARAMETER  MaxSpeed or PortNumber or Is64BitCapable is NULL.
+  @return EFI_DEVICE_ERROR       An error was encountered.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2GetCapability (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT UINT8                 *MaxSpeed,
+  OUT UINT8                 *PortNumber,
+  OUT UINT8                 *Is64BitCapable
+  )
+{
+  USB_HC_DEV          *Uhc;
+  UINT32              Offset;
+  UINT16              PortSC;
+  UINT32              Index;
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if ((NULL == MaxSpeed) || (NULL == PortNumber) || (NULL == Is64BitCapable)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *MaxSpeed       = EFI_USB_SPEED_FULL;
+  *Is64BitCapable = (UINT8) FALSE;
+
+  *PortNumber = 0;
+
+  for (Index = 0; Index < USB_MAX_ROOTHUB_PORT; Index++) {
+    Offset  = USBPORTSC_OFFSET + Index * 2;
+    PortSC  = UhciReadReg (Uhc->PciIo, Offset);
+
+    //
+    // Port status's bit 7 is reserved and always returns 1 if
+    // the port number is valid. Intel's UHCI (in EHCI controller)
+    // returns 0 in this bit if port number is invalid. Also, if
+    // PciIo IoRead returns error, 0xFFFF is returned to caller.
+    //
+    if (((PortSC & 0x80) == 0) || (PortSC == 0xFFFF)) {
+      break;
+    }
+    (*PortNumber)++;
+  }
+
+  Uhc->RootPorts = *PortNumber;
+
+  DEBUG ((EFI_D_INFO, "Uhci2GetCapability: %d ports\n", (UINT32)Uhc->RootPorts));
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieves the current status of a USB root hub port according to UEFI 2.0 spec.
+
+  @param  This                    A pointer to the EFI_USB2_HC_PROTOCOL.
+  @param  PortNumber              The port to get status.
+  @param  PortStatus              A pointer to the current port status bits and  port
+                                  status change bits.
+
+  @return EFI_SUCCESS             status of the USB root hub port was returned in PortStatus.
+  @return EFI_INVALID_PARAMETER   PortNumber is invalid.
+  @return EFI_DEVICE_ERROR        Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2GetRootHubPortStatus (
+  IN   EFI_USB2_HC_PROTOCOL   *This,
+  IN   UINT8                  PortNumber,
+  OUT  EFI_USB_PORT_STATUS    *PortStatus
+  )
+{
+  USB_HC_DEV          *Uhc;
+  UINT32              Offset;
+  UINT16              PortSC;
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (PortNumber >= Uhc->RootPorts) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Offset                        = USBPORTSC_OFFSET + PortNumber * 2;
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  PortSC                        = UhciReadReg (Uhc->PciIo, Offset);
+
+  if ((PortSC & USBPORTSC_CCS) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_CONNECTION;
+  }
+
+  if ((PortSC & USBPORTSC_PED) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_ENABLE;
+  }
+
+  if ((PortSC & USBPORTSC_SUSP) != 0) {
+    DEBUG ((EFI_D_INFO, "Uhci2GetRootHubPortStatus: port %d is suspended\n", PortNumber));
+    PortStatus->PortStatus |= USB_PORT_STAT_SUSPEND;
+  }
+
+  if ((PortSC & USBPORTSC_PR) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_RESET;
+  }
+
+  if ((PortSC & USBPORTSC_LSDA) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+  }
+
+  //
+  // CHC will always return one in port owner bit
+  //
+  PortStatus->PortStatus |= USB_PORT_STAT_OWNER;
+
+  if ((PortSC & USBPORTSC_CSC) != 0) {
+    PortStatus->PortChangeStatus |= USB_PORT_STAT_C_CONNECTION;
+  }
+
+  if ((PortSC & USBPORTSC_PEDC) != 0) {
+    PortStatus->PortChangeStatus |= USB_PORT_STAT_C_ENABLE;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Sets a feature for the specified root hub port according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL.
+  @param  PortNumber             Specifies the root hub port whose feature  is
+                                 requested to be set.
+  @param  PortFeature            Indicates the feature selector associated  with the
+                                 feature set request.
+
+  @return EFI_SUCCESS            PortFeature was set for the root port.
+  @return EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @return EFI_DEVICE_ERROR       Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2SetRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL    *This,
+  IN UINT8                   PortNumber,
+  IN EFI_USB_PORT_FEATURE    PortFeature
+  )
+{
+  USB_HC_DEV          *Uhc;
+  EFI_TPL             OldTpl;
+  UINT32              Offset;
+  UINT16              PortSC;
+  UINT16              Command;
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if (PortNumber >= Uhc->RootPorts) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Offset  = USBPORTSC_OFFSET + PortNumber * 2;
+
+  OldTpl  = gBS->RaiseTPL (UHCI_TPL);
+  PortSC  = UhciReadReg (Uhc->PciIo, Offset);
+
+  switch (PortFeature) {
+  case EfiUsbPortSuspend:
+    Command = UhciReadReg (Uhc->PciIo, USBCMD_OFFSET);
+    if ((Command & USBCMD_EGSM) == 0) {
+      //
+      // if global suspend is not active, can set port suspend
+      //
+      PortSC &= 0xfff5;
+      PortSC |= USBPORTSC_SUSP;
+    }
+    break;
+
+  case EfiUsbPortReset:
+    PortSC &= 0xfff5;
+    PortSC |= USBPORTSC_PR;
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // No action
+    //
+    break;
+
+  case EfiUsbPortEnable:
+    PortSC &= 0xfff5;
+    PortSC |= USBPORTSC_PED;
+    break;
+
+  default:
+    gBS->RestoreTPL (OldTpl);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhciWriteReg (Uhc->PciIo, Offset, PortSC);
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Clears a feature for the specified root hub port according to Uefi 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber             Specifies the root hub port whose feature  is
+                                 requested to be cleared.
+  @param  PortFeature            Indicates the feature selector associated with the
+                                 feature clear request.
+
+  @return EFI_SUCCESS            PortFeature was cleared for the USB root hub port.
+  @return EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @return EFI_DEVICE_ERROR       Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2ClearRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL    *This,
+  IN UINT8                   PortNumber,
+  IN EFI_USB_PORT_FEATURE    PortFeature
+  )
+{
+  USB_HC_DEV          *Uhc;
+  EFI_TPL             OldTpl;
+  UINT32              Offset;
+  UINT16              PortSC;
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if (PortNumber >= Uhc->RootPorts) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Offset  = USBPORTSC_OFFSET + PortNumber * 2;
+
+  OldTpl  = gBS->RaiseTPL (UHCI_TPL);
+  PortSC  = UhciReadReg (Uhc->PciIo, Offset);
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    PortSC &= 0xfff5;
+    PortSC &= ~USBPORTSC_PED;
+    break;
+
+  case EfiUsbPortSuspend:
+    //
+    // Cause a resume on the specified port if in suspend mode.
+    //
+    PortSC &= 0xfff5;
+    PortSC &= ~USBPORTSC_SUSP;
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // No action
+    //
+    break;
+
+  case EfiUsbPortReset:
+    PortSC &= 0xfff5;
+    PortSC &= ~USBPORTSC_PR;
+    break;
+
+  case EfiUsbPortConnectChange:
+    PortSC &= 0xfff5;
+    PortSC |= USBPORTSC_CSC;
+    break;
+
+  case EfiUsbPortEnableChange:
+    PortSC &= 0xfff5;
+    PortSC |= USBPORTSC_PEDC;
+    break;
+
+  case EfiUsbPortSuspendChange:
+    //
+    // Root hub does not support this
+    //
+    break;
+
+  case EfiUsbPortOverCurrentChange:
+    //
+    // Root hub does not support this
+    //
+    break;
+
+  case EfiUsbPortResetChange:
+    //
+    // Root hub does not support this
+    //
+    break;
+
+  default:
+    gBS->RestoreTPL (OldTpl);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhciWriteReg (Uhc->PciIo, Offset, PortSC);
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Submits control transfer to a target USB device accroding to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress          Target device address.
+  @param  DeviceSpeed            Device speed.
+  @param  MaximumPacketLength    Maximum packet size of the target endpoint.
+  @param  Request                USB device request to send.
+  @param  TransferDirection      Data direction of the Data stage in control transfer.
+  @param  Data                   Data to transmit/receive in data stage.
+  @param  DataLength             Length of the data.
+  @param  TimeOut                Maximum time, in microseconds, for transfer to complete.
+  @param  Translator             Transaction translator to be used by this device.
+  @param  TransferResult         Variable to receive the transfer result.
+
+  @return EFI_SUCCESS            The control transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES   Failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @return EFI_TIMEOUT            Failed due to timeout.
+  @return EFI_DEVICE_ERROR       Failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2ControlTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                 *This,
+  IN     UINT8                                DeviceAddress,
+  IN     UINT8                                DeviceSpeed,
+  IN     UINTN                                MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST               *Request,
+  IN     EFI_USB_DATA_DIRECTION               TransferDirection,
+  IN OUT VOID                                 *Data,
+  IN OUT UINTN                                *DataLength,
+  IN     UINTN                                TimeOut,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR   *Translator,
+  OUT    UINT32                               *TransferResult
+  )
+{
+  USB_HC_DEV              *Uhc;
+  UHCI_TD_SW              *TDs;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  UHCI_QH_RESULT          QhResult;
+  UINT8                   PktId;
+  UINT8                   *RequestPhy;
+  VOID                    *RequestMap;
+  UINT8                   *DataPhy;
+  VOID                    *DataMap;
+  BOOLEAN                 IsSlowDevice;
+  UINTN                   TransferDataLength;
+
+  Uhc         = UHC_FROM_USB2_HC_PROTO (This);
+  TDs         = NULL;
+  DataPhy     = NULL;
+  DataMap     = NULL;
+  RequestPhy  = NULL;
+  RequestMap  = NULL;
+
+  IsSlowDevice  = (BOOLEAN) ((EFI_USB_SPEED_LOW == DeviceSpeed) ? TRUE : FALSE);
+
+  //
+  // Parameters Checking
+  //
+  if (Request == NULL || TransferResult == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsSlowDevice && (MaximumPacketLength != 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8) &&  (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64)) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) && (Data == NULL || DataLength == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (TransferDirection == EfiUsbNoData) {
+    TransferDataLength = 0;
+  } else {
+    TransferDataLength = *DataLength;
+  }
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  //
+  // If errors exist that cause host controller halt,
+  // clear status then return EFI_DEVICE_ERROR.
+  //
+  UhciAckAllInterrupt (Uhc);
+
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  OldTpl = gBS->RaiseTPL (UHCI_TPL);
+
+  //
+  // Map the Request and data for bus master access,
+  // then create a list of TD for this transfer
+  //
+  Status = UhciMapUserRequest (Uhc, Request, &RequestPhy, &RequestMap);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = UhciMapUserData (Uhc, TransferDirection, Data, DataLength, &PktId, &DataPhy, &DataMap);
+
+  if (EFI_ERROR (Status)) {
+    Uhc->PciIo->Unmap (Uhc->PciIo, RequestMap);
+    goto ON_EXIT;
+  }
+
+  TDs = UhciCreateCtrlTds (
+          Uhc,
+          DeviceAddress,
+          PktId,
+          (UINT8*)Request,
+          RequestPhy,
+          (UINT8*)Data,
+          DataPhy,
+          TransferDataLength,
+          (UINT8) MaximumPacketLength,
+          IsSlowDevice
+          );
+
+  if (TDs == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto UNMAP_DATA;
+  }
+
+  //
+  // According to the speed of the end point, link
+  // the TD to corrosponding queue head, then check
+  // the execution result
+  //
+  UhciLinkTdToQh (Uhc, Uhc->CtrlQh, TDs);
+  Status = UhciExecuteTransfer (Uhc, Uhc->CtrlQh, TDs, TimeOut, IsSlowDevice, &QhResult);
+  UhciUnlinkTdFromQh (Uhc->CtrlQh, TDs);
+
+  Uhc->PciIo->Flush (Uhc->PciIo);
+
+  *TransferResult = QhResult.Result;
+
+  if (DataLength != NULL) {
+    *DataLength = QhResult.Complete;
+  }
+
+  UhciDestoryTds (Uhc, TDs);
+
+UNMAP_DATA:
+  Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+  Uhc->PciIo->Unmap (Uhc->PciIo, RequestMap);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress          Target device address.
+  @param  EndPointAddress        Endpoint number and direction.
+  @param  DeviceSpeed            Device speed.
+  @param  MaximumPacketLength    Maximum packet size of the target endpoint.
+  @param  DataBuffersNumber      Number of data buffers prepared for the transfer.
+  @param  Data                   Array of pointers to the buffers of data.
+  @param  DataLength             On input, size of the data buffer, On output,
+                                 actually transferred data size.
+  @param  DataToggle             On input, data toggle to use; On output, next data toggle.
+  @param  TimeOut                Maximum time out, in microseconds.
+  @param  Translator             A pointr to the transaction translator data.
+  @param  TransferResult         Variable to receive transfer result.
+
+  @return EFI_SUCCESS            The bulk transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES   Failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @return EFI_TIMEOUT            Failed due to timeout.
+  @return EFI_DEVICE_ERROR       Failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2BulkTransfer (
+  IN     EFI_USB2_HC_PROTOCOL               *This,
+  IN     UINT8                              DeviceAddress,
+  IN     UINT8                              EndPointAddress,
+  IN     UINT8                              DeviceSpeed,
+  IN     UINTN                              MaximumPacketLength,
+  IN     UINT8                              DataBuffersNumber,
+  IN OUT VOID                               *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN OUT UINTN                              *DataLength,
+  IN OUT UINT8                              *DataToggle,
+  IN     UINTN                              TimeOut,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR *Translator,
+  OUT    UINT32                             *TransferResult
+  )
+{
+  EFI_USB_DATA_DIRECTION  Direction;
+  EFI_TPL                 OldTpl;
+  USB_HC_DEV              *Uhc;
+  UHCI_TD_SW              *TDs;
+  UHCI_QH_SW              *BulkQh;
+  UHCI_QH_RESULT          QhResult;
+  EFI_STATUS              Status;
+  UINT8                   PktId;
+  UINT8                   *DataPhy;
+  VOID                    *DataMap;
+
+  Uhc     = UHC_FROM_USB2_HC_PROTO (This);
+  DataPhy = NULL;
+  DataMap = NULL;
+
+  if (DeviceSpeed == EFI_USB_SPEED_LOW) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DataLength == NULL) || (*DataLength == 0) || (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8) && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_OUT_OF_RESOURCES;
+
+  //
+  // If has errors that cause host controller halt,
+  // then return EFI_DEVICE_ERROR directly.
+  //
+  UhciAckAllInterrupt (Uhc);
+
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  OldTpl = gBS->RaiseTPL (UHCI_TPL);
+
+  //
+  // Map the source data buffer for bus master access,
+  // then create a list of TDs
+  //
+  if ((EndPointAddress & 0x80) != 0) {
+    Direction = EfiUsbDataIn;
+  } else {
+    Direction = EfiUsbDataOut;
+  }
+
+  Status = UhciMapUserData (Uhc, Direction, *Data, DataLength, &PktId, &DataPhy, &DataMap);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = EFI_OUT_OF_RESOURCES;
+  TDs    = UhciCreateBulkOrIntTds (
+             Uhc,
+             DeviceAddress,
+             EndPointAddress,
+             PktId,
+             (UINT8 *)*Data,
+             DataPhy,
+             *DataLength,
+             DataToggle,
+             (UINT8) MaximumPacketLength,
+             FALSE
+             );
+
+  if (TDs == NULL) {
+    Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+    goto ON_EXIT;
+  }
+
+
+  //
+  // Link the TDs to bulk queue head. According to the platfore
+  // defintion of UHCI_NO_BW_RECLAMATION, BulkQh is either configured
+  // to do full speed bandwidth reclamation or not.
+  //
+  BulkQh = Uhc->BulkQh;
+
+  UhciLinkTdToQh (Uhc, BulkQh, TDs);
+  Status = UhciExecuteTransfer (Uhc, BulkQh, TDs, TimeOut, FALSE, &QhResult);
+  UhciUnlinkTdFromQh (BulkQh, TDs);
+
+  Uhc->PciIo->Flush (Uhc->PciIo);
+
+  *TransferResult = QhResult.Result;
+  *DataToggle     = QhResult.NextToggle;
+  *DataLength     = QhResult.Complete;
+
+  UhciDestoryTds (Uhc, TDs);
+  Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Submits an asynchronous interrupt transfer to an
+  interrupt endpoint of a USB device according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress          Target device address.
+  @param  EndPointAddress        Endpoint number and direction.
+  @param  DeviceSpeed            Device speed.
+  @param  MaximumPacketLength    Maximum packet size of the target endpoint.
+  @param  IsNewTransfer          If TRUE, submit a new transfer, if FALSE cancel old transfer.
+  @param  DataToggle             On input, data toggle to use; On output, next data toggle.
+  @param  PollingInterval        Interrupt poll rate in milliseconds.
+  @param  DataLength             On input, size of the data buffer, On output,
+                                 actually transferred data size.
+  @param  Translator             A pointr to the transaction translator data.
+  @param  CallBackFunction       Function to call periodically.
+  @param  Context                User context.
+
+  @return EFI_SUCCESS            Transfer was submitted.
+  @return EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @return EFI_OUT_OF_RESOURCES   Failed due to a lack of resources.
+  @return EFI_DEVICE_ERROR       Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2AsyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL               *This,
+  IN     UINT8                              DeviceAddress,
+  IN     UINT8                              EndPointAddress,
+  IN     UINT8                              DeviceSpeed,
+  IN     UINTN                              MaximumPacketLength,
+  IN     BOOLEAN                            IsNewTransfer,
+  IN OUT UINT8                              *DataToggle,
+  IN     UINTN                              PollingInterval,
+  IN     UINTN                              DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK    CallBackFunction,
+  IN     VOID                               *Context
+  )
+{
+  USB_HC_DEV          *Uhc;
+  BOOLEAN             IsSlowDevice;
+  UHCI_QH_SW          *Qh;
+  UHCI_TD_SW          *IntTds;
+  EFI_TPL             OldTpl;
+  EFI_STATUS          Status;
+  UINT8               *DataPtr;
+  UINT8               *DataPhy;
+  UINT8               PktId;
+
+  Uhc       = UHC_FROM_USB2_HC_PROTO (This);
+  Qh        = NULL;
+  IntTds    = NULL;
+  DataPtr   = NULL;
+  DataPhy   = NULL;
+
+  IsSlowDevice  = (BOOLEAN) ((EFI_USB_SPEED_LOW == DeviceSpeed) ? TRUE : FALSE);
+
+  if ((EndPointAddress & 0x80) == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Delete Async interrupt transfer request
+  //
+  if (!IsNewTransfer) {
+    OldTpl = gBS->RaiseTPL (UHCI_TPL);
+    Status = UhciRemoveAsyncReq (Uhc, DeviceAddress, EndPointAddress, DataToggle);
+
+    gBS->RestoreTPL (OldTpl);
+    return Status;
+  }
+
+  if (PollingInterval < 1 || PollingInterval > 255) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DataLength == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If has errors that cause host controller halt,
+  // then return EFI_DEVICE_ERROR directly.
+  //
+  UhciAckAllInterrupt (Uhc);
+
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((EndPointAddress & 0x80) == 0) {
+    PktId = OUTPUT_PACKET_ID;
+  } else {
+    PktId = INPUT_PACKET_ID;
+  }
+
+  //
+  // Allocate and map source data buffer for bus master access.
+  //
+  DataPtr = UsbHcAllocateMem (Uhc->MemPool, DataLength);
+
+  if (DataPtr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DataPhy = (UINT8 *) (UINTN) UsbHcGetPciAddressForHostMem (Uhc->MemPool, DataPtr, DataLength);
+
+  OldTpl = gBS->RaiseTPL (UHCI_TPL);
+
+  Qh = UhciCreateQh (Uhc, PollingInterval);
+
+  if (Qh == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto FREE_DATA;
+  }
+
+  IntTds = UhciCreateBulkOrIntTds (
+             Uhc,
+             DeviceAddress,
+             EndPointAddress,
+             PktId,
+             DataPtr,
+             DataPhy,
+             DataLength,
+             DataToggle,
+             (UINT8) MaximumPacketLength,
+             IsSlowDevice
+             );
+
+  if (IntTds == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto DESTORY_QH;
+  }
+
+  UhciLinkTdToQh (Uhc, Qh, IntTds);
+
+  //
+  // Save QH-TD structures to async Interrupt transfer list,
+  // for monitor interrupt transfer execution routine use.
+  //
+  Status = UhciCreateAsyncReq (
+             Uhc,
+             Qh,
+             IntTds,
+             DeviceAddress,
+             EndPointAddress,
+             DataLength,
+             PollingInterval,
+             DataPtr,
+             CallBackFunction,
+             Context,
+             IsSlowDevice
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto DESTORY_QH;
+  }
+
+  UhciLinkQhToFrameList (Uhc, Qh);
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+
+DESTORY_QH:
+  UsbHcFreeMem (Uhc->MemPool, Qh, sizeof (UHCI_QH_SW));
+
+FREE_DATA:
+  UsbHcFreeMem (Uhc->MemPool, DataPtr, DataLength);
+  Uhc->PciIo->Flush (Uhc->PciIo);
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Submits synchronous interrupt transfer to an interrupt endpoint
+  of a USB device according to UEFI 2.0 spec.
+
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress          Target device address.
+  @param  EndPointAddress        Endpoint number and direction.
+  @param  DeviceSpeed            Device speed.
+  @param  MaximumPacketLength    Maximum packet size of the target endpoint.
+  @param  Data                   Array of pointers to the buffers of data.
+  @param  DataLength             On input, size of the data buffer, On output,
+                                 actually transferred data size.
+  @param  DataToggle             On input, data toggle to use; On output, next data toggle.
+  @param  TimeOut                Maximum time out, in microseconds.
+  @param  Translator             A pointr to the transaction translator data.
+  @param  TransferResult         Variable to receive transfer result.
+
+  @return EFI_SUCCESS            The transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES   Failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @return EFI_TIMEOUT            Failed due to timeout.
+  @return EFI_DEVICE_ERROR       Failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2SyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                      *This,
+  IN     UINT8                                     DeviceAddress,
+  IN     UINT8                                     EndPointAddress,
+  IN     UINT8                                     DeviceSpeed,
+  IN     UINTN                                     MaximumPacketLength,
+  IN OUT VOID                                      *Data,
+  IN OUT UINTN                                     *DataLength,
+  IN OUT UINT8                                     *DataToggle,
+  IN     UINTN                                     TimeOut,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR        *Translator,
+  OUT    UINT32                                    *TransferResult
+  )
+{
+  EFI_STATUS          Status;
+  USB_HC_DEV          *Uhc;
+  UHCI_TD_SW          *TDs;
+  UHCI_QH_RESULT      QhResult;
+  EFI_TPL             OldTpl;
+  UINT8               *DataPhy;
+  VOID                *DataMap;
+  UINT8               PktId;
+  BOOLEAN             IsSlowDevice;
+
+  Uhc     = UHC_FROM_USB2_HC_PROTO (This);
+  DataPhy = NULL;
+  DataMap = NULL;
+  TDs     = NULL;
+
+  if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IsSlowDevice  = (BOOLEAN) ((EFI_USB_SPEED_LOW == DeviceSpeed) ? TRUE : FALSE);
+
+  if ((DataLength == NULL) || (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataLength == 0) || (MaximumPacketLength > 64)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsSlowDevice && (MaximumPacketLength > 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+
+  UhciAckAllInterrupt (Uhc);
+
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    return Status;
+  }
+
+  OldTpl = gBS->RaiseTPL (UHCI_TPL);
+
+  //
+  // Map the source data buffer for bus master access.
+  // Create Tds list, then link it to the UHC's interrupt list
+  //
+  Status = UhciMapUserData (
+             Uhc,
+             EfiUsbDataIn,
+             Data,
+             DataLength,
+             &PktId,
+             &DataPhy,
+             &DataMap
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  TDs = UhciCreateBulkOrIntTds (
+          Uhc,
+          DeviceAddress,
+          EndPointAddress,
+          PktId,
+          (UINT8 *)Data,
+          DataPhy,
+          *DataLength,
+          DataToggle,
+          (UINT8) MaximumPacketLength,
+          IsSlowDevice
+          );
+
+  if (TDs == NULL) {
+    Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+
+  UhciLinkTdToQh (Uhc, Uhc->SyncIntQh, TDs);
+
+  Status = UhciExecuteTransfer (Uhc, Uhc->SyncIntQh, TDs, TimeOut, IsSlowDevice, &QhResult);
+
+  UhciUnlinkTdFromQh (Uhc->SyncIntQh, TDs);
+  Uhc->PciIo->Flush (Uhc->PciIo);
+
+  *TransferResult = QhResult.Result;
+  *DataToggle     = QhResult.NextToggle;
+  *DataLength     = QhResult.Complete;
+
+  UhciDestoryTds (Uhc, TDs);
+  Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Submits isochronous transfer to a target USB device according to UEFI 2.0 spec.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and direction.
+  @param  DeviceSpeed           Device speed.
+  @param  MaximumPacketLength   Maximum packet size of the target endpoint.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data.
+  @param  DataLength            On input, size of the data buffer, On output,
+                                actually transferred data size.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        Variable to receive transfer result.
+
+  @return EFI_UNSUPPORTED
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2IsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL               *This,
+  IN     UINT8                              DeviceAddress,
+  IN     UINT8                              EndPointAddress,
+  IN     UINT8                              DeviceSpeed,
+  IN     UINTN                              MaximumPacketLength,
+  IN     UINT8                              DataBuffersNumber,
+  IN OUT VOID                               *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                              DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR *Translator,
+  OUT    UINT32                             *TransferResult
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Submits Async isochronous transfer to a target USB device according to UEFI 2.0 spec.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and direction.
+  @param  DeviceSpeed           Device speed.
+  @param  MaximumPacketLength   Maximum packet size of the target endpoint.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data.
+  @param  DataLength            On input, size of the data buffer, On output,
+                                actually transferred data size.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  IsochronousCallBack   Function to call when the transfer complete.
+  @param  Context               Pass to the call back function as parameter.
+
+  @return EFI_UNSUPPORTED
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2AsyncIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     IsochronousCallBack,
+  IN     VOID                                *Context
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Entry point for EFI drivers.
+
+  @param  ImageHandle      EFI_HANDLE.
+  @param  SystemTable      EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS      Driver is successfully loaded.
+  @return Others           Failed.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gUhciDriverBinding,
+           ImageHandle,
+           &gUhciComponentName,
+           &gUhciComponentName2
+           );
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has UsbHcProtocol installed will be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  EFI_STATUS            OpenStatus;
+  EFI_STATUS            Status;
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  USB_CLASSC            UsbClassCReg;
+
+  //
+  // Test whether there is PCI IO Protocol attached on the controller handle.
+  //
+  OpenStatus = gBS->OpenProtocol (
+                      Controller,
+                      &gEfiPciIoProtocolGuid,
+                      (VOID **) &PciIo,
+                      This->DriverBindingHandle,
+                      Controller,
+                      EFI_OPEN_PROTOCOL_BY_DRIVER
+                      );
+
+  if (EFI_ERROR (OpenStatus)) {
+    return OpenStatus;
+  }
+
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (USB_CLASSC) / sizeof (UINT8),
+                        &UsbClassCReg
+                        );
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Test whether the controller belongs to UHCI type
+  //
+  if ((UsbClassCReg.BaseCode != PCI_CLASS_SERIAL) ||
+      (UsbClassCReg.SubClassCode != PCI_CLASS_SERIAL_USB) ||
+      (UsbClassCReg.ProgInterface != PCI_IF_UHCI)
+      ) {
+
+    Status = EFI_UNSUPPORTED;
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+
+}
+
+
+/**
+  Allocate and initialize the empty UHCI device.
+
+  @param  PciIo                  The PCIIO to use.
+  @param  DevicePath             The device path of host controller.
+  @param  OriginalPciAttributes  The original PCI attributes.
+
+  @return Allocated UHCI device. If err, return NULL.
+
+**/
+USB_HC_DEV *
+UhciAllocateDev (
+  IN EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN UINT64                    OriginalPciAttributes
+  )
+{
+  USB_HC_DEV  *Uhc;
+  EFI_STATUS  Status;
+
+  Uhc = AllocateZeroPool (sizeof (USB_HC_DEV));
+
+  if (Uhc == NULL) {
+    return NULL;
+  }
+
+  //
+  // This driver supports both USB_HC_PROTOCOL and USB2_HC_PROTOCOL.
+  // USB_HC_PROTOCOL is for EFI 1.1 backward compability.
+  //
+  Uhc->Signature                        = USB_HC_DEV_SIGNATURE;
+  Uhc->Usb2Hc.GetCapability             = Uhci2GetCapability;
+  Uhc->Usb2Hc.Reset                     = Uhci2Reset;
+  Uhc->Usb2Hc.GetState                  = Uhci2GetState;
+  Uhc->Usb2Hc.SetState                  = Uhci2SetState;
+  Uhc->Usb2Hc.ControlTransfer           = Uhci2ControlTransfer;
+  Uhc->Usb2Hc.BulkTransfer              = Uhci2BulkTransfer;
+  Uhc->Usb2Hc.AsyncInterruptTransfer    = Uhci2AsyncInterruptTransfer;
+  Uhc->Usb2Hc.SyncInterruptTransfer     = Uhci2SyncInterruptTransfer;
+  Uhc->Usb2Hc.IsochronousTransfer       = Uhci2IsochronousTransfer;
+  Uhc->Usb2Hc.AsyncIsochronousTransfer  = Uhci2AsyncIsochronousTransfer;
+  Uhc->Usb2Hc.GetRootHubPortStatus      = Uhci2GetRootHubPortStatus;
+  Uhc->Usb2Hc.SetRootHubPortFeature     = Uhci2SetRootHubPortFeature;
+  Uhc->Usb2Hc.ClearRootHubPortFeature   = Uhci2ClearRootHubPortFeature;
+  Uhc->Usb2Hc.MajorRevision             = 0x1;
+  Uhc->Usb2Hc.MinorRevision             = 0x1;
+
+  Uhc->PciIo                 = PciIo;
+  Uhc->DevicePath            = DevicePath;
+  Uhc->OriginalPciAttributes = OriginalPciAttributes;
+  Uhc->MemPool               = UsbHcInitMemPool (PciIo, TRUE, 0);
+
+  if (Uhc->MemPool == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  InitializeListHead (&Uhc->AsyncIntList);
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  UhciMonitorAsyncReqList,
+                  Uhc,
+                  &Uhc->AsyncIntMonitor
+                  );
+
+  if (EFI_ERROR (Status)) {
+    UsbHcFreeMemPool (Uhc->MemPool);
+    goto ON_ERROR;
+  }
+
+  return Uhc;
+
+ON_ERROR:
+  FreePool (Uhc);
+  return NULL;
+}
+
+
+/**
+  Free the UHCI device and release its associated resources.
+
+  @param  Uhc     The UHCI device to release.
+
+**/
+VOID
+UhciFreeDev (
+  IN USB_HC_DEV           *Uhc
+  )
+{
+  if (Uhc->AsyncIntMonitor != NULL) {
+    gBS->CloseEvent (Uhc->AsyncIntMonitor);
+  }
+
+  if (Uhc->ExitBootServiceEvent != NULL) {
+    gBS->CloseEvent (Uhc->ExitBootServiceEvent);
+  }
+  
+  if (Uhc->MemPool != NULL) {
+    UsbHcFreeMemPool (Uhc->MemPool);
+  }
+
+  if (Uhc->CtrlNameTable != NULL) {
+    FreeUnicodeStringTable (Uhc->CtrlNameTable);
+  }
+
+  FreePool (Uhc);
+}
+
+
+/**
+  Uninstall all Uhci Interface.
+
+  @param  Controller           Controller handle.
+  @param  This                 Protocol instance pointer.
+
+**/
+VOID
+UhciCleanDevUp (
+  IN  EFI_HANDLE           Controller,
+  IN  EFI_USB2_HC_PROTOCOL *This
+  )
+{
+  USB_HC_DEV          *Uhc;
+  EFI_STATUS          Status;
+
+  //
+  // Uninstall the USB_HC and USB_HC2 protocol, then disable the controller
+  //
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  &Uhc->Usb2Hc
+                  );
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+  UhciFreeAllAsyncReq (Uhc);
+  UhciDestoryFrameList (Uhc);
+
+  //
+  // Restore original PCI attributes
+  //
+  Uhc->PciIo->Attributes (
+                  Uhc->PciIo,
+                  EfiPciIoAttributeOperationSet,
+                  Uhc->OriginalPciAttributes,
+                  NULL
+                  );
+
+  UhciFreeDev (Uhc);
+}
+
+/**
+  One notified function to stop the Host Controller when gBS->ExitBootServices() called.
+
+  @param  Event                   Pointer to this event
+  @param  Context                 Event handler private data
+
+**/
+VOID
+EFIAPI
+UhcExitBootService (
+  EFI_EVENT                      Event,
+  VOID                           *Context
+  )
+{
+  USB_HC_DEV   *Uhc;
+
+  Uhc = (USB_HC_DEV *) Context;
+
+  //
+  // Stop the Host Controller
+  //
+  UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+
+  //
+  // Reset the Host Controller
+  //
+  UhciSetRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_HCRESET);
+  gBS->Stall (UHC_ROOT_PORT_RECOVERY_STALL);
+}
+
+/**
+  Starting the Usb UHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @retval EFI_SUCCESS          This driver supports this device.
+  @retval EFI_UNSUPPORTED      This driver does not support this device.
+  @retval EFI_DEVICE_ERROR     This driver cannot be started due to device Error.
+                               EFI_OUT_OF_RESOURCES- Failed due to resource shortage.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  EFI_STATUS          Status;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  USB_HC_DEV          *Uhc;
+  UINT64              Supports;
+  UINT64              OriginalPciAttributes;
+  BOOLEAN             PciAttributesSaved;
+  EFI_DEVICE_PATH_PROTOCOL  *HcDevicePath;
+
+  //
+  // Open PCIIO, then enable the EHC device and turn off emulation
+  //
+  Uhc = NULL;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open Device Path Protocol for on USB host controller
+  //
+  HcDevicePath = NULL;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &HcDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  PciAttributesSaved = FALSE;
+  //
+  // Save original PCI attributes
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &OriginalPciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto CLOSE_PCIIO;
+  }
+  PciAttributesSaved = TRUE;
+
+  //
+  // Robustnesss improvement such as for UoL
+  // Default is not required.
+  //
+  if (FeaturePcdGet (PcdTurnOffUsbLegacySupport)) {
+    UhciTurnOffUsbEmulation (PciIo);
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status = PciIo->Attributes (
+                      PciIo,
+                      EfiPciIoAttributeOperationEnable,
+                      Supports,
+                      NULL
+                      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto CLOSE_PCIIO;
+  }
+
+  Uhc = UhciAllocateDev (PciIo, HcDevicePath, OriginalPciAttributes);
+
+  if (Uhc == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto CLOSE_PCIIO;
+  }
+
+  //
+  // Allocate and Init Host Controller's Frame List Entry
+  //
+  Status = UhciInitFrameList (Uhc);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto FREE_UHC;
+  }
+
+  Status = gBS->SetTimer (
+                  Uhc->AsyncIntMonitor,
+                  TimerPeriodic,
+                  UHC_ASYNC_POLL_INTERVAL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_UHC;
+  }
+
+  //
+  // Install USB2_HC_PROTOCOL
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  &Uhc->Usb2Hc,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_UHC;
+  }
+
+  //
+  // Create event to stop the HC when exit boot service.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  UhcExitBootService,
+                  Uhc,
+                  &gEfiEventExitBootServicesGuid,
+                  &Uhc->ExitBootServiceEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto UNINSTALL_USBHC;
+  }
+
+  //
+  // Install the component name protocol
+  //
+  Uhc->CtrlNameTable = NULL;
+
+  AddUnicodeString2 (
+    "eng",
+    gUhciComponentName.SupportedLanguages,
+    &Uhc->CtrlNameTable,
+    L"Usb Universal Host Controller",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gUhciComponentName2.SupportedLanguages,
+    &Uhc->CtrlNameTable,
+    L"Usb Universal Host Controller",
+    FALSE
+    );
+
+
+  //
+  // Start the UHCI hardware, also set its reclamation point to 64 bytes
+  //
+  UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, USBCMD_RS | USBCMD_MAXP);
+
+  return EFI_SUCCESS;
+  
+UNINSTALL_USBHC:
+  gBS->UninstallMultipleProtocolInterfaces (
+         Controller,
+         &gEfiUsb2HcProtocolGuid,
+         &Uhc->Usb2Hc,
+         NULL
+         );
+
+FREE_UHC:
+  UhciFreeDev (Uhc);
+
+CLOSE_PCIIO:
+  if (PciAttributesSaved) {
+    //
+    // Restore original PCI attributes
+    //
+    PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSet,
+                    OriginalPciAttributes,
+                    NULL
+                    );
+  }
+
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiPciIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS
+  @return others
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN UINTN                           NumberOfChildren,
+  IN EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_USB2_HC_PROTOCOL  *Usb2Hc;
+  EFI_STATUS            Status;
+
+   Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  //
+  // Test whether the Controller handler passed in is a valid
+  // Usb controller handle that should be supported, if not,
+  // return the error status directly
+  //
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UhciCleanDevUp (Controller, Usb2Hc);
+
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiPciIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.h
new file mode 100644
index 0000000000..8635d8231c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.h
@@ -0,0 +1,221 @@
+/** @file
+
+  The definition for UHCI driver model and HC protocol routines.
+
+Copyright (c) 2004 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_UHCI_H_
+#define _EFI_UHCI_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/Usb2HostController.h>
+#include <Protocol/UsbHostController.h>
+#include <Protocol/PciIo.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Pci.h>
+
+typedef struct _USB_HC_DEV  USB_HC_DEV;
+
+#include "UsbHcMem.h"
+#include "UhciQueue.h"
+#include "UhciReg.h"
+#include "UhciSched.h"
+#include "UhciDebug.h"
+#include "ComponentName.h"
+
+//
+// UHC timeout experience values
+//
+
+#define UHC_1_MICROSECOND             1
+#define UHC_1_MILLISECOND             (1000 * UHC_1_MICROSECOND)
+#define UHC_1_SECOND                  (1000 * UHC_1_MILLISECOND)
+
+//
+// UHCI register operation timeout, set by experience
+//
+#define UHC_GENERIC_TIMEOUT           UHC_1_SECOND
+
+//
+// Wait for force global resume(FGR) complete, refers to
+// specification[UHCI11-2.1.1]
+//
+#define UHC_FORCE_GLOBAL_RESUME_STALL (20 * UHC_1_MILLISECOND)
+
+//
+// Wait for roothub port reset and recovery, reset stall
+// is set by experience, and recovery stall refers to
+// specification[UHCI11-2.1.1]
+//
+#define UHC_ROOT_PORT_RESET_STALL     (50 * UHC_1_MILLISECOND)
+#define UHC_ROOT_PORT_RECOVERY_STALL  (10 * UHC_1_MILLISECOND)
+
+//
+// Sync and Async transfer polling interval, set by experience,
+// and the unit of Async is 100us.
+//
+#define UHC_SYNC_POLL_INTERVAL        (1 * UHC_1_MILLISECOND)
+#define UHC_ASYNC_POLL_INTERVAL       EFI_TIMER_PERIOD_MILLISECONDS(1)
+
+//
+// UHC raises TPL to TPL_NOTIFY to serialize all its operations
+// to protect shared data structures.
+//
+#define  UHCI_TPL                     TPL_NOTIFY
+
+#define  USB_HC_DEV_SIGNATURE         SIGNATURE_32 ('u', 'h', 'c', 'i')
+
+#pragma pack(1)
+typedef struct {
+  UINT8               ProgInterface;
+  UINT8               SubClassCode;
+  UINT8               BaseCode;
+} USB_CLASSC;
+#pragma pack()
+
+#define UHC_FROM_USB2_HC_PROTO(This)  CR(This, USB_HC_DEV, Usb2Hc, USB_HC_DEV_SIGNATURE)
+
+//
+// USB_HC_DEV support the UHCI hardware controller. It schedules
+// the asynchronous interrupt transfer with the same method as
+// EHCI: a reversed tree structure. For synchronous interrupt,
+// control and bulk transfer, it uses three static queue head to
+// schedule them. SyncIntQh is for interrupt transfer. LsCtrlQh is
+// for LOW speed control transfer, and FsCtrlBulkQh is for FULL
+// speed control or bulk transfer. This is because FULL speed contrl
+// or bulk transfer can reclaim the unused bandwidth. Some USB
+// device requires this bandwidth reclamation capability.
+//
+struct _USB_HC_DEV {
+  UINT32                    Signature;
+  EFI_USB2_HC_PROTOCOL      Usb2Hc;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  UINT64                    OriginalPciAttributes;
+
+  //
+  // Schedule data structures
+  //
+  UINT32                    *FrameBase; // the buffer pointed by this pointer is used to store pci bus address of the QH descriptor.
+  UINT32                    *FrameBaseHostAddr; // the buffer pointed by this pointer is used to store host memory address of the QH descriptor.
+  UHCI_QH_SW                *SyncIntQh;
+  UHCI_QH_SW                *CtrlQh;
+  UHCI_QH_SW                *BulkQh;
+
+  //
+  // Structures to maintain asynchronus interrupt transfers.
+  // When asynchronous interrutp transfer is unlinked from
+  // the frame list, the hardware may still hold a pointer
+  // to it. To synchronize with hardware, its resoureces are
+  // released in two steps using Recycle and RecycleWait.
+  // Check the asynchronous interrupt management routines.
+  //
+  LIST_ENTRY                AsyncIntList;
+  EFI_EVENT                 AsyncIntMonitor;
+  UHCI_ASYNC_REQUEST        *Recycle;
+  UHCI_ASYNC_REQUEST        *RecycleWait;
+
+
+  UINTN                     RootPorts;
+  USBHC_MEM_POOL            *MemPool;
+  EFI_UNICODE_STRING_TABLE  *CtrlNameTable;
+  VOID                      *FrameMapping;
+
+  //
+  // ExitBootServicesEvent is used to stop the EHC DMA operation 
+  // after exit boot service.
+  //
+  EFI_EVENT                 ExitBootServiceEvent;
+};
+
+extern EFI_DRIVER_BINDING_PROTOCOL   gUhciDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gUhciComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUhciComponentName2;
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has UsbHcProtocol installed will be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Starting the Usb UHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @retval EFI_SUCCESS          This driver supports this device.
+  @retval EFI_UNSUPPORTED      This driver does not support this device.
+  @retval EFI_DEVICE_ERROR     This driver cannot be started due to device Error.
+                               EFI_OUT_OF_RESOURCES- Failed due to resource shortage.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS
+  @return others
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN UINTN                           NumberOfChildren,
+  IN EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.c
new file mode 100644
index 0000000000..9bf53fd411
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.c
@@ -0,0 +1,77 @@
+/** @file
+
+  This file provides the information dump support for Uhci when in debug mode.
+
+Copyright (c) 2007, 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 "Uhci.h"
+
+/**
+  Dump the content of QH structure.
+
+  @param  QhSw    Pointer to software QH structure.
+
+**/
+VOID
+UhciDumpQh (
+  IN UHCI_QH_SW    *QhSw
+  )
+{
+  DEBUG ((EFI_D_VERBOSE, "&QhSw @ 0x%p\n", QhSw));
+  DEBUG ((EFI_D_VERBOSE, "QhSw.NextQh    - 0x%p\n", QhSw->NextQh));
+  DEBUG ((EFI_D_VERBOSE, "QhSw.TDs       - 0x%p\n", QhSw->TDs));
+  DEBUG ((EFI_D_VERBOSE, "QhSw.QhHw:\n"));
+  DEBUG ((EFI_D_VERBOSE, " Horizon  Link - %x\n", QhSw->QhHw.HorizonLink));
+  DEBUG ((EFI_D_VERBOSE, " Vertical Link - %x\n\n", QhSw->QhHw.VerticalLink));
+}
+
+
+/**
+  Dump the content of TD structure.
+
+  @param  TdSw    Pointer to software TD structure.
+
+**/
+VOID
+UhciDumpTds (
+  IN UHCI_TD_SW           *TdSw
+  )
+{
+  UHCI_TD_SW              *CurTdSw;
+
+  CurTdSw = TdSw;
+
+  while (CurTdSw != NULL) {
+    DEBUG ((EFI_D_VERBOSE, "TdSw @ 0x%p\n",           CurTdSw));
+    DEBUG ((EFI_D_VERBOSE, "TdSw.NextTd   - 0x%p\n",  CurTdSw->NextTd));
+    DEBUG ((EFI_D_VERBOSE, "TdSw.DataLen  - %d\n",    CurTdSw->DataLen));
+    DEBUG ((EFI_D_VERBOSE, "TdSw.Data     - 0x%p\n",  CurTdSw->Data));
+    DEBUG ((EFI_D_VERBOSE, "TdHw:\n"));
+    DEBUG ((EFI_D_VERBOSE, " NextLink     - 0x%x\n",  CurTdSw->TdHw.NextLink));
+    DEBUG ((EFI_D_VERBOSE, " ActualLen    - %d\n",    CurTdSw->TdHw.ActualLen));
+    DEBUG ((EFI_D_VERBOSE, " Status       - 0x%x\n",  CurTdSw->TdHw.Status));
+    DEBUG ((EFI_D_VERBOSE, " IOC          - %d\n",    CurTdSw->TdHw.IntOnCpl));
+    DEBUG ((EFI_D_VERBOSE, " IsIsoCh      - %d\n",    CurTdSw->TdHw.IsIsoch));
+    DEBUG ((EFI_D_VERBOSE, " LowSpeed     - %d\n",    CurTdSw->TdHw.LowSpeed));
+    DEBUG ((EFI_D_VERBOSE, " ErrorCount   - %d\n",    CurTdSw->TdHw.ErrorCount));
+    DEBUG ((EFI_D_VERBOSE, " ShortPacket  - %d\n",    CurTdSw->TdHw.ShortPacket));
+    DEBUG ((EFI_D_VERBOSE, " PidCode      - 0x%x\n",  CurTdSw->TdHw.PidCode));
+    DEBUG ((EFI_D_VERBOSE, " DevAddr      - %d\n",    CurTdSw->TdHw.DeviceAddr));
+    DEBUG ((EFI_D_VERBOSE, " EndPoint     - %d\n",    CurTdSw->TdHw.EndPoint));
+    DEBUG ((EFI_D_VERBOSE, " DataToggle   - %d\n",    CurTdSw->TdHw.DataToggle));
+    DEBUG ((EFI_D_VERBOSE, " MaxPacketLen - %d\n",    CurTdSw->TdHw.MaxPacketLen));
+    DEBUG ((EFI_D_VERBOSE, " DataBuffer   - 0x%x\n\n",CurTdSw->TdHw.DataBuffer));
+
+    CurTdSw = CurTdSw->NextTd;
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.h
new file mode 100644
index 0000000000..abdb64fa36
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.h
@@ -0,0 +1,47 @@
+/** @file
+
+  This file contains the definination for host controller debug support routines
+
+Copyright (c) 2007, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_UHCI_DEBUG_H_
+#define _EFI_UHCI_DEBUG_H_
+
+
+/**
+  Dump the content of QH structure.
+
+  @param  QhSw    Pointer to software QH structure.
+
+  @return None.
+
+**/
+VOID
+UhciDumpQh (
+  IN UHCI_QH_SW         *QhSw
+  );
+
+
+/**
+  Dump the content of TD structure.
+
+  @param  TdSw    Pointer to software TD structure.
+
+  @return None.
+
+**/
+VOID
+UhciDumpTds (
+  IN UHCI_TD_SW           *TdSw
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.inf b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.inf
new file mode 100644
index 0000000000..9c5ff7b6fc
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.inf
@@ -0,0 +1,86 @@
+## @file
+#  The UhciDxe driver is responsible for managing the behavior of UHCI controller. 
+#  It implements the interfaces of monitoring the status of all ports and transferring
+#  Control, Bulk, Interrupt and Isochronous requests to Usb1.x device
+#
+#  Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UhciDxe
+  MODULE_UNI_FILE                = UhciDxe.uni
+  FILE_GUID                      = 2FB92EFA-2EE0-4bae-9EB6-7464125E1EF7
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  
+  ENTRY_POINT                    = UhciDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gUhciDriverBinding                        
+#  COMPONENT_NAME                =  gUhciComponentName
+#  COMPONENT_NAME2               =  gUhciComponentName2
+#
+
+[Sources]
+  UhciSched.c
+  UhciDebug.c
+  UsbHcMem.h
+  UhciDebug.h
+  UhciQueue.c
+  UhciReg.c
+  UsbHcMem.c
+  UhciQueue.h
+  Uhci.c
+  Uhci.h
+  UhciReg.h
+  UhciSched.h
+  ComponentName.c
+  ComponentName.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdTurnOffUsbLegacySupport  ## CONSUMES
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  PcdLib
+  ReportStatusCodeLib
+
+[Guids]
+  gEfiEventExitBootServicesGuid                 ## SOMETIMES_CONSUMES ## Event
+
+[Protocols]
+  gEfiPciIoProtocolGuid                         ## TO_START
+  gEfiUsb2HcProtocolGuid                        ## BY_START
+
+# [Event]
+# EVENT_TYPE_PERIODIC_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.uni b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.uni
new file mode 100644
index 0000000000..d011d39f1c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxeExtra.uni
new file mode 100644
index 0000000000..71d9f6cf77
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.c
new file mode 100644
index 0000000000..1a0aa6e636
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.c
@@ -0,0 +1,707 @@
+/** @file
+
+  The UHCI register operation routines.
+
+Copyright (c) 2007 - 2010, 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 "Uhci.h"
+
+
+/**
+  Map address of request structure buffer.
+
+  @param  Uhc                The UHCI device.
+  @param  Request            The user request buffer.
+  @param  MappedAddr         Mapped address of request.
+  @param  Map                Identificaion of this mapping to return.
+
+  @return EFI_SUCCESS        Success.
+  @return EFI_DEVICE_ERROR   Fail to map the user request.
+
+**/
+EFI_STATUS
+UhciMapUserRequest (
+  IN  USB_HC_DEV          *Uhc,
+  IN  OUT VOID            *Request,
+  OUT UINT8               **MappedAddr,
+  OUT VOID                **Map
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 Len;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  Len    = sizeof (EFI_USB_DEVICE_REQUEST);
+  Status = Uhc->PciIo->Map (
+                         Uhc->PciIo,
+                         EfiPciIoOperationBusMasterRead,
+                         Request,
+                         &Len,
+                         &PhyAddr,
+                         Map
+                         );
+
+  if (!EFI_ERROR (Status)) {
+    *MappedAddr = (UINT8 *) (UINTN) PhyAddr;
+  }
+
+  return Status;
+}
+
+
+/**
+  Map address of user data buffer.
+
+  @param  Uhc                The UHCI device.
+  @param  Direction          Direction of the data transfer.
+  @param  Data               The user data buffer.
+  @param  Len                Length of the user data.
+  @param  PktId              Packet identificaion.
+  @param  MappedAddr         Mapped address to return.
+  @param  Map                Identificaion of this mapping to return.
+
+  @return EFI_SUCCESS        Success.
+  @return EFI_DEVICE_ERROR   Fail to map the user data.
+
+**/
+EFI_STATUS
+UhciMapUserData (
+  IN  USB_HC_DEV              *Uhc,
+  IN  EFI_USB_DATA_DIRECTION  Direction,
+  IN  VOID                    *Data,
+  IN  OUT UINTN               *Len,
+  OUT UINT8                   *PktId,
+  OUT UINT8                   **MappedAddr,
+  OUT VOID                    **Map
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  Status = EFI_SUCCESS;
+
+  switch (Direction) {
+  case EfiUsbDataIn:
+    //
+    // BusMasterWrite means cpu read
+    //
+    *PktId = INPUT_PACKET_ID;
+    Status = Uhc->PciIo->Map (
+                           Uhc->PciIo,
+                           EfiPciIoOperationBusMasterWrite,
+                           Data,
+                           Len,
+                           &PhyAddr,
+                           Map
+                           );
+
+    if (EFI_ERROR (Status)) {
+      goto EXIT;
+    }
+
+    *MappedAddr = (UINT8 *) (UINTN) PhyAddr;
+    break;
+
+  case EfiUsbDataOut:
+    *PktId = OUTPUT_PACKET_ID;
+    Status = Uhc->PciIo->Map (
+                           Uhc->PciIo,
+                           EfiPciIoOperationBusMasterRead,
+                           Data,
+                           Len,
+                           &PhyAddr,
+                           Map
+                           );
+
+    if (EFI_ERROR (Status)) {
+      goto EXIT;
+    }
+
+    *MappedAddr = (UINT8 *) (UINTN) PhyAddr;
+    break;
+
+  case EfiUsbNoData:
+    if ((Len != NULL) && (*Len != 0)) {
+      Status    = EFI_INVALID_PARAMETER;
+      goto EXIT;
+    }
+
+    *PktId      = OUTPUT_PACKET_ID;
+    *MappedAddr = NULL;
+    *Map        = NULL;
+    break;
+
+  default:
+    Status      = EFI_INVALID_PARAMETER;
+  }
+
+EXIT:
+  return Status;
+}
+
+
+/**
+  Link the TD To QH.
+
+  @param  Uhc         The UHCI device.
+  @param  Qh          The queue head for the TD to link to.
+  @param  Td          The TD to link.
+
+**/
+VOID
+UhciLinkTdToQh (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_QH_SW           *Qh,
+  IN UHCI_TD_SW           *Td
+  )
+{
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Td, sizeof (UHCI_TD_HW));
+
+  ASSERT ((Qh != NULL) && (Td != NULL));
+
+  Qh->QhHw.VerticalLink = QH_VLINK (PhyAddr, FALSE);
+  Qh->TDs               = (VOID *) Td;
+}
+
+
+/**
+  Unlink TD from the QH.
+
+  @param  Qh          The queue head to unlink from.
+  @param  Td          The TD to unlink.
+
+**/
+VOID
+UhciUnlinkTdFromQh (
+  IN UHCI_QH_SW           *Qh,
+  IN UHCI_TD_SW           *Td
+  )
+{
+  ASSERT ((Qh != NULL) && (Td != NULL));
+
+  Qh->QhHw.VerticalLink = QH_VLINK (NULL, TRUE);
+  Qh->TDs               = NULL;
+}
+
+
+/**
+  Append a new TD To the previous TD.
+
+  @param  Uhc         The UHCI device.
+  @param  PrevTd      Previous UHCI_TD_SW to be linked to.
+  @param  ThisTd      TD to link.
+
+**/
+VOID
+UhciAppendTd (
+  IN USB_HC_DEV     *Uhc,
+  IN UHCI_TD_SW     *PrevTd,
+  IN UHCI_TD_SW     *ThisTd
+  )
+{
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, ThisTd, sizeof (UHCI_TD_HW));
+
+  ASSERT ((PrevTd != NULL) && (ThisTd != NULL));
+
+  PrevTd->TdHw.NextLink = TD_LINK (PhyAddr, TRUE, FALSE);
+  PrevTd->NextTd        = (VOID *) ThisTd;
+}
+
+
+/**
+  Delete a list of TDs.
+
+  @param  Uhc         The UHCI device.
+  @param  FirstTd     TD link list head.
+
+  @return None.
+
+**/
+VOID
+UhciDestoryTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_TD_SW           *FirstTd
+  )
+{
+  UHCI_TD_SW            *NextTd;
+  UHCI_TD_SW            *ThisTd;
+
+  NextTd = FirstTd;
+
+  while (NextTd != NULL) {
+    ThisTd  = NextTd;
+    NextTd  = ThisTd->NextTd;
+    UsbHcFreeMem (Uhc->MemPool, ThisTd, sizeof (UHCI_TD_SW));
+  }
+}
+
+
+/**
+  Create an initialize a new queue head.
+
+  @param  Uhc         The UHCI device.
+  @param  Interval    The polling interval for the queue.
+
+  @return The newly created queue header.
+
+**/
+UHCI_QH_SW *
+UhciCreateQh (
+  IN  USB_HC_DEV        *Uhc,
+  IN  UINTN             Interval
+  )
+{
+  UHCI_QH_SW            *Qh;
+
+  Qh = UsbHcAllocateMem (Uhc->MemPool, sizeof (UHCI_QH_SW));
+
+  if (Qh == NULL) {
+    return NULL;
+  }
+
+  Qh->QhHw.HorizonLink  = QH_HLINK (NULL, TRUE);
+  Qh->QhHw.VerticalLink = QH_VLINK (NULL, TRUE);
+  Qh->Interval          = UhciConvertPollRate(Interval);
+  Qh->TDs               = NULL;
+  Qh->NextQh            = NULL;
+
+  return Qh;
+}
+
+
+/**
+  Create and intialize a TD.
+
+  @param  Uhc         The UHCI device.
+
+  @return The newly allocated and initialized TD.
+
+**/
+UHCI_TD_SW *
+UhciCreateTd (
+  IN  USB_HC_DEV          *Uhc
+  )
+{
+  UHCI_TD_SW              *Td;
+
+  Td     = UsbHcAllocateMem (Uhc->MemPool, sizeof (UHCI_TD_SW));
+  if (Td == NULL) {
+    return NULL;
+  }
+
+  Td->TdHw.NextLink = TD_LINK (NULL, FALSE, TRUE);
+  Td->NextTd        = NULL;
+  Td->Data          = NULL;
+  Td->DataLen       = 0;
+
+  return Td;
+}
+
+
+/**
+  Create and initialize a TD for Setup Stage of a control transfer.
+
+  @param  Uhc         The UHCI device.
+  @param  DevAddr     Device address.
+  @param  Request     A pointer to cpu memory address of Device request.
+  @param  RequestPhy  A pointer to pci memory address of Device request.
+  @param  IsLow       Full speed or low speed.
+
+  @return The created setup Td Pointer.
+
+**/
+UHCI_TD_SW *
+UhciCreateSetupTd (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               *Request,
+  IN  UINT8               *RequestPhy,
+  IN  BOOLEAN             IsLow
+  )
+{
+  UHCI_TD_SW              *Td;
+
+  Td = UhciCreateTd (Uhc);
+
+  if (Td == NULL) {
+    return NULL;
+  }
+
+  Td->TdHw.NextLink     = TD_LINK (NULL, TRUE, TRUE);
+  Td->TdHw.ShortPacket  = FALSE;
+  Td->TdHw.IsIsoch      = FALSE;
+  Td->TdHw.IntOnCpl     = FALSE;
+  Td->TdHw.ErrorCount   = 0x03;
+  Td->TdHw.Status      |= USBTD_ACTIVE;
+  Td->TdHw.DataToggle   = 0;
+  Td->TdHw.EndPoint     = 0;
+  Td->TdHw.LowSpeed     = IsLow ? 1 : 0;
+  Td->TdHw.DeviceAddr   = DevAddr & 0x7F;
+  Td->TdHw.MaxPacketLen = (UINT32) (sizeof (EFI_USB_DEVICE_REQUEST) - 1);
+  Td->TdHw.PidCode      = SETUP_PACKET_ID;
+  Td->TdHw.DataBuffer   = (UINT32) (UINTN) RequestPhy;
+
+  Td->Data              = Request;
+  Td->DataLen           = (UINT16) sizeof (EFI_USB_DEVICE_REQUEST);
+
+  return Td;
+}
+
+
+/**
+  Create a TD for data.
+
+  @param  Uhc         The UHCI device.
+  @param  DevAddr     Device address.
+  @param  Endpoint    Endpoint number.
+  @param  DataPtr     A pointer to cpu memory address of Data buffer.
+  @param  DataPhyPtr  A pointer to pci memory address of Data buffer.
+  @param  Len         Data length.
+  @param  PktId       Packet ID.
+  @param  Toggle      Data toggle value.
+  @param  IsLow       Full speed or low speed.
+
+  @return Data Td pointer if success, otherwise NULL.
+
+**/
+UHCI_TD_SW *
+UhciCreateDataTd (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               Endpoint,
+  IN  UINT8               *DataPtr,
+  IN  UINT8               *DataPhyPtr,
+  IN  UINTN               Len,
+  IN  UINT8               PktId,
+  IN  UINT8               Toggle,
+  IN  BOOLEAN             IsLow
+  )
+{
+  UHCI_TD_SW  *Td;
+
+  //
+  // Code as length - 1, and the max valid length is 0x500
+  //
+  ASSERT (Len <= 0x500);
+
+  Td  = UhciCreateTd (Uhc);
+
+  if (Td == NULL) {
+    return NULL;
+  }
+
+  Td->TdHw.NextLink     = TD_LINK (NULL, TRUE, TRUE);
+  Td->TdHw.ShortPacket  = FALSE;
+  Td->TdHw.IsIsoch      = FALSE;
+  Td->TdHw.IntOnCpl     = FALSE;
+  Td->TdHw.ErrorCount   = 0x03;
+  Td->TdHw.Status       = USBTD_ACTIVE;
+  Td->TdHw.LowSpeed     = IsLow ? 1 : 0;
+  Td->TdHw.DataToggle   = Toggle & 0x01;
+  Td->TdHw.EndPoint     = Endpoint & 0x0F;
+  Td->TdHw.DeviceAddr   = DevAddr & 0x7F;
+  Td->TdHw.MaxPacketLen = (UINT32) (Len - 1);
+  Td->TdHw.PidCode      = (UINT8) PktId;
+  Td->TdHw.DataBuffer   = (UINT32) (UINTN) DataPhyPtr;
+
+  Td->Data              = DataPtr;
+  Td->DataLen           = (UINT16) Len;
+
+  return Td;
+}
+
+
+/**
+  Create TD for the Status Stage of control transfer.
+
+  @param  Uhc         The UHCI device.
+  @param  DevAddr     Device address.
+  @param  PktId       Packet ID.
+  @param  IsLow       Full speed or low speed.
+
+  @return Status Td Pointer.
+
+**/
+UHCI_TD_SW *
+UhciCreateStatusTd (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               PktId,
+  IN  BOOLEAN             IsLow
+  )
+{
+  UHCI_TD_SW              *Td;
+
+  Td = UhciCreateTd (Uhc);
+
+  if (Td == NULL) {
+    return NULL;
+  }
+
+  Td->TdHw.NextLink     = TD_LINK (NULL, TRUE, TRUE);
+  Td->TdHw.ShortPacket  = FALSE;
+  Td->TdHw.IsIsoch      = FALSE;
+  Td->TdHw.IntOnCpl     = FALSE;
+  Td->TdHw.ErrorCount   = 0x03;
+  Td->TdHw.Status      |= USBTD_ACTIVE;
+  Td->TdHw.MaxPacketLen = 0x7FF;      //0x7FF: there is no data (refer to UHCI spec)
+  Td->TdHw.DataToggle   = 1;
+  Td->TdHw.EndPoint     = 0;
+  Td->TdHw.LowSpeed     = IsLow ? 1 : 0;
+  Td->TdHw.DeviceAddr   = DevAddr & 0x7F;
+  Td->TdHw.PidCode      = (UINT8) PktId;
+  Td->TdHw.DataBuffer   = (UINT32) (UINTN) NULL;
+
+  Td->Data              = NULL;
+  Td->DataLen           = 0;
+
+  return Td;
+}
+
+
+/**
+  Create Tds list for Control Transfer.
+
+  @param  Uhc         The UHCI device.
+  @param  DeviceAddr  The device address.
+  @param  DataPktId   Packet Identification of Data Tds.
+  @param  Request     A pointer to cpu memory address of request structure buffer to transfer.
+  @param  RequestPhy  A pointer to pci memory address of request structure buffer to transfer.
+  @param  Data        A pointer to cpu memory address of user data buffer to transfer.
+  @param  DataPhy     A pointer to pci memory address of user data buffer to transfer.
+  @param  DataLen     Length of user data to transfer.
+  @param  MaxPacket   Maximum packet size for control transfer.
+  @param  IsLow       Full speed or low speed.
+
+  @return The Td list head for the control transfer.
+
+**/
+UHCI_TD_SW *
+UhciCreateCtrlTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UINT8                DeviceAddr,
+  IN UINT8                DataPktId,
+  IN UINT8                *Request,
+  IN UINT8                *RequestPhy,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN UINT8                MaxPacket,
+  IN BOOLEAN              IsLow
+  )
+{
+  UHCI_TD_SW                *SetupTd;
+  UHCI_TD_SW                *FirstDataTd;
+  UHCI_TD_SW                *DataTd;
+  UHCI_TD_SW                *PrevDataTd;
+  UHCI_TD_SW                *StatusTd;
+  UINT8                     DataToggle;
+  UINT8                     StatusPktId;
+  UINTN                     ThisTdLen;
+
+
+  DataTd      = NULL;
+  SetupTd     = NULL;
+  FirstDataTd = NULL;
+  PrevDataTd  = NULL;
+  StatusTd    = NULL;
+
+  //
+  // Create setup packets for the transfer
+  //
+  SetupTd = UhciCreateSetupTd (Uhc, DeviceAddr, Request, RequestPhy, IsLow);
+
+  if (SetupTd == NULL) {
+    return NULL;
+  }
+
+  //
+  // Create data packets for the transfer
+  //
+  DataToggle = 1;
+
+  while (DataLen > 0) {
+    //
+    // PktSize is the data load size in each Td.
+    //
+    ThisTdLen = (DataLen > MaxPacket ? MaxPacket : DataLen);
+
+    DataTd = UhciCreateDataTd (
+               Uhc,
+               DeviceAddr,
+               0,
+               Data,  //cpu memory address
+               DataPhy, //Pci memory address
+               ThisTdLen,
+               DataPktId,
+               DataToggle,
+               IsLow
+               );
+
+    if (DataTd == NULL) {
+      goto FREE_TD;
+    }
+
+    if (FirstDataTd == NULL) {
+      FirstDataTd         = DataTd;
+      FirstDataTd->NextTd = NULL;
+    } else {
+      UhciAppendTd (Uhc, PrevDataTd, DataTd);
+    }
+
+    DataToggle ^= 1;
+    PrevDataTd = DataTd;
+    Data += ThisTdLen;
+    DataPhy += ThisTdLen;
+    DataLen -= ThisTdLen;
+  }
+
+  //
+  // Status packet is on the opposite direction to data packets
+  //
+  if (OUTPUT_PACKET_ID == DataPktId) {
+    StatusPktId = INPUT_PACKET_ID;
+  } else {
+    StatusPktId = OUTPUT_PACKET_ID;
+  }
+
+  StatusTd = UhciCreateStatusTd (Uhc, DeviceAddr, StatusPktId, IsLow);
+
+  if (StatusTd == NULL) {
+    goto FREE_TD;
+  }
+
+  //
+  // Link setup Td -> data Tds -> status Td together
+  //
+  if (FirstDataTd != NULL) {
+    UhciAppendTd (Uhc, SetupTd, FirstDataTd);
+    UhciAppendTd (Uhc, PrevDataTd, StatusTd);
+  } else {
+    UhciAppendTd (Uhc, SetupTd, StatusTd);
+  }
+
+  return SetupTd;
+
+FREE_TD:
+  if (SetupTd != NULL) {
+    UhciDestoryTds (Uhc, SetupTd);
+  }
+
+  if (FirstDataTd != NULL) {
+    UhciDestoryTds (Uhc, FirstDataTd);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Create Tds list for Bulk/Interrupt Transfer.
+
+  @param  Uhc         USB_HC_DEV.
+  @param  DevAddr     Address of Device.
+  @param  EndPoint    Endpoint Number.
+  @param  PktId       Packet Identification of Data Tds.
+  @param  Data        A pointer to cpu memory address of user data buffer to transfer.
+  @param  DataPhy     A pointer to pci memory address of user data buffer to transfer.
+  @param  DataLen     Length of user data to transfer.
+  @param  DataToggle  Data Toggle Pointer.
+  @param  MaxPacket   Maximum packet size for Bulk/Interrupt transfer.
+  @param  IsLow       Is Low Speed Device.
+
+  @return The Tds list head for the bulk transfer.
+
+**/
+UHCI_TD_SW *
+UhciCreateBulkOrIntTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UINT8                DevAddr,
+  IN UINT8                EndPoint,
+  IN UINT8                PktId,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN OUT UINT8            *DataToggle,
+  IN UINT8                MaxPacket,
+  IN BOOLEAN              IsLow
+  )
+{
+  UHCI_TD_SW              *DataTd;
+  UHCI_TD_SW              *FirstDataTd;
+  UHCI_TD_SW              *PrevDataTd;
+  UINTN                   ThisTdLen;
+
+  DataTd      = NULL;
+  FirstDataTd = NULL;
+  PrevDataTd  = NULL;
+
+  //
+  // Create data packets for the transfer
+  //
+  while (DataLen > 0) {
+    //
+    // PktSize is the data load size that each Td.
+    //
+    ThisTdLen = DataLen;
+
+    if (DataLen > MaxPacket) {
+      ThisTdLen = MaxPacket;
+    }
+
+    DataTd = UhciCreateDataTd (
+               Uhc,
+               DevAddr,
+               EndPoint,
+               Data,
+               DataPhy,
+               ThisTdLen,
+               PktId,
+               *DataToggle,
+               IsLow
+               );
+
+    if (DataTd == NULL) {
+      goto FREE_TD;
+    }
+
+    if (PktId == INPUT_PACKET_ID) {
+      DataTd->TdHw.ShortPacket = TRUE;
+    }
+
+    if (FirstDataTd == NULL) {
+      FirstDataTd         = DataTd;
+      FirstDataTd->NextTd = NULL;
+    } else {
+      UhciAppendTd (Uhc, PrevDataTd, DataTd);
+    }
+
+    *DataToggle ^= 1;
+    PrevDataTd   = DataTd;
+    Data        += ThisTdLen;
+    DataPhy     += ThisTdLen;
+    DataLen     -= ThisTdLen;
+  }
+
+  return FirstDataTd;
+
+FREE_TD:
+  if (FirstDataTd != NULL) {
+    UhciDestoryTds (Uhc, FirstDataTd);
+  }
+
+  return NULL;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.h
new file mode 100644
index 0000000000..9ddca38302
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.h
@@ -0,0 +1,272 @@
+/** @file
+
+  The definition for UHCI register operation routines.
+
+Copyright (c) 2007, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_UHCI_QUEUE_H_
+#define _EFI_UHCI_QUEUE_H_
+
+//
+// Macroes used to set various links in UHCI's driver.
+// In this UHCI driver, QH's horizontal link always pointers to other QH,
+// and its vertical link always pointers to TD. TD's next pointer always
+// pointers to other sibling TD. Frame link always pointers to QH because
+// ISO transfer isn't supported.
+//
+// We should use UINT32 to access these pointers to void race conditions
+// with hardware.
+//
+#define QH_HLINK(Pointer, Terminate)  \
+        (((UINT32) ((UINTN) (Pointer)) & 0xFFFFFFF0) | 0x02 | ((Terminate) ? 0x01 : 0))
+
+#define QH_VLINK(Pointer, Terminate)  \
+        (((UINT32) ((UINTN) (Pointer)) & 0xFFFFFFF0) | ((Terminate) ? 0x01 : 0))
+
+#define TD_LINK(Pointer, VertFirst, Terminate) \
+        (((UINT32) ((UINTN) (Pointer)) & 0xFFFFFFF0) | \
+         ((VertFirst) ? 0x04 : 0) | ((Terminate) ? 0x01 : 0))
+
+#define LINK_TERMINATED(Link) (((Link) & 0x01) != 0)
+
+#define UHCI_ADDR(QhOrTd)     ((VOID *) (UINTN) ((QhOrTd) & 0xFFFFFFF0))
+
+#pragma pack(1)
+//
+// Both links in QH has this internal structure:
+//   Next pointer: 28, Reserved: 2, NextIsQh: 1, Terminate: 1
+// This is the same as frame list entry.
+//
+typedef struct {
+  UINT32              HorizonLink;
+  UINT32              VerticalLink;
+} UHCI_QH_HW;
+
+//
+// Next link in TD has this internal structure:
+//   Next pointer: 28, Reserved: 1, Vertical First: 1, NextIsQh: 1, Terminate: 1
+//
+typedef struct {
+  UINT32              NextLink;
+  UINT32              ActualLen   : 11;
+  UINT32              Reserved1   : 5;
+  UINT32              Status      : 8;
+  UINT32              IntOnCpl    : 1;
+  UINT32              IsIsoch     : 1;
+  UINT32              LowSpeed    : 1;
+  UINT32              ErrorCount  : 2;
+  UINT32              ShortPacket : 1;
+  UINT32              Reserved2   : 2;
+  UINT32              PidCode     : 8;
+  UINT32              DeviceAddr  : 7;
+  UINT32              EndPoint    : 4;
+  UINT32              DataToggle  : 1;
+  UINT32              Reserved3   : 1;
+  UINT32              MaxPacketLen: 11;
+  UINT32              DataBuffer;
+} UHCI_TD_HW;
+#pragma pack()
+
+typedef struct _UHCI_TD_SW  UHCI_TD_SW;
+typedef struct _UHCI_QH_SW  UHCI_QH_SW;
+
+struct _UHCI_QH_SW {
+  UHCI_QH_HW        QhHw;
+  UHCI_QH_SW        *NextQh;
+  UHCI_TD_SW        *TDs;
+  UINTN             Interval;
+};
+
+struct _UHCI_TD_SW {
+  UHCI_TD_HW        TdHw;
+  UHCI_TD_SW        *NextTd;
+  UINT8             *Data;
+  UINT16            DataLen;
+};
+
+
+/**
+  Link the TD To QH.
+
+  @param  Uhc         The UHCI device.
+  @param  Qh          The queue head for the TD to link to.
+  @param  Td          The TD to link.
+
+**/
+VOID
+UhciLinkTdToQh (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_QH_SW           *Qh,
+  IN UHCI_TD_SW           *Td
+  );
+
+
+/**
+  Unlink TD from the QH.
+
+  @param  Qh          The queue head to unlink from.
+  @param  Td          The TD to unlink.
+
+  @return None.
+
+**/
+VOID
+UhciUnlinkTdFromQh (
+  IN UHCI_QH_SW           *Qh,
+  IN UHCI_TD_SW           *Td
+  );
+
+
+/**
+  Map address of request structure buffer.
+
+  @param  Uhc                The UHCI device.
+  @param  Request            The user request buffer.
+  @param  MappedAddr         Mapped address of request.
+  @param  Map                Identificaion of this mapping to return.
+
+  @return EFI_SUCCESS        Success.
+  @return EFI_DEVICE_ERROR   Fail to map the user request.
+
+**/
+EFI_STATUS
+UhciMapUserRequest (
+  IN  USB_HC_DEV          *Uhc,
+  IN  OUT VOID            *Request,
+  OUT UINT8               **MappedAddr,
+  OUT VOID                **Map
+  );
+
+
+/**
+  Map address of user data buffer.
+
+  @param  Uhc                The UHCI device.
+  @param  Direction          Direction of the data transfer.
+  @param  Data               The user data buffer.
+  @param  Len                Length of the user data.
+  @param  PktId              Packet identificaion.
+  @param  MappedAddr         Mapped address to return.
+  @param  Map                Identificaion of this mapping to return.
+
+  @return EFI_SUCCESS        Success.
+  @return EFI_DEVICE_ERROR   Fail to map the user data.
+
+**/
+EFI_STATUS
+UhciMapUserData (
+  IN  USB_HC_DEV              *Uhc,
+  IN  EFI_USB_DATA_DIRECTION  Direction,
+  IN  VOID                    *Data,
+  IN  OUT UINTN               *Len,
+  OUT UINT8                   *PktId,
+  OUT UINT8                   **MappedAddr,
+  OUT VOID                    **Map
+  );
+
+
+/**
+  Delete a list of TDs.
+
+  @param  Uhc         The UHCI device.
+  @param  FirstTd     TD link list head.
+
+  @return None.
+
+**/
+VOID
+UhciDestoryTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_TD_SW           *FirstTd
+  );
+
+
+/**
+  Create an initialize a new queue head.
+
+  @param  Uhc         The UHCI device.
+  @param  Interval    The polling interval for the queue.
+
+  @return The newly created queue header.
+
+**/
+UHCI_QH_SW *
+UhciCreateQh (
+  IN  USB_HC_DEV        *Uhc,
+  IN  UINTN             Interval
+  );
+
+
+/**
+  Create Tds list for Control Transfer.
+
+  @param  Uhc         The UHCI device.
+  @param  DeviceAddr  The device address.
+  @param  DataPktId   Packet Identification of Data Tds.
+  @param  Request     A pointer to cpu memory address of request structure buffer to transfer.
+  @param  RequestPhy  A pointer to pci memory address of request structure buffer to transfer.
+  @param  Data        A pointer to cpu memory address of user data buffer to transfer.
+  @param  DataPhy     A pointer to pci memory address of user data buffer to transfer.
+  @param  DataLen     Length of user data to transfer.
+  @param  MaxPacket   Maximum packet size for control transfer.
+  @param  IsLow       Full speed or low speed.
+
+  @return The Td list head for the control transfer.
+
+**/
+UHCI_TD_SW *
+UhciCreateCtrlTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UINT8                DeviceAddr,
+  IN UINT8                DataPktId,
+  IN UINT8                *Request,
+  IN UINT8                *RequestPhy,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN UINT8                MaxPacket,
+  IN BOOLEAN              IsLow
+  );
+
+
+/**
+  Create Tds list for Bulk/Interrupt Transfer.
+
+  @param  Uhc         USB_HC_DEV.
+  @param  DevAddr     Address of Device.
+  @param  EndPoint    Endpoint Number.
+  @param  PktId       Packet Identification of Data Tds.
+  @param  Data        A pointer to cpu memory address of user data buffer to transfer.
+  @param  DataPhy     A pointer to pci memory address of user data buffer to transfer.
+  @param  DataLen     Length of user data to transfer.
+  @param  DataToggle  Data Toggle Pointer.
+  @param  MaxPacket   Maximum packet size for Bulk/Interrupt transfer.
+  @param  IsLow       Is Low Speed Device.
+
+  @return The Tds list head for the bulk transfer.
+
+**/
+UHCI_TD_SW *
+UhciCreateBulkOrIntTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UINT8                DevAddr,
+  IN UINT8                EndPoint,
+  IN UINT8                PktId,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN OUT UINT8            *DataToggle,
+  IN UINT8                MaxPacket,
+  IN BOOLEAN              IsLow
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.c
new file mode 100644
index 0000000000..0fd16284b3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.c
@@ -0,0 +1,281 @@
+/** @file
+
+  The UHCI register operation routines.
+
+Copyright (c) 2007, 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 "Uhci.h"
+
+
+/**
+  Read a UHCI register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+
+  @return Content of register.
+
+**/
+UINT16
+UhciReadReg (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset
+  )
+{
+  UINT16      Data;
+  EFI_STATUS  Status;
+
+  Status = PciIo->Io.Read (
+                      PciIo,
+                      EfiPciIoWidthUint16,
+                      USB_BAR_INDEX,
+                      Offset,
+                      1,
+                      &Data
+                      );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UhciReadReg: PciIo Io.Read error: %r at offset %d\n", Status, Offset));
+
+    Data = 0xFFFF;
+  }
+
+  return Data;
+}
+
+
+/**
+  Write data to UHCI register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Data         Data to write.
+
+**/
+VOID
+UhciWriteReg (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Data
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = PciIo->Io.Write (
+                      PciIo,
+                      EfiPciIoWidthUint16,
+                      USB_BAR_INDEX,
+                      Offset,
+                      1,
+                      &Data
+                      );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UhciWriteReg: PciIo Io.Write error: %r at offset %d\n", Status, Offset));
+  }
+}
+
+
+/**
+  Set a bit of the UHCI Register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Bit          The bit to set.
+
+**/
+VOID
+UhciSetRegBit (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Bit
+  )
+{
+  UINT16  Data;
+
+  Data = UhciReadReg (PciIo, Offset);
+  Data = (UINT16) (Data |Bit);
+  UhciWriteReg (PciIo, Offset, Data);
+}
+
+
+/**
+  Clear a bit of the UHCI Register.
+
+  @param  PciIo        The PCI_IO protocol to access the PCI.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Bit          The bit to clear.
+
+**/
+VOID
+UhciClearRegBit (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Bit
+  )
+{
+  UINT16  Data;
+
+  Data = UhciReadReg (PciIo, Offset);
+  Data = (UINT16) (Data & ~Bit);
+  UhciWriteReg (PciIo, Offset, Data);
+}
+
+
+/**
+  Clear all the interrutp status bits, these bits
+  are Write-Clean.
+
+  @param  Uhc          The UHCI device.
+
+**/
+VOID
+UhciAckAllInterrupt (
+  IN  USB_HC_DEV          *Uhc
+  )
+{
+  UhciWriteReg (Uhc->PciIo, USBSTS_OFFSET, 0x3F);
+
+  //
+  // If current HC is halted, re-enable it. Host Controller Process Error
+  // is a temporary error status.
+  //
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    DEBUG ((EFI_D_ERROR, "UhciAckAllInterrupt: re-enable the UHCI from system error\n"));
+    Uhc->Usb2Hc.SetState (&Uhc->Usb2Hc, EfiUsbHcStateOperational);
+  }
+}
+
+
+/**
+  Stop the host controller.
+
+  @param  Uhc          The UHCI device.
+  @param  Timeout      Max time allowed.
+
+  @retval EFI_SUCCESS  The host controller is stopped.
+  @retval EFI_TIMEOUT  Failed to stop the host controller.
+
+**/
+EFI_STATUS
+UhciStopHc (
+  IN USB_HC_DEV        *Uhc,
+  IN UINTN             Timeout
+  )
+{
+  UINT16                UsbSts;
+  UINTN                 Index;
+
+  UhciClearRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_RS);
+
+  //
+  // ensure the HC is in halt status after send the stop command
+  // Timeout is in us unit.
+  //
+  for (Index = 0; Index < (Timeout / 50) + 1; Index++) {
+    UsbSts = UhciReadReg (Uhc->PciIo, USBSTS_OFFSET);
+
+    if ((UsbSts & USBSTS_HCH) == USBSTS_HCH) {
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (50);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+
+/**
+  Check whether the host controller operates well.
+
+  @param  PciIo        The PCI_IO protocol to use.
+
+  @retval TRUE         Host controller is working.
+  @retval FALSE        Host controller is halted or system error.
+
+**/
+BOOLEAN
+UhciIsHcWorking (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo
+  )
+{
+  UINT16                UsbSts;
+
+  UsbSts = UhciReadReg (PciIo, USBSTS_OFFSET);
+
+  if ((UsbSts & (USBSTS_HCPE | USBSTS_HSE | USBSTS_HCH)) != 0) {
+    DEBUG ((EFI_D_ERROR, "UhciIsHcWorking: current USB state is %x\n", UsbSts));
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Set the UHCI frame list base address. It can't use
+  UhciWriteReg which access memory in UINT16.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Addr         Address to set.
+
+**/
+VOID
+UhciSetFrameListBaseAddr (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN VOID                    *Addr
+  )
+{
+  EFI_STATUS              Status;
+  UINT32                  Data;
+
+  Data = (UINT32) ((UINTN) Addr & 0xFFFFF000);
+
+  Status = PciIo->Io.Write (
+                       PciIo,
+                       EfiPciIoWidthUint32,
+                       USB_BAR_INDEX,
+                       (UINT64) USB_FRAME_BASE_OFFSET,
+                       1,
+                       &Data
+                       );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UhciSetFrameListBaseAddr: PciIo Io.Write error: %r\n", Status));
+  }
+}
+
+
+/**
+  Disable USB Emulation.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL protocol to use.
+
+**/
+VOID
+UhciTurnOffUsbEmulation (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo
+  )
+{
+  UINT16            Command;
+
+  Command = 0;
+
+  PciIo->Pci.Write (
+               PciIo,
+               EfiPciIoWidthUint16,
+               USB_EMULATION_OFFSET,
+               1,
+               &Command
+               );
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.h
new file mode 100644
index 0000000000..7212ad0dfa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.h
@@ -0,0 +1,248 @@
+/** @file
+
+  The definition for UHCI register operation routines.
+
+Copyright (c) 2007 - 2010, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_UHCI_REG_H_
+#define _EFI_UHCI_REG_H_
+
+//
+// UHCI register offset
+//
+
+#define UHCI_FRAME_NUM        1024
+
+//
+// Register offset and PCI related staff
+//
+#define USB_BAR_INDEX         4
+
+#define USBCMD_OFFSET         0
+#define USBSTS_OFFSET         2
+#define USBINTR_OFFSET        4
+#define USBPORTSC_OFFSET      0x10
+#define USB_FRAME_NO_OFFSET   6
+#define USB_FRAME_BASE_OFFSET 8
+#define USB_EMULATION_OFFSET  0xC0
+
+//
+// Packet IDs
+//
+#define SETUP_PACKET_ID       0x2D
+#define INPUT_PACKET_ID       0x69
+#define OUTPUT_PACKET_ID      0xE1
+#define ERROR_PACKET_ID       0x55
+
+//
+// USB port status and control bit definition.
+//
+#define USBPORTSC_CCS         BIT0  // Current Connect Status
+#define USBPORTSC_CSC         BIT1  // Connect Status Change
+#define USBPORTSC_PED         BIT2  // Port Enable / Disable
+#define USBPORTSC_PEDC        BIT3  // Port Enable / Disable Change
+#define USBPORTSC_LSL         BIT4  // Line Status Low BIT
+#define USBPORTSC_LSH         BIT5  // Line Status High BIT
+#define USBPORTSC_RD          BIT6  // Resume Detect
+#define USBPORTSC_LSDA        BIT8  // Low Speed Device Attached
+#define USBPORTSC_PR          BIT9  // Port Reset
+#define USBPORTSC_SUSP        BIT12 // Suspend
+
+//
+// UHCI Spec said it must implement 2 ports each host at least,
+// and if more, check whether the bit7 of PORTSC is always 1.
+// So here assume the max of port number each host is 16.
+//
+#define USB_MAX_ROOTHUB_PORT  0x0F
+
+//
+// Command register bit definitions
+//
+#define USBCMD_RS             BIT0  // Run/Stop
+#define USBCMD_HCRESET        BIT1  // Host reset
+#define USBCMD_GRESET         BIT2  // Global reset
+#define USBCMD_EGSM           BIT3  // Global Suspend Mode
+#define USBCMD_FGR            BIT4  // Force Global Resume
+#define USBCMD_SWDBG          BIT5  // SW Debug mode
+#define USBCMD_CF             BIT6  // Config Flag (sw only)
+#define USBCMD_MAXP           BIT7  // Max Packet (0 = 32, 1 = 64)
+
+//
+// USB Status register bit definitions
+//
+#define USBSTS_USBINT         BIT0  // Interrupt due to IOC
+#define USBSTS_ERROR          BIT1  // Interrupt due to error
+#define USBSTS_RD             BIT2  // Resume Detect
+#define USBSTS_HSE            BIT3  // Host System Error
+#define USBSTS_HCPE           BIT4  // Host Controller Process Error
+#define USBSTS_HCH            BIT5  // HC Halted
+
+#define USBTD_ACTIVE          BIT7  // TD is still active
+#define USBTD_STALLED         BIT6  // TD is stalled
+#define USBTD_BUFFERR         BIT5  // Buffer underflow or overflow
+#define USBTD_BABBLE          BIT4  // Babble condition
+#define USBTD_NAK             BIT3  // NAK is received
+#define USBTD_CRC             BIT2  // CRC/Time out error
+#define USBTD_BITSTUFF        BIT1  // Bit stuff error
+
+
+/**
+  Read a UHCI register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+
+  @return Content of register.
+
+**/
+UINT16
+UhciReadReg (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset
+  );
+
+
+
+/**
+  Write data to UHCI register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Data         Data to write.
+
+  @return None.
+
+**/
+VOID
+UhciWriteReg (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Data
+  );
+
+
+
+/**
+  Set a bit of the UHCI Register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Bit          The bit to set.
+
+  @return None.
+
+**/
+VOID
+UhciSetRegBit (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Bit
+  );
+
+
+
+/**
+  Clear a bit of the UHCI Register.
+
+  @param  PciIo        The PCI_IO protocol to access the PCI.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Bit          The bit to clear.
+
+  @return None.
+
+**/
+VOID
+UhciClearRegBit (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Bit
+  );
+
+
+/**
+  Clear all the interrutp status bits, these bits
+  are Write-Clean.
+
+  @param  Uhc          The UHCI device.
+
+  @return None.
+
+**/
+VOID
+UhciAckAllInterrupt (
+  IN  USB_HC_DEV          *Uhc
+  );
+
+
+/**
+  Stop the host controller.
+
+  @param  Uhc          The UHCI device.
+  @param  Timeout      Max time allowed.
+
+  @retval EFI_SUCCESS  The host controller is stopped.
+  @retval EFI_TIMEOUT  Failed to stop the host controller.
+
+**/
+EFI_STATUS
+UhciStopHc (
+  IN USB_HC_DEV         *Uhc,
+  IN UINTN              Timeout
+  );
+
+
+
+/**
+  Check whether the host controller operates well.
+
+  @param  PciIo        The PCI_IO protocol to use.
+
+  @retval TRUE         Host controller is working.
+  @retval FALSE        Host controller is halted or system error.
+
+**/
+BOOLEAN
+UhciIsHcWorking (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo
+  );
+
+
+/**
+  Set the UHCI frame list base address. It can't use
+  UhciWriteReg which access memory in UINT16.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Addr         Address to set.
+
+  @return None.
+
+**/
+VOID
+UhciSetFrameListBaseAddr (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN VOID                    *Addr
+  );
+
+
+/**
+  Disable USB Emulation.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL protocol to use.
+
+  @return None.
+
+**/
+VOID
+UhciTurnOffUsbEmulation (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.c
new file mode 100644
index 0000000000..90f010c998
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.c
@@ -0,0 +1,1045 @@
+/** @file
+
+  The EHCI register operation routines.
+
+Copyright (c) 2007 - 2013, 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 "Uhci.h"
+
+
+/**
+  Create Frame List Structure.
+
+  @param  Uhc                    UHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_UNSUPPORTED        Map memory fail.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+UhciInitFrameList (
+  IN USB_HC_DEV         *Uhc
+  )
+{
+  EFI_PHYSICAL_ADDRESS  MappedAddr;
+  EFI_STATUS            Status;
+  VOID                  *Buffer;
+  VOID                  *Mapping;
+  UINTN                 Pages;
+  UINTN                 Bytes;
+  UINTN                 Index;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  //
+  // The Frame List is a common buffer that will be
+  // accessed by both the cpu and the usb bus master
+  // at the same time. The Frame List ocupies 4K bytes,
+  // and must be aligned on 4-Kbyte boundaries.
+  //
+  Bytes = 4096;
+  Pages = EFI_SIZE_TO_PAGES (Bytes);
+
+  Status = Uhc->PciIo->AllocateBuffer (
+                         Uhc->PciIo,
+                         AllocateAnyPages,
+                         EfiBootServicesData,
+                         Pages,
+                         &Buffer,
+                         0
+                         );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = Uhc->PciIo->Map (
+                         Uhc->PciIo,
+                         EfiPciIoOperationBusMasterCommonBuffer,
+                         Buffer,
+                         &Bytes,
+                         &MappedAddr,
+                         &Mapping
+                         );
+
+  if (EFI_ERROR (Status) || (Bytes != 4096)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  Uhc->FrameBase           = (UINT32 *) (UINTN) Buffer;
+  Uhc->FrameMapping        = Mapping;
+
+  //
+  // Tell the Host Controller where the Frame List lies,
+  // by set the Frame List Base Address Register.
+  //
+  UhciSetFrameListBaseAddr (Uhc->PciIo, (VOID *) (UINTN) MappedAddr);
+
+  //
+  // Allocate the QH used by sync interrupt/control/bulk transfer.
+  // FS ctrl/bulk queue head is set to loopback so additional BW
+  // can be reclaimed. Notice, LS don't support bulk transfer and
+  // also doesn't support BW reclamation.
+  //
+  Uhc->SyncIntQh  = UhciCreateQh (Uhc, 1);
+  Uhc->CtrlQh     = UhciCreateQh (Uhc, 1);
+  Uhc->BulkQh     = UhciCreateQh (Uhc, 1);
+
+  if ((Uhc->SyncIntQh == NULL) || (Uhc->CtrlQh == NULL) || (Uhc->BulkQh == NULL)) {
+    Uhc->PciIo->Unmap (Uhc->PciIo, Mapping);
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  //
+  //                                                +-------------+
+  //                                                |             |
+  // Link the three together: SyncIntQh->CtrlQh->BulkQh <---------+
+  // Each frame entry is linked to this sequence of QH. These QH
+  // will remain on the schedul, never got removed
+  //
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Uhc->CtrlQh, sizeof (UHCI_QH_HW));
+  Uhc->SyncIntQh->QhHw.HorizonLink  = QH_HLINK (PhyAddr, FALSE);
+  Uhc->SyncIntQh->NextQh            = Uhc->CtrlQh;
+
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Uhc->BulkQh, sizeof (UHCI_QH_HW));
+  Uhc->CtrlQh->QhHw.HorizonLink     = QH_HLINK (PhyAddr, FALSE);
+  Uhc->CtrlQh->NextQh               = Uhc->BulkQh;
+
+  //
+  // Some old platform such as Intel's Tiger 4 has a difficult time
+  // in supporting the full speed bandwidth reclamation in the previous
+  // mentioned form. Most new platforms don't suffer it.
+  //
+  Uhc->BulkQh->QhHw.HorizonLink     = QH_HLINK (PhyAddr, FALSE);
+
+  Uhc->BulkQh->NextQh               = NULL;
+
+  Uhc->FrameBaseHostAddr = AllocateZeroPool (4096);
+  if (Uhc->FrameBaseHostAddr == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Uhc->SyncIntQh, sizeof (UHCI_QH_HW));
+  for (Index = 0; Index < UHCI_FRAME_NUM; Index++) {
+    Uhc->FrameBase[Index] = QH_HLINK (PhyAddr, FALSE);
+    Uhc->FrameBaseHostAddr[Index] = (UINT32)(UINTN)Uhc->SyncIntQh;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  if (Uhc->SyncIntQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->SyncIntQh, sizeof (UHCI_QH_SW));
+  }
+
+  if (Uhc->CtrlQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->CtrlQh, sizeof (UHCI_QH_SW));
+  }
+
+  if (Uhc->BulkQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->BulkQh, sizeof (UHCI_QH_SW));
+  }
+
+  Uhc->PciIo->FreeBuffer (Uhc->PciIo, Pages, Buffer);
+  return Status;
+}
+
+
+/**
+  Destory FrameList buffer.
+
+  @param  Uhc                    The UHCI device.
+
+**/
+VOID
+UhciDestoryFrameList (
+  IN USB_HC_DEV           *Uhc
+  )
+{
+  //
+  // Unmap the common buffer for framelist entry,
+  // and free the common buffer.
+  // Uhci's frame list occupy 4k memory.
+  //
+  Uhc->PciIo->Unmap (Uhc->PciIo, Uhc->FrameMapping);
+
+  Uhc->PciIo->FreeBuffer (
+                Uhc->PciIo,
+                EFI_SIZE_TO_PAGES (4096),
+                (VOID *) Uhc->FrameBase
+                );
+
+  if (Uhc->FrameBaseHostAddr != NULL) {
+    FreePool (Uhc->FrameBaseHostAddr);
+  }
+
+  if (Uhc->SyncIntQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->SyncIntQh, sizeof (UHCI_QH_SW));
+  }
+
+  if (Uhc->CtrlQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->CtrlQh, sizeof (UHCI_QH_SW));
+  }
+
+  if (Uhc->BulkQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->BulkQh, sizeof (UHCI_QH_SW));
+  }
+
+  Uhc->FrameBase           = NULL;
+  Uhc->FrameBaseHostAddr   = NULL;
+  Uhc->SyncIntQh           = NULL;
+  Uhc->CtrlQh              = NULL;
+  Uhc->BulkQh              = NULL;
+}
+
+
+/**
+  Convert the poll rate to the maxium 2^n that is smaller
+  than Interval.
+
+  @param  Interval               The poll rate to convert.
+
+  @return The converted poll rate.
+
+**/
+UINTN
+UhciConvertPollRate (
+  IN  UINTN               Interval
+  )
+{
+  UINTN                   BitCount;
+
+  ASSERT (Interval != 0);
+
+  //
+  // Find the index (1 based) of the highest non-zero bit
+  //
+  BitCount = 0;
+
+  while (Interval != 0) {
+    Interval >>= 1;
+    BitCount++;
+  }
+
+  return (UINTN)1 << (BitCount - 1);
+}
+
+
+/**
+  Link a queue head (for asynchronous interrupt transfer) to
+  the frame list.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head to link into.
+
+**/
+VOID
+UhciLinkQhToFrameList (
+  USB_HC_DEV              *Uhc,
+  UHCI_QH_SW              *Qh
+  )
+{
+  UINTN                   Index;
+  UHCI_QH_SW              *Prev;
+  UHCI_QH_SW              *Next;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+  EFI_PHYSICAL_ADDRESS    QhPciAddr;
+
+  ASSERT ((Uhc->FrameBase != NULL) && (Qh != NULL));
+
+  QhPciAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Qh, sizeof (UHCI_QH_HW));
+
+  for (Index = 0; Index < UHCI_FRAME_NUM; Index += Qh->Interval) {
+    //
+    // First QH can't be NULL because we always keep static queue
+    // heads on the frame list
+    //
+    ASSERT (!LINK_TERMINATED (Uhc->FrameBase[Index]));
+    Next  = (UHCI_QH_SW*)(UINTN)Uhc->FrameBaseHostAddr[Index];
+    Prev  = NULL;
+
+    //
+    // Now, insert the queue head (Qh) into this frame:
+    // 1. Find a queue head with the same poll interval, just insert
+    //    Qh after this queue head, then we are done.
+    //
+    // 2. Find the position to insert the queue head into:
+    //      Previous head's interval is bigger than Qh's
+    //      Next head's interval is less than Qh's
+    //    Then, insert the Qh between then
+    //
+    // This method is very much the same as that used by EHCI.
+    // Because each QH's interval is round down to 2^n, poll
+    // rate is correct.
+    //
+    while (Next->Interval > Qh->Interval) {
+      Prev  = Next;
+      Next  = Next->NextQh;
+      ASSERT (Next != NULL);
+    }
+
+    //
+    // The entry may have been linked into the frame by early insertation.
+    // For example: if insert a Qh with Qh.Interval == 4, and there is a Qh
+    // with Qh.Interval == 8 on the frame. If so, we are done with this frame.
+    // It isn't necessary to compare all the QH with the same interval to
+    // Qh. This is because if there is other QH with the same interval, Qh
+    // should has been inserted after that at FrameBase[0] and at FrameBase[0] it is
+    // impossible (Next == Qh)
+    //
+    if (Next == Qh) {
+      continue;
+    }
+
+    if (Next->Interval == Qh->Interval) {
+      //
+      // If there is a QH with the same interval, it locates at
+      // FrameBase[0], and we can simply insert it after this QH. We
+      // are all done.
+      //
+      ASSERT ((Index == 0) && (Qh->NextQh == NULL));
+
+      Prev                    = Next;
+      Next                    = Next->NextQh;
+
+      Qh->NextQh              = Next;
+      Prev->NextQh            = Qh;
+
+      Qh->QhHw.HorizonLink    = Prev->QhHw.HorizonLink;
+
+      Prev->QhHw.HorizonLink  = QH_HLINK (QhPciAddr, FALSE);
+      break;
+    }
+
+    //
+    // OK, find the right position, insert it in. If Qh's next
+    // link has already been set, it is in position. This is
+    // guarranted by 2^n polling interval.
+    //
+    if (Qh->NextQh == NULL) {
+      Qh->NextQh            = Next;
+      PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Next, sizeof (UHCI_QH_HW));
+      Qh->QhHw.HorizonLink  = QH_HLINK (PhyAddr, FALSE);
+    }
+
+    if (Prev == NULL) {
+      Uhc->FrameBase[Index]           = QH_HLINK (QhPciAddr, FALSE);
+      Uhc->FrameBaseHostAddr[Index]   = (UINT32)(UINTN)Qh;
+    } else {
+      Prev->NextQh            = Qh;
+      Prev->QhHw.HorizonLink  = QH_HLINK (QhPciAddr, FALSE);
+    }
+  }
+}
+
+
+/**
+  Unlink QH from the frame list is easier: find all
+  the precedence node, and pointer there next to QhSw's
+  next.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head to unlink.
+
+**/
+VOID
+UhciUnlinkQhFromFrameList (
+  USB_HC_DEV              *Uhc,
+  UHCI_QH_SW              *Qh
+  )
+{
+  UINTN                   Index;
+  UHCI_QH_SW              *Prev;
+  UHCI_QH_SW              *This;
+
+  ASSERT ((Uhc->FrameBase != NULL) && (Qh != NULL));
+
+  for (Index = 0; Index < UHCI_FRAME_NUM; Index += Qh->Interval) {
+    //
+    // Frame link can't be NULL because we always keep static
+    // queue heads on the frame list
+    //
+    ASSERT (!LINK_TERMINATED (Uhc->FrameBase[Index]));
+    This  = (UHCI_QH_SW*)(UINTN)Uhc->FrameBaseHostAddr[Index];
+    Prev  = NULL;
+
+    //
+    // Walk through the frame's QH list to find the
+    // queue head to remove
+    //
+    while ((This != NULL) && (This != Qh)) {
+      Prev  = This;
+      This  = This->NextQh;
+    }
+
+    //
+    // Qh may have already been unlinked from this frame
+    // by early action.
+    //
+    if (This == NULL) {
+      continue;
+    }
+
+    if (Prev == NULL) {
+      //
+      // Qh is the first entry in the frame
+      //
+      Uhc->FrameBase[Index]           = Qh->QhHw.HorizonLink;
+      Uhc->FrameBaseHostAddr[Index]   = (UINT32)(UINTN)Qh->NextQh;
+    } else {
+      Prev->NextQh            = Qh->NextQh;
+      Prev->QhHw.HorizonLink  = Qh->QhHw.HorizonLink;
+    }
+  }
+}
+
+
+/**
+  Check TDs Results.
+
+  @param  Uhc                    This UHCI device.
+  @param  Td                     UHCI_TD_SW to check.
+  @param  IsLow                  Is Low Speed Device.
+  @param  QhResult               Return the result of this TD list.
+
+  @return Whether the TD's result is finialized.
+
+**/
+BOOLEAN
+UhciCheckTdStatus (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UHCI_TD_SW          *Td,
+  IN  BOOLEAN             IsLow,
+  OUT UHCI_QH_RESULT      *QhResult
+  )
+{
+  UINTN                   Len;
+  UINT8                   State;
+  UHCI_TD_HW              *TdHw;
+  BOOLEAN                 Finished;
+
+  Finished             = TRUE;
+
+  //
+  // Initialize the data toggle to that of the first
+  // TD. The next toggle to use is either:
+  // 1. first TD's toggle if no TD is executed OK
+  // 2. the next toggle of last executed-OK TD
+  //
+  QhResult->Result     = EFI_USB_NOERROR;
+  QhResult->NextToggle = (UINT8)Td->TdHw.DataToggle;
+  QhResult->Complete   = 0;
+
+  while (Td != NULL) {
+    TdHw  = &Td->TdHw;
+    State = (UINT8)TdHw->Status;
+
+    //
+    // UHCI will set STALLED bit when it abort the execution
+    // of TD list. There are several reasons:
+    //   1. BABBLE error happened
+    //   2. Received a STALL response
+    //   3. Error count decreased to zero.
+    //
+    // It also set CRC/Timeout/NAK/Buffer Error/BitStuff Error
+    // bits when corresponding conditions happen. But these
+    // conditions are not deadly, that is a TD can successfully
+    // completes even these bits are set. But it is likely that
+    // upper layer won't distinguish these condtions. So, only
+    // set these bits when TD is actually halted.
+    //
+    if ((State & USBTD_STALLED) != 0) {
+      if ((State & USBTD_BABBLE) != 0) {
+        QhResult->Result |= EFI_USB_ERR_BABBLE;
+
+      } else if (TdHw->ErrorCount != 0) {
+        QhResult->Result |= EFI_USB_ERR_STALL;
+      }
+
+      if ((State & USBTD_CRC) != 0) {
+        QhResult->Result |= EFI_USB_ERR_CRC;
+      }
+
+      if ((State & USBTD_BUFFERR) != 0) {
+        QhResult->Result |= EFI_USB_ERR_BUFFER;
+      }
+
+      if ((Td->TdHw.Status & USBTD_BITSTUFF) != 0) {
+        QhResult->Result |= EFI_USB_ERR_BITSTUFF;
+      }
+
+      if (TdHw->ErrorCount == 0) {
+        QhResult->Result |= EFI_USB_ERR_TIMEOUT;
+      }
+
+      Finished = TRUE;
+      goto ON_EXIT;
+
+    } else if ((State & USBTD_ACTIVE) != 0) {
+      //
+      // The TD is still active, no need to check further.
+      //
+      QhResult->Result |= EFI_USB_ERR_NOTEXECUTE;
+
+      Finished = FALSE;
+      goto ON_EXIT;
+
+    } else {
+      //
+      // Update the next data toggle, it is always the
+      // next to the last known-good TD's data toggle if
+      // any TD is executed OK
+      //
+      QhResult->NextToggle = (UINT8) (1 - (UINT8)TdHw->DataToggle);
+
+      //
+      // This TD is finished OK or met short packet read. Update the
+      // transfer length if it isn't a SETUP.
+      //
+      Len = (TdHw->ActualLen + 1) & 0x7FF;
+
+      if (TdHw->PidCode != SETUP_PACKET_ID) {
+        QhResult->Complete += Len;
+      }
+
+      //
+      // Short packet condition for full speed input TD, also
+      // terminate the transfer
+      //
+      if (!IsLow && (TdHw->ShortPacket == 1) && (Len < Td->DataLen)) {
+        DEBUG ((EFI_D_VERBOSE, "UhciCheckTdStatus: short packet read occured\n"));
+
+        Finished = TRUE;
+        goto ON_EXIT;
+      }
+    }
+
+    Td = Td->NextTd;
+  }
+
+ON_EXIT:
+  //
+  // Check whether HC is halted. Don't move this up. It must be
+  // called after data toggle is successfully updated.
+  //
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    QhResult->Result |= EFI_USB_ERR_SYSTEM;
+    Finished  = TRUE;
+  }
+
+  if (Finished) {
+    Uhc->PciIo->Flush (Uhc->PciIo);
+  }
+
+  UhciAckAllInterrupt (Uhc);
+  return Finished;
+}
+
+
+/**
+  Check the result of the transfer.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head of the transfer.
+  @param  Td                     The first TDs of the transfer.
+  @param  TimeOut                TimeOut value in milliseconds.
+  @param  IsLow                  Is Low Speed Device.
+  @param  QhResult               The variable to return result.
+
+  @retval EFI_SUCCESS            The transfer finished with success.
+  @retval EFI_DEVICE_ERROR       Transfer failed.
+
+**/
+EFI_STATUS
+UhciExecuteTransfer (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UHCI_QH_SW          *Qh,
+  IN  UHCI_TD_SW          *Td,
+  IN  UINTN               TimeOut,
+  IN  BOOLEAN             IsLow,
+  OUT UHCI_QH_RESULT      *QhResult
+  )
+{
+  UINTN                   Index;
+  UINTN                   Delay;
+  BOOLEAN                 Finished;
+  EFI_STATUS              Status;
+  BOOLEAN                 InfiniteLoop;
+
+  Finished     = FALSE;
+  Status       = EFI_SUCCESS;
+  Delay        = TimeOut * UHC_1_MILLISECOND;
+  InfiniteLoop = FALSE;
+
+  //
+  // According to UEFI spec section 16.2.4, If Timeout is 0, then the caller
+  // must wait for the function to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR
+  // is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  for (Index = 0; InfiniteLoop || (Index < Delay); Index++) {
+    Finished = UhciCheckTdStatus (Uhc, Td, IsLow, QhResult);
+
+    //
+    // Transfer is OK or some error occured (TD inactive)
+    //
+    if (Finished) {
+      break;
+    }
+
+    gBS->Stall (UHC_1_MICROSECOND);
+  }
+
+  if (!Finished) {
+    DEBUG ((EFI_D_ERROR, "UhciExecuteTransfer: execution not finished for %dms\n", (UINT32)TimeOut));
+    UhciDumpQh (Qh);
+    UhciDumpTds (Td);
+
+    Status = EFI_TIMEOUT;
+
+  } else if (QhResult->Result != EFI_USB_NOERROR) {
+    DEBUG ((EFI_D_ERROR, "UhciExecuteTransfer: execution failed with result %x\n", QhResult->Result));
+    UhciDumpQh (Qh);
+    UhciDumpTds (Td);
+
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+
+/**
+  Update Async Request, QH and TDs.
+
+  @param  Uhc                    The UHCI device.
+  @param  AsyncReq               The UHCI asynchronous transfer to update.
+  @param  Result                 Transfer reslut.
+  @param  NextToggle             The toggle of next data.
+
+**/
+VOID
+UhciUpdateAsyncReq (
+  IN USB_HC_DEV          *Uhc,
+  IN UHCI_ASYNC_REQUEST  *AsyncReq,
+  IN UINT32              Result,
+  IN UINT32              NextToggle
+  )
+{
+  UHCI_QH_SW              *Qh;
+  UHCI_TD_SW              *FirstTd;
+  UHCI_TD_SW              *Td;
+
+  Qh          = AsyncReq->QhSw;
+  FirstTd     = AsyncReq->FirstTd;
+
+  if (Result == EFI_USB_NOERROR) {
+    //
+    // The last transfer succeeds. Then we need to update
+    // the Qh and Td for next round of transfer.
+    // 1. Update the TD's data toggle
+    // 2. Activate all the TDs
+    // 3. Link the TD to the queue head again since during
+    //    execution, queue head's TD pointer is changed by
+    //    hardware.
+    //
+    for (Td = FirstTd; Td != NULL; Td = Td->NextTd) {
+      Td->TdHw.DataToggle = NextToggle;
+      NextToggle         ^= 1;
+      Td->TdHw.Status    |= USBTD_ACTIVE;
+    }
+
+    UhciLinkTdToQh (Uhc, Qh, FirstTd);
+    return ;
+  }
+}
+
+
+/**
+  Create Async Request node, and Link to List.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head of the transfer.
+  @param  FirstTd                First TD of the transfer.
+  @param  DevAddr                Device Address.
+  @param  EndPoint               EndPoint Address.
+  @param  DataLen                Data length.
+  @param  Interval               Polling Interval when inserted to frame list.
+  @param  Data                   Data buffer, unmapped.
+  @param  Callback               Callback after interrupt transfeer.
+  @param  Context                Callback Context passed as function parameter.
+  @param  IsLow                  Is Low Speed.
+
+  @retval EFI_SUCCESS            An asynchronous transfer is created.
+  @retval EFI_INVALID_PARAMETER  Paremeter is error.
+  @retval EFI_OUT_OF_RESOURCES   Failed because of resource shortage.
+
+**/
+EFI_STATUS
+UhciCreateAsyncReq (
+  IN USB_HC_DEV                       *Uhc,
+  IN UHCI_QH_SW                       *Qh,
+  IN UHCI_TD_SW                       *FirstTd,
+  IN UINT8                            DevAddr,
+  IN UINT8                            EndPoint,
+  IN UINTN                            DataLen,
+  IN UINTN                            Interval,
+  IN UINT8                            *Data,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  Callback,
+  IN VOID                             *Context,
+  IN BOOLEAN                          IsLow
+  )
+{
+  UHCI_ASYNC_REQUEST      *AsyncReq;
+
+  AsyncReq = AllocatePool (sizeof (UHCI_ASYNC_REQUEST));
+
+  if (AsyncReq == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Fill Request field. Data is allocated host memory, not mapped
+  //
+  AsyncReq->Signature   = UHCI_ASYNC_INT_SIGNATURE;
+  AsyncReq->DevAddr     = DevAddr;
+  AsyncReq->EndPoint    = EndPoint;
+  AsyncReq->DataLen     = DataLen;
+  AsyncReq->Interval    = UhciConvertPollRate(Interval);
+  AsyncReq->Data        = Data;
+  AsyncReq->Callback    = Callback;
+  AsyncReq->Context     = Context;
+  AsyncReq->QhSw        = Qh;
+  AsyncReq->FirstTd     = FirstTd;
+  AsyncReq->IsLow       = IsLow;
+
+  //
+  // Insert the new interrupt transfer to the head of the list.
+  // The interrupt transfer's monitor function scans the whole
+  // list from head to tail. The new interrupt transfer MUST be
+  // added to the head of the list.
+  //
+  InsertHeadList (&(Uhc->AsyncIntList), &(AsyncReq->Link));
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Free an asynchronous request's resource such as memory.
+
+  @param  Uhc                    The UHCI device.
+  @param  AsyncReq               The asynchronous request to free.
+
+**/
+VOID
+UhciFreeAsyncReq (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_ASYNC_REQUEST   *AsyncReq
+  )
+{
+  ASSERT ((Uhc != NULL) && (AsyncReq != NULL));
+
+  UhciDestoryTds (Uhc, AsyncReq->FirstTd);
+  UsbHcFreeMem (Uhc->MemPool, AsyncReq->QhSw, sizeof (UHCI_QH_SW));
+
+  if (AsyncReq->Data != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, AsyncReq->Data, AsyncReq->DataLen);
+  }
+
+  gBS->FreePool (AsyncReq);
+}
+
+
+/**
+  Unlink an asynchronous request's from UHC's asynchronus list.
+  also remove the queue head from the frame list. If FreeNow,
+  release its resource also. Otherwise, add the request to the
+  UHC's recycle list to wait for a while before release the memory.
+  Until then, hardware won't hold point to the request.
+
+  @param  Uhc                    The UHCI device.
+  @param  AsyncReq               The asynchronous request to free.
+  @param  FreeNow                If TRUE, free the resource immediately, otherwise
+                                 add the request to recycle wait list.
+
+**/
+VOID
+UhciUnlinkAsyncReq (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_ASYNC_REQUEST   *AsyncReq,
+  IN BOOLEAN              FreeNow
+  )
+{
+  ASSERT ((Uhc != NULL) && (AsyncReq != NULL));
+
+  RemoveEntryList (&(AsyncReq->Link));
+  UhciUnlinkQhFromFrameList (Uhc, AsyncReq->QhSw);
+
+  if (FreeNow) {
+    UhciFreeAsyncReq (Uhc, AsyncReq);
+  } else {
+    //
+    // To sychronize with hardware, mark the queue head as inactive
+    // then add AsyncReq to UHC's recycle list
+    //
+    AsyncReq->QhSw->QhHw.VerticalLink = QH_VLINK (NULL, TRUE);
+    AsyncReq->Recycle = Uhc->RecycleWait;
+    Uhc->RecycleWait  = AsyncReq;
+  }
+}
+
+
+/**
+  Delete Async Interrupt QH and TDs.
+
+  @param  Uhc                    The UHCI device.
+  @param  DevAddr                Device Address.
+  @param  EndPoint               EndPoint Address.
+  @param  Toggle                 The next data toggle to use.
+
+  @retval EFI_SUCCESS            The request is deleted.
+  @retval EFI_INVALID_PARAMETER  Paremeter is error.
+  @retval EFI_NOT_FOUND          The asynchronous isn't found.
+
+**/
+EFI_STATUS
+UhciRemoveAsyncReq (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               EndPoint,
+  OUT UINT8               *Toggle
+  )
+{
+  EFI_STATUS          Status;
+  UHCI_ASYNC_REQUEST  *AsyncReq;
+  UHCI_QH_RESULT      QhResult;
+  LIST_ENTRY          *Link;
+  BOOLEAN             Found;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // If no asynchronous interrupt transaction exists
+  //
+  if (IsListEmpty (&(Uhc->AsyncIntList))) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Find the asynchronous transfer to this device/endpoint pair
+  //
+  Found = FALSE;
+  Link  = Uhc->AsyncIntList.ForwardLink;
+
+  do {
+    AsyncReq  = UHCI_ASYNC_INT_FROM_LINK (Link);
+    Link      = Link->ForwardLink;
+
+    if ((AsyncReq->DevAddr == DevAddr) && (AsyncReq->EndPoint == EndPoint)) {
+      Found = TRUE;
+      break;
+    }
+
+  } while (Link != &(Uhc->AsyncIntList));
+
+  if (!Found) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Check the result of the async transfer then update it
+  // to get the next data toggle to use.
+  //
+  UhciCheckTdStatus (Uhc, AsyncReq->FirstTd, AsyncReq->IsLow, &QhResult);
+  *Toggle = QhResult.NextToggle;
+
+  //
+  // Don't release the request now, keep it to synchronize with hardware.
+  //
+  UhciUnlinkAsyncReq (Uhc, AsyncReq, FALSE);
+  return Status;
+}
+
+
+/**
+  Recycle the asynchronouse request. When a queue head
+  is unlinked from frame list, host controller hardware
+  may still hold a cached pointer to it. To synchronize
+  with hardware, the request is released in two steps:
+  first it is linked to the UHC's RecycleWait list. At
+  the next time UhciMonitorAsyncReqList is fired, it is
+  moved to UHC's Recylelist. Then, at another timer
+  activation, all the requests on Recycle list is freed.
+  This guarrantes that each unlink queue head keeps
+  existing for at least 50ms, far enough for the hardware
+  to clear its cache.
+
+  @param  Uhc                    The UHCI device.
+
+**/
+VOID
+UhciRecycleAsyncReq (
+  IN USB_HC_DEV           *Uhc
+  )
+{
+  UHCI_ASYNC_REQUEST      *Req;
+  UHCI_ASYNC_REQUEST      *Next;
+
+  Req = Uhc->Recycle;
+
+  while (Req != NULL) {
+    Next = Req->Recycle;
+    UhciFreeAsyncReq (Uhc, Req);
+    Req  = Next;
+  }
+
+  Uhc->Recycle     = Uhc->RecycleWait;
+  Uhc->RecycleWait = NULL;
+}
+
+
+
+/**
+  Release all the asynchronous transfers on the lsit.
+
+  @param  Uhc                    The UHCI device.
+
+**/
+VOID
+UhciFreeAllAsyncReq (
+  IN USB_HC_DEV           *Uhc
+  )
+{
+  LIST_ENTRY              *Head;
+  UHCI_ASYNC_REQUEST      *AsyncReq;
+
+  //
+  // Call UhciRecycleAsyncReq twice. The requests on Recycle
+  // will be released at the first call; The requests on
+  // RecycleWait will be released at the second call.
+  //
+  UhciRecycleAsyncReq (Uhc);
+  UhciRecycleAsyncReq (Uhc);
+
+  Head = &(Uhc->AsyncIntList);
+
+  if (IsListEmpty (Head)) {
+    return;
+  }
+
+  while (!IsListEmpty (Head)) {
+    AsyncReq  = UHCI_ASYNC_INT_FROM_LINK (Head->ForwardLink);
+    UhciUnlinkAsyncReq (Uhc, AsyncReq, TRUE);
+  }
+}
+
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                  The event of the time.
+  @param  Context                Context of the event, pointer to USB_HC_DEV.
+
+**/
+VOID
+EFIAPI
+UhciMonitorAsyncReqList (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  UHCI_ASYNC_REQUEST      *AsyncReq;
+  LIST_ENTRY              *Link;
+  USB_HC_DEV              *Uhc;
+  VOID                    *Data;
+  BOOLEAN                 Finished;
+  UHCI_QH_RESULT          QhResult;
+
+  Uhc = (USB_HC_DEV *) Context;
+
+  //
+  // Recycle the asynchronous requests expired, and promote
+  // requests waiting to be recycled the next time when this
+  // timer expires
+  //
+  UhciRecycleAsyncReq (Uhc);
+
+  if (IsListEmpty (&(Uhc->AsyncIntList))) {
+    return ;
+  }
+
+  //
+  // This loop must be delete safe
+  //
+  Link = Uhc->AsyncIntList.ForwardLink;
+
+  do {
+    AsyncReq  = UHCI_ASYNC_INT_FROM_LINK (Link);
+    Link      = Link->ForwardLink;
+
+    Finished = UhciCheckTdStatus (Uhc, AsyncReq->FirstTd, AsyncReq->IsLow, &QhResult);
+
+    if (!Finished) {
+      continue;
+    }
+
+    //
+    // Copy the data to temporary buffer if there are some
+    // data transferred. We may have zero-length packet
+    //
+    Data = NULL;
+
+    if (QhResult.Complete != 0) {
+      Data = AllocatePool (QhResult.Complete);
+
+      if (Data == NULL) {
+        return ;
+      }
+
+      CopyMem (Data, AsyncReq->FirstTd->Data, QhResult.Complete);
+    }
+
+    UhciUpdateAsyncReq (Uhc, AsyncReq, QhResult.Result, QhResult.NextToggle);
+
+    //
+    // Now, either transfer is SUCCESS or met errors since
+    // we have skipped to next transfer earlier if current
+    // transfer is still active.
+    //
+    if (QhResult.Result == EFI_USB_NOERROR) {
+      AsyncReq->Callback (Data, QhResult.Complete, AsyncReq->Context, QhResult.Result);
+    } else {
+      //
+      // Leave error recovery to its related device driver.
+      // A common case of the error recovery is to re-submit
+      // the interrupt transfer. When an interrupt transfer
+      // is re-submitted, its position in the linked list is
+      // changed. It is inserted to the head of the linked
+      // list, while this function scans the whole list from
+      // head to tail. Thus, the re-submitted interrupt transfer's
+      // callback function will not be called again in this round.
+      //
+      AsyncReq->Callback (NULL, 0, AsyncReq->Context, QhResult.Result);
+    }
+
+    if (Data != NULL) {
+      gBS->FreePool (Data);
+    }
+  } while (Link != &(Uhc->AsyncIntList));
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.h
new file mode 100644
index 0000000000..f6d4fc40a7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.h
@@ -0,0 +1,271 @@
+/** @file
+
+  The definition for EHCI register operation routines.
+
+Copyright (c) 2007 - 2010, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_UHCI_SCHED_H_
+#define _EFI_UHCI_SCHED_H_
+
+
+#define UHCI_ASYNC_INT_SIGNATURE  SIGNATURE_32 ('u', 'h', 'c', 'a')
+//
+// The failure mask for USB transfer return status. If any of
+// these bit is set, the transfer failed. EFI_USB_ERR_NOEXECUTE
+// and EFI_USB_ERR_NAK are not considered as error condition:
+// the transfer is still going on.
+//
+#define USB_ERR_FAIL_MASK  (EFI_USB_ERR_STALL   | EFI_USB_ERR_BUFFER | \
+                            EFI_USB_ERR_BABBLE  | EFI_USB_ERR_CRC    | \
+                            EFI_USB_ERR_TIMEOUT | EFI_USB_ERR_BITSTUFF | \
+                            EFI_USB_ERR_SYSTEM)
+
+
+//
+// Structure to return the result of UHCI QH execution.
+// Result is the final result of the QH's QTD. NextToggle
+// is the next data toggle to use. Complete is the actual
+// length of data transferred.
+//
+typedef struct {
+  UINT32                  Result;
+  UINT8                   NextToggle;
+  UINTN                   Complete;
+} UHCI_QH_RESULT;
+
+typedef struct _UHCI_ASYNC_REQUEST  UHCI_ASYNC_REQUEST;
+
+//
+// Structure used to manager the asynchronous interrupt transfers.
+//
+struct _UHCI_ASYNC_REQUEST{
+  UINTN                           Signature;
+  LIST_ENTRY                      Link;
+  UHCI_ASYNC_REQUEST              *Recycle;
+
+  //
+  // Endpoint attributes
+  //
+  UINT8                           DevAddr;
+  UINT8                           EndPoint;
+  BOOLEAN                         IsLow;
+  UINTN                           Interval;
+
+  //
+  // Data and UHC structures
+  //
+  UHCI_QH_SW                      *QhSw;
+  UHCI_TD_SW                      *FirstTd;
+  UINT8                           *Data;      // Allocated host memory, not mapped memory
+  UINTN                           DataLen;
+  VOID                            *Mapping;
+
+  //
+  // User callback and its context
+  //
+  EFI_ASYNC_USB_TRANSFER_CALLBACK Callback;
+  VOID                            *Context;
+};
+
+#define UHCI_ASYNC_INT_FROM_LINK(a) \
+          CR (a, UHCI_ASYNC_REQUEST, Link, UHCI_ASYNC_INT_SIGNATURE)
+
+
+/**
+  Create Frame List Structure.
+
+  @param  Uhc                    The UHCI device.
+
+  @return EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @return EFI_UNSUPPORTED        Map memory fail.
+  @return EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+UhciInitFrameList (
+  IN USB_HC_DEV         *Uhc
+  );
+
+/**
+  Destory FrameList buffer.
+
+  @param  Uhc                    The UHCI device.
+
+  @return None.
+
+**/
+VOID
+UhciDestoryFrameList (
+  IN USB_HC_DEV           *Uhc
+  );
+
+
+/**
+  Convert the poll rate to the maxium 2^n that is smaller
+  than Interval.
+
+  @param  Interval               The poll rate to convert.
+
+  @return The converted poll rate.
+
+**/
+UINTN
+UhciConvertPollRate (
+  IN  UINTN               Interval
+  );
+
+
+/**
+  Link a queue head (for asynchronous interrupt transfer) to
+  the frame list.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head to link into.
+
+**/
+VOID
+UhciLinkQhToFrameList (
+  USB_HC_DEV              *Uhc,
+  UHCI_QH_SW              *Qh
+  );
+
+
+/**
+  Unlink QH from the frame list is easier: find all
+  the precedence node, and pointer there next to QhSw's
+  next.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head to unlink.
+
+**/
+VOID
+UhciUnlinkQhFromFrameList (
+  USB_HC_DEV              *Uhc,
+  UHCI_QH_SW              *Qh
+  );
+
+
+/**
+  Check the result of the transfer.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head of the transfer.
+  @param  Td                     The first TDs of the transfer.
+  @param  TimeOut                TimeOut value in milliseconds.
+  @param  IsLow                  Is Low Speed Device.
+  @param  QhResult               The variable to return result.
+
+  @retval EFI_SUCCESS            The transfer finished with success.
+  @retval EFI_DEVICE_ERROR       Transfer failed.
+
+**/
+EFI_STATUS
+UhciExecuteTransfer (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UHCI_QH_SW          *Qh,
+  IN  UHCI_TD_SW          *Td,
+  IN  UINTN               TimeOut,
+  IN  BOOLEAN             IsLow,
+  OUT UHCI_QH_RESULT      *QhResult
+  );
+
+
+/**
+  Create Async Request node, and Link to List.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head of the transfer.
+  @param  FirstTd                First TD of the transfer.
+  @param  DevAddr                Device Address.
+  @param  EndPoint               EndPoint Address.
+  @param  DataLen                Data length.
+  @param  Interval               Polling Interval when inserted to frame list.
+  @param  Data                   Data buffer, unmapped.
+  @param  Callback               Callback after interrupt transfeer.
+  @param  Context                Callback Context passed as function parameter.
+  @param  IsLow                  Is Low Speed.
+
+  @retval EFI_SUCCESS            An asynchronous transfer is created.
+  @retval EFI_INVALID_PARAMETER  Paremeter is error.
+  @retval EFI_OUT_OF_RESOURCES   Failed because of resource shortage.
+
+**/
+EFI_STATUS
+UhciCreateAsyncReq (
+  IN USB_HC_DEV                       *Uhc,
+  IN UHCI_QH_SW                       *Qh,
+  IN UHCI_TD_SW                       *FirstTd,
+  IN UINT8                            DevAddr,
+  IN UINT8                            EndPoint,
+  IN UINTN                            DataLen,
+  IN UINTN                            Interval,
+  IN UINT8                            *Data,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  Callback,
+  IN VOID                             *Context,
+  IN BOOLEAN                          IsLow
+  );
+
+
+/**
+  Delete Async Interrupt QH and TDs.
+
+  @param  Uhc                    The UHCI device.
+  @param  DevAddr                Device Address.
+  @param  EndPoint               EndPoint Address.
+  @param  Toggle                 The next data toggle to use.
+
+  @retval EFI_SUCCESS            The request is deleted.
+  @retval EFI_INVALID_PARAMETER  Paremeter is error.
+  @retval EFI_NOT_FOUND          The asynchronous isn't found.
+
+**/
+EFI_STATUS
+UhciRemoveAsyncReq (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               EndPoint,
+  OUT UINT8               *Toggle
+  );
+
+
+/**
+  Release all the asynchronous transfers on the lsit.
+
+  @param  Uhc                    The UHCI device.
+
+  @return None.
+
+**/
+VOID
+UhciFreeAllAsyncReq (
+  IN USB_HC_DEV           *Uhc
+  );
+
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                  The event of the time.
+  @param  Context                Context of the event, pointer to USB_HC_DEV.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+UhciMonitorAsyncReqList (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.c
new file mode 100644
index 0000000000..e42d482885
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.c
@@ -0,0 +1,564 @@
+/** @file
+
+  The routine procedure for uhci memory allocate/free.
+
+Copyright (c) 2007 - 2010, 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 "Uhci.h"
+
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pool           The buffer pool to allocate memory for.
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Pages
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  VOID                    *BufHost;
+  VOID                    *Mapping;
+  EFI_PHYSICAL_ADDRESS    MappedAddr;
+  UINTN                   Bytes;
+  EFI_STATUS              Status;
+
+  PciIo = Pool->PciIo;
+
+  Block = AllocateZeroPool (sizeof (USBHC_MEM_BLOCK));
+  if (Block == NULL) {
+    return NULL;
+  }
+
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (USBHC_MEM_UNIT * 8);
+  Block->Bits     = AllocateZeroPool (Block->BitsLen);
+
+  if (Block->Bits == NULL) {
+    gBS->FreePool (Block);
+    return NULL;
+  }
+
+  //
+  // Allocate the number of Pages of memory, then map it for
+  // bus master read and write.
+  //
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    Pages,
+                    &BufHost,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_BITARRAY;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (Pages);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    BufHost,
+                    &Bytes,
+                    &MappedAddr,
+                    &Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (Pages))) {
+    goto FREE_BUFFER;
+  }
+
+  //
+  // Check whether the data structure used by the host controller
+  // should be restricted into the same 4G
+  //
+  if (Pool->Check4G && (Pool->Which4G != USB_HC_HIGH_32BIT (MappedAddr))) {
+    PciIo->Unmap (PciIo, Mapping);
+    goto FREE_BUFFER;
+  }
+
+  Block->BufHost  = BufHost;
+  Block->Buf      = (UINT8 *) ((UINTN) MappedAddr);
+  Block->Mapping  = Mapping;
+
+  return Block;
+
+FREE_BUFFER:
+  PciIo->FreeBuffer (PciIo, Pages, BufHost);
+
+FREE_BITARRAY:
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+  return NULL;
+}
+
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL       *Pool,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+
+  ASSERT ((Pool != NULL) && (Block != NULL));
+
+  PciIo = Pool->PciIo;
+
+  //
+  // Unmap the common buffer then free the structures
+  //
+  PciIo->Unmap (PciIo, Block->Mapping);
+  PciIo->FreeBuffer (PciIo, EFI_SIZE_TO_PAGES (Block->BufLen), Block->BufHost);
+
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+}
+
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block           The memory block to allocate memory from.
+  @param  Units           Number of memory units to allocate.
+
+  @return EFI_SUCCESS     The needed memory is allocated.
+  @return EFI_NOT_FOUND   Can't find the free memory.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN  USBHC_MEM_BLOCK     *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | (UINT8) USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->Buf + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  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.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+
+/**
+  Is the memory block empty?
+
+  @param  Block     The memory block to check.
+
+  @return TRUE      The memory block is empty.
+  @return FALSE     The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  Check4G               Whether the host controller requires allocated memory
+                                from one 4G address space.
+  @param  Which4G               The 4G memory area each memory allocated should be from.
+
+  @return EFI_SUCCESS           The memory pool is initialized.
+  @return EFI_OUT_OF_RESOURCE   Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  )
+{
+  USBHC_MEM_POOL          *Pool;
+
+  Pool = AllocatePool (sizeof (USBHC_MEM_POOL));
+
+  if (Pool == NULL) {
+    return Pool;
+  }
+
+  Pool->PciIo   = PciIo;
+  Pool->Check4G = Check4G;
+  Pool->Which4G = Which4G;
+  Pool->Head    = UsbHcAllocMemBlock (Pool, USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    gBS->FreePool (Pool);
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+
+/**
+  Release the memory management pool.
+
+  @param  Pool               The USB memory pool to free.
+
+  @return EFI_SUCCESS        The memory pool is freed.
+  @return EFI_DEVICE_ERROR   Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+{
+  USBHC_MEM_BLOCK *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UsbHcUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    UsbHcUnlinkMemBlock (Pool->Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+  gBS->FreePool (Pool);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool           The host controller's memory pool.
+  @param  Size           Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  USBHC_MEM_BLOCK         *NewBlock;
+  VOID                    *Mem;
+  UINTN                   AllocSize;
+  UINTN                   Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+
+  NewBlock = UsbHcAllocMemBlock (Pool, Pages);
+
+  if (NewBlock == NULL) {
+    DEBUG ((EFI_D_INFO, "UsbHcAllocateMem: failed to allocate block\n"));
+    return NULL;
+  }
+
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->Buf <= ToFree) && ((ToFree + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->Buf) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->Buf) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    UsbHcUnlinkMemBlock (Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.h
new file mode 100644
index 0000000000..c53d0b78f2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.h
@@ -0,0 +1,161 @@
+/** @file
+
+  This file contains the definination for host controller memory management routines
+
+Copyright (c) 2007 - 2010, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_EHCI_MEM_H_
+#define _EFI_EHCI_MEM_H_
+
+#define USB_HC_BIT(a)                  ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+#define USB_HC_HIGH_32BIT(Addr64)    \
+          ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+struct _USBHC_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen;
+  UINT8                   *Buf;
+  UINT8                   *BufHost;
+  UINTN                   BufLen;   // Memory size in bytes
+  VOID                    *Mapping;
+  USBHC_MEM_BLOCK         *Next;
+};
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB
+// host controller. EHCI requires the control memory and transfer
+// data to be on the same 4G memory.
+//
+typedef struct _USBHC_MEM_POOL {
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  BOOLEAN                 Check4G;
+  UINT32                  Which4G;
+  USBHC_MEM_BLOCK         *Head;
+} USBHC_MEM_POOL;
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT           64
+
+#define USBHC_MEM_UNIT_MASK      (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_DEFAULT_PAGES  16
+
+#define USBHC_MEM_ROUND(Len)  (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo               The PciIo that can be used to access the host controller.
+  @param  Check4G             Whether the host controller requires allocated memory
+                              from one 4G address space.
+  @param  Which4G             The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS         The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  );
+
+
+/**
+  Release the memory management pool.
+
+  @param  Pool               The USB memory pool to free.
+
+  @return EFI_SUCCESS        The memory pool is freed.
+  @return EFI_DEVICE_ERROR   Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  );
+
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool  The host controller's memory pool.
+  @param  Size  Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  );
+
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool  The memory pool of the host controller.
+  @param  Mem   The memory to free.
+  @param  Size  The size of the memory to free.
+
+  @return None.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**
+  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
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.c b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.c
new file mode 100644
index 0000000000..368b75d753
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.c
@@ -0,0 +1,3219 @@
+/** @file
+PEIM to produce gPeiUsbHostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2006 - 2014, 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 "UhcPeim.h"
+
+/**
+  Initializes Usb Host Controller.
+
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            PPI successfully installed.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcPeimEntry (
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  PEI_USB_CONTROLLER_PPI      *ChipSetUsbControllerPpi;
+  EFI_STATUS                  Status;
+  UINT8                       Index;
+  UINTN                       ControllerType;
+  UINTN                       BaseAddress;
+  UINTN                       MemPages;
+  USB_UHC_DEV                 *UhcDev;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  Status = PeiServicesLocatePpi (
+             &gPeiUsbControllerPpiGuid,
+             0,
+             NULL,
+             (VOID **) &ChipSetUsbControllerPpi
+             );
+  //
+  // If failed to locate, it is a bug in dispather as depex has gPeiUsbControllerPpiGuid.
+  //
+  ASSERT_EFI_ERROR (Status);
+
+  Index = 0;
+  while (TRUE) {
+    Status = ChipSetUsbControllerPpi->GetUsbController (
+                                        (EFI_PEI_SERVICES **) PeiServices,
+                                        ChipSetUsbControllerPpi,
+                                        Index,
+                                        &ControllerType,
+                                        &BaseAddress
+                                        );
+    //
+    // When status is error, meant no controller is found
+    //
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    //
+    // This PEIM is for UHC type controller.
+    //
+    if (ControllerType != PEI_UHCI_CONTROLLER) {
+      Index++;
+      continue;
+    }
+
+    MemPages = sizeof (USB_UHC_DEV) / EFI_PAGE_SIZE + 1;
+
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesData,
+               MemPages,
+               &TempPtr
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    UhcDev = (USB_UHC_DEV *) ((UINTN) TempPtr);
+    UhcDev->Signature   = USB_UHC_DEV_SIGNATURE;
+    UhcDev->UsbHostControllerBaseAddress = (UINT32) BaseAddress;
+
+    //
+    // Init local memory management service
+    //
+    Status = InitializeMemoryManagement (UhcDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Initialize Uhc's hardware
+    //
+    Status = InitializeUsbHC (UhcDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    UhcDev->UsbHostControllerPpi.ControlTransfer          = UhcControlTransfer;
+    UhcDev->UsbHostControllerPpi.BulkTransfer             = UhcBulkTransfer;
+    UhcDev->UsbHostControllerPpi.GetRootHubPortNumber     = UhcGetRootHubPortNumber;
+    UhcDev->UsbHostControllerPpi.GetRootHubPortStatus     = UhcGetRootHubPortStatus;
+    UhcDev->UsbHostControllerPpi.SetRootHubPortFeature    = UhcSetRootHubPortFeature;
+    UhcDev->UsbHostControllerPpi.ClearRootHubPortFeature  = UhcClearRootHubPortFeature;
+
+    UhcDev->PpiDescriptor.Flags = (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+    UhcDev->PpiDescriptor.Guid  = &gPeiUsbHostControllerPpiGuid;
+    UhcDev->PpiDescriptor.Ppi   = &UhcDev->UsbHostControllerPpi;
+
+    Status = PeiServicesInstallPpi (&UhcDev->PpiDescriptor);
+    if (EFI_ERROR (Status)) {
+      Index++;
+      continue;
+    }
+
+    Index++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress          The target device address.
+  @param  DeviceSpeed            Target device speed.
+  @param  MaximumPacketLength    Maximum packet size the default control transfer 
+                                 endpoint is capable of sending or receiving.
+  @param  Request                USB device request to send.
+  @param  TransferDirection      Specifies the data direction for the data stage.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+  @param  TimeOut                Indicates the maximum timeout, in millisecond.
+                                 If Timeout is 0, then the caller must wait for the function
+                                 to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  TransferResult         Return the result of this control transfer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcControlTransfer (
+  IN     EFI_PEI_SERVICES               **PeiServices,
+  IN     PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN     UINT8                          DeviceAddress,
+  IN     UINT8                          DeviceSpeed,
+  IN     UINT8                          MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST         *Request,
+  IN     EFI_USB_DATA_DIRECTION         TransferDirection,
+  IN OUT VOID                           *Data                 OPTIONAL,
+  IN OUT UINTN                          *DataLength           OPTIONAL,
+  IN     UINTN                          TimeOut,
+  OUT    UINT32                         *TransferResult
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      StatusReg;
+  UINT8       PktID;
+  QH_STRUCT   *PtrQH;
+  TD_STRUCT   *PtrTD;
+  TD_STRUCT   *PtrPreTD;
+  TD_STRUCT   *PtrSetupTD;
+  TD_STRUCT   *PtrStatusTD;
+  EFI_STATUS  Status;
+  UINT32      DataLen;
+  UINT8       *PtrDataSource;
+  UINT8       *Ptr;
+  UINT8       DataToggle;
+
+  UhcDev      = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+
+  StatusReg   = UhcDev->UsbHostControllerBaseAddress + USBSTS;
+
+  PktID       = INPUT_PACKET_ID;
+
+  if (Request == NULL || TransferResult == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // if errors exist that cause host controller halt,
+  // then return EFI_DEVICE_ERROR.
+  //
+
+  if (!IsStatusOK (UhcDev, StatusReg)) {
+    ClearStatusReg (UhcDev, StatusReg);
+    *TransferResult = EFI_USB_ERR_SYSTEM;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ClearStatusReg (UhcDev, StatusReg);
+
+  //
+  // generate Setup Stage TD
+  //
+
+  PtrQH = UhcDev->ConfigQH;
+
+  GenSetupStageTD (
+    UhcDev,
+    DeviceAddress,
+    0,
+    DeviceSpeed,
+    (UINT8 *) Request,
+    (UINT8) sizeof (EFI_USB_DEVICE_REQUEST),
+    &PtrSetupTD
+    );
+
+  //
+  // link setup TD structures to QH structure
+  //
+  LinkTDToQH (PtrQH, PtrSetupTD);
+
+  PtrPreTD = PtrSetupTD;
+
+  //
+  //  Data Stage of Control Transfer
+  //
+  switch (TransferDirection) {
+
+  case EfiUsbDataIn:
+    PktID         = INPUT_PACKET_ID;
+    PtrDataSource = Data;
+    DataLen       = (UINT32) *DataLength;
+    Ptr           = PtrDataSource;
+    break;
+
+  case EfiUsbDataOut:
+    PktID         = OUTPUT_PACKET_ID;
+    PtrDataSource = Data;
+    DataLen       = (UINT32) *DataLength;
+    Ptr           = PtrDataSource;
+    break;
+
+  //
+  // no data stage
+  //
+  case EfiUsbNoData:
+    if (*DataLength != 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    PktID         = OUTPUT_PACKET_ID;
+    PtrDataSource = NULL;
+    DataLen       = 0;
+    Ptr           = NULL;
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DataToggle  = 1;
+
+  PtrTD       = PtrSetupTD;
+  while (DataLen > 0) {
+    //
+    // create TD structures and link together
+    //
+    UINT8 PacketSize;
+
+    //
+    // PacketSize is the data load size of each TD carries.
+    //
+    PacketSize = (UINT8) DataLen;
+    if (DataLen > MaximumPacketLength) {
+      PacketSize = MaximumPacketLength;
+    }
+
+    GenDataTD (
+      UhcDev,
+      DeviceAddress,
+      0,
+      Ptr,
+      PacketSize,
+      PktID,
+      DataToggle,
+      DeviceSpeed,
+      &PtrTD
+      );
+
+    //
+    // Link two TDs in vertical depth
+    //
+    LinkTDToTD (PtrPreTD, PtrTD);
+    PtrPreTD = PtrTD;
+
+    DataToggle ^= 1;
+    Ptr += PacketSize;
+    DataLen -= PacketSize;
+  }
+
+  //
+  // PtrPreTD points to the last TD before the Setup-Stage TD.
+  //
+  PtrPreTD = PtrTD;
+
+  //
+  // Status Stage of Control Transfer
+  //
+  if (PktID == OUTPUT_PACKET_ID) {
+    PktID = INPUT_PACKET_ID;
+  } else {
+    PktID = OUTPUT_PACKET_ID;
+  }
+  //
+  // create Status Stage TD structure
+  //
+  CreateStatusTD (
+    UhcDev,
+    DeviceAddress,
+    0,
+    PktID,
+    DeviceSpeed,
+    &PtrStatusTD
+    );
+
+  LinkTDToTD (PtrPreTD, PtrStatusTD);
+
+  //
+  // Poll QH-TDs execution and get result.
+  // detail status is returned
+  //
+  Status = ExecuteControlTransfer (
+            UhcDev,
+            PtrSetupTD,
+            DataLength,
+            TimeOut,
+            TransferResult
+            );
+
+  //
+  // TRUE means must search other framelistindex
+  //
+  SetQHVerticalValidorInvalid(PtrQH, FALSE);
+  DeleteQueuedTDs (UhcDev, PtrSetupTD);
+
+  //
+  // if has errors that cause host controller halt, then return EFI_DEVICE_ERROR directly.
+  //
+  if (!IsStatusOK (UhcDev, StatusReg)) {
+
+    ClearStatusReg (UhcDev, StatusReg);
+    *TransferResult |= EFI_USB_ERR_SYSTEM;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ClearStatusReg (UhcDev, StatusReg);
+
+  return Status;
+}
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of 
+                                sending or receiving.
+  @param  Data                  Array of pointers to the buffers of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to use of 
+                                the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+                                If Timeout is 0, then the caller must wait for the function
+                                to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcBulkTransfer (
+  IN     EFI_PEI_SERVICES               **PeiServices,
+  IN     PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN     UINT8                          DeviceAddress,
+  IN     UINT8                          EndPointAddress,
+  IN     UINT8                          MaximumPacketLength,
+  IN OUT VOID                           *Data,
+  IN OUT UINTN                          *DataLength,
+  IN OUT UINT8                          *DataToggle,
+  IN     UINTN                          TimeOut,
+  OUT    UINT32                         *TransferResult
+  )
+{
+  USB_UHC_DEV             *UhcDev;
+  UINT32                  StatusReg;
+
+  UINT32                  DataLen;
+
+  QH_STRUCT               *PtrQH;
+  TD_STRUCT               *PtrFirstTD;
+  TD_STRUCT               *PtrTD;
+  TD_STRUCT               *PtrPreTD;
+
+  UINT8                   PktID;
+  UINT8                   *PtrDataSource;
+  UINT8                   *Ptr;
+
+  BOOLEAN                 IsFirstTD;
+
+  EFI_STATUS              Status;
+
+  EFI_USB_DATA_DIRECTION  TransferDirection;
+
+  BOOLEAN                 ShortPacketEnable;
+
+  UINT16                  CommandContent;
+
+  UhcDev = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+
+  //
+  // Enable the maximum packet size (64bytes)
+  // that can be used for full speed bandwidth reclamation
+  // at the end of a frame.
+  //
+  CommandContent = USBReadPortW (UhcDev, UhcDev->UsbHostControllerBaseAddress + USBCMD);
+  if ((CommandContent & USBCMD_MAXP) != USBCMD_MAXP) {
+    CommandContent |= USBCMD_MAXP;
+    USBWritePortW (UhcDev, UhcDev->UsbHostControllerBaseAddress + USBCMD, CommandContent);
+  }
+
+  StatusReg   = UhcDev->UsbHostControllerBaseAddress + USBSTS;
+
+  //
+  // these code lines are added here per complier's strict demand
+  //
+  PktID             = INPUT_PACKET_ID;
+  PtrTD             = NULL;
+  PtrFirstTD        = NULL;
+  PtrPreTD          = NULL;
+  DataLen           = 0;
+  Ptr               = NULL;
+
+  ShortPacketEnable = FALSE;
+
+  if ((DataLength == 0) || (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (MaximumPacketLength != 8 && MaximumPacketLength != 16
+      && MaximumPacketLength != 32 && MaximumPacketLength != 64) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // if has errors that cause host controller halt, then return EFI_DEVICE_ERROR directly.
+  //
+  if (!IsStatusOK (UhcDev, StatusReg)) {
+
+    ClearStatusReg (UhcDev, StatusReg);
+    *TransferResult = EFI_USB_ERR_SYSTEM;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ClearStatusReg (UhcDev, StatusReg);
+
+  if ((EndPointAddress & 0x80) != 0) {
+    TransferDirection = EfiUsbDataIn;
+  } else {
+    TransferDirection = EfiUsbDataOut;
+  }
+
+  switch (TransferDirection) {
+
+  case EfiUsbDataIn:
+    ShortPacketEnable = TRUE;
+    PktID             = INPUT_PACKET_ID;
+    PtrDataSource     = Data;
+    DataLen           = (UINT32) *DataLength;
+    Ptr               = PtrDataSource;
+    break;
+
+  case EfiUsbDataOut:
+    PktID         = OUTPUT_PACKET_ID;
+    PtrDataSource = Data;
+    DataLen       = (UINT32) *DataLength;
+    Ptr           = PtrDataSource;
+    break;
+
+  default:
+    break;
+  }
+
+  PtrQH = UhcDev->BulkQH;
+
+  IsFirstTD = TRUE;
+  while (DataLen > 0) {
+    //
+    // create TD structures and link together
+    //
+    UINT8 PacketSize;
+
+    PacketSize = (UINT8) DataLen;
+    if (DataLen > MaximumPacketLength) {
+      PacketSize = MaximumPacketLength;
+    }
+
+    GenDataTD (
+      UhcDev,
+      DeviceAddress,
+      EndPointAddress,
+      Ptr,
+      PacketSize,
+      PktID,
+      *DataToggle,
+      USB_FULL_SPEED_DEVICE,
+      &PtrTD
+      );
+
+    //
+    // Enable short packet detection.
+    // (default action is disabling short packet detection)
+    //
+    if (ShortPacketEnable) {
+      EnableorDisableTDShortPacket (PtrTD, TRUE);
+    }
+
+    if (IsFirstTD) {
+      PtrFirstTD            = PtrTD;
+      PtrFirstTD->PtrNextTD = NULL;
+      IsFirstTD             = FALSE;
+    } else {
+      //
+      // Link two TDs in vertical depth
+      //
+      LinkTDToTD (PtrPreTD, PtrTD);
+    }
+
+    PtrPreTD = PtrTD;
+
+    *DataToggle ^= 1;
+    Ptr += PacketSize;
+    DataLen -= PacketSize;
+  }
+  //
+  // link TD structures to QH structure
+  //
+  LinkTDToQH (PtrQH, PtrFirstTD);
+
+  //
+  // Execute QH-TD and get result
+  //
+  //
+  // detail status is put into the Result field in the pIRP
+  // the Data Toggle value is also re-updated to the value
+  // of the last successful TD
+  //
+  Status = ExecBulkTransfer (
+            UhcDev,
+            PtrFirstTD,
+            DataLength,
+            DataToggle,
+            TimeOut,
+            TransferResult
+            );
+
+  //
+  // Delete Bulk transfer TD structure
+  //
+  DeleteQueuedTDs (UhcDev, PtrFirstTD);
+
+  //
+  // if has errors that cause host controller halt, then return EFI_DEVICE_ERROR directly.
+  //
+  if (!IsStatusOK (UhcDev, StatusReg)) {
+
+    ClearStatusReg (UhcDev, StatusReg);
+    *TransferResult |= EFI_USB_ERR_SYSTEM;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ClearStatusReg (UhcDev, StatusReg);
+
+  return Status;
+}
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices   The pointer to the PEI Services Table.
+  @param[in]  This          The pointer to this instance of the 
+                            PEI_USB_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber    The pointer to the number of the root hub ports.                                
+                                
+  @retval EFI_SUCCESS           The port number was retrieved successfully.
+  @retval EFI_INVALID_PARAMETER PortNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcGetRootHubPortNumber (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  OUT UINT8                         *PortNumber
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      PSAddr;
+  UINT16      RHPortControl;
+  UINT32      Index;
+
+  UhcDev = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+
+  if (PortNumber == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *PortNumber = 0;
+
+  for (Index = 0; Index < 2; Index++) {
+    PSAddr = UhcDev->UsbHostControllerBaseAddress + USBPORTSC1 + Index * 2;
+    RHPortControl = USBReadPortW (UhcDev, PSAddr);
+    //
+    // Port Register content is valid
+    //
+    if (RHPortControl != 0xff) {
+      (*PortNumber)++;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the current status of a USB root hub port.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  PortNumber             The root hub port to retrieve the state from.  
+  @param  PortStatus             Variable to receive the port state.
+
+  @retval EFI_SUCCESS            The status of the USB root hub port specified.
+                                 by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcGetRootHubPortStatus (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN  UINT8                         PortNumber,
+  OUT EFI_USB_PORT_STATUS           *PortStatus
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      PSAddr;
+  UINT16      RHPortStatus;
+  UINT8       TotalPortNumber;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhcGetRootHubPortNumber (PeiServices, This, &TotalPortNumber);
+  if (PortNumber > TotalPortNumber) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhcDev                        = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+  PSAddr                        = UhcDev->UsbHostControllerBaseAddress + USBPORTSC1 + PortNumber * 2;
+
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  RHPortStatus = USBReadPortW (UhcDev, PSAddr);
+
+  //
+  // Current Connect Status
+  //
+  if ((RHPortStatus & USBPORTSC_CCS) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_CONNECTION;
+  }
+  //
+  // Port Enabled/Disabled
+  //
+  if ((RHPortStatus & USBPORTSC_PED) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_ENABLE;
+  }
+  //
+  // Port Suspend
+  //
+  if ((RHPortStatus & USBPORTSC_SUSP) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_SUSPEND;
+  }
+  //
+  // Port Reset
+  //
+  if ((RHPortStatus & USBPORTSC_PR) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_RESET;
+  }
+  //
+  // Low Speed Device Attached
+  //
+  if ((RHPortStatus & USBPORTSC_LSDA) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+  }
+  //
+  //   Fill Port Status Change bits
+  //
+  //
+  // Connect Status Change
+  //
+  if ((RHPortStatus & USBPORTSC_CSC) != 0) {
+    PortStatus->PortChangeStatus |= USB_PORT_STAT_C_CONNECTION;
+  }
+  //
+  // Port Enabled/Disabled Change
+  //
+  if ((RHPortStatus & USBPORTSC_PEDC) != 0) {
+    PortStatus->PortChangeStatus |= USB_PORT_STAT_C_ENABLE;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_TIMEOUT            The time out occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcSetRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      PSAddr;
+  UINT32      CommandRegAddr;
+  UINT16      RHPortControl;
+  UINT8       TotalPortNumber;
+
+  UhcGetRootHubPortNumber (PeiServices, This, &TotalPortNumber);
+  if (PortNumber > TotalPortNumber) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhcDev          = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+  PSAddr          = UhcDev->UsbHostControllerBaseAddress + USBPORTSC1 + PortNumber * 2;
+  CommandRegAddr  = UhcDev->UsbHostControllerBaseAddress + USBCMD;
+
+  RHPortControl = USBReadPortW (UhcDev, PSAddr);
+
+  switch (PortFeature) {
+
+  case EfiUsbPortSuspend:
+    if ((USBReadPortW (UhcDev, CommandRegAddr) & USBCMD_EGSM) == 0) {
+      //
+      // if global suspend is not active, can set port suspend
+      //
+      RHPortControl &= 0xfff5;
+      RHPortControl |= USBPORTSC_SUSP;
+    }
+    break;
+
+  case EfiUsbPortReset:
+    RHPortControl &= 0xfff5;
+    RHPortControl |= USBPORTSC_PR;
+    //
+    // Set the reset bit
+    //
+    break;
+
+  case EfiUsbPortPower:
+    break;
+
+  case EfiUsbPortEnable:
+    RHPortControl &= 0xfff5;
+    RHPortControl |= USBPORTSC_PED;
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  USBWritePortW (UhcDev, PSAddr, RHPortControl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Clears a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  PortNumber            Specifies the root hub port whose feature
+                                is requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was cleared 
+                                 for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcClearRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      PSAddr;
+  UINT16      RHPortControl;
+  UINT8       TotalPortNumber;
+
+  UhcGetRootHubPortNumber (PeiServices, This, &TotalPortNumber);
+
+  if (PortNumber > TotalPortNumber) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhcDev  = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+  PSAddr  = UhcDev->UsbHostControllerBaseAddress + USBPORTSC1 + PortNumber * 2;
+
+  RHPortControl = USBReadPortW (UhcDev, PSAddr);
+
+  switch (PortFeature) {
+  //
+  // clear PORT_ENABLE feature means disable port.
+  //
+  case EfiUsbPortEnable:
+    RHPortControl &= 0xfff5;
+    RHPortControl &= ~USBPORTSC_PED;
+    break;
+
+  //
+  // clear PORT_SUSPEND feature means resume the port.
+  // (cause a resume on the specified port if in suspend mode)
+  //
+  case EfiUsbPortSuspend:
+    RHPortControl &= 0xfff5;
+    RHPortControl &= ~USBPORTSC_SUSP;
+    break;
+
+  //
+  // no operation
+  //
+  case EfiUsbPortPower:
+    break;
+
+  //
+  // clear PORT_RESET means clear the reset signal.
+  //
+  case EfiUsbPortReset:
+    RHPortControl &= 0xfff5;
+    RHPortControl &= ~USBPORTSC_PR;
+    break;
+
+  //
+  // clear connect status change
+  //
+  case EfiUsbPortConnectChange:
+    RHPortControl &= 0xfff5;
+    RHPortControl |= USBPORTSC_CSC;
+    break;
+
+  //
+  // clear enable/disable status change
+  //
+  case EfiUsbPortEnableChange:
+    RHPortControl &= 0xfff5;
+    RHPortControl |= USBPORTSC_PEDC;
+    break;
+
+  //
+  // root hub does not support this request
+  //
+  case EfiUsbPortSuspendChange:
+    break;
+
+  //
+  // root hub does not support this request
+  //
+  case EfiUsbPortOverCurrentChange:
+    break;
+
+  //
+  // root hub does not support this request
+  //
+  case EfiUsbPortResetChange:
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  USBWritePortW (UhcDev, PSAddr, RHPortControl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UHCI.
+
+  @param  UhcDev                 UHCI Device.
+
+  @retval EFI_SUCCESS            UHCI successfully initialized.
+  @retval EFI_OUT_OF_RESOURCES   Resource can not be allocated.
+
+**/
+EFI_STATUS
+InitializeUsbHC (
+  IN USB_UHC_DEV          *UhcDev
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      FrameListBaseAddrReg;
+  UINT32      CommandReg;
+  UINT16      Command;
+
+  //
+  // Create and Initialize Frame List For the Host Controller.
+  //
+  Status = CreateFrameList (UhcDev);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  FrameListBaseAddrReg  = UhcDev->UsbHostControllerBaseAddress + USBFLBASEADD;
+  CommandReg            = UhcDev->UsbHostControllerBaseAddress + USBCMD;
+
+  //
+  // Set Frame List Base Address to the specific register to inform the hardware.
+  //
+  SetFrameListBaseAddress (UhcDev, FrameListBaseAddrReg, (UINT32) (UINTN) (UhcDev->FrameListEntry));
+
+  Command = USBReadPortW (UhcDev, CommandReg);
+  Command |= USBCMD_GRESET;
+  USBWritePortW (UhcDev, CommandReg, Command);
+
+  MicroSecondDelay (50 * 1000);
+
+
+  Command &= ~USBCMD_GRESET;
+
+  USBWritePortW (UhcDev, CommandReg, Command);
+
+  //
+  //UHCI spec page120 reset recovery time
+  //
+  MicroSecondDelay (20 * 1000);
+
+  //
+  // Set Run/Stop bit to 1.
+  //
+  Command = USBReadPortW (UhcDev, CommandReg);
+  Command |= USBCMD_RS | USBCMD_MAXP;
+  USBWritePortW (UhcDev, CommandReg, Command);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create Frame List Structure.
+
+  @param  UhcDev                 UHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+CreateFrameList (
+  USB_UHC_DEV             *UhcDev
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  FrameListBaseAddr;
+  FRAMELIST_ENTRY       *FrameListPtr;
+  UINTN                 Index;
+
+  //
+  // The Frame List ocupies 4K bytes,
+  // and must be aligned on 4-Kbyte boundaries.
+  //
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             1,
+             &FrameListBaseAddr
+             );
+
+  if (Status != EFI_SUCCESS) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  //Create Control QH and Bulk QH and link them into Framelist Entry
+  //
+  Status = CreateQH(UhcDev, &UhcDev->ConfigQH);
+  if (Status != EFI_SUCCESS) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  ASSERT (UhcDev->ConfigQH != NULL);
+
+  Status = CreateQH(UhcDev, &UhcDev->BulkQH);
+  if (Status != EFI_SUCCESS) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  ASSERT (UhcDev->BulkQH != NULL);
+
+  //
+  //Set the corresponding QH pointer 
+  //
+  SetQHHorizontalLinkPtr(UhcDev->ConfigQH, UhcDev->BulkQH);
+  SetQHHorizontalQHorTDSelect (UhcDev->ConfigQH, TRUE);
+  SetQHHorizontalValidorInvalid (UhcDev->ConfigQH, TRUE);
+
+  UhcDev->FrameListEntry = (FRAMELIST_ENTRY *) ((UINTN) FrameListBaseAddr);
+
+  FrameListPtr = UhcDev->FrameListEntry;
+
+  for (Index = 0; Index < 1024; Index++) {
+    FrameListPtr->FrameListPtrTerminate = 0;
+    FrameListPtr->FrameListPtr          = (UINT32)(UINTN)UhcDev->ConfigQH >> 4;
+    FrameListPtr->FrameListPtrQSelect   = 1;
+    FrameListPtr->FrameListRsvd         = 0;
+    FrameListPtr ++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read a 16bit width data from Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+
+  @retval the register content read.
+
+**/
+UINT16
+USBReadPortW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port
+  )
+{
+  return IoRead16 (Port);
+}
+
+/**
+  Write a 16bit width data into Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+  @param  Data    The data written into the register.
+
+**/
+VOID
+USBWritePortW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port,
+  IN  UINT16        Data
+  )
+{
+  IoWrite16 (Port, Data);
+}
+
+/**
+  Write a 32bit width data into Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+  @param  Data    The data written into the register.
+
+**/
+VOID
+USBWritePortDW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port,
+  IN  UINT32        Data
+  )
+{
+  IoWrite32 (Port, Data);
+}
+
+/**
+  Clear the content of UHCI's Status Register.
+  
+  @param  UhcDev       The UHCI device.
+  @param  StatusAddr   The IO space address of the register.
+
+**/
+VOID
+ClearStatusReg (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        StatusAddr
+  )
+{
+  //
+  // Clear the content of UHCI's Status Register
+  //
+  USBWritePortW (UhcDev, StatusAddr, 0x003F);
+}
+
+/**
+  Check whether the host controller operates well.
+
+  @param  UhcDev        The UHCI device.
+  @param  StatusRegAddr The io address of status register.
+
+  @retval TRUE          Host controller is working.
+  @retval FALSE         Host controller is halted or system error.
+
+**/
+BOOLEAN
+IsStatusOK (
+  IN USB_UHC_DEV     *UhcDev,
+  IN UINT32          StatusRegAddr
+  )
+{
+  UINT16  StatusValue;
+
+  StatusValue = USBReadPortW (UhcDev, StatusRegAddr);
+
+  if ((StatusValue & (USBSTS_HCPE | USBSTS_HSE | USBSTS_HCH)) != 0) {
+    return FALSE;
+  } else {
+    return TRUE;
+  }
+}
+
+/**
+  Get Current Frame Number.
+
+  @param  UhcDev          The UHCI device.
+  @param  FrameNumberAddr The address of frame list register.
+
+  @retval The content of the frame list register.
+
+**/
+UINT16
+GetCurrentFrameNumber (
+  IN USB_UHC_DEV   *UhcDev,
+  IN UINT32        FrameNumberAddr
+  )
+{
+  //
+  // Gets value in the USB frame number register.
+  //
+  return (UINT16) (USBReadPortW (UhcDev, FrameNumberAddr) & 0x03FF);
+}
+
+/**
+  Set Frame List Base Address.
+
+  @param  UhcDev           The UHCI device.
+  @param  FrameListRegAddr The address of frame list register.
+  @param  Addr             The address of frame list table.
+
+**/
+VOID
+SetFrameListBaseAddress (
+  IN USB_UHC_DEV   *UhcDev,
+  IN UINT32        FrameListRegAddr,
+  IN UINT32        Addr
+  )
+{
+  //
+  // Sets value in the USB Frame List Base Address register.
+  //
+  USBWritePortDW (UhcDev, FrameListRegAddr, (UINT32) (Addr & 0xFFFFF000));
+}
+
+/**
+  Create QH and initialize.
+
+  @param  UhcDev               The UHCI device.
+  @param  PtrQH                Place to store QH_STRUCT pointer.
+
+  @retval EFI_OUT_OF_RESOURCES Can't allocate memory resources.
+  @retval EFI_SUCCESS        Success.
+
+**/
+EFI_STATUS
+CreateQH (
+  IN  USB_UHC_DEV   *UhcDev,
+  OUT QH_STRUCT     **PtrQH
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // allocate align memory for QH_STRUCT
+  //
+  Status = AllocateTDorQHStruct (UhcDev, sizeof(QH_STRUCT), (void **)PtrQH);
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // init each field of the QH_STRUCT
+  //
+  SetQHHorizontalValidorInvalid (*PtrQH, FALSE);
+  SetQHVerticalValidorInvalid (*PtrQH, FALSE);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set the horizontal link pointer in QH.
+
+  @param  PtrQH               Place to store QH_STRUCT pointer.
+  @param  PtrNext             Place to the next QH_STRUCT.
+
+**/
+VOID
+SetQHHorizontalLinkPtr (
+  IN QH_STRUCT  *PtrQH,
+  IN VOID       *PtrNext
+  )
+{
+  //
+  // Since the QH_STRUCT is aligned on 16-byte boundaries,
+  // Only the highest 28bit of the address is valid
+  // (take 32bit address as an example).
+  //
+  PtrQH->QueueHead.QHHorizontalPtr = (UINT32) (UINTN) PtrNext >> 4;
+}
+
+/**
+  Get the horizontal link pointer in QH.
+
+  @param  PtrQH     Place to store QH_STRUCT pointer.
+
+  @retval The horizontal link pointer in QH.
+
+**/
+VOID *
+GetQHHorizontalLinkPtr (
+  IN QH_STRUCT  *PtrQH
+  )
+{
+  //
+  // Restore the 28bit address to 32bit address
+  // (take 32bit address as an example)
+  //
+  return (VOID *) (UINTN) ((PtrQH->QueueHead.QHHorizontalPtr) << 4);
+}
+
+/**
+  Set a QH or TD horizontally to be connected with a specific QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsQH       Specify QH or TD is connected.
+
+**/
+VOID
+SetQHHorizontalQHorTDSelect (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsQH
+  )
+{
+  //
+  // if QH is connected, the specified bit is set,
+  // if TD is connected, the specified bit is cleared.
+  //
+  PtrQH->QueueHead.QHHorizontalQSelect = IsQH ? 1 : 0;
+}
+
+/**
+  Set the horizontal validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsValid    Specify the horizontal linker is valid or not.
+
+**/
+VOID
+SetQHHorizontalValidorInvalid (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsValid
+  )
+{
+  //
+  // Valid means the horizontal link pointer is valid,
+  // else, it's invalid.
+  //
+  PtrQH->QueueHead.QHHorizontalTerminate = IsValid ? 0 : 1;
+}
+
+/**
+  Set the vertical link pointer in QH.
+
+  @param  PtrQH       Place to store QH_STRUCT pointer.
+  @param  PtrNext     Place to the next QH_STRUCT.
+
+**/
+VOID
+SetQHVerticalLinkPtr (
+  IN QH_STRUCT  *PtrQH,
+  IN VOID       *PtrNext
+  )
+{
+  //
+  // Since the QH_STRUCT is aligned on 16-byte boundaries,
+  // Only the highest 28bit of the address is valid
+  // (take 32bit address as an example).
+  //
+  PtrQH->QueueHead.QHVerticalPtr = (UINT32) (UINTN) PtrNext >> 4;
+}
+
+/**
+  Set a QH or TD vertically to be connected with a specific QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsQH       Specify QH or TD is connected.
+
+**/
+VOID
+SetQHVerticalQHorTDSelect (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsQH
+  )
+{
+  //
+  // Set the specified bit if the Vertical Link Pointer pointing to a QH,
+  // Clear the specified bit if the Vertical Link Pointer pointing to a TD.
+  //
+  PtrQH->QueueHead.QHVerticalQSelect = IsQH ? 1 : 0;
+}
+
+/**
+  Set the vertical validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsValid    Specify the vertical linker is valid or not.
+
+**/
+VOID
+SetQHVerticalValidorInvalid (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsValid
+  )
+{
+  //
+  // If TRUE, meaning the Vertical Link Pointer field is valid,
+  // else, the field is invalid.
+  //
+  PtrQH->QueueHead.QHVerticalTerminate = IsValid ? 0 : 1;
+}
+
+/**
+  Get the vertical validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+
+  @retval The vertical linker is valid or not.
+
+**/
+BOOLEAN
+GetQHHorizontalValidorInvalid (
+  IN QH_STRUCT  *PtrQH
+  )
+{
+  //
+  // If TRUE, meaning the Horizontal Link Pointer field is valid,
+  // else, the field is invalid.
+  //
+  return (BOOLEAN) (!(PtrQH->QueueHead.QHHorizontalTerminate));
+}
+
+/**
+  Allocate TD or QH Struct.
+
+  @param  UhcDev                 The UHCI device.
+  @param  Size                   The size of allocation.
+  @param  PtrStruct              Place to store TD_STRUCT pointer.
+
+  @return EFI_SUCCESS            Allocate successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+AllocateTDorQHStruct (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT32          Size,
+  OUT VOID            **PtrStruct
+  )
+{
+  EFI_STATUS  Status;
+
+  Status      = EFI_SUCCESS;
+  *PtrStruct  = NULL;
+
+  Status = UhcAllocatePool (
+            UhcDev,
+            (UINT8 **) PtrStruct,
+            Size
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ZeroMem (*PtrStruct, Size);
+
+  return Status;
+}
+
+/**
+  Create a TD Struct.
+
+  @param  UhcDev                 The UHCI device.
+  @param  PtrTD                  Place to store TD_STRUCT pointer.
+
+  @return EFI_SUCCESS            Allocate successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+CreateTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  OUT TD_STRUCT       **PtrTD
+  )
+{
+  EFI_STATUS  Status;
+  //
+  // create memory for TD_STRUCT, and align the memory.
+  //
+  Status = AllocateTDorQHStruct (UhcDev, sizeof(TD_STRUCT), (void **)PtrTD);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Make TD ready.
+  //
+  SetTDLinkPtrValidorInvalid (*PtrTD, FALSE);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Generate Setup Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  DeviceSpeed  Device Speed.
+  @param  DevRequest   Device reuquest.
+  @param  RequestLen   Request length.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate setup stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+GenSetupStageTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           DeviceSpeed,
+  IN  UINT8           *DevRequest,
+  IN  UINT8           RequestLen,
+  OUT TD_STRUCT       **PtrTD
+  )
+{
+  TD_STRUCT   *TdStruct;
+  EFI_STATUS  Status;
+
+  Status = CreateTD (UhcDev, &TdStruct);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SetTDLinkPtr (TdStruct, NULL);
+
+  //
+  // Depth first fashion
+  //
+  SetTDLinkPtrDepthorBreadth (TdStruct, TRUE);
+
+  //
+  // initialize as the last TD in the QH context,
+  // this field will be updated in the TD linkage process.
+  //
+  SetTDLinkPtrValidorInvalid (TdStruct, FALSE);
+
+  //
+  // Disable Short Packet Detection by default
+  //
+  EnableorDisableTDShortPacket (TdStruct, FALSE);
+
+  //
+  // Max error counter is 3, retry 3 times when error encountered.
+  //
+  SetTDControlErrorCounter (TdStruct, 3);
+
+  //
+  // set device speed attribute
+  // (TRUE - Slow Device; FALSE - Full Speed Device)
+  //
+  switch (DeviceSpeed) {
+  case USB_SLOW_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (TdStruct, TRUE);
+    break;
+
+  case USB_FULL_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (TdStruct, FALSE);
+    break;
+  }
+  //
+  // Non isochronous transfer TD
+  //
+  SetTDControlIsochronousorNot (TdStruct, FALSE);
+
+  //
+  // Interrupt On Complete bit be set to zero,
+  // Disable IOC interrupt.
+  //
+  SetorClearTDControlIOC (TdStruct, FALSE);
+
+  //
+  // Set TD Active bit
+  //
+  SetTDStatusActiveorInactive (TdStruct, TRUE);
+
+  SetTDTokenMaxLength (TdStruct, RequestLen);
+
+  SetTDTokenDataToggle0 (TdStruct);
+
+  SetTDTokenEndPoint (TdStruct, Endpoint);
+
+  SetTDTokenDeviceAddress (TdStruct, DevAddr);
+
+  SetTDTokenPacketID (TdStruct, SETUP_PACKET_ID);
+
+  TdStruct->PtrTDBuffer      = (UINT8 *) DevRequest;
+  TdStruct->TDBufferLength = RequestLen;
+  SetTDDataBuffer (TdStruct);
+
+  *PtrTD = TdStruct;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Generate Data Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  PtrData      Data buffer.
+  @param  Len          Data length.
+  @param  PktID        PacketID.
+  @param  Toggle       Data toggle value.
+  @param  DeviceSpeed  Device Speed.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate data stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+GenDataTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           *PtrData,
+  IN  UINT8           Len,
+  IN  UINT8           PktID,
+  IN  UINT8           Toggle,
+  IN  UINT8           DeviceSpeed,
+  OUT TD_STRUCT       **PtrTD
+  )
+{
+  TD_STRUCT   *TdStruct;
+  EFI_STATUS  Status;
+
+  Status = CreateTD (UhcDev, &TdStruct);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SetTDLinkPtr (TdStruct, NULL);
+
+  //
+  // Depth first fashion
+  //
+  SetTDLinkPtrDepthorBreadth (TdStruct, TRUE);
+
+  //
+  // Link pointer pointing to TD struct
+  //
+  SetTDLinkPtrQHorTDSelect (TdStruct, FALSE);
+
+  //
+  // initialize as the last TD in the QH context,
+  // this field will be updated in the TD linkage process.
+  //
+  SetTDLinkPtrValidorInvalid (TdStruct, FALSE);
+
+  //
+  // Disable short packet detect
+  //
+  EnableorDisableTDShortPacket (TdStruct, FALSE);
+  //
+  // Max error counter is 3
+  //
+  SetTDControlErrorCounter (TdStruct, 3);
+
+  //
+  // set device speed attribute
+  // (TRUE - Slow Device; FALSE - Full Speed Device)
+  //
+  switch (DeviceSpeed) {
+  case USB_SLOW_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (TdStruct, TRUE);
+    break;
+
+  case USB_FULL_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (TdStruct, FALSE);
+    break;
+  }
+  //
+  // Non isochronous transfer TD
+  //
+  SetTDControlIsochronousorNot (TdStruct, FALSE);
+
+  //
+  // Disable Interrupt On Complete
+  // Disable IOC interrupt.
+  //
+  SetorClearTDControlIOC (TdStruct, FALSE);
+
+  //
+  // Set Active bit
+  //
+  SetTDStatusActiveorInactive (TdStruct, TRUE);
+
+  SetTDTokenMaxLength (TdStruct, Len);
+
+  if (Toggle != 0) {
+    SetTDTokenDataToggle1 (TdStruct);
+  } else {
+    SetTDTokenDataToggle0 (TdStruct);
+  }
+
+  SetTDTokenEndPoint (TdStruct, Endpoint);
+
+  SetTDTokenDeviceAddress (TdStruct, DevAddr);
+
+  SetTDTokenPacketID (TdStruct, PktID);
+
+  TdStruct->PtrTDBuffer      = (UINT8 *) PtrData;
+  TdStruct->TDBufferLength = Len;
+  SetTDDataBuffer (TdStruct);
+
+  *PtrTD = TdStruct;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Generate Status Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  PktID        PacketID.
+  @param  DeviceSpeed  Device Speed.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate status stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+CreateStatusTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           PktID,
+  IN  UINT8           DeviceSpeed,
+  OUT TD_STRUCT       **PtrTD
+  )
+{
+  TD_STRUCT   *PtrTDStruct;
+  EFI_STATUS  Status;
+
+  Status = CreateTD (UhcDev, &PtrTDStruct);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SetTDLinkPtr (PtrTDStruct, NULL);
+
+  //
+  // Depth first fashion
+  //
+  SetTDLinkPtrDepthorBreadth (PtrTDStruct, TRUE);
+
+  //
+  // initialize as the last TD in the QH context,
+  // this field will be updated in the TD linkage process.
+  //
+  SetTDLinkPtrValidorInvalid (PtrTDStruct, FALSE);
+
+  //
+  // Disable short packet detect
+  //
+  EnableorDisableTDShortPacket (PtrTDStruct, FALSE);
+
+  //
+  // Max error counter is 3
+  //
+  SetTDControlErrorCounter (PtrTDStruct, 3);
+
+  //
+  // set device speed attribute
+  // (TRUE - Slow Device; FALSE - Full Speed Device)
+  //
+  switch (DeviceSpeed) {
+  case USB_SLOW_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (PtrTDStruct, TRUE);
+    break;
+
+  case USB_FULL_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (PtrTDStruct, FALSE);
+    break;
+  }
+  //
+  // Non isochronous transfer TD
+  //
+  SetTDControlIsochronousorNot (PtrTDStruct, FALSE);
+
+  //
+  // Disable Interrupt On Complete
+  // Disable IOC interrupt.
+  //
+  SetorClearTDControlIOC (PtrTDStruct, FALSE);
+
+  //
+  // Set TD Active bit
+  //
+  SetTDStatusActiveorInactive (PtrTDStruct, TRUE);
+
+  SetTDTokenMaxLength (PtrTDStruct, 0);
+
+  SetTDTokenDataToggle1 (PtrTDStruct);
+
+  SetTDTokenEndPoint (PtrTDStruct, Endpoint);
+
+  SetTDTokenDeviceAddress (PtrTDStruct, DevAddr);
+
+  SetTDTokenPacketID (PtrTDStruct, PktID);
+
+  PtrTDStruct->PtrTDBuffer      = NULL;
+  PtrTDStruct->TDBufferLength = 0;
+  SetTDDataBuffer (PtrTDStruct);
+
+  *PtrTD = PtrTDStruct;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set the link pointer validor bit in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  IsValid      Specify the linker pointer is valid or not.
+
+**/
+VOID
+SetTDLinkPtrValidorInvalid (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsValid
+  )
+{
+  //
+  // Valid means the link pointer is valid,
+  // else, it's invalid.
+  //
+  PtrTDStruct->TDData.TDLinkPtrTerminate = (IsValid ? 0 : 1);
+}
+
+/**
+  Set the Link Pointer pointing to a QH or TD.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  IsQH          Specify QH or TD is connected.
+
+**/
+VOID
+SetTDLinkPtrQHorTDSelect (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsQH
+  )
+{
+  //
+  // Indicate whether the Link Pointer pointing to a QH or TD
+  //
+  PtrTDStruct->TDData.TDLinkPtrQSelect = (IsQH ? 1 : 0);
+}
+
+/**
+  Set the traverse is depth-first or breadth-first.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  IsDepth       Specify the traverse is depth-first or breadth-first.
+
+**/
+VOID
+SetTDLinkPtrDepthorBreadth (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsDepth
+  )
+{
+  //
+  // If TRUE, indicating the host controller should process in depth first fashion,
+  // else, the host controller should process in breadth first fashion
+  //
+  PtrTDStruct->TDData.TDLinkPtrDepthSelect = (IsDepth ? 1 : 0);
+}
+
+/**
+  Set TD Link Pointer in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  PtrNext      Place to the next TD_STRUCT.
+
+**/
+VOID
+SetTDLinkPtr (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  VOID      *PtrNext
+  )
+{
+  //
+  // Set TD Link Pointer. Since QH,TD align on 16-byte boundaries,
+  // only the highest 28 bits are valid. (if take 32bit address as an example)
+  //
+  PtrTDStruct->TDData.TDLinkPtr = (UINT32) (UINTN) PtrNext >> 4;
+}
+
+/**
+  Get TD Link Pointer.
+
+  @param  PtrTDStruct     Place to store TD_STRUCT pointer.
+
+  @retval Get TD Link Pointer in TD.
+
+**/
+VOID *
+GetTDLinkPtr (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Get TD Link Pointer. Restore it back to 32bit
+  // (if take 32bit address as an example)
+  //
+  return (VOID *) (UINTN) ((PtrTDStruct->TDData.TDLinkPtr) << 4);
+}
+
+/**
+  Get the information about whether the Link Pointer field pointing to
+  a QH or a TD.
+
+  @param  PtrTDStruct     Place to store TD_STRUCT pointer.
+
+  @retval whether the Link Pointer field pointing to a QH or a TD.
+
+**/
+BOOLEAN
+IsTDLinkPtrQHOrTD (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Get the information about whether the Link Pointer field pointing to
+  // a QH or a TD.
+  //
+  return (BOOLEAN) (PtrTDStruct->TDData.TDLinkPtrQSelect);
+}
+
+/**
+  Enable/Disable short packet detection mechanism.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  IsEnable     Enable or disable short packet detection mechanism.
+
+**/
+VOID
+EnableorDisableTDShortPacket (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsEnable
+  )
+{
+  //
+  // TRUE means enable short packet detection mechanism.
+  //
+  PtrTDStruct->TDData.TDStatusSPD = (IsEnable ? 1 : 0);
+}
+
+/**
+  Set the max error counter in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  MaxErrors    The number of allowable error.
+
+**/
+VOID
+SetTDControlErrorCounter (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT8     MaxErrors
+  )
+{
+  //
+  // valid value of MaxErrors is 0,1,2,3
+  //
+  if (MaxErrors > 3) {
+    MaxErrors = 3;
+  }
+
+  PtrTDStruct->TDData.TDStatusErr = MaxErrors;
+}
+
+/**
+  Set the TD is targeting a low-speed device or not.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsLowSpeedDevice  Whether The device is low-speed.
+
+**/
+VOID
+SetTDLoworFullSpeedDevice (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsLowSpeedDevice
+  )
+{
+  //
+  // TRUE means the TD is targeting at a Low-speed device
+  //
+  PtrTDStruct->TDData.TDStatusLS = (IsLowSpeedDevice ? 1 : 0);
+}
+
+/**
+  Set the TD is isochronous transfer type or not.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsIsochronous     Whether the transaction isochronous transfer type.
+
+**/
+VOID
+SetTDControlIsochronousorNot (
+  IN  TD_STRUCT   *PtrTDStruct,
+  IN  BOOLEAN     IsIsochronous
+  )
+{
+  //
+  // TRUE means the TD belongs to Isochronous transfer type.
+  //
+  PtrTDStruct->TDData.TDStatusIOS = (IsIsochronous ? 1 : 0);
+}
+
+/**
+  Set if UCHI should issue an interrupt on completion of the frame
+  in which this TD is executed
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsSet             Whether HC should issue an interrupt on completion.
+
+**/
+VOID
+SetorClearTDControlIOC (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsSet
+  )
+{
+  //
+  // If this bit is set, it indicates that the host controller should issue
+  // an interrupt on completion of the frame in which this TD is executed.
+  //
+  PtrTDStruct->TDData.TDStatusIOC = IsSet ? 1 : 0;
+}
+
+/**
+  Set if the TD is active and can be executed.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsActive          Whether the TD is active and can be executed.
+
+**/
+VOID
+SetTDStatusActiveorInactive (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsActive
+  )
+{
+  //
+  // If this bit is set, it indicates that the TD is active and can be
+  // executed.
+  //
+  if (IsActive) {
+    PtrTDStruct->TDData.TDStatus |= 0x80;
+  } else {
+    PtrTDStruct->TDData.TDStatus &= 0x7F;
+  }
+}
+
+/**
+  Specifies the maximum number of data bytes allowed for the transfer.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  MaxLen        The maximum number of data bytes allowed.
+
+  @retval The allowed maximum number of data.
+**/
+UINT16
+SetTDTokenMaxLength (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT16    MaxLen
+  )
+{
+  //
+  // Specifies the maximum number of data bytes allowed for the transfer.
+  // the legal value extent is 0 ~ 0x500.
+  //
+  if (MaxLen > 0x500) {
+    MaxLen = 0x500;
+  }
+
+  PtrTDStruct->TDData.TDTokenMaxLen = MaxLen - 1;
+
+  return MaxLen;
+}
+
+/**
+  Set the data toggle bit to DATA1.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDTokenDataToggle1 (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Set the data toggle bit to DATA1
+  //
+  PtrTDStruct->TDData.TDTokenDataToggle = 1;
+}
+
+/**
+  Set the data toggle bit to DATA0.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDTokenDataToggle0 (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Set the data toggle bit to DATA0
+  //
+  PtrTDStruct->TDData.TDTokenDataToggle = 0;
+}
+
+/**
+  Set EndPoint Number the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  EndPoint      The Endport number of the target.
+
+**/
+VOID
+SetTDTokenEndPoint (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINTN     EndPoint
+  )
+{
+  //
+  // Set EndPoint Number the TD is targeting at.
+  //
+  PtrTDStruct->TDData.TDTokenEndPt = (UINT8) EndPoint;
+}
+
+/**
+  Set Device Address the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  DevAddr       The Device Address of the target.
+
+**/
+VOID
+SetTDTokenDeviceAddress (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINTN     DevAddr
+  )
+{
+  //
+  // Set Device Address the TD is targeting at.
+  //
+  PtrTDStruct->TDData.TDTokenDevAddr = (UINT8) DevAddr;
+}
+
+/**
+  Set Packet Identification the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  PacketID      The Packet Identification of the target.
+
+**/
+VOID
+SetTDTokenPacketID (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT8     PacketID
+  )
+{
+  //
+  // Set the Packet Identification to be used for this transaction.
+  //
+  PtrTDStruct->TDData.TDTokenPID = PacketID;
+}
+
+/**
+  Set the beginning address of the data buffer that will be used
+  during the transaction.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDDataBuffer (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Set the beginning address of the data buffer that will be used
+  // during the transaction.
+  //
+  PtrTDStruct->TDData.TDBufferPtr = (UINT32) (UINTN) (PtrTDStruct->PtrTDBuffer);
+}
+
+/**
+  Detect whether the TD is active.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The TD is active or not.
+
+**/
+BOOLEAN
+IsTDStatusActive (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether the TD is active.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x80);
+}
+
+/**
+  Detect whether the TD is stalled.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The TD is stalled or not.
+
+**/
+BOOLEAN
+IsTDStatusStalled (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether the device/endpoint addressed by this TD is stalled.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x40);
+}
+
+/**
+  Detect whether Data Buffer Error is happened.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Data Buffer Error is happened or not.
+
+**/
+BOOLEAN
+IsTDStatusBufferError (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether Data Buffer Error is happened.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x20);
+}
+
+/**
+  Detect whether Babble Error is happened.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Babble Error is happened or not.
+
+**/
+BOOLEAN
+IsTDStatusBabbleError (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether Babble Error is happened.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x10);
+}
+
+/**
+  Detect whether NAK is received.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The NAK is received or not.
+
+**/
+BOOLEAN
+IsTDStatusNAKReceived (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether NAK is received.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x08);
+}
+
+/**
+  Detect whether CRC/Time Out Error is encountered.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The CRC/Time Out Error is encountered or not.
+
+**/
+BOOLEAN
+IsTDStatusCRCTimeOutError (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether CRC/Time Out Error is encountered.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x04);
+}
+
+/**
+  Detect whether Bitstuff Error is received.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Bitstuff Error is received or not.
+
+**/
+BOOLEAN
+IsTDStatusBitStuffError (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether Bitstuff Error is received.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x02);
+}
+
+/**
+  Retrieve the actual number of bytes that were tansferred.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The actual number of bytes that were tansferred.
+
+**/
+UINT16
+GetTDStatusActualLength (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Retrieve the actual number of bytes that were tansferred.
+  // the value is encoded as n-1. so return the decoded value.
+  //
+  return (UINT16) ((PtrTDStruct->TDData.TDStatusActualLength) + 1);
+}
+
+/**
+  Retrieve the information of whether the Link Pointer field is valid or not.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The linker pointer field is valid or not.
+
+**/
+BOOLEAN
+GetTDLinkPtrValidorInvalid (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Retrieve the information of whether the Link Pointer field
+  // is valid or not.
+  //
+  if ((PtrTDStruct->TDData.TDLinkPtrTerminate & BIT0) != 0) {
+    return FALSE;
+  } else {
+    return TRUE;
+  }
+
+}
+
+/**
+  Count TD Number from PtrFirstTD.
+
+  @param  PtrFirstTD   Place to store TD_STRUCT pointer.
+
+  @retval The queued TDs number.
+
+**/
+UINTN
+CountTDsNumber (
+  IN  TD_STRUCT *PtrFirstTD
+  )
+{
+  UINTN     Number;
+  TD_STRUCT *Ptr;
+
+  //
+  // Count the queued TDs number.
+  //
+  Number  = 0;
+  Ptr     = PtrFirstTD;
+  while (Ptr != 0) {
+    Ptr = (TD_STRUCT *) Ptr->PtrNextTD;
+    Number++;
+  }
+
+  return Number;
+}
+
+/**
+  Link TD To QH.
+
+  @param  PtrQH   Place to store QH_STRUCT pointer.
+  @param  PtrTD   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+LinkTDToQH (
+  IN  QH_STRUCT *PtrQH,
+  IN  TD_STRUCT *PtrTD
+  )
+{
+  if (PtrQH == NULL || PtrTD == NULL) {
+    return ;
+  }
+  //
+  //  Validate QH Vertical Ptr field
+  //
+  SetQHVerticalValidorInvalid (PtrQH, TRUE);
+
+  //
+  //  Vertical Ptr pointing to TD structure
+  //
+  SetQHVerticalQHorTDSelect (PtrQH, FALSE);
+
+  SetQHVerticalLinkPtr (PtrQH, (VOID *) PtrTD);
+
+  PtrQH->PtrDown = (VOID *) PtrTD;
+}
+
+/**
+  Link TD To TD.
+
+  @param  PtrPreTD  Place to store TD_STRUCT pointer.
+  @param  PtrTD     Place to store TD_STRUCT pointer.
+
+**/
+VOID
+LinkTDToTD (
+  IN  TD_STRUCT *PtrPreTD,
+  IN  TD_STRUCT *PtrTD
+  )
+{
+  if (PtrPreTD == NULL || PtrTD == NULL) {
+    return ;
+  }
+  //
+  // Depth first fashion
+  //
+  SetTDLinkPtrDepthorBreadth (PtrPreTD, TRUE);
+
+  //
+  // Link pointer pointing to TD struct
+  //
+  SetTDLinkPtrQHorTDSelect (PtrPreTD, FALSE);
+
+  //
+  // Validate the link pointer valid bit
+  //
+  SetTDLinkPtrValidorInvalid (PtrPreTD, TRUE);
+
+  SetTDLinkPtr (PtrPreTD, PtrTD);
+
+  PtrPreTD->PtrNextTD = (VOID *) PtrTD;
+
+  PtrTD->PtrNextTD    = NULL;
+}
+
+/**
+  Execute Control Transfer.
+
+  @param  UhcDev            The UCHI device.
+  @param  PtrTD             A pointer to TD_STRUCT data.
+  @param  ActualLen         Actual transfer Length.
+  @param  TimeOut           TimeOut value.
+  @param  TransferResult    Transfer Result.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+ExecuteControlTransfer (
+  IN  USB_UHC_DEV *UhcDev,
+  IN  TD_STRUCT   *PtrTD,
+  OUT UINTN       *ActualLen,
+  IN  UINTN       TimeOut,
+  OUT UINT32      *TransferResult
+  )
+{
+  UINTN   ErrTDPos;
+  UINTN   Delay;
+  BOOLEAN InfiniteLoop;
+
+  ErrTDPos          = 0;
+  *TransferResult   = EFI_USB_NOERROR;
+  *ActualLen        = 0;
+  InfiniteLoop      = FALSE;
+
+  Delay = TimeOut * STALL_1_MILLI_SECOND;
+  //
+  // If Timeout is 0, then the caller must wait for the function to be completed
+  // until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  do {
+
+    CheckTDsResults (PtrTD, TransferResult, &ErrTDPos, ActualLen);
+
+    //
+    // TD is inactive, means the control transfer is end.
+    //
+    if ((*TransferResult & EFI_USB_ERR_NOTEXECUTE) != EFI_USB_ERR_NOTEXECUTE) {
+      break;
+    }
+    MicroSecondDelay (STALL_1_MICRO_SECOND);
+    Delay--;
+
+  } while (InfiniteLoop || (Delay != 0));
+
+  if (*TransferResult != EFI_USB_NOERROR) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Execute Bulk Transfer.
+
+  @param  UhcDev            The UCHI device.
+  @param  PtrTD             A pointer to TD_STRUCT data.
+  @param  ActualLen         Actual transfer Length.
+  @param  DataToggle        DataToggle value.
+  @param  TimeOut           TimeOut value.
+  @param  TransferResult    Transfer Result.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+ExecBulkTransfer (
+  IN     USB_UHC_DEV *UhcDev,
+  IN     TD_STRUCT   *PtrTD,
+  IN OUT UINTN       *ActualLen,
+  IN     UINT8       *DataToggle,
+  IN     UINTN       TimeOut,
+  OUT    UINT32      *TransferResult
+  )
+{
+  UINTN   ErrTDPos;
+  UINTN   ScrollNum;
+  UINTN   Delay;
+  BOOLEAN InfiniteLoop;
+
+  ErrTDPos          = 0;
+  *TransferResult   = EFI_USB_NOERROR;
+  *ActualLen        = 0;
+  InfiniteLoop      = FALSE;
+
+  Delay = TimeOut * STALL_1_MILLI_SECOND;
+  //
+  // If Timeout is 0, then the caller must wait for the function to be completed
+  // until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  do {
+
+    CheckTDsResults (PtrTD, TransferResult, &ErrTDPos, ActualLen);
+    //
+    // TD is inactive, thus meaning bulk transfer's end.
+    //
+    if ((*TransferResult & EFI_USB_ERR_NOTEXECUTE) != EFI_USB_ERR_NOTEXECUTE) {
+      break;
+    }
+    MicroSecondDelay (STALL_1_MICRO_SECOND);
+    Delay--;
+
+  } while (InfiniteLoop || (Delay != 0));
+
+  //
+  // has error
+  //
+  if (*TransferResult != EFI_USB_NOERROR) {
+    //
+    // scroll the Data Toggle back to the last success TD
+    //
+    ScrollNum = CountTDsNumber (PtrTD) - ErrTDPos;
+    if ((ScrollNum % 2) != 0) {
+      *DataToggle ^= 1;
+    }
+
+  //
+  // If error, wait 100ms to retry by upper layer
+  //
+    MicroSecondDelay (100 * 1000);
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Delete Queued TDs.
+
+  @param  UhcDev       The UCHI device.
+  @param  PtrFirstTD   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+DeleteQueuedTDs (
+  IN USB_UHC_DEV     *UhcDev,
+  IN TD_STRUCT       *PtrFirstTD
+  )
+{
+  TD_STRUCT *Tptr1;
+
+  TD_STRUCT *Tptr2;
+
+  Tptr1 = PtrFirstTD;
+  //
+  // Delete all the TDs in a queue.
+  //
+  while (Tptr1 != NULL) {
+
+    Tptr2 = Tptr1;
+
+    if (!GetTDLinkPtrValidorInvalid (Tptr2)) {
+      Tptr1 = NULL;
+    } else {
+      //
+      // has more than one TD in the queue.
+      //
+      Tptr1 = GetTDLinkPtr (Tptr2);
+    }
+
+    UhcFreePool (UhcDev, (UINT8 *) Tptr2, sizeof (TD_STRUCT));
+  }
+
+  return ;
+}
+
+/**
+  Check TDs Results.
+
+  @param  PtrTD               A pointer to TD_STRUCT data.
+  @param  Result              The result to return.
+  @param  ErrTDPos            The Error TD position.
+  @param  ActualTransferSize  Actual transfer size.
+
+  @retval The TD is executed successfully or not.
+
+**/
+BOOLEAN
+CheckTDsResults (
+  IN  TD_STRUCT               *PtrTD,
+  OUT UINT32                  *Result,
+  OUT UINTN                   *ErrTDPos,
+  OUT UINTN                   *ActualTransferSize
+  )
+{
+  UINTN Len;
+
+  *Result   = EFI_USB_NOERROR;
+  *ErrTDPos = 0;
+
+  //
+  // Init to zero.
+  //
+  *ActualTransferSize = 0;
+
+  while (PtrTD != NULL) {
+
+    if (IsTDStatusActive (PtrTD)) {
+      *Result |= EFI_USB_ERR_NOTEXECUTE;
+    }
+
+    if (IsTDStatusStalled (PtrTD)) {
+      *Result |= EFI_USB_ERR_STALL;
+    }
+
+    if (IsTDStatusBufferError (PtrTD)) {
+      *Result |= EFI_USB_ERR_BUFFER;
+    }
+
+    if (IsTDStatusBabbleError (PtrTD)) {
+      *Result |= EFI_USB_ERR_BABBLE;
+    }
+
+    if (IsTDStatusNAKReceived (PtrTD)) {
+      *Result |= EFI_USB_ERR_NAK;
+    }
+
+    if (IsTDStatusCRCTimeOutError (PtrTD)) {
+      *Result |= EFI_USB_ERR_TIMEOUT;
+    }
+
+    if (IsTDStatusBitStuffError (PtrTD)) {
+      *Result |= EFI_USB_ERR_BITSTUFF;
+    }
+    //
+    // Accumulate actual transferred data length in each TD.
+    //
+    Len = GetTDStatusActualLength (PtrTD) & 0x7FF;
+    *ActualTransferSize += Len;
+
+    //
+    // if any error encountered, stop processing the left TDs.
+    //
+    if ((*Result) != 0) {
+      return FALSE;
+    }
+
+    PtrTD = (TD_STRUCT *) (PtrTD->PtrNextTD);
+    //
+    // Record the first Error TD's position in the queue,
+    // this value is zero-based.
+    //
+    (*ErrTDPos)++;
+  }
+
+  return TRUE;
+}
+
+/**
+  Create Memory Block.
+
+  @param  UhcDev                   The UCHI device.
+  @param  MemoryHeader             The Pointer to allocated memory block.
+  @param  MemoryBlockSizeInPages   The page size of memory block to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+CreateMemoryBlock (
+  IN  USB_UHC_DEV           *UhcDev,
+  OUT MEMORY_MANAGE_HEADER  **MemoryHeader,
+  IN  UINTN                 MemoryBlockSizeInPages
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  TempPtr;
+  UINTN                 MemPages;
+  UINT8                 *Ptr;
+
+  //
+  // Memory Block uses MemoryBlockSizeInPages pages,
+  // memory management header and bit array use 1 page
+  //
+  MemPages = MemoryBlockSizeInPages + 1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             MemPages,
+             &TempPtr
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Ptr = (UINT8 *) ((UINTN) TempPtr);
+
+  ZeroMem (Ptr, MemPages * EFI_PAGE_SIZE);
+
+  *MemoryHeader = (MEMORY_MANAGE_HEADER *) Ptr;
+  //
+  // adjust Ptr pointer to the next empty memory
+  //
+  Ptr += sizeof (MEMORY_MANAGE_HEADER);
+  //
+  // Set Bit Array initial address
+  //
+  (*MemoryHeader)->BitArrayPtr  = Ptr;
+
+  (*MemoryHeader)->Next         = NULL;
+
+  //
+  // Memory block initial address
+  //
+  Ptr = (UINT8 *) ((UINTN) TempPtr);
+  Ptr += EFI_PAGE_SIZE;
+  (*MemoryHeader)->MemoryBlockPtr = Ptr;
+  //
+  // set Memory block size
+  //
+  (*MemoryHeader)->MemoryBlockSizeInBytes = MemoryBlockSizeInPages * EFI_PAGE_SIZE;
+  //
+  // each bit in Bit Array will manage 32byte memory in memory block
+  //
+  (*MemoryHeader)->BitArraySizeInBytes = ((*MemoryHeader)->MemoryBlockSizeInBytes / 32) / 8;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UHCI memory management.
+
+  @param  UhcDev                 The UCHI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+InitializeMemoryManagement (
+  IN USB_UHC_DEV           *UhcDev
+  )
+{
+  MEMORY_MANAGE_HEADER  *MemoryHeader;
+  EFI_STATUS            Status;
+  UINTN                 MemPages;
+
+  MemPages  = NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES;
+  Status    = CreateMemoryBlock (UhcDev, &MemoryHeader, MemPages);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UhcDev->Header1 = MemoryHeader;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UHCI memory management.
+
+  @param  UhcDev           The UCHI device.
+  @param  Pool             Buffer pointer to store the buffer pointer.
+  @param  AllocSize        The size of the pool to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+UhcAllocatePool (
+  IN  USB_UHC_DEV     *UhcDev,
+  OUT UINT8           **Pool,
+  IN  UINTN           AllocSize
+  )
+{
+  MEMORY_MANAGE_HEADER  *MemoryHeader;
+  MEMORY_MANAGE_HEADER  *TempHeaderPtr;
+  MEMORY_MANAGE_HEADER  *NewMemoryHeader;
+  UINTN                 RealAllocSize;
+  UINTN                 MemoryBlockSizeInPages;
+  EFI_STATUS            Status;
+
+  *Pool = NULL;
+
+  MemoryHeader = UhcDev->Header1;
+
+  //
+  // allocate unit is 32 byte (align on 32 byte)
+  //
+  if ((AllocSize & 0x1F) != 0) {
+    RealAllocSize = (AllocSize / 32 + 1) * 32;
+  } else {
+    RealAllocSize = AllocSize;
+  }
+
+  Status = EFI_NOT_FOUND;
+  for (TempHeaderPtr = MemoryHeader; TempHeaderPtr != NULL; TempHeaderPtr = TempHeaderPtr->Next) {
+
+    Status = AllocMemInMemoryBlock (
+              TempHeaderPtr,
+              (VOID **) Pool,
+              RealAllocSize / 32
+              );
+    if (!EFI_ERROR (Status)) {
+      return EFI_SUCCESS;
+    }
+  }
+  //
+  // There is no enough memory,
+  // Create a new Memory Block
+  //
+  //
+  // if pool size is larger than NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES,
+  // just allocate a large enough memory block.
+  //
+  if (RealAllocSize > (NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES * EFI_PAGE_SIZE)) {
+    MemoryBlockSizeInPages = RealAllocSize / EFI_PAGE_SIZE + 1;
+  } else {
+    MemoryBlockSizeInPages = NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES;
+  }
+
+  Status = CreateMemoryBlock (UhcDev, &NewMemoryHeader, MemoryBlockSizeInPages);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Link the new Memory Block to the Memory Header list
+  //
+  InsertMemoryHeaderToList (MemoryHeader, NewMemoryHeader);
+
+  Status = AllocMemInMemoryBlock (
+            NewMemoryHeader,
+            (VOID **) Pool,
+            RealAllocSize / 32
+            );
+  return Status;
+}
+
+/**
+  Alloc Memory In MemoryBlock.
+
+  @param  MemoryHeader           The pointer to memory manage header.
+  @param  Pool                   Buffer pointer to store the buffer pointer.
+  @param  NumberOfMemoryUnit     The size of the pool to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+AllocMemInMemoryBlock (
+  IN  MEMORY_MANAGE_HEADER  *MemoryHeader,
+  OUT VOID                  **Pool,
+  IN  UINTN                 NumberOfMemoryUnit
+  )
+{
+  UINTN TempBytePos;
+  UINTN FoundBytePos;
+  UINT8 Index;
+  UINT8 FoundBitPos;
+  UINT8 ByteValue;
+  UINT8 BitValue;
+  UINTN NumberOfZeros;
+  UINTN Count;
+
+  FoundBytePos  = 0;
+  FoundBitPos   = 0;
+
+  ByteValue     = MemoryHeader->BitArrayPtr[0];
+  NumberOfZeros = 0;
+  Index             = 0;
+  for (TempBytePos = 0; TempBytePos < MemoryHeader->BitArraySizeInBytes;) {
+    //
+    // Pop out BitValue from a byte in TempBytePos.
+    //
+    BitValue = (UINT8)(ByteValue & 0x1);
+
+    if (BitValue == 0) {
+      //
+      // Found a free bit, the NumberOfZeros only record the number of those consecutive zeros
+      //
+      NumberOfZeros++;
+      //
+      // Found enough consecutive free space, break the loop
+      //
+      if (NumberOfZeros >= NumberOfMemoryUnit) {
+        break;
+      }
+    } else {
+      //
+      // Encountering a '1', meant the bit is ocupied.
+      //
+      if (NumberOfZeros >= NumberOfMemoryUnit) {
+        //
+        // Found enough consecutive free space,break the loop
+        //
+        break;
+      } else {
+        //
+        // the NumberOfZeros only record the number of those consecutive zeros,
+        // so reset the NumberOfZeros to 0 when encountering '1' before finding
+        // enough consecutive '0's
+        //
+        NumberOfZeros = 0;
+        //
+        // reset the (FoundBytePos,FoundBitPos) to the position of '1'
+        //
+        FoundBytePos  = TempBytePos;
+        FoundBitPos   = Index;
+      }
+    }
+    //
+    // right shift the byte
+    //
+    ByteValue /= 2;
+
+    //
+    // step forward a bit
+    //
+    Index++;
+    if (Index == 8) {
+      //
+      // step forward a byte, getting the byte value,
+      // and reset the bit pos.
+      //
+      TempBytePos += 1;
+      ByteValue = MemoryHeader->BitArrayPtr[TempBytePos];
+      Index     = 0;
+    }
+  }
+
+  if (NumberOfZeros < NumberOfMemoryUnit) {
+    return EFI_NOT_FOUND;
+  }
+  //
+  // Found enough free space.
+  //
+  //
+  // The values recorded in (FoundBytePos,FoundBitPos) have two conditions:
+  //  1)(FoundBytePos,FoundBitPos) record the position
+  //    of the last '1' before the consecutive '0's, it must
+  //    be adjusted to the start position of the consecutive '0's.
+  //  2)the start address of the consecutive '0's is just the start of
+  //    the bitarray. so no need to adjust the values of (FoundBytePos,FoundBitPos).
+  //
+  if ((MemoryHeader->BitArrayPtr[0] & BIT0) != 0) {
+    FoundBitPos += 1;
+  }
+  //
+  // Have the (FoundBytePos,FoundBitPos) make sense.
+  //
+  if (FoundBitPos > 7) {
+    FoundBytePos += 1;
+    FoundBitPos -= 8;
+  }
+  //
+  // Set the memory as allocated
+  //
+  for (TempBytePos = FoundBytePos, Index = FoundBitPos, Count = 0; Count < NumberOfMemoryUnit; Count++) {
+
+    MemoryHeader->BitArrayPtr[TempBytePos] = (UINT8) (MemoryHeader->BitArrayPtr[TempBytePos] | (1 << Index));
+    Index++;
+    if (Index == 8) {
+      TempBytePos += 1;
+      Index = 0;
+    }
+  }
+
+  *Pool = MemoryHeader->MemoryBlockPtr + (FoundBytePos * 8 + FoundBitPos) * 32;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Uhci Free Pool.
+
+  @param  UhcDev                 The UHCI device.
+  @param  Pool                   A pointer to store the buffer address.
+  @param  AllocSize              The size of the pool to be freed.
+
+**/
+VOID
+UhcFreePool (
+  IN USB_UHC_DEV     *UhcDev,
+  IN UINT8           *Pool,
+  IN UINTN           AllocSize
+  )
+{
+  MEMORY_MANAGE_HEADER  *MemoryHeader;
+  MEMORY_MANAGE_HEADER  *TempHeaderPtr;
+  UINTN                 StartBytePos;
+  UINTN                 Index;
+  UINT8                 StartBitPos;
+  UINT8                 Index2;
+  UINTN                 Count;
+  UINTN                 RealAllocSize;
+
+  MemoryHeader = UhcDev->Header1;
+
+  //
+  // allocate unit is 32 byte (align on 32 byte)
+  //
+  if ((AllocSize & 0x1F) != 0) {
+    RealAllocSize = (AllocSize / 32 + 1) * 32;
+  } else {
+    RealAllocSize = AllocSize;
+  }
+
+  for (TempHeaderPtr = MemoryHeader; TempHeaderPtr != NULL;
+       TempHeaderPtr = TempHeaderPtr->Next) {
+
+    if ((Pool >= TempHeaderPtr->MemoryBlockPtr) &&
+        ((Pool + RealAllocSize) <= (TempHeaderPtr->MemoryBlockPtr +
+                                    TempHeaderPtr->MemoryBlockSizeInBytes))) {
+
+      //
+      // Pool is in the Memory Block area,
+      // find the start byte and bit in the bit array
+      //
+      StartBytePos  = ((Pool - TempHeaderPtr->MemoryBlockPtr) / 32) / 8;
+      StartBitPos   = (UINT8) (((Pool - TempHeaderPtr->MemoryBlockPtr) / 32) % 8);
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Index = StartBytePos, Index2 = StartBitPos, Count = 0; Count < (RealAllocSize / 32); Count++) {
+
+        TempHeaderPtr->BitArrayPtr[Index] = (UINT8) (TempHeaderPtr->BitArrayPtr[Index] ^ (1 << Index2));
+        Index2++;
+        if (Index2 == 8) {
+          Index += 1;
+          Index2 = 0;
+        }
+      }
+      //
+      // break the loop
+      //
+      break;
+    }
+  }
+
+}
+
+/**
+  Insert a new memory header into list.
+
+  @param  MemoryHeader         A pointer to the memory header list.
+  @param  NewMemoryHeader      A new memory header to be inserted into the list.
+
+**/
+VOID
+InsertMemoryHeaderToList (
+  IN MEMORY_MANAGE_HEADER  *MemoryHeader,
+  IN MEMORY_MANAGE_HEADER  *NewMemoryHeader
+  )
+{
+  MEMORY_MANAGE_HEADER  *TempHeaderPtr;
+
+  for (TempHeaderPtr = MemoryHeader; TempHeaderPtr != NULL; TempHeaderPtr = TempHeaderPtr->Next) {
+    if (TempHeaderPtr->Next == NULL) {
+      TempHeaderPtr->Next = NewMemoryHeader;
+      break;
+    }
+  }
+}
+
+/**
+  Judge the memory block in the memory header is empty or not.
+
+  @param  MemoryHeaderPtr   A pointer to the memory header list.
+
+  @retval Whether the memory block in the memory header is empty or not.
+
+**/
+BOOLEAN
+IsMemoryBlockEmptied (
+  IN MEMORY_MANAGE_HEADER  *MemoryHeaderPtr
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < MemoryHeaderPtr->BitArraySizeInBytes; Index++) {
+    if (MemoryHeaderPtr->BitArrayPtr[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  remove a memory header from list.
+
+  @param  FirstMemoryHeader   A pointer to the memory header list.
+  @param  FreeMemoryHeader    A memory header to be removed into the list.
+
+**/
+VOID
+DelinkMemoryBlock (
+  IN MEMORY_MANAGE_HEADER    *FirstMemoryHeader,
+  IN MEMORY_MANAGE_HEADER    *FreeMemoryHeader
+  )
+{
+  MEMORY_MANAGE_HEADER  *TempHeaderPtr;
+
+  if ((FirstMemoryHeader == NULL) || (FreeMemoryHeader == NULL)) {
+    return ;
+  }
+
+  for (TempHeaderPtr = FirstMemoryHeader; TempHeaderPtr != NULL; TempHeaderPtr = TempHeaderPtr->Next) {
+
+    if (TempHeaderPtr->Next == FreeMemoryHeader) {
+      //
+      // Link the before and after
+      //
+      TempHeaderPtr->Next = FreeMemoryHeader->Next;
+      break;
+    }
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.h b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.h
new file mode 100644
index 0000000000..460db7eab9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.h
@@ -0,0 +1,1333 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2006 - 2013, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _RECOVERY_UHC_H_
+#define _RECOVERY_UHC_H_
+
+
+#include <PiPei.h>
+
+#include <Ppi/UsbController.h>
+#include <Ppi/UsbHostController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/IoLib.h>
+#include <Library/PeiServicesLib.h>
+
+#define USB_SLOW_SPEED_DEVICE 0x01
+#define USB_FULL_SPEED_DEVICE 0x02
+
+//
+// One memory block uses 16 page
+//
+#define NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES 16
+
+#define USBCMD                            0       /* Command Register Offset 00-01h */
+#define USBCMD_RS                         BIT0    /* Run/Stop */
+#define USBCMD_HCRESET                    BIT1    /* Host reset */
+#define USBCMD_GRESET                     BIT2    /* Global reset */
+#define USBCMD_EGSM                       BIT3    /* Global Suspend Mode */
+#define USBCMD_FGR                        BIT4    /* Force Global Resume */
+#define USBCMD_SWDBG                      BIT5    /* SW Debug mode */
+#define USBCMD_CF                         BIT6    /* Config Flag (sw only) */
+#define USBCMD_MAXP                       BIT7    /* Max Packet (0 = 32, 1 = 64) */
+
+/* Status register */
+#define USBSTS        2       /* Status Register Offset 02-03h */
+#define USBSTS_USBINT BIT0    /* Interrupt due to IOC */
+#define USBSTS_ERROR  BIT1    /* Interrupt due to error */
+#define USBSTS_RD     BIT2    /* Resume Detect */
+#define USBSTS_HSE    BIT3    /* Host System Error - basically PCI problems */
+#define USBSTS_HCPE   BIT4    /* Host Controller Process Error - the scripts were buggy */
+#define USBSTS_HCH    BIT5    /* HC Halted */
+
+/* Interrupt enable register */
+#define USBINTR         4       /* Interrupt Enable Register 04-05h */
+#define USBINTR_TIMEOUT BIT0    /* Timeout/CRC error enable */
+#define USBINTR_RESUME  BIT1    /* Resume interrupt enable */
+#define USBINTR_IOC     BIT2    /* Interrupt On Complete enable */
+#define USBINTR_SP      BIT3    /* Short packet interrupt enable */
+
+/* Frame Number Register Offset 06-08h */
+#define USBFRNUM  6
+
+/* Frame List Base Address Register Offset 08-0Bh */
+#define USBFLBASEADD  8
+
+/* Start of Frame Modify Register Offset 0Ch */
+#define USBSOF  0x0c
+
+/* USB port status and control registers */
+#define USBPORTSC1            0x10      /*Port 1 offset 10-11h */
+#define USBPORTSC2            0x12      /*Port 2 offset 12-13h */
+
+#define USBPORTSC_CCS         BIT0      /* Current Connect Status ("device present") */
+#define USBPORTSC_CSC         BIT1      /* Connect Status Change */
+#define USBPORTSC_PED         BIT2      /* Port Enable / Disable */
+#define USBPORTSC_PEDC        BIT3      /* Port Enable / Disable Change */
+#define USBPORTSC_LSL         BIT4      /* Line Status Low bit*/
+#define USBPORTSC_LSH         BIT5      /* Line Status High bit*/
+#define USBPORTSC_RD          BIT6      /* Resume Detect */
+#define USBPORTSC_LSDA        BIT8      /* Low Speed Device Attached */
+#define USBPORTSC_PR          BIT9      /* Port Reset */
+#define USBPORTSC_SUSP        BIT12     /* Suspend */
+
+#define SETUP_PACKET_ID       0x2D
+#define INPUT_PACKET_ID       0x69
+#define OUTPUT_PACKET_ID      0xE1
+#define ERROR_PACKET_ID       0x55
+
+#define STALL_1_MICRO_SECOND  1
+#define STALL_1_MILLI_SECOND  1000
+
+
+#pragma pack(1)
+
+typedef struct {
+  UINT32  FrameListPtrTerminate : 1;
+  UINT32  FrameListPtrQSelect : 1;
+  UINT32  FrameListRsvd : 2;
+  UINT32  FrameListPtr : 28;
+} FRAMELIST_ENTRY;
+
+typedef struct {
+  UINT32  QHHorizontalTerminate : 1;
+  UINT32  QHHorizontalQSelect : 1;
+  UINT32  QHHorizontalRsvd : 2;
+  UINT32  QHHorizontalPtr : 28;
+  UINT32  QHVerticalTerminate : 1;
+  UINT32  QHVerticalQSelect : 1;
+  UINT32  QHVerticalRsvd : 2;
+  UINT32  QHVerticalPtr : 28;
+} QUEUE_HEAD;
+
+typedef struct {
+  QUEUE_HEAD  QueueHead;
+  UINT32      Reserved1;
+  UINT32      Reserved2;
+  VOID        *PtrNext;
+  VOID        *PtrDown;
+  VOID        *Reserved3;
+  UINT32      Reserved4;
+} QH_STRUCT;
+
+typedef struct {
+  UINT32  TDLinkPtrTerminate : 1;
+  UINT32  TDLinkPtrQSelect : 1;
+  UINT32  TDLinkPtrDepthSelect : 1;
+  UINT32  TDLinkPtrRsvd : 1;
+  UINT32  TDLinkPtr : 28;
+  UINT32  TDStatusActualLength : 11;
+  UINT32  TDStatusRsvd : 5;
+  UINT32  TDStatus : 8;
+  UINT32  TDStatusIOC : 1;
+  UINT32  TDStatusIOS : 1;
+  UINT32  TDStatusLS : 1;
+  UINT32  TDStatusErr : 2;
+  UINT32  TDStatusSPD : 1;
+  UINT32  TDStatusRsvd2 : 2;
+  UINT32  TDTokenPID : 8;
+  UINT32  TDTokenDevAddr : 7;
+  UINT32  TDTokenEndPt : 4;
+  UINT32  TDTokenDataToggle : 1;
+  UINT32  TDTokenRsvd : 1;
+  UINT32  TDTokenMaxLen : 11;
+  UINT32  TDBufferPtr;
+} TD;
+
+typedef struct {
+  TD      TDData;
+  UINT8   *PtrTDBuffer;
+  VOID    *PtrNextTD;
+  VOID    *PtrNextQH;
+  UINT16  TDBufferLength;
+  UINT16  Reserved;
+} TD_STRUCT;
+
+#pragma pack()
+
+typedef struct _MEMORY_MANAGE_HEADER MEMORY_MANAGE_HEADER;
+
+struct _MEMORY_MANAGE_HEADER {
+  UINT8                         *BitArrayPtr;
+  UINTN                         BitArraySizeInBytes;
+  UINT8                         *MemoryBlockPtr;
+  UINTN                         MemoryBlockSizeInBytes;
+  MEMORY_MANAGE_HEADER          *Next;
+};
+
+#define USB_UHC_DEV_SIGNATURE SIGNATURE_32 ('p', 'u', 'h', 'c')
+typedef struct {
+  UINTN                       Signature;
+  PEI_USB_HOST_CONTROLLER_PPI UsbHostControllerPpi;
+  EFI_PEI_PPI_DESCRIPTOR      PpiDescriptor;
+
+  UINT32                      UsbHostControllerBaseAddress;
+  FRAMELIST_ENTRY             *FrameListEntry;
+  QH_STRUCT                   *ConfigQH;
+  QH_STRUCT                   *BulkQH;
+  //
+  // Header1 used for QH,TD memory blocks management
+  //
+  MEMORY_MANAGE_HEADER        *Header1;
+
+} USB_UHC_DEV;
+
+#define PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS(a)  CR (a, USB_UHC_DEV, UsbHostControllerPpi, USB_UHC_DEV_SIGNATURE)
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress          The target device address.
+  @param  DeviceSpeed            Target device speed.
+  @param  MaximumPacketLength    Maximum packet size the default control transfer 
+                                 endpoint is capable of sending or receiving.
+  @param  Request                USB device request to send.
+  @param  TransferDirection      Specifies the data direction for the data stage.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+  @param  TimeOut                Indicates the maximum timeout, in millisecond.
+  @param  TransferResult         Return the result of this control transfer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcControlTransfer (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    * This,
+  IN     UINT8                      DeviceAddress,
+  IN     UINT8                      DeviceSpeed,
+  IN     UINT8                      MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST     * Request,
+  IN     EFI_USB_DATA_DIRECTION     TransferDirection,
+  IN OUT VOID                       *Data OPTIONAL,
+  IN OUT UINTN                      *DataLength OPTIONAL,
+  IN     UINTN                      TimeOut,
+  OUT    UINT32                     *TransferResult
+  );
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of 
+                                sending or receiving.
+  @param  Data                  Array of pointers to the buffers of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to use of 
+                                the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcBulkTransfer (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN  UINT8                         DeviceAddress,
+  IN  UINT8                         EndPointAddress,
+  IN  UINT8                         MaximumPacketLength,
+  IN OUT VOID                       *Data,
+  IN OUT UINTN                      *DataLength,
+  IN OUT UINT8                      *DataToggle,
+  IN  UINTN                         TimeOut,
+  OUT UINT32                        *TransferResult
+  );
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices   The pointer to the PEI Services Table.
+  @param[in]  This          The pointer to this instance of the 
+                            PEI_USB_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber    The pointer to the number of the root hub ports.                                
+                                
+  @retval EFI_SUCCESS           The port number was retrieved successfully.
+  @retval EFI_INVALID_PARAMETER PortNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcGetRootHubPortNumber (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  OUT UINT8                         *PortNumber
+  );
+
+/**
+  Retrieves the current status of a USB root hub port.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  PortNumber             The root hub port to retrieve the state from.  
+  @param  PortStatus             Variable to receive the port state.
+
+  @retval EFI_SUCCESS            The status of the USB root hub port specified.
+                                 by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcGetRootHubPortStatus (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN  UINT8                         PortNumber,
+  OUT EFI_USB_PORT_STATUS           *PortStatus
+  );
+
+/**
+  Sets a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_TIMEOUT            The time out occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcSetRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  );
+
+/**
+  Clears a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  PortNumber            Specifies the root hub port whose feature
+                                is requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was cleared 
+                                 for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcClearRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  );
+
+/**
+  Initialize UHCI.
+
+  @param  UhcDev                 UHCI Device.
+
+  @retval EFI_SUCCESS            UHCI successfully initialized.
+  @retval EFI_OUT_OF_RESOURCES   Resource can not be allocated.
+
+**/
+EFI_STATUS
+InitializeUsbHC (
+  IN USB_UHC_DEV          *UhcDev
+  );
+
+/**
+  Create Frame List Structure.
+
+  @param  UhcDev                 UHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+CreateFrameList (
+  USB_UHC_DEV             *UhcDev
+  );
+
+/**
+  Read a 16bit width data from Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+
+  @retval the register content read.
+
+**/
+UINT16
+USBReadPortW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port
+  );
+
+/**
+  Write a 16bit width data into Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+  @param  Data    The data written into the register.
+
+**/
+VOID
+USBWritePortW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port,
+  IN  UINT16        Data
+  );
+
+/**
+  Write a 32bit width data into Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+  @param  Data    The data written into the register.
+
+**/
+VOID
+USBWritePortDW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port,
+  IN  UINT32        Data
+  );
+
+/**
+  Clear the content of UHCI's Status Register.
+  
+  @param  UhcDev       The UHCI device.
+  @param  StatusAddr   The IO space address of the register.
+
+**/
+VOID
+ClearStatusReg (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        StatusAddr
+  );
+
+/**
+  Check whether the host controller operates well.
+
+  @param  UhcDev        The UHCI device.
+  @param  StatusRegAddr The io address of status register.
+
+  @retval TRUE          Host controller is working.
+  @retval FALSE         Host controller is halted or system error.
+
+**/
+BOOLEAN
+IsStatusOK (
+  IN USB_UHC_DEV     *UhcDev,
+  IN UINT32          StatusRegAddr
+  );
+
+/**
+  Get Current Frame Number.
+
+  @param  UhcDev          The UHCI device.
+  @param  FrameNumberAddr The address of frame list register.
+
+  @retval The content of the frame list register.
+
+**/
+UINT16
+GetCurrentFrameNumber (
+  IN USB_UHC_DEV   *UhcDev,
+  IN UINT32        FrameNumberAddr
+  );
+
+/**
+  Set Frame List Base Address.
+
+  @param  UhcDev           The UHCI device.
+  @param  FrameListRegAddr The address of frame list register.
+  @param  Addr             The address of frame list table.
+
+**/
+VOID
+SetFrameListBaseAddress (
+  IN USB_UHC_DEV   *UhcDev,
+  IN UINT32        FrameListRegAddr,
+  IN UINT32        Addr
+  );
+
+/**
+  Create QH and initialize.
+
+  @param  UhcDev               The UHCI device.
+  @param  PtrQH                Place to store QH_STRUCT pointer.
+
+  @retval EFI_OUT_OF_RESOURCES Can't allocate memory resources.
+  @retval EFI_SUCCESS        Success.
+
+**/
+EFI_STATUS
+CreateQH (
+  IN  USB_UHC_DEV  *UhcDev,
+  OUT QH_STRUCT    **PtrQH
+  );
+
+/**
+  Set the horizontal link pointer in QH.
+
+  @param  PtrQH               Place to store QH_STRUCT pointer.
+  @param  PtrNext             Place to the next QH_STRUCT.
+
+**/
+VOID
+SetQHHorizontalLinkPtr (
+  IN QH_STRUCT  *PtrQH,
+  IN VOID       *PtrNext
+  );
+
+/**
+  Get the horizontal link pointer in QH.
+
+  @param  PtrQH     Place to store QH_STRUCT pointer.
+
+  @retval The horizontal link pointer in QH.
+
+**/
+VOID  *
+GetQHHorizontalLinkPtr (
+  IN QH_STRUCT  *PtrQH
+  );
+
+/**
+  Set a QH or TD horizontally to be connected with a specific QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsQH       Specify QH or TD is connected.
+
+**/
+VOID
+SetQHHorizontalQHorTDSelect (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsQH
+  );
+
+/**
+  Set the horizontal validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsValid    Specify the horizontal linker is valid or not.
+
+**/
+VOID
+SetQHHorizontalValidorInvalid (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsValid
+  );
+
+/**
+  Set the vertical link pointer in QH.
+
+  @param  PtrQH       Place to store QH_STRUCT pointer.
+  @param  PtrNext     Place to the next QH_STRUCT.
+
+**/
+VOID
+SetQHVerticalLinkPtr (
+  IN QH_STRUCT  *PtrQH,
+  IN VOID       *PtrNext
+  );
+
+/**
+  Set a QH or TD vertically to be connected with a specific QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsQH       Specify QH or TD is connected.
+
+**/
+VOID
+SetQHVerticalQHorTDSelect (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsQH
+  );
+
+/**
+  Set the vertical validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsValid    Specify the vertical linker is valid or not.
+
+**/
+VOID
+SetQHVerticalValidorInvalid (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsValid
+  );
+
+/**
+  Get the vertical validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+
+  @retval The vertical linker is valid or not.
+
+**/
+BOOLEAN
+GetQHHorizontalValidorInvalid (
+  IN QH_STRUCT  *PtrQH
+  );
+
+/**
+  Allocate TD or QH Struct.
+
+  @param  UhcDev                 The UHCI device.
+  @param  Size                   The size of allocation.
+  @param  PtrStruct              Place to store TD_STRUCT pointer.
+
+  @return EFI_SUCCESS            Allocate successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+AllocateTDorQHStruct (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Size,
+  OUT VOID          **PtrStruct
+  );
+
+/**
+  Create a TD Struct.
+
+  @param  UhcDev                 The UHCI device.
+  @param  PtrTD                  Place to store TD_STRUCT pointer.
+
+  @return EFI_SUCCESS            Allocate successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+CreateTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  OUT TD_STRUCT       **PtrTD
+  );
+
+/**
+  Generate Setup Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  DeviceSpeed  Device Speed.
+  @param  DevRequest   Device reuquest.
+  @param  RequestLen   Request length.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate setup stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+GenSetupStageTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           DeviceSpeed,
+  IN  UINT8           *DevRequest,
+  IN  UINT8           RequestLen,
+  OUT TD_STRUCT       **PtrTD
+  );
+
+/**
+  Generate Data Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  PtrData      Data buffer.
+  @param  Len          Data length.
+  @param  PktID        PacketID.
+  @param  Toggle       Data toggle value.
+  @param  DeviceSpeed  Device Speed.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate data stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+GenDataTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           *PtrData,
+  IN  UINT8           Len,
+  IN  UINT8           PktID,
+  IN  UINT8           Toggle,
+  IN  UINT8           DeviceSpeed,
+  OUT TD_STRUCT       **PtrTD
+  );
+
+/**
+  Generate Status Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  PktID        PacketID.
+  @param  DeviceSpeed  Device Speed.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate status stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+CreateStatusTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           PktID,
+  IN  UINT8           DeviceSpeed,
+  OUT TD_STRUCT       **PtrTD
+  );
+
+/**
+  Set the link pointer validor bit in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  IsValid      Specify the linker pointer is valid or not.
+
+**/
+VOID
+SetTDLinkPtrValidorInvalid (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsValid
+  );
+
+/**
+  Set the Link Pointer pointing to a QH or TD.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  IsQH          Specify QH or TD is connected.
+
+**/
+VOID
+SetTDLinkPtrQHorTDSelect (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsQH
+  );
+
+/**
+  Set the traverse is depth-first or breadth-first.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  IsDepth       Specify the traverse is depth-first or breadth-first.
+
+**/
+VOID
+SetTDLinkPtrDepthorBreadth (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsDepth
+  );
+
+/**
+  Set TD Link Pointer in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  PtrNext      Place to the next TD_STRUCT.
+
+**/
+VOID
+SetTDLinkPtr (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  VOID      *PtrNext
+  );
+
+/**
+  Get TD Link Pointer.
+
+  @param  PtrTDStruct     Place to store TD_STRUCT pointer.
+
+  @retval Get TD Link Pointer in TD.
+
+**/
+VOID*
+GetTDLinkPtr (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Get the information about whether the Link Pointer field pointing to
+  a QH or a TD.
+
+  @param  PtrTDStruct     Place to store TD_STRUCT pointer.
+
+  @retval whether the Link Pointer field pointing to a QH or a TD.
+
+**/
+BOOLEAN
+IsTDLinkPtrQHOrTD (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Enable/Disable short packet detection mechanism.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  IsEnable     Enable or disable short packet detection mechanism.
+
+**/
+VOID
+EnableorDisableTDShortPacket (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsEnable
+  );
+
+/**
+  Set the max error counter in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  MaxErrors    The number of allowable error.
+
+**/
+VOID
+SetTDControlErrorCounter (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT8     MaxErrors
+  );
+
+/**
+  Set the TD is targeting a low-speed device or not.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsLowSpeedDevice  Whether The device is low-speed.
+
+**/
+VOID
+SetTDLoworFullSpeedDevice (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsLowSpeedDevice
+  );
+
+/**
+  Set the TD is isochronous transfer type or not.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsIsochronous     Whether the transaction isochronous transfer type.
+
+**/
+VOID
+SetTDControlIsochronousorNot (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsIsochronous
+  );
+
+/**
+  Set if UCHI should issue an interrupt on completion of the frame
+  in which this TD is executed
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsSet             Whether HC should issue an interrupt on completion.
+
+**/
+VOID
+SetorClearTDControlIOC (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsSet
+  );
+
+/**
+  Set if the TD is active and can be executed.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsActive          Whether the TD is active and can be executed.
+
+**/
+VOID
+SetTDStatusActiveorInactive (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsActive
+  );
+
+/**
+  Specifies the maximum number of data bytes allowed for the transfer.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  MaxLen        The maximum number of data bytes allowed.
+
+  @retval The allowed maximum number of data.
+**/
+UINT16
+SetTDTokenMaxLength (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT16    MaxLen
+  );
+
+/**
+  Set the data toggle bit to DATA1.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDTokenDataToggle1 (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Set the data toggle bit to DATA0.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDTokenDataToggle0 (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Set EndPoint Number the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  EndPoint      The Endport number of the target.
+
+**/
+VOID
+SetTDTokenEndPoint (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINTN     EndPoint
+  );
+
+/**
+  Set Device Address the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  DevAddr       The Device Address of the target.
+
+**/
+VOID
+SetTDTokenDeviceAddress (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINTN     DevAddr
+  );
+
+/**
+  Set Packet Identification the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  PacketID      The Packet Identification of the target.
+
+**/
+VOID
+SetTDTokenPacketID (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT8     PacketID
+  );
+
+/**
+  Set the beginning address of the data buffer that will be used
+  during the transaction.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDDataBuffer (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether the TD is active.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The TD is active or not.
+
+**/
+BOOLEAN
+IsTDStatusActive (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether the TD is stalled.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The TD is stalled or not.
+
+**/
+BOOLEAN
+IsTDStatusStalled (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether Data Buffer Error is happened.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Data Buffer Error is happened or not.
+
+**/
+BOOLEAN
+IsTDStatusBufferError (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether Babble Error is happened.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Babble Error is happened or not.
+
+**/
+BOOLEAN
+IsTDStatusBabbleError (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether NAK is received.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The NAK is received or not.
+
+**/
+BOOLEAN
+IsTDStatusNAKReceived (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether CRC/Time Out Error is encountered.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The CRC/Time Out Error is encountered or not.
+
+**/
+BOOLEAN
+IsTDStatusCRCTimeOutError (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether Bitstuff Error is received.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Bitstuff Error is received or not.
+
+**/
+BOOLEAN
+IsTDStatusBitStuffError (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Retrieve the actual number of bytes that were tansferred.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The actual number of bytes that were tansferred.
+
+**/
+UINT16
+GetTDStatusActualLength (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Retrieve the information of whether the Link Pointer field is valid or not.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The linker pointer field is valid or not.
+
+**/
+BOOLEAN
+GetTDLinkPtrValidorInvalid (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Count TD Number from PtrFirstTD.
+
+  @param  PtrFirstTD   Place to store TD_STRUCT pointer.
+
+  @retval The queued TDs number.
+
+**/
+UINTN
+CountTDsNumber (
+  IN  TD_STRUCT *PtrFirstTD
+  );
+
+/**
+  Link TD To QH.
+
+  @param  PtrQH   Place to store QH_STRUCT pointer.
+  @param  PtrTD   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+LinkTDToQH (
+  IN  QH_STRUCT *PtrQH,
+  IN  TD_STRUCT *PtrTD
+  );
+
+/**
+  Link TD To TD.
+
+  @param  PtrPreTD  Place to store TD_STRUCT pointer.
+  @param  PtrTD     Place to store TD_STRUCT pointer.
+
+**/
+VOID
+LinkTDToTD (
+  IN  TD_STRUCT *PtrPreTD,
+  IN  TD_STRUCT *PtrTD
+  );
+
+/**
+  Execute Control Transfer.
+
+  @param  UhcDev            The UCHI device.
+  @param  PtrTD             A pointer to TD_STRUCT data.
+  @param  ActualLen         Actual transfer Length.
+  @param  TimeOut           TimeOut value.
+  @param  TransferResult    Transfer Result.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+ExecuteControlTransfer (
+  IN  USB_UHC_DEV *UhcDev,
+  IN  TD_STRUCT   *PtrTD,
+  OUT UINTN       *ActualLen,
+  IN  UINTN       TimeOut,
+  OUT UINT32      *TransferResult
+  );
+
+/**
+  Execute Bulk Transfer.
+
+  @param  UhcDev            The UCHI device.
+  @param  PtrTD             A pointer to TD_STRUCT data.
+  @param  ActualLen         Actual transfer Length.
+  @param  DataToggle        DataToggle value.
+  @param  TimeOut           TimeOut value.
+  @param  TransferResult    Transfer Result.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+ExecBulkTransfer (
+  IN     USB_UHC_DEV *UhcDev,
+  IN     TD_STRUCT   *PtrTD,
+  IN OUT UINTN     *ActualLen,
+  IN     UINT8     *DataToggle,
+  IN     UINTN     TimeOut,
+  OUT    UINT32    *TransferResult
+  );
+
+/**
+  Delete Queued TDs.
+
+  @param  UhcDev       The UCHI device.
+  @param  PtrFirstTD   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+DeleteQueuedTDs (
+  IN USB_UHC_DEV     *UhcDev,
+  IN TD_STRUCT       *PtrFirstTD
+  );
+
+/**
+  Check TDs Results.
+
+  @param  PtrTD               A pointer to TD_STRUCT data.
+  @param  Result              The result to return.
+  @param  ErrTDPos            The Error TD position.
+  @param  ActualTransferSize  Actual transfer size.
+
+  @retval The TD is executed successfully or not.
+
+**/
+BOOLEAN
+CheckTDsResults (
+  IN  TD_STRUCT               *PtrTD,
+  OUT UINT32                  *Result,
+  OUT UINTN                   *ErrTDPos,
+  OUT UINTN                   *ActualTransferSize
+  );
+
+/**
+  Create Memory Block.
+
+  @param  UhcDev                   The UCHI device.
+  @param  MemoryHeader             The Pointer to allocated memory block.
+  @param  MemoryBlockSizeInPages   The page size of memory block to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+CreateMemoryBlock (
+  IN  USB_UHC_DEV           *UhcDev,
+  OUT MEMORY_MANAGE_HEADER  **MemoryHeader,
+  IN  UINTN                 MemoryBlockSizeInPages
+  );
+
+/**
+  Initialize UHCI memory management.
+
+  @param  UhcDev                 The UCHI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+InitializeMemoryManagement (
+  IN USB_UHC_DEV           *UhcDev
+  );
+
+/**
+  Initialize UHCI memory management.
+
+  @param  UhcDev           The UCHI device.
+  @param  Pool             Buffer pointer to store the buffer pointer.
+  @param  AllocSize        The size of the pool to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+UhcAllocatePool (
+  IN  USB_UHC_DEV     *UhcDev,
+  OUT UINT8           **Pool,
+  IN  UINTN           AllocSize
+  );
+
+/**
+  Alloc Memory In MemoryBlock.
+
+  @param  MemoryHeader           The pointer to memory manage header.
+  @param  Pool                   Buffer pointer to store the buffer pointer.
+  @param  NumberOfMemoryUnit     The size of the pool to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+AllocMemInMemoryBlock (
+  IN  MEMORY_MANAGE_HEADER  *MemoryHeader,
+  OUT VOID                  **Pool,
+  IN  UINTN                 NumberOfMemoryUnit
+  );
+
+/**
+  Uhci Free Pool.
+
+  @param  UhcDev                 The UHCI device.
+  @param  Pool                   A pointer to store the buffer address.
+  @param  AllocSize              The size of the pool to be freed.
+
+**/
+VOID
+UhcFreePool (
+  IN USB_UHC_DEV     *UhcDev,
+  IN UINT8           *Pool,
+  IN UINTN           AllocSize
+  );
+
+/**
+  Insert a new memory header into list.
+
+  @param  MemoryHeader         A pointer to the memory header list.
+  @param  NewMemoryHeader      A new memory header to be inserted into the list.
+
+**/
+VOID
+InsertMemoryHeaderToList (
+  IN MEMORY_MANAGE_HEADER  *MemoryHeader,
+  IN MEMORY_MANAGE_HEADER  *NewMemoryHeader
+  );
+
+/**
+  Judge the memory block in the memory header is empty or not.
+
+  @param  MemoryHeaderPtr   A pointer to the memory header list.
+
+  @retval Whether the memory block in the memory header is empty or not.
+
+**/
+BOOLEAN
+IsMemoryBlockEmptied (
+  IN MEMORY_MANAGE_HEADER  *MemoryHeaderPtr
+  );
+
+/**
+  remove a memory header from list.
+
+  @param  FirstMemoryHeader   A pointer to the memory header list.
+  @param  FreeMemoryHeader    A memory header to be removed into the list.
+
+**/
+VOID
+DelinkMemoryBlock (
+  IN MEMORY_MANAGE_HEADER    *FirstMemoryHeader,
+  IN MEMORY_MANAGE_HEADER    *FreeMemoryHeader
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.inf b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.inf
new file mode 100644
index 0000000000..7baa07612d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.inf
@@ -0,0 +1,64 @@
+## @file
+# The UhcPeim driver is responsible for managing the behavior of UHCI controller at PEI phase.
+#
+# It produces gPeiUsbHostControllerPpiGuid based on gPeiUsbControllerPpiGuid which is used
+# to enable recovery function from USB Drivers.
+#
+# Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions
+# of the BSD License which accompanies this distribution.  The
+# full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UhciPei
+  MODULE_UNI_FILE                = UhciPei.uni
+  FILE_GUID                      = C463CEAC-FC57-4f36-88B7-356C750C3BCA
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = UhcPeimEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  UhcPeim.c
+  UhcPeim.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  IoLib
+  TimerLib
+  BaseMemoryLib
+  PeiServicesLib
+  PeimEntryPoint
+  DebugLib
+
+
+[Ppis]
+  gPeiUsbHostControllerPpiGuid                  ## PRODUCES
+  gPeiUsbControllerPpiGuid                      ## CONSUMES
+
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gPeiUsbControllerPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.uni b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.uni
new file mode 100644
index 0000000000..b3cfb23139
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPeiExtra.uni
new file mode 100644
index 0000000000..5cde35591a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.c
new file mode 100644
index 0000000000..706aa292c8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.c
@@ -0,0 +1,224 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for XHCI driver.
+
+Copyright (c) 2011, 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 "Xhci.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gXhciComponentName = {
+  XhciComponentNameGetDriverName,
+  XhciComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gXhciComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) XhciComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) XhciComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mXhciDriverNameTable[] = {
+  { "eng;en", L"Usb Xhci Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+XhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mXhciDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gXhciComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is not a valid EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+XhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_HANDLE                   ChildHandle OPTIONAL,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **ControllerName
+  )
+{
+  EFI_STATUS           Status;
+  EFI_USB2_HC_PROTOCOL *Usb2Hc;
+  USB_XHCI_INSTANCE    *XhciDev;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gXhciDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  gXhciDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  XhciDev = XHC_FROM_THIS (Usb2Hc);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           XhciDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gXhciComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.h
new file mode 100644
index 0000000000..c3c44ccdfb
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.h
@@ -0,0 +1,146 @@
+/** @file
+
+  This file contains the delarations for componet name routines.
+
+Copyright (c) 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_COMPONENT_NAME_H_
+#define _EFI_COMPONENT_NAME_H_
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+XhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is not a valid EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+XhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_HANDLE                   ChildHandle OPTIONAL,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **ControllerName
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.c
new file mode 100644
index 0000000000..54c69d78ed
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.c
@@ -0,0 +1,758 @@
+/** @file
+
+  Routine procedures for memory allocate/free.
+
+Copyright (c) 2013 - 2015, 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 "Xhci.h"
+
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pool           The buffer pool to allocate memory for.
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Pages
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  VOID                    *BufHost;
+  VOID                    *Mapping;
+  EFI_PHYSICAL_ADDRESS    MappedAddr;
+  UINTN                   Bytes;
+  EFI_STATUS              Status;
+
+  PciIo = Pool->PciIo;
+
+  Block = AllocateZeroPool (sizeof (USBHC_MEM_BLOCK));
+  if (Block == NULL) {
+    return NULL;
+  }
+
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (USBHC_MEM_UNIT * 8);
+  Block->Bits     = AllocateZeroPool (Block->BitsLen);
+
+  if (Block->Bits == NULL) {
+    gBS->FreePool (Block);
+    return NULL;
+  }
+
+  //
+  // Allocate the number of Pages of memory, then map it for
+  // bus master read and write.
+  //
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    Pages,
+                    &BufHost,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_BITARRAY;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (Pages);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    BufHost,
+                    &Bytes,
+                    &MappedAddr,
+                    &Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (Pages))) {
+    goto FREE_BUFFER;
+  }
+
+  Block->BufHost  = BufHost;
+  Block->Buf      = (UINT8 *) ((UINTN) MappedAddr);
+  Block->Mapping  = Mapping;
+
+  return Block;
+
+FREE_BUFFER:
+  PciIo->FreeBuffer (PciIo, Pages, BufHost);
+
+FREE_BITARRAY:
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+  return NULL;
+}
+
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL       *Pool,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+
+  ASSERT ((Pool != NULL) && (Block != NULL));
+
+  PciIo = Pool->PciIo;
+
+  //
+  // Unmap the common buffer then free the structures
+  //
+  PciIo->Unmap (PciIo, Block->Mapping);
+  PciIo->FreeBuffer (PciIo, EFI_SIZE_TO_PAGES (Block->BufLen), Block->BufHost);
+
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+}
+
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block          The memory block to allocate memory from.
+  @param  Units          Number of memory units to allocate.
+
+  @return The pointer to the allocated memory. If couldn't allocate the needed memory,
+          the return value is NULL.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN  USBHC_MEM_BLOCK     *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->BufHost + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  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
+UsbHcGetPciAddrForHostAddr (
+  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;
+}
+
+/**
+  Calculate the corresponding host address according to the pci address.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to pci memory.
+  @param  Size           The size of the memory region.
+
+  @return                The host memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetHostAddrForPciAddr (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINTN                   AllocSize;
+  EFI_PHYSICAL_ADDRESS    HostAddr;
+  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->Buf <= (UINT8 *) Mem) && (((UINT8 *) Mem + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      break;
+    }
+  }
+
+  ASSERT ((Block != NULL));
+  //
+  // calculate the pci memory address for host memory address.
+  //
+  Offset = (UINT8 *)Mem - Block->Buf;
+  HostAddr = (EFI_PHYSICAL_ADDRESS)(UINTN) (Block->BufHost + Offset);
+  return HostAddr;
+}
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head           The head of the memory pool's block list.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+
+/**
+  Is the memory block empty?
+
+  @param  Block   The memory block to check.
+
+  @retval TRUE    The memory block is empty.
+  @retval FALSE   The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo                The PciIo that can be used to access the host controller.
+
+  @retval EFI_SUCCESS          The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE  Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo
+  )
+{
+  USBHC_MEM_POOL          *Pool;
+
+  Pool = AllocatePool (sizeof (USBHC_MEM_POOL));
+
+  if (Pool == NULL) {
+    return Pool;
+  }
+
+  Pool->PciIo   = PciIo;
+  Pool->Head    = UsbHcAllocMemBlock (Pool, USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    gBS->FreePool (Pool);
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+
+/**
+  Release the memory management pool.
+
+  @param  Pool              The USB memory pool to free.
+
+  @retval EFI_SUCCESS       The memory pool is freed.
+  @retval EFI_DEVICE_ERROR  Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+{
+  USBHC_MEM_BLOCK *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UsbHcUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    UsbHcUnlinkMemBlock (Pool->Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+  gBS->FreePool (Pool);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool           The host controller's memory pool.
+  @param  Size           Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  USBHC_MEM_BLOCK         *NewBlock;
+  VOID                    *Mem;
+  UINTN                   AllocSize;
+  UINTN                   Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+
+  NewBlock = UsbHcAllocMemBlock (Pool, Pages);
+
+  if (NewBlock == NULL) {
+    DEBUG ((EFI_D_ERROR, "UsbHcAllocateMem: failed to allocate block\n"));
+    return NULL;
+  }
+
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->BufHost <= ToFree) && ((ToFree + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    UsbHcUnlinkMemBlock (Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
+
+/**  
+  Allocates pages at a specified alignment that are suitable for an EfiPciIoOperationBusMasterCommonBuffer mapping.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  Pages                 The number of pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @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           Success to allocate aligned pages.
+  @retval EFI_INVALID_PARAMETER Pages or Alignment is not valid.
+  @retval EFI_OUT_OF_RESOURCES  Do not have enough resources to allocate memory.
+  
+
+**/
+EFI_STATUS
+UsbHcAllocateAlignedPages (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN UINTN                  Pages,
+  IN UINTN                  Alignment,
+  OUT VOID                  **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS  *DeviceAddress,
+  OUT VOID                  **Mapping
+  )
+{
+  EFI_STATUS            Status;
+  VOID                  *Memory;
+  UINTN                 AlignedMemory;
+  UINTN                 AlignmentMask;
+  UINTN                 UnalignedPages;
+  UINTN                 RealPages;
+  UINTN                 Bytes;
+
+  //
+  // Alignment must be a power of two or zero.
+  //
+  ASSERT ((Alignment & (Alignment - 1)) == 0);
+  
+  if ((Alignment & (Alignment - 1)) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+ 
+  if (Pages == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Alignment > EFI_PAGE_SIZE) {
+    //
+    // Calculate the total number of pages since alignment is larger than page size.
+    //
+    AlignmentMask  = Alignment - 1;
+    RealPages      = Pages + EFI_SIZE_TO_PAGES (Alignment);
+    //
+    // Make sure that Pages plus EFI_SIZE_TO_PAGES (Alignment) does not overflow.
+    //
+    ASSERT (RealPages > Pages);
+ 
+    Status = PciIo->AllocateBuffer (
+                      PciIo,
+                      AllocateAnyPages,
+                      EfiBootServicesData,
+                      Pages,
+                      &Memory,
+                      0
+                      );    
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    AlignedMemory  = ((UINTN) Memory + AlignmentMask) & ~AlignmentMask;
+    UnalignedPages = EFI_SIZE_TO_PAGES (AlignedMemory - (UINTN) Memory);
+    if (UnalignedPages > 0) {
+      //
+      // Free first unaligned page(s).
+      //
+      Status = PciIo->FreeBuffer (PciIo, UnalignedPages, Memory);
+      ASSERT_EFI_ERROR (Status);
+    }
+    Memory         = (VOID *)(UINTN)(AlignedMemory + EFI_PAGES_TO_SIZE (Pages));
+    UnalignedPages = RealPages - Pages - UnalignedPages;
+    if (UnalignedPages > 0) {
+      //
+      // Free last unaligned page(s).
+      //
+      Status = PciIo->FreeBuffer (PciIo, UnalignedPages, Memory);
+      ASSERT_EFI_ERROR (Status);
+    }
+  } else {
+    //
+    // Do not over-allocate pages in this case.
+    //
+    Status = PciIo->AllocateBuffer (
+                      PciIo,
+                      AllocateAnyPages,
+                      EfiBootServicesData,
+                      Pages,
+                      &Memory,
+                      0
+                      );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    AlignedMemory  = (UINTN) Memory;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (Pages);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    (VOID *) AlignedMemory,
+                    &Bytes,
+                    DeviceAddress,
+                    Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (Pages))) {
+    Status = PciIo->FreeBuffer (PciIo, Pages, (VOID *) AlignedMemory);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  *HostAddress = (VOID *) AlignedMemory;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Frees memory that was allocated with UsbHcAllocateAlignedPages().
+  
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  Pages                 The number of 4 KB pages to free.
+  @param  Mapping               The mapping value returned from Map().
+
+**/
+VOID
+UsbHcFreeAlignedPages (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN VOID                   *HostAddress,
+  IN UINTN                  Pages,
+  VOID                      *Mapping
+  )
+{
+  EFI_STATUS      Status;
+  
+  ASSERT (Pages != 0);
+  
+  Status = PciIo->Unmap (PciIo, Mapping);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = PciIo->FreeBuffer (
+                    PciIo,
+                    Pages,
+                    HostAddress
+                    );     
+  ASSERT_EFI_ERROR (Status);
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.h
new file mode 100644
index 0000000000..1907685ddd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.h
@@ -0,0 +1,213 @@
+/** @file
+
+  This file contains the definination for host controller memory management routines.
+
+Copyright (c) 2013, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_XHCI_MEM_H_
+#define _EFI_XHCI_MEM_H_
+
+#define USB_HC_BIT(a)                  ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+struct _USBHC_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen;
+  UINT8                   *Buf;
+  UINT8                   *BufHost;
+  UINTN                   BufLen;   // Memory size in bytes
+  VOID                    *Mapping;
+  USBHC_MEM_BLOCK         *Next;
+};
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB
+// host controller. XHCI requires the control memory and transfer
+// data to be on the same 4G memory.
+//
+typedef struct _USBHC_MEM_POOL {
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  BOOLEAN                 Check4G;
+  UINT32                  Which4G;
+  USBHC_MEM_BLOCK         *Head;
+} USBHC_MEM_POOL;
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT           64
+
+#define USBHC_MEM_UNIT_MASK      (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_DEFAULT_PAGES  16
+
+#define USBHC_MEM_ROUND(Len)  (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo               The PciIo that can be used to access the host controller.
+
+  @retval EFI_SUCCESS         The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo
+  );
+
+
+/**
+  Release the memory management pool.
+
+  @param   Pool               The USB memory pool to free.
+
+  @retval EFI_SUCCESS       The memory pool is freed.
+  @retval EFI_DEVICE_ERROR  Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  );
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool  The host controller's memory pool.
+  @param  Size  Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  );
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool  The memory pool of the host controller.
+  @param  Mem   The memory to free.
+  @param  Size  The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**
+  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
+UsbHcGetPciAddrForHostAddr (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**
+  Calculate the corresponding host address according to the pci address.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to pci memory.
+  @param  Size           The size of the memory region.
+
+  @return                The host memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetHostAddrForPciAddr (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**  
+  Allocates pages at a specified alignment that are suitable for an EfiPciIoOperationBusMasterCommonBuffer mapping.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  Pages                 The number of pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @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           Success to allocate aligned pages.
+  @retval EFI_INVALID_PARAMETER Pages or Alignment is not valid.
+  @retval EFI_OUT_OF_RESOURCES  Do not have enough resources to allocate memory.
+  
+
+**/
+EFI_STATUS
+UsbHcAllocateAlignedPages (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN UINTN                  Pages,
+  IN UINTN                  Alignment,
+  OUT VOID                  **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS  *DeviceAddress,
+  OUT VOID                  **Mapping
+  );
+  
+/**
+  Frees memory that was allocated with UsbHcAllocateAlignedPages().
+  
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  Pages                 The number of pages to free.
+  @param  Mapping               The mapping value returned from Map().
+
+**/
+VOID
+UsbHcFreeAlignedPages (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN VOID                   *HostAddress,
+  IN UINTN                  Pages,
+  VOID                      *Mapping
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.c
new file mode 100644
index 0000000000..39c28ab7d8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.c
@@ -0,0 +1,2250 @@
+/** @file
+  The XHCI controller driver.
+
+Copyright (c) 2011 - 2015, 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 "Xhci.h"
+
+//
+// Two arrays used to translate the XHCI port state (change)
+// to the UEFI protocol's port state (change).
+//
+USB_PORT_STATE_MAP  mUsbPortStateMap[] = {
+  {XHC_PORTSC_CCS,   USB_PORT_STAT_CONNECTION},
+  {XHC_PORTSC_PED,   USB_PORT_STAT_ENABLE},
+  {XHC_PORTSC_OCA,   USB_PORT_STAT_OVERCURRENT},
+  {XHC_PORTSC_RESET, USB_PORT_STAT_RESET}
+};
+
+USB_PORT_STATE_MAP  mUsbPortChangeMap[] = {
+  {XHC_PORTSC_CSC, USB_PORT_STAT_C_CONNECTION},
+  {XHC_PORTSC_PEC, USB_PORT_STAT_C_ENABLE},
+  {XHC_PORTSC_OCC, USB_PORT_STAT_C_OVERCURRENT},
+  {XHC_PORTSC_PRC, USB_PORT_STAT_C_RESET}
+};
+
+USB_CLEAR_PORT_MAP mUsbClearPortChangeMap[] = {
+  {XHC_PORTSC_CSC, EfiUsbPortConnectChange},
+  {XHC_PORTSC_PEC, EfiUsbPortEnableChange},
+  {XHC_PORTSC_OCC, EfiUsbPortOverCurrentChange},
+  {XHC_PORTSC_PRC, EfiUsbPortResetChange}
+};
+
+USB_PORT_STATE_MAP  mUsbHubPortStateMap[] = {
+  {XHC_HUB_PORTSC_CCS,   USB_PORT_STAT_CONNECTION},
+  {XHC_HUB_PORTSC_PED,   USB_PORT_STAT_ENABLE},
+  {XHC_HUB_PORTSC_OCA,   USB_PORT_STAT_OVERCURRENT},
+  {XHC_HUB_PORTSC_RESET, USB_PORT_STAT_RESET}
+};
+
+USB_PORT_STATE_MAP  mUsbHubPortChangeMap[] = {
+  {XHC_HUB_PORTSC_CSC, USB_PORT_STAT_C_CONNECTION},
+  {XHC_HUB_PORTSC_PEC, USB_PORT_STAT_C_ENABLE},
+  {XHC_HUB_PORTSC_OCC, USB_PORT_STAT_C_OVERCURRENT},
+  {XHC_HUB_PORTSC_PRC, USB_PORT_STAT_C_RESET}
+};
+
+USB_CLEAR_PORT_MAP mUsbHubClearPortChangeMap[] = {
+  {XHC_HUB_PORTSC_CSC, EfiUsbPortConnectChange},
+  {XHC_HUB_PORTSC_PEC, EfiUsbPortEnableChange},
+  {XHC_HUB_PORTSC_OCC, EfiUsbPortOverCurrentChange},
+  {XHC_HUB_PORTSC_PRC, EfiUsbPortResetChange},
+  {XHC_HUB_PORTSC_BHRC, Usb3PortBHPortResetChange}
+};
+
+EFI_DRIVER_BINDING_PROTOCOL  gXhciDriverBinding = {
+  XhcDriverBindingSupported,
+  XhcDriverBindingStart,
+  XhcDriverBindingStop,
+  0x30,
+  NULL,
+  NULL
+};
+
+//
+// Template for Xhci's Usb2 Host Controller Protocol Instance.
+//
+EFI_USB2_HC_PROTOCOL gXhciUsb2HcTemplate = {
+  XhcGetCapability,
+  XhcReset,
+  XhcGetState,
+  XhcSetState,
+  XhcControlTransfer,
+  XhcBulkTransfer,
+  XhcAsyncInterruptTransfer,
+  XhcSyncInterruptTransfer,
+  XhcIsochronousTransfer,
+  XhcAsyncIsochronousTransfer,
+  XhcGetRootHubPortStatus,
+  XhcSetRootHubPortFeature,
+  XhcClearRootHubPortFeature,
+  0x3,
+  0x0
+};
+
+/**
+  Retrieves the capability of root hub ports.
+
+  @param  This                  The EFI_USB2_HC_PROTOCOL instance.
+  @param  MaxSpeed              Max speed supported by the controller.
+  @param  PortNumber            Number of the root hub ports.
+  @param  Is64BitCapable        Whether the controller supports 64-bit memory
+                                addressing.
+
+  @retval EFI_SUCCESS           Host controller capability were retrieved successfully.
+  @retval EFI_INVALID_PARAMETER Either of the three capability pointer is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetCapability (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT UINT8                 *MaxSpeed,
+  OUT UINT8                 *PortNumber,
+  OUT UINT8                 *Is64BitCapable
+  )
+{
+  USB_XHCI_INSTANCE  *Xhc;
+  EFI_TPL            OldTpl;
+
+  if ((MaxSpeed == NULL) || (PortNumber == NULL) || (Is64BitCapable == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc             = XHC_FROM_THIS (This);
+  *MaxSpeed       = EFI_USB_SPEED_SUPER;
+  *PortNumber     = (UINT8) (Xhc->HcSParams1.Data.MaxPorts);
+  *Is64BitCapable = (UINT8) (Xhc->HcCParams.Data.Ac64);
+  DEBUG ((EFI_D_INFO, "XhcGetCapability: %d ports, 64 bit %d\n", *PortNumber, *Is64BitCapable));
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Provides software reset for the USB host controller.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  Attributes            A bit mask of the reset operation to perform.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcReset (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT16                Attributes
+  )
+{
+  USB_XHCI_INSTANCE  *Xhc;
+  EFI_STATUS         Status;
+  EFI_TPL            OldTpl;
+
+  Xhc = XHC_FROM_THIS (This);
+  
+  if (Xhc->DevicePath != NULL) {
+    //
+    // Report Status Code to indicate reset happens
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_IO_BUS_USB | EFI_IOB_PC_RESET),
+      Xhc->DevicePath
+      );
+  }  
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  switch (Attributes) {
+  case EFI_USB_HC_RESET_GLOBAL:
+  //
+  // Flow through, same behavior as Host Controller Reset
+  //
+  case EFI_USB_HC_RESET_HOST_CONTROLLER:
+    if ((Xhc->DebugCapSupOffset != 0xFFFFFFFF) && ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset) & 0xFF) == XHC_CAP_USB_DEBUG) &&
+        ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset + XHC_DC_DCCTRL) & BIT0) != 0)) {
+      Status = EFI_SUCCESS;
+      goto ON_EXIT;
+    }
+    //
+    // Host Controller must be Halt when Reset it
+    //
+    if (!XhcIsHalt (Xhc)) {
+      Status = XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        Status = EFI_DEVICE_ERROR;
+        goto ON_EXIT;
+      }
+    }
+
+    Status = XhcResetHC (Xhc, XHC_RESET_TIMEOUT);
+    ASSERT (!(XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_CNR)));
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+    //
+    // Clean up the asynchronous transfers, currently only
+    // interrupt supports asynchronous operation.
+    //
+    XhciDelAllAsyncIntTransfers (Xhc);
+    XhcFreeSched (Xhc);
+
+    XhcInitSched (Xhc);
+    break;
+
+  case EFI_USB_HC_RESET_GLOBAL_WITH_DEBUG:
+  case EFI_USB_HC_RESET_HOST_WITH_DEBUG:
+    Status = EFI_UNSUPPORTED;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcReset: status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Retrieve the current state of the USB host controller.
+
+  @param  This                   This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Variable to return the current host controller
+                                 state.
+
+  @retval EFI_SUCCESS            Host controller state was returned in State.
+  @retval EFI_INVALID_PARAMETER  State is NULL.
+  @retval EFI_DEVICE_ERROR       An error was encountered while attempting to
+                                 retrieve the host controller's current state.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetState (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT EFI_USB_HC_STATE      *State
+  )
+{
+  USB_XHCI_INSTANCE  *Xhc;
+  EFI_TPL            OldTpl;
+
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = XHC_FROM_THIS (This);
+
+  if (XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT)) {
+    *State = EfiUsbHcStateHalt;
+  } else {
+    *State = EfiUsbHcStateOperational;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcGetState: current state %d\n", *State));
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets the USB host controller to a specific state.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                 The state of the host controller that will be set.
+
+  @retval EFI_SUCCESS           The USB host controller was successfully placed
+                                in the state specified by State.
+  @retval EFI_INVALID_PARAMETER State is invalid.
+  @retval EFI_DEVICE_ERROR      Failed to set the state due to device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetState (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN EFI_USB_HC_STATE      State
+  )
+{
+  USB_XHCI_INSTANCE   *Xhc;
+  EFI_STATUS          Status;
+  EFI_USB_HC_STATE    CurState;
+  EFI_TPL             OldTpl;
+
+  Status = XhcGetState (This, &CurState);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (CurState == State) {
+    return EFI_SUCCESS;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = XHC_FROM_THIS (This);
+
+  switch (State) {
+  case EfiUsbHcStateHalt:
+    Status = XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateOperational:
+    if (XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HSE)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    //
+    // Software must not write a one to this field unless the host controller
+    // is in the Halted state. Doing so will yield undefined results.
+    // refers to Spec[XHCI1.0-2.3.1]
+    //
+    if (!XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    Status = XhcRunHC (Xhc, XHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateSuspend:
+    Status = EFI_UNSUPPORTED;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcSetState: status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            The root hub port to retrieve the state from.
+                                This value is zero-based.
+  @param  PortStatus            Variable to receive the port state.
+
+  @retval EFI_SUCCESS           The status of the USB root hub port specified.
+                                by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetRootHubPortStatus (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  IN  UINT8                 PortNumber,
+  OUT EFI_USB_PORT_STATUS   *PortStatus
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  UINTN                   Index;
+  UINTN                   MapSize;
+  EFI_STATUS              Status;
+  USB_DEV_ROUTE           ParentRouteChart;
+  EFI_TPL                 OldTpl;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc       = XHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = Xhc->HcSParams1.Data.MaxPorts;
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset                       = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  PortStatus->PortStatus       = 0;
+  PortStatus->PortChangeStatus = 0;
+
+  State = XhcReadOpReg (Xhc, Offset);
+
+  //
+  // According to XHCI 1.0 spec, bit 10~13 of the root port status register identifies the speed of the attached device.
+  //
+  switch ((State & XHC_PORTSC_PS) >> 10) {
+  case 2:
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+    break;
+
+  case 3:
+    PortStatus->PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+    break;
+
+  case 4:
+    PortStatus->PortStatus |= USB_PORT_STAT_SUPER_SPEED;
+    break;
+
+  default:
+    break;
+  }
+
+  //
+  // Convert the XHCI port/port change state to UEFI status
+  //
+  MapSize = sizeof (mUsbPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbPortStateMap[Index].HwState)) {
+      PortStatus->PortStatus = (UINT16) (PortStatus->PortStatus | mUsbPortStateMap[Index].UefiState);
+    }
+  }
+  //
+  // Bit5~8 reflects its current link state.
+  //
+  if ((State & XHC_PORTSC_PLS) >> 5 == 3) {
+    PortStatus->PortStatus |= USB_PORT_STAT_SUSPEND;
+  }
+
+  MapSize = sizeof (mUsbPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbPortChangeMap[Index].HwState)) {
+      PortStatus->PortChangeStatus = (UINT16) (PortStatus->PortChangeStatus | mUsbPortChangeMap[Index].UefiState);
+    }
+  }
+
+  MapSize = sizeof (mUsbClearPortChangeMap) / sizeof (USB_CLEAR_PORT_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbClearPortChangeMap[Index].HwState)) {
+      XhcClearRootHubPortFeature (This, PortNumber, (EFI_USB_PORT_FEATURE)mUsbClearPortChangeMap[Index].Selector);
+    }
+  }
+
+  //
+  // Poll the root port status register to enable/disable corresponding device slot if there is a device attached/detached.
+  // For those devices behind hub, we get its attach/detach event by hooking Get_Port_Status request at control transfer for those hub.
+  //
+  ParentRouteChart.Dword = 0;
+  XhcPollPortStatusChange (Xhc, ParentRouteChart, PortNumber, PortStatus);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT8                 PortNumber,
+  IN EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = XHC_FROM_THIS (This);
+  Status = EFI_SUCCESS;
+
+  TotalPort = (Xhc->HcSParams1.Data.MaxPorts);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  State  = XhcReadOpReg (Xhc, Offset);
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bit
+  //
+  State &= ~ (BIT1 | BIT17 | BIT18 | BIT19 | BIT20 | BIT21 | BIT22 | BIT23);
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Ports may only be enabled by the xHC. Software cannot enable a port by writing a '1' to this flag.
+    // A port may be disabled by software writing a '1' to this flag.
+    //
+    Status = EFI_SUCCESS;
+    break;
+
+  case EfiUsbPortSuspend:
+    State |= XHC_PORTSC_LWS;
+    XhcWriteOpReg (Xhc, Offset, State);
+    State &= ~XHC_PORTSC_PLS;
+    State |= (3 << 5) ;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    DEBUG ((EFI_D_INFO, "XhcUsbPortReset!\n"));
+    //
+    // Make sure Host Controller not halt before reset it
+    //
+    if (XhcIsHalt (Xhc)) {
+      Status = XhcRunHC (Xhc, XHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_INFO, "XhcSetRootHubPortFeature :failed to start HC - %r\n", Status));
+        break;
+      }
+    }
+
+    //
+    // 4.3.1 Resetting a Root Hub Port
+    // 1) Write the PORTSC register with the Port Reset (PR) bit set to '1'.
+    //
+    State |= XHC_PORTSC_RESET;
+    XhcWriteOpReg (Xhc, Offset, State);
+    XhcWaitOpRegBit(Xhc, Offset, XHC_PORTSC_PRC, TRUE, XHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // Not supported, ignore the operation
+    //
+    Status = EFI_SUCCESS;
+    break;
+
+  case EfiUsbPortOwner:
+    //
+    // XHCI root hub port don't has the owner bit, ignore the operation
+    //
+    Status = EFI_SUCCESS;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcSetRootHubPortFeature: status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Specifies the root hub port whose feature is
+                                requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was cleared
+                                for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcClearRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT8                 PortNumber,
+  IN EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc       = XHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Xhc->HcSParams1.Data.MaxPorts);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset = XHC_PORTSC_OFFSET + (0x10 * PortNumber);
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bit
+  //
+  State  = XhcReadOpReg (Xhc, Offset);
+  State &= ~ (BIT1 | BIT17 | BIT18 | BIT19 | BIT20 | BIT21 | BIT22 | BIT23);
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Ports may only be enabled by the xHC. Software cannot enable a port by writing a '1' to this flag.
+    // A port may be disabled by software writing a '1' to this flag.
+    //
+    State |= XHC_PORTSC_PED;
+    State &= ~XHC_PORTSC_RESET;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    State |= XHC_PORTSC_LWS;
+    XhcWriteOpReg (Xhc, Offset, State);
+    State &= ~XHC_PORTSC_PLS;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // PORTSC_RESET BIT(4) bit is RW1S attribute, which means Write-1-to-set status:
+    // Register bits indicate status when read, a clear bit may be set by
+    // writing a '1'. Writing a '0' to RW1S bits has no effect.
+    //
+    break;
+
+  case EfiUsbPortOwner:
+    //
+    // XHCI root hub port don't has the owner bit, ignore the operation
+    //
+    break;
+
+  case EfiUsbPortConnectChange:
+    //
+    // Clear connect status change
+    //
+    State |= XHC_PORTSC_CSC;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortEnableChange:
+    //
+    // Clear enable status change
+    //
+    State |= XHC_PORTSC_PEC;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortOverCurrentChange:
+    //
+    // Clear PortOverCurrent change
+    //
+    State |= XHC_PORTSC_OCC;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortResetChange:
+    //
+    // Clear Port Reset change
+    //
+    State |= XHC_PORTSC_PRC;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+  case EfiUsbPortSuspendChange:
+    //
+    // Not supported or not related operation
+    //
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcClearRootHubPortFeature: status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits control transfer to a target USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         The target device address.
+  @param  DeviceSpeed           Target device speed.
+  @param  MaximumPacketLength   Maximum packet size the default control transfer
+                                endpoint is capable of sending or receiving.
+  @param  Request               USB device request to send.
+  @param  TransferDirection     Specifies the data direction for the data stage
+  @param  Data                  Data buffer to be transmitted or received from USB
+                                device.
+  @param  DataLength            The size (in bytes) of the data buffer.
+  @param  Timeout               Indicates the maximum timeout, in millisecond.
+  @param  Translator            Transaction translator to be used by this device.
+  @param  TransferResult        Return the result of this control transfer.
+
+  @retval EFI_SUCCESS           Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcControlTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST              *Request,
+  IN     EFI_USB_DATA_DIRECTION              TransferDirection,
+  IN OUT VOID                                *Data,
+  IN OUT UINTN                               *DataLength,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  URB                     *Urb;
+  UINT8                   Endpoint;
+  UINT8                   Index;
+  UINT8                   DescriptorType;
+  UINT8                   SlotId;
+  UINT8                   TTT;
+  UINT8                   MTT;
+  UINT32                  MaxPacket0;
+  EFI_USB_HUB_DESCRIPTOR  *HubDesc;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  EFI_STATUS              RecoveryStatus;
+  UINTN                   MapSize;
+  EFI_USB_PORT_STATUS     PortStatus;
+  UINT32                  State;
+  EFI_USB_DEVICE_REQUEST  ClearPortRequest;
+  UINTN                   Len;
+
+  //
+  // Validate parameters
+  //
+  if ((Request == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbDataIn) &&
+      (TransferDirection != EfiUsbDataOut) &&
+      (TransferDirection != EfiUsbNoData)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection == EfiUsbNoData) &&
+      ((Data != NULL) || (*DataLength != 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) &&
+     ((Data == NULL) || (*DataLength == 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8)  && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64) &&
+      (MaximumPacketLength != 512)
+      ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_SUPER) && (MaximumPacketLength != 512)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc             = XHC_FROM_THIS (This);
+
+  Status          = EFI_DEVICE_ERROR;
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Len             = 0;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcControlTransfer: HC halted at entrance\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Hook the Set_Address request from UsbBus.
+  // According to XHCI 1.0 spec, the Set_Address request is replaced by XHCI's Address_Device cmd.
+  //
+  if ((Request->Request     == USB_REQ_SET_ADDRESS) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE))) {
+    //
+    // Reset the BusDevAddr field of all disabled entries in UsbDevContext array firstly.
+    // This way is used to clean the history to avoid using wrong device address by XhcAsyncInterruptTransfer().
+    //
+    for (Index = 0; Index < 255; Index++) {
+      if (!Xhc->UsbDevContext[Index + 1].Enabled &&
+          (Xhc->UsbDevContext[Index + 1].SlotId == 0) &&
+          (Xhc->UsbDevContext[Index + 1].BusDevAddr == (UINT8)Request->Value)) {
+        Xhc->UsbDevContext[Index + 1].BusDevAddr = 0;
+      }
+    }
+
+    if (Xhc->UsbDevContext[SlotId].XhciDevAddr == 0) {
+      Status = EFI_DEVICE_ERROR;
+      goto ON_EXIT;
+    }
+    //
+    // The actual device address has been assigned by XHCI during initializing the device slot.
+    // So we just need establish the mapping relationship between the device address requested from UsbBus
+    // and the actual device address assigned by XHCI. The the following invocations through EFI_USB2_HC_PROTOCOL interface
+    // can find out the actual device address by it.
+    //
+    Xhc->UsbDevContext[SlotId].BusDevAddr = (UINT8)Request->Value;
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  // Note that we encode the direction in address although default control
+  // endpoint is bidirectional. XhcCreateUrb expects this
+  // combination of Ep addr and its direction.
+  //
+  Endpoint = (UINT8) (0 | ((TransferDirection == EfiUsbDataIn) ? 0x80 : 0));
+  Urb = XhcCreateUrb (
+          Xhc,
+          DeviceAddress,
+          Endpoint,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_CTRL_TRANSFER,
+          Request,
+          Data,
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcControlTransfer: failed to create URB"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcExecTransfer (Xhc, FALSE, Urb, Timeout);
+
+  //
+  // Get the status from URB. The result is updated in XhcCheckUrbResult
+  // which is called by XhcExecTransfer
+  //
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcControlTransfer: XhcDequeueTrbFromEndpoint failed\n"));
+    }
+    goto FREE_URB;
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcControlTransfer: XhcRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+      goto FREE_URB;
+    } else {
+      goto FREE_URB;
+    }
+  }
+
+  Xhc->PciIo->Flush (Xhc->PciIo);
+  
+  if (Urb->DataMap != NULL) {
+    Status = Xhc->PciIo->Unmap (Xhc->PciIo, Urb->DataMap);
+    ASSERT_EFI_ERROR (Status);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto FREE_URB;
+    }  
+  }
+
+  //
+  // Hook Get_Descriptor request from UsbBus as we need evaluate context and configure endpoint.
+  // Hook Get_Status request form UsbBus as we need trace device attach/detach event happened at hub.
+  // Hook Set_Config request from UsbBus as we need configure device endpoint.
+  //
+  if ((Request->Request     == USB_REQ_GET_DESCRIPTOR) &&
+      ((Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE)) || 
+      ((Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_CLASS, USB_TARGET_DEVICE))))) {
+    DescriptorType = (UINT8)(Request->Value >> 8);
+    if ((DescriptorType == USB_DESC_TYPE_DEVICE) && ((*DataLength == sizeof (EFI_USB_DEVICE_DESCRIPTOR)) || ((DeviceSpeed == EFI_USB_SPEED_FULL) && (*DataLength == 8)))) {
+        ASSERT (Data != NULL);
+        //
+        // Store a copy of device scriptor as hub device need this info to configure endpoint.
+        //
+        CopyMem (&Xhc->UsbDevContext[SlotId].DevDesc, Data, *DataLength);
+        if (Xhc->UsbDevContext[SlotId].DevDesc.BcdUSB == 0x0300) {
+          //
+          // If it's a usb3.0 device, then its max packet size is a 2^n.
+          //
+          MaxPacket0 = 1 << Xhc->UsbDevContext[SlotId].DevDesc.MaxPacketSize0;
+        } else {
+          MaxPacket0 = Xhc->UsbDevContext[SlotId].DevDesc.MaxPacketSize0;
+        }
+        Xhc->UsbDevContext[SlotId].ConfDesc = AllocateZeroPool (Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations * sizeof (EFI_USB_CONFIG_DESCRIPTOR *));
+        if (Xhc->HcCParams.Data.Csz == 0) {
+          Status = XhcEvaluateContext (Xhc, SlotId, MaxPacket0);
+        } else {
+          Status = XhcEvaluateContext64 (Xhc, SlotId, MaxPacket0);
+        }
+    } else if (DescriptorType == USB_DESC_TYPE_CONFIG) {
+      ASSERT (Data != NULL);
+      if (*DataLength == ((UINT16 *)Data)[1]) {
+        //
+        // Get configuration value from request, Store the configuration descriptor for Configure_Endpoint cmd.
+        //
+        Index = (UINT8)Request->Value;
+        ASSERT (Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations);
+        Xhc->UsbDevContext[SlotId].ConfDesc[Index] = AllocateZeroPool(*DataLength);
+        CopyMem (Xhc->UsbDevContext[SlotId].ConfDesc[Index], Data, *DataLength);
+        //
+        // Default to use AlternateSetting 0 for all interfaces.
+        //
+        Xhc->UsbDevContext[SlotId].ActiveAlternateSetting = AllocateZeroPool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]->NumInterfaces * sizeof (UINT8));
+      }
+    } else if (((DescriptorType == USB_DESC_TYPE_HUB) ||
+               (DescriptorType == USB_DESC_TYPE_HUB_SUPER_SPEED)) && (*DataLength > 2)) {
+      ASSERT (Data != NULL);
+      HubDesc = (EFI_USB_HUB_DESCRIPTOR *)Data;
+      ASSERT (HubDesc->NumPorts <= 15);
+      //
+      // The bit 5,6 of HubCharacter field of Hub Descriptor is TTT.
+      //
+      TTT = (UINT8)((HubDesc->HubCharacter & (BIT5 | BIT6)) >> 5);
+      if (Xhc->UsbDevContext[SlotId].DevDesc.DeviceProtocol == 2) {
+        //
+        // Don't support multi-TT feature for super speed hub now.
+        //
+        MTT = 0;
+        DEBUG ((EFI_D_ERROR, "XHCI: Don't support multi-TT feature for Hub now. (force to disable MTT)\n"));
+      } else {
+        MTT = 0;
+      }
+
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcConfigHubContext (Xhc, SlotId, HubDesc->NumPorts, TTT, MTT);
+      } else {
+        Status = XhcConfigHubContext64 (Xhc, SlotId, HubDesc->NumPorts, TTT, MTT);
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_SET_CONFIG) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE))) {
+    //
+    // Hook Set_Config request from UsbBus as we need configure device endpoint.
+    //
+    for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+      if (Xhc->UsbDevContext[SlotId].ConfDesc[Index]->ConfigurationValue == (UINT8)Request->Value) {
+        if (Xhc->HcCParams.Data.Csz == 0) {
+          Status = XhcSetConfigCmd (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+        } else {
+          Status = XhcSetConfigCmd64 (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+        }
+        break;
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_SET_INTERFACE) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_INTERFACE))) {
+    //
+    // Hook Set_Interface request from UsbBus as we need configure interface setting.
+    // Request->Value indicates AlterlateSetting to set
+    // Request->Index indicates Interface to set
+    //
+    if (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[(UINT8) Request->Index] != (UINT8) Request->Value) {
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcSetInterface (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Xhc->UsbDevContext[SlotId].ActiveConfiguration - 1], Request);
+      } else {
+        Status = XhcSetInterface64 (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Xhc->UsbDevContext[SlotId].ActiveConfiguration - 1], Request);
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_GET_STATUS) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_CLASS, USB_TARGET_OTHER))) {
+    ASSERT (Data != NULL);
+    //
+    // Hook Get_Status request from UsbBus to keep track of the port status change.
+    //
+    State                       = *(UINT32 *)Data;
+    PortStatus.PortStatus       = 0;
+    PortStatus.PortChangeStatus = 0;
+
+    if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+      //
+      // For super speed hub, its bit10~12 presents the attached device speed.
+      //
+      if ((State & XHC_PORTSC_PS) >> 10 == 0) {
+        PortStatus.PortStatus |= USB_PORT_STAT_SUPER_SPEED;
+      }
+    } else {
+      //
+      // For high or full/low speed hub, its bit9~10 presents the attached device speed.
+      //
+      if (XHC_BIT_IS_SET (State, BIT9)) {
+        PortStatus.PortStatus |= USB_PORT_STAT_LOW_SPEED;
+      } else if (XHC_BIT_IS_SET (State, BIT10)) {
+        PortStatus.PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+      }
+    }
+
+    //
+    // Convert the XHCI port/port change state to UEFI status
+    //
+    MapSize = sizeof (mUsbHubPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubPortStateMap[Index].HwState)) {
+        PortStatus.PortStatus = (UINT16) (PortStatus.PortStatus | mUsbHubPortStateMap[Index].UefiState);
+      }
+    }
+
+    MapSize = sizeof (mUsbHubPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubPortChangeMap[Index].HwState)) {
+        PortStatus.PortChangeStatus = (UINT16) (PortStatus.PortChangeStatus | mUsbHubPortChangeMap[Index].UefiState);
+      }
+    }
+
+    MapSize = sizeof (mUsbHubClearPortChangeMap) / sizeof (USB_CLEAR_PORT_MAP);
+
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubClearPortChangeMap[Index].HwState)) {
+        ZeroMem (&ClearPortRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+        ClearPortRequest.RequestType  = USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_CLASS, USB_TARGET_OTHER);
+        ClearPortRequest.Request      = (UINT8) USB_REQ_CLEAR_FEATURE;
+        ClearPortRequest.Value        = mUsbHubClearPortChangeMap[Index].Selector;
+        ClearPortRequest.Index        = Request->Index;
+        ClearPortRequest.Length       = 0;
+
+        XhcControlTransfer (
+          This, 
+          DeviceAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          &ClearPortRequest,
+          EfiUsbNoData,
+          NULL,
+          &Len,
+          Timeout,
+          Translator,
+          TransferResult
+          );
+      }
+    }
+
+    XhcPollPortStatusChange (Xhc, Xhc->UsbDevContext[SlotId].RouteString, (UINT8)Request->Index, &PortStatus);
+
+    *(UINT32 *)Data = *(UINT32*)&PortStatus;
+  }
+
+FREE_URB:
+  FreePool (Urb);
+
+ON_EXIT:
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcControlTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support bulk
+                                transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of
+                                sending or receiving.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data to transmit
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to
+                                use of the subsequent bulk transfer.
+  @param  Timeout               Indicates the maximum time, in millisecond, which
+                                the transfer is allowed to complete.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcBulkTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN OUT UINTN                               *DataLength,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  URB                     *Urb;
+  UINT8                   SlotId;
+  EFI_STATUS              Status;
+  EFI_STATUS              RecoveryStatus;
+  EFI_TPL                 OldTpl;
+
+  //
+  // Validate the parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (Data[0] == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 0) && (*DataToggle != 1)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((EFI_USB_SPEED_HIGH == DeviceSpeed) && (MaximumPacketLength > 512)) ||
+      ((EFI_USB_SPEED_SUPER == DeviceSpeed) && (MaximumPacketLength > 1024))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc             = XHC_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: HC is halted\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  Urb = XhcCreateUrb (
+          Xhc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_BULK_TRANSFER,
+          NULL,
+          Data[0],
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcExecTransfer (Xhc, FALSE, Urb, Timeout);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcBulkTransfer: XhcDequeueTrbFromEndpoint failed\n"));
+    }
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: XhcRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+  Xhc->PciIo->Flush (Xhc->PciIo);
+  XhcFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Submits an asynchronous interrupt transfer to an
+  interrupt endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+  @param  IsNewTransfer         If TRUE, to submit an new asynchronous interrupt
+                                transfer If FALSE, to remove the specified
+                                asynchronous interrupt.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  PollingInterval       The he interval, in milliseconds, that the transfer
+                                is polled.
+  @param  DataLength            The length of data to receive at the rate specified
+                                by  PollingInterval.
+  @param  Translator            Transaction translator to use.
+  @param  CallBackFunction      Function to call at the rate specified by
+                                PollingInterval.
+  @param  Context               Context to CallBackFunction.
+
+  @retval EFI_SUCCESS           The request has been successfully submitted or canceled.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request failed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcAsyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     BOOLEAN                             IsNewTransfer,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               PollingInterval,
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     CallBackFunction,
+  IN     VOID                                *Context OPTIONAL
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  URB                     *Urb;
+  EFI_STATUS              Status;
+  UINT8                   SlotId;
+  UINT8                   Index;
+  UINT8                   *Data;
+  EFI_TPL                 OldTpl;
+
+  //
+  // Validate parameters
+  //
+  if (!XHCI_IS_DATAIN (EndPointAddress)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsNewTransfer) {
+    if (DataLength == 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((*DataToggle != 1) && (*DataToggle != 0)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((PollingInterval > 255) || (PollingInterval < 1)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = XHC_FROM_THIS (This);
+
+  //
+  // Delete Async interrupt transfer request.
+  //
+  if (!IsNewTransfer) {
+    //
+    // The delete request may happen after device is detached.
+    //
+    for (Index = 0; Index < 255; Index++) {
+      if (Xhc->UsbDevContext[Index + 1].BusDevAddr == DeviceAddress) {
+        break;
+      }
+    }
+
+    if (Index == 255) {
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;
+    }
+
+    Status = XhciDelAsyncIntTransfer (Xhc, DeviceAddress, EndPointAddress);
+    DEBUG ((EFI_D_INFO, "XhcAsyncInterruptTransfer: remove old transfer for addr %d, Status = %r\n", DeviceAddress, Status));
+    goto ON_EXIT;
+  }
+
+  Status = EFI_SUCCESS;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcAsyncInterruptTransfer: HC is halt\n"));
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  Data = AllocateZeroPool (DataLength);
+
+  if (Data == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcAsyncInterruptTransfer: failed to allocate buffer\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Urb = XhcCreateUrb (
+          Xhc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_INT_TRANSFER_ASYNC,
+          NULL,
+          Data,
+          DataLength,
+          CallBackFunction,
+          Context
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcAsyncInterruptTransfer: failed to create URB\n"));
+    FreePool (Data);
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  InsertHeadList (&Xhc->AsyncIntTransfers, &Urb->UrbList);
+  //
+  // Ring the doorbell
+  //
+  Status = RingIntTransferDoorBell (Xhc, Urb);
+
+ON_EXIT:
+  Xhc->PciIo->Flush (Xhc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits synchronous interrupt transfer to an interrupt endpoint
+  of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+                                of sending or receiving.
+  @param  Data                  Buffer of data that will be transmitted to  USB
+                                device or received from USB device.
+  @param  DataLength            On input, the size, in bytes, of the data buffer; On
+                                output, the number of bytes transferred.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  Timeout               Maximum time, in second, to complete.
+  @param  Translator            Transaction translator to use.
+  @param  TransferResult        Variable to receive the transfer result.
+
+  @return EFI_SUCCESS           The transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER Some parameters are invalid.
+  @return EFI_TIMEOUT           The transfer failed due to timeout.
+  @return EFI_DEVICE_ERROR      The failed due to host controller or device error
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN OUT VOID                                *Data,
+  IN OUT UINTN                               *DataLength,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  URB                     *Urb;
+  UINT8                   SlotId;
+  EFI_STATUS              Status;
+  EFI_STATUS              RecoveryStatus;
+  EFI_TPL                 OldTpl;
+
+  //
+  // Validates parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8))  ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((DeviceSpeed == EFI_USB_SPEED_HIGH) && (MaximumPacketLength > 3072))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc     = XHC_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "EhcSyncInterruptTransfer: HC is halt\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  Urb = XhcCreateUrb (
+          Xhc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_INT_TRANSFER_SYNC,
+          NULL,
+          Data,
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcSyncInterruptTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcExecTransfer (Xhc, FALSE, Urb, Timeout);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcSyncInterruptTransfer: XhcDequeueTrbFromEndpoint failed\n"));
+    }
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcSyncInterruptTransfer: XhcRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+  Xhc->PciIo->Flush (Xhc->PciIo);
+  XhcFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSyncInterruptTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  TransferResult       Variable to receive the transfer result.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer is unsupported.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Submits Async isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  IsochronousCallBack  Function to be called when the transfer complete.
+  @param  Context              Context passed to the call back function as
+                               parameter.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcAsyncIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     IsochronousCallBack,
+  IN     VOID                                *Context
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Entry point for EFI drivers.
+
+  @param  ImageHandle       EFI_HANDLE.
+  @param  SystemTable       EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS       Success.
+  @retval Others            Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gXhciDriverBinding,
+           ImageHandle,
+           &gXhciComponentName,
+           &gXhciComponentName2
+           );
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has Usb2HcProtocol installed will
+  be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  EFI_STATUS              Status;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  USB_CLASSC              UsbClassCReg;
+
+  //
+  // Test whether there is PCI IO Protocol attached on the controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (USB_CLASSC) / sizeof (UINT8),
+                        &UsbClassCReg
+                        );
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Test whether the controller belongs to Xhci type
+  //
+  if ((UsbClassCReg.BaseCode != PCI_CLASS_SERIAL) ||
+      (UsbClassCReg.SubClassCode != PCI_CLASS_SERIAL_USB) ||
+      (UsbClassCReg.ProgInterface != PCI_IF_XHCI)) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+/**
+  Create and initialize a USB_XHCI_INSTANCE structure.
+
+  @param  PciIo                  The PciIo on this device.
+  @param  DevicePath             The device path of host controller.
+  @param  OriginalPciAttributes  Original PCI attributes.
+
+  @return The allocated and initialized USB_XHCI_INSTANCE structure if created,
+          otherwise NULL.
+
+**/
+USB_XHCI_INSTANCE*
+XhcCreateUsbHc (
+  IN EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN UINT64                    OriginalPciAttributes
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  EFI_STATUS              Status;
+  UINT32                  PageSize;
+  UINT16                  ExtCapReg;
+
+  Xhc = AllocateZeroPool (sizeof (USB_XHCI_INSTANCE));
+
+  if (Xhc == NULL) {
+    return NULL;
+  }
+
+  //
+  // Initialize private data structure
+  //
+  Xhc->Signature             = XHCI_INSTANCE_SIG;
+  Xhc->PciIo                 = PciIo;
+  Xhc->DevicePath            = DevicePath;
+  Xhc->OriginalPciAttributes = OriginalPciAttributes;
+  CopyMem (&Xhc->Usb2Hc, &gXhciUsb2HcTemplate, sizeof (EFI_USB2_HC_PROTOCOL));
+
+  InitializeListHead (&Xhc->AsyncIntTransfers);
+
+  //
+  // Be caution that the Offset passed to XhcReadCapReg() should be Dword align
+  //
+  Xhc->CapLength        = XhcReadCapReg8 (Xhc, XHC_CAPLENGTH_OFFSET);
+  Xhc->HcSParams1.Dword = XhcReadCapReg (Xhc, XHC_HCSPARAMS1_OFFSET);
+  Xhc->HcSParams2.Dword = XhcReadCapReg (Xhc, XHC_HCSPARAMS2_OFFSET);
+  Xhc->HcCParams.Dword  = XhcReadCapReg (Xhc, XHC_HCCPARAMS_OFFSET);
+  Xhc->DBOff            = XhcReadCapReg (Xhc, XHC_DBOFF_OFFSET);
+  Xhc->RTSOff           = XhcReadCapReg (Xhc, XHC_RTSOFF_OFFSET);
+
+  //
+  // This PageSize field defines the page size supported by the xHC implementation.
+  // This xHC supports a page size of 2^(n+12) if bit n is Set. For example,
+  // if bit 0 is Set, the xHC supports 4k byte page sizes.
+  //
+  PageSize      = XhcReadOpReg(Xhc, XHC_PAGESIZE_OFFSET) & XHC_PAGESIZE_MASK;
+  Xhc->PageSize = 1 << (HighBitSet32(PageSize) + 12);
+
+  ExtCapReg            = (UINT16) (Xhc->HcCParams.Data.ExtCapReg);
+  Xhc->ExtCapRegBase   = ExtCapReg << 2;
+  Xhc->UsbLegSupOffset = XhcGetCapabilityAddr (Xhc, XHC_CAP_USB_LEGACY);
+  Xhc->DebugCapSupOffset = XhcGetCapabilityAddr (Xhc, XHC_CAP_USB_DEBUG);
+
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: Capability length 0x%x\n", Xhc->CapLength));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: HcSParams1 0x%x\n", Xhc->HcSParams1));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: HcSParams2 0x%x\n", Xhc->HcSParams2));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: HcCParams 0x%x\n", Xhc->HcCParams));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: DBOff 0x%x\n", Xhc->DBOff));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: RTSOff 0x%x\n", Xhc->RTSOff));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: UsbLegSupOffset 0x%x\n", Xhc->UsbLegSupOffset));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: DebugCapSupOffset 0x%x\n", Xhc->DebugCapSupOffset));
+
+  //
+  // Create AsyncRequest Polling Timer
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  XhcMonitorAsyncRequests,
+                  Xhc,
+                  &Xhc->PollTimer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return Xhc;
+
+ON_ERROR:
+  FreePool (Xhc);
+  return NULL;
+}
+
+/**
+  One notified function to stop the Host Controller when gBS->ExitBootServices() called.
+
+  @param  Event                   Pointer to this event
+  @param  Context                 Event handler private data
+
+**/
+VOID
+EFIAPI
+XhcExitBootService (
+  EFI_EVENT  Event,
+  VOID       *Context
+  )
+
+{
+  USB_XHCI_INSTANCE    *Xhc;
+  EFI_PCI_IO_PROTOCOL  *PciIo;
+
+  Xhc = (USB_XHCI_INSTANCE*) Context;
+  PciIo = Xhc->PciIo;
+
+  //
+  // Stop AsyncRequest Polling timer then stop the XHCI driver
+  // and uninstall the XHCI protocl.
+  //
+  gBS->SetTimer (Xhc->PollTimer, TimerCancel, 0);
+  XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+
+  if (Xhc->PollTimer != NULL) {
+    gBS->CloseEvent (Xhc->PollTimer);
+  }
+
+  XhcClearBiosOwnership (Xhc);
+
+  //
+  // Restore original PCI attributes
+  //
+  PciIo->Attributes (
+                  PciIo,
+                  EfiPciIoAttributeOperationSet,
+                  Xhc->OriginalPciAttributes,
+                  NULL
+                  );
+}
+
+/**
+  Starting the Usb XHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          supports this device.
+  @return EFI_UNSUPPORTED      do not support this device.
+  @return EFI_DEVICE_ERROR     cannot be started due to device Error.
+  @return EFI_OUT_OF_RESOURCES cannot allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  EFI_STATUS              Status;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  UINT64                  Supports;
+  UINT64                  OriginalPciAttributes;
+  BOOLEAN                 PciAttributesSaved;
+  USB_XHCI_INSTANCE       *Xhc;
+  EFI_DEVICE_PATH_PROTOCOL  *HcDevicePath;
+
+  //
+  // Open the PciIo Protocol, then enable the USB host controller
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open Device Path Protocol for on USB host controller
+  //
+  HcDevicePath = NULL;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &HcDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  PciAttributesSaved = FALSE;
+  //
+  // Save original PCI attributes
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &OriginalPciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto CLOSE_PCIIO;
+  }
+  PciAttributesSaved = TRUE;
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status = PciIo->Attributes (
+                      PciIo,
+                      EfiPciIoAttributeOperationEnable,
+                      Supports,
+                      NULL
+                      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDriverBindingStart: failed to enable controller\n"));
+    goto CLOSE_PCIIO;
+  }
+
+  //
+  // Create then install USB2_HC_PROTOCOL
+  //
+  Xhc = XhcCreateUsbHc (PciIo, HcDevicePath, OriginalPciAttributes);
+
+  if (Xhc == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcDriverBindingStart: failed to create USB2_HC\n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  XhcSetBiosOwnership (Xhc);
+
+  XhcResetHC (Xhc, XHC_RESET_TIMEOUT);
+  ASSERT (XhcIsHalt (Xhc));
+
+  //
+  // After Chip Hardware Reset wait until the Controller Not Ready (CNR) flag
+  // in the USBSTS is '0' before writing any xHC Operational or Runtime registers.
+  //
+  ASSERT (!(XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_CNR)));
+
+  //
+  // Initialize the schedule
+  //
+  XhcInitSched (Xhc);
+
+  //
+  // Start the Host Controller
+  //
+  XhcRunHC(Xhc, XHC_GENERIC_TIMEOUT);
+
+  //
+  // Start the asynchronous interrupt monitor
+  //
+  Status = gBS->SetTimer (Xhc->PollTimer, TimerPeriodic, XHC_ASYNC_TIMER_INTERVAL);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDriverBindingStart: failed to start async interrupt monitor\n"));
+    XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+    goto FREE_POOL;
+  }
+
+  //
+  // Create event to stop the HC when exit boot service.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  XhcExitBootService,
+                  Xhc,
+                  &gEfiEventExitBootServicesGuid,
+                  &Xhc->ExitBootServiceEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto FREE_POOL;
+  }
+
+  //
+  // Install the component name protocol, don't fail the start
+  // because of something for display.
+  //
+  AddUnicodeString2 (
+    "eng",
+    gXhciComponentName.SupportedLanguages,
+    &Xhc->ControllerNameTable,
+    L"eXtensible Host Controller (USB 3.0)",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gXhciComponentName2.SupportedLanguages,
+    &Xhc->ControllerNameTable,
+    L"eXtensible Host Controller (USB 3.0)",
+    FALSE
+    );
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &Xhc->Usb2Hc
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDriverBindingStart: failed to install USB2_HC Protocol\n"));
+    goto FREE_POOL;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcDriverBindingStart: XHCI started for controller @ %x\n", Controller));
+  return EFI_SUCCESS;
+
+FREE_POOL:
+  gBS->CloseEvent (Xhc->PollTimer);
+  XhcFreeSched (Xhc);
+  FreePool (Xhc);
+
+CLOSE_PCIIO:
+  if (PciAttributesSaved) {
+    //
+    // Restore original PCI attributes
+    //
+    PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSet,
+                    OriginalPciAttributes,
+                    NULL
+                    );
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS          Success.
+  @return EFI_DEVICE_ERROR     Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  )
+{
+  EFI_STATUS            Status;
+  EFI_USB2_HC_PROTOCOL  *Usb2Hc;
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  USB_XHCI_INSTANCE     *Xhc;
+  UINT8                 Index;
+
+  //
+  // Test whether the Controller handler passed in is a valid
+  // Usb controller handle that should be supported, if not,
+  // return the error status directly
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  Usb2Hc
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Xhc   = XHC_FROM_THIS (Usb2Hc);
+  PciIo = Xhc->PciIo;
+
+  //
+  // Stop AsyncRequest Polling timer then stop the XHCI driver
+  // and uninstall the XHCI protocl.
+  //
+  gBS->SetTimer (Xhc->PollTimer, TimerCancel, 0);
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0)) {
+      continue;
+    }
+    if (Xhc->HcCParams.Data.Csz == 0) {
+      XhcDisableSlotCmd (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+    } else {
+      XhcDisableSlotCmd64 (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+    }
+  }
+
+  if (Xhc->PollTimer != NULL) {
+    gBS->CloseEvent (Xhc->PollTimer);
+  }
+
+  if (Xhc->ExitBootServiceEvent != NULL) {
+    gBS->CloseEvent (Xhc->ExitBootServiceEvent);
+  }
+
+  XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+  XhcClearBiosOwnership (Xhc);
+  XhciDelAllAsyncIntTransfers (Xhc);
+  XhcFreeSched (Xhc);
+
+  if (Xhc->ControllerNameTable) {
+    FreeUnicodeStringTable (Xhc->ControllerNameTable);
+  }
+
+  //
+  // Restore original PCI attributes
+  //
+  PciIo->Attributes (
+           PciIo,
+           EfiPciIoAttributeOperationSet,
+           Xhc->OriginalPciAttributes,
+           NULL
+           );
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  FreePool (Xhc);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.h
new file mode 100644
index 0000000000..7999151b3f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.h
@@ -0,0 +1,727 @@
+/** @file
+
+  Provides some data structure definitions used by the XHCI host controller driver.
+
+Copyright (c) 2011 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_XHCI_H_
+#define _EFI_XHCI_H_
+
+#include <Uefi.h>
+
+#include <Protocol/Usb2HostController.h>
+#include <Protocol/PciIo.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DebugLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Pci.h>
+
+typedef struct _USB_XHCI_INSTANCE    USB_XHCI_INSTANCE;
+typedef struct _USB_DEV_CONTEXT      USB_DEV_CONTEXT;
+
+#include "XhciReg.h"
+#include "XhciSched.h"
+#include "ComponentName.h"
+#include "UsbHcMem.h"
+
+//
+// The unit is microsecond, setting it as 1us.
+//
+#define XHC_1_MICROSECOND            (1)
+//
+// The unit is microsecond, setting it as 1ms.
+//
+#define XHC_1_MILLISECOND            (1000)
+//
+// XHC generic timeout experience values.
+// The unit is millisecond, setting it as 10s.
+//
+#define XHC_GENERIC_TIMEOUT          (10 * 1000)
+//
+// XHC reset timeout experience values.
+// The unit is millisecond, setting it as 1s.
+//
+#define XHC_RESET_TIMEOUT            (1000)
+//
+// XHC async transfer timer interval, set by experience.
+// The unit is 100us, takes 1ms as interval.
+//
+#define XHC_ASYNC_TIMER_INTERVAL     EFI_TIMER_PERIOD_MILLISECONDS(1)
+
+//
+// XHC raises TPL to TPL_NOTIFY to serialize all its operations
+// to protect shared data structures.
+//
+#define XHC_TPL                      TPL_NOTIFY
+
+#define CMD_RING_TRB_NUMBER          0x100
+#define TR_RING_TRB_NUMBER           0x100
+#define ERST_NUMBER                  0x01
+#define EVENT_RING_TRB_NUMBER        0x200
+
+#define CMD_INTER                    0
+#define CTRL_INTER                   1
+#define BULK_INTER                   2
+#define INT_INTER                    3
+#define INT_INTER_ASYNC              4
+
+//
+// Iterate through the doule linked list. This is delete-safe.
+// Don't touch NextEntry
+//
+#define EFI_LIST_FOR_EACH_SAFE(Entry, NextEntry, ListHead) \
+  for (Entry = (ListHead)->ForwardLink, NextEntry = Entry->ForwardLink;\
+      Entry != (ListHead); Entry = NextEntry, NextEntry = Entry->ForwardLink)
+
+#define EFI_LIST_CONTAINER(Entry, Type, Field) BASE_CR(Entry, Type, Field)
+
+#define XHC_LOW_32BIT(Addr64)          ((UINT32)(((UINTN)(Addr64)) & 0xFFFFFFFF))
+#define XHC_HIGH_32BIT(Addr64)         ((UINT32)(RShiftU64((UINT64)(UINTN)(Addr64), 32) & 0xFFFFFFFF))
+#define XHC_BIT_IS_SET(Data, Bit)      ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define XHC_REG_BIT_IS_SET(Xhc, Offset, Bit) \
+          (XHC_BIT_IS_SET(XhcReadOpReg ((Xhc), (Offset)), (Bit)))
+
+#define XHCI_IS_DATAIN(EndpointAddr)   XHC_BIT_IS_SET((EndpointAddr), 0x80)
+
+#define XHCI_INSTANCE_SIG              SIGNATURE_32 ('x', 'h', 'c', 'i')
+#define XHC_FROM_THIS(a)               CR(a, USB_XHCI_INSTANCE, Usb2Hc, XHCI_INSTANCE_SIG)
+
+#define USB_DESC_TYPE_HUB              0x29
+#define USB_DESC_TYPE_HUB_SUPER_SPEED  0x2a
+
+//
+// The RequestType in EFI_USB_DEVICE_REQUEST is composed of
+// three fields: One bit direction, 2 bit type, and 5 bit
+// target.
+//
+#define USB_REQUEST_TYPE(Dir, Type, Target) \
+          ((UINT8)((((Dir) == EfiUsbDataIn ? 0x01 : 0) << 7) | (Type) | (Target)))
+
+//
+// Xhci Data and Ctrl Structures
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                     ProgInterface;
+  UINT8                     SubClassCode;
+  UINT8                     BaseCode;
+} USB_CLASSC;
+
+typedef struct {
+  UINT8                     Length;
+  UINT8                     DescType;
+  UINT8                     NumPorts;
+  UINT16                    HubCharacter;
+  UINT8                     PwrOn2PwrGood;
+  UINT8                     HubContrCurrent;
+  UINT8                     Filler[16];
+} EFI_USB_HUB_DESCRIPTOR;
+#pragma pack()
+
+struct _USB_DEV_CONTEXT {
+  //
+  // Whether this entry in UsbDevContext array is used or not.
+  //
+  BOOLEAN                   Enabled;
+  //
+  // The slot id assigned to the new device through XHCI's Enable_Slot cmd.
+  //
+  UINT8                     SlotId;
+  //
+  // The route string presented an attached usb device.
+  //
+  USB_DEV_ROUTE             RouteString;
+  //
+  // The route string of parent device if it exists. Otherwise it's zero.
+  //
+  USB_DEV_ROUTE             ParentRouteString;
+  //
+  // The actual device address assigned by XHCI through Address_Device command.
+  //
+  UINT8                     XhciDevAddr;
+  //
+  // The requested device address from UsbBus driver through Set_Address standard usb request.
+  // As XHCI spec replaces this request with Address_Device command, we have to record the
+  // requested device address and establish a mapping relationship with the actual device address.
+  // Then UsbBus driver just need to be aware of the requested device address to access usb device
+  // through EFI_USB2_HC_PROTOCOL. Xhci driver would be responsible for translating it to actual
+  // device address and access the actual device.
+  //
+  UINT8                     BusDevAddr;
+  //
+  // The pointer to the input device context.
+  //
+  VOID                      *InputContext;
+  //
+  // The pointer to the output device context.
+  //
+  VOID                      *OutputContext;
+  //
+  // The transfer queue for every endpoint.
+  //
+  VOID                      *EndpointTransferRing[31];
+  //
+  // The device descriptor which is stored to support XHCI's Evaluate_Context cmd.
+  //
+  EFI_USB_DEVICE_DESCRIPTOR DevDesc;
+  //
+  // As a usb device may include multiple configuration descriptors, we dynamically allocate an array
+  // to store them.
+  // Note that every configuration descriptor stored here includes those lower level descriptors,
+  // such as Interface descriptor, Endpoint descriptor, and so on.
+  // These information is used to support XHCI's Config_Endpoint cmd.
+  //
+  EFI_USB_CONFIG_DESCRIPTOR **ConfDesc;
+  //
+  // A device has an active Configuration.
+  //
+  UINT8                     ActiveConfiguration;
+  //
+  // Every interface has an active AlternateSetting.
+  //
+  UINT8                     *ActiveAlternateSetting;
+};
+
+struct _USB_XHCI_INSTANCE {
+  UINT32                    Signature;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  UINT64                    OriginalPciAttributes;
+  USBHC_MEM_POOL            *MemPool;
+
+  EFI_USB2_HC_PROTOCOL      Usb2Hc;
+
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+
+  //
+  // ExitBootServicesEvent is used to set OS semaphore and
+  // stop the XHC DMA operation after exit boot service.
+  //
+  EFI_EVENT                 ExitBootServiceEvent;
+  EFI_EVENT                 PollTimer;
+  LIST_ENTRY                AsyncIntTransfers;
+
+  UINT8                     CapLength;    ///< Capability Register Length
+  XHC_HCSPARAMS1            HcSParams1;   ///< Structural Parameters 1
+  XHC_HCSPARAMS2            HcSParams2;   ///< Structural Parameters 2
+  XHC_HCCPARAMS             HcCParams;    ///< Capability Parameters
+  UINT32                    DBOff;        ///< Doorbell Offset
+  UINT32                    RTSOff;       ///< Runtime Register Space Offset
+  UINT16                    MaxInterrupt;
+  UINT32                    PageSize;
+  UINT64                    *ScratchBuf;
+  VOID                      *ScratchMap;
+  UINT32                    MaxScratchpadBufs;
+  UINT64                    *ScratchEntry;
+  UINTN                     *ScratchEntryMap;
+  UINT32                    ExtCapRegBase;
+  UINT32                    UsbLegSupOffset;
+  UINT32                    DebugCapSupOffset;
+  UINT64                    *DCBAA;
+  VOID                      *DCBAAMap;
+  UINT32                    MaxSlotsEn;
+  //
+  // Cmd Transfer Ring
+  //
+  TRANSFER_RING             CmdRing;
+  //
+  // EventRing
+  //
+  EVENT_RING                EventRing;
+  //
+  // Misc
+  //
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+
+  //
+  // Store device contexts managed by XHCI instance
+  // The array supports up to 255 devices, entry 0 is reserved and should not be used.
+  //
+  USB_DEV_CONTEXT           UsbDevContext[256];
+};
+
+
+extern EFI_DRIVER_BINDING_PROTOCOL      gXhciDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL      gXhciComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL     gXhciComponentName2;
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has Usb2HcProtocol installed will
+  be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+/**
+  Starting the Usb XHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          supports this device.
+  @return EFI_UNSUPPORTED      do not support this device.
+  @return EFI_DEVICE_ERROR     cannot be started due to device Error.
+  @return EFI_OUT_OF_RESOURCES cannot allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS          Success.
+  @return EFI_DEVICE_ERROR     Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  );
+
+/**
+  Retrieves the capability of root hub ports.
+
+  @param  This                  The EFI_USB2_HC_PROTOCOL instance.
+  @param  MaxSpeed              Max speed supported by the controller.
+  @param  PortNumber            Number of the root hub ports.
+  @param  Is64BitCapable        Whether the controller supports 64-bit memory
+                                addressing.
+
+  @retval EFI_SUCCESS           Host controller capability were retrieved successfully.
+  @retval EFI_INVALID_PARAMETER Either of the three capability pointer is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetCapability (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT UINT8                 *MaxSpeed,
+  OUT UINT8                 *PortNumber,
+  OUT UINT8                 *Is64BitCapable
+  );
+
+/**
+  Provides software reset for the USB host controller.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  Attributes            A bit mask of the reset operation to perform.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcReset (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT16                Attributes
+  );
+
+/**
+  Retrieve the current state of the USB host controller.
+
+  @param  This                   This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Variable to return the current host controller
+                                 state.
+
+  @retval EFI_SUCCESS            Host controller state was returned in State.
+  @retval EFI_INVALID_PARAMETER  State is NULL.
+  @retval EFI_DEVICE_ERROR       An error was encountered while attempting to
+                                 retrieve the host controller's current state.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetState (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT EFI_USB_HC_STATE      *State
+  );
+
+/**
+  Sets the USB host controller to a specific state.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                 The state of the host controller that will be set.
+
+  @retval EFI_SUCCESS           The USB host controller was successfully placed
+                                in the state specified by State.
+  @retval EFI_INVALID_PARAMETER State is invalid.
+  @retval EFI_DEVICE_ERROR      Failed to set the state due to device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetState (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN EFI_USB_HC_STATE      State
+  );
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            The root hub port to retrieve the state from.
+                                This value is zero-based.
+  @param  PortStatus            Variable to receive the port state.
+
+  @retval EFI_SUCCESS           The status of the USB root hub port specified.
+                                by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetRootHubPortStatus (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  IN  UINT8                 PortNumber,
+  OUT EFI_USB_PORT_STATUS   *PortStatus
+  );
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT8                 PortNumber,
+  IN EFI_USB_PORT_FEATURE  PortFeature
+  );
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Specifies the root hub port whose feature is
+                                requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was cleared
+                                for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcClearRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT8                 PortNumber,
+  IN EFI_USB_PORT_FEATURE  PortFeature
+  );
+
+/**
+  Submits control transfer to a target USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         The target device address.
+  @param  DeviceSpeed           Target device speed.
+  @param  MaximumPacketLength   Maximum packet size the default control transfer
+                                endpoint is capable of sending or receiving.
+  @param  Request               USB device request to send.
+  @param  TransferDirection     Specifies the data direction for the data stage
+  @param  Data                  Data buffer to be transmitted or received from USB
+                                device.
+  @param  DataLength            The size (in bytes) of the data buffer.
+  @param  Timeout               Indicates the maximum timeout, in millisecond.
+  @param  Translator            Transaction translator to be used by this device.
+  @param  TransferResult        Return the result of this control transfer.
+
+  @retval EFI_SUCCESS           Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcControlTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST              *Request,
+  IN     EFI_USB_DATA_DIRECTION              TransferDirection,
+  IN OUT VOID                                *Data,
+  IN OUT UINTN                               *DataLength,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  );
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support bulk
+                                transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of
+                                sending or receiving.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data to transmit
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to
+                                use of the subsequent bulk transfer.
+  @param  Timeout               Indicates the maximum time, in millisecond, which
+                                the transfer is allowed to complete.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcBulkTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN OUT UINTN                               *DataLength,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  );
+
+/**
+  Submits an asynchronous interrupt transfer to an
+  interrupt endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+  @param  IsNewTransfer         If TRUE, to submit an new asynchronous interrupt
+                                transfer If FALSE, to remove the specified
+                                asynchronous interrupt.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  PollingInterval       The he interval, in milliseconds, that the transfer
+                                is polled.
+  @param  DataLength            The length of data to receive at the rate specified
+                                by  PollingInterval.
+  @param  Translator            Transaction translator to use.
+  @param  CallBackFunction      Function to call at the rate specified by
+                                PollingInterval.
+  @param  Context               Context to CallBackFunction.
+
+  @retval EFI_SUCCESS           The request has been successfully submitted or canceled.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request failed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcAsyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     BOOLEAN                             IsNewTransfer,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               PollingInterval,
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     CallBackFunction,
+  IN     VOID                                *Context OPTIONAL
+  );
+
+/**
+  Submits synchronous interrupt transfer to an interrupt endpoint
+  of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+                                of sending or receiving.
+  @param  Data                  Buffer of data that will be transmitted to  USB
+                                device or received from USB device.
+  @param  DataLength            On input, the size, in bytes, of the data buffer; On
+                                output, the number of bytes transferred.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  Timeout               Maximum time, in second, to complete.
+  @param  Translator            Transaction translator to use.
+  @param  TransferResult        Variable to receive the transfer result.
+
+  @return EFI_SUCCESS           The transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER Some parameters are invalid.
+  @return EFI_TIMEOUT           The transfer failed due to timeout.
+  @return EFI_DEVICE_ERROR      The failed due to host controller or device error
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN OUT VOID                                *Data,
+  IN OUT UINTN                               *DataLength,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  );
+
+/**
+  Submits isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  TransferResult       Variable to receive the transfer result.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer is unsupported.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  );
+
+/**
+  Submits Async isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  IsochronousCallBack  Function to be called when the transfer complete.
+  @param  Context              Context passed to the call back function as
+                               parameter.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcAsyncIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     IsochronousCallBack,
+  IN     VOID                                *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.inf b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.inf
new file mode 100644
index 0000000000..614938ac89
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.inf
@@ -0,0 +1,76 @@
+## @file
+#  The XhciDxe driver is responsible for managing the behavior of XHCI controller.
+#  It implements the interfaces of monitoring the status of all ports and transferring
+#  Control, Bulk, Interrupt and Isochronous requests to those attached usb LS/FS/HS/SS devices.
+#
+#  Copyright (c) 2011 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = XhciDxe
+  MODULE_UNI_FILE                = XhciDxe.uni
+  FILE_GUID                      = B7F50E91-A759-412c-ADE4-DCD03E7F7C28
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = XhcDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gXhciDriverBinding
+#  COMPONENT_NAME                =  gXhciComponentName
+#  COMPONENT_NAME2               =  gXhciComponentName2
+#
+
+[Sources]
+  Xhci.c
+  XhciReg.c
+  XhciSched.c
+  UsbHcMem.c
+  UsbHcMem.h  
+  ComponentName.c
+  ComponentName.h
+  Xhci.h
+  XhciReg.h
+  XhciSched.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  ReportStatusCodeLib
+
+[Guids]
+  gEfiEventExitBootServicesGuid                 ## SOMETIMES_CONSUMES ## Event
+
+[Protocols]
+  gEfiPciIoProtocolGuid                         ## TO_START
+  gEfiUsb2HcProtocolGuid                        ## BY_START
+
+# [Event]
+# EVENT_TYPE_PERIODIC_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  XhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.uni b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.uni
new file mode 100644
index 0000000000..eea95dfa2b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxeExtra.uni
new file mode 100644
index 0000000000..9c598b1868
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c
new file mode 100644
index 0000000000..d0f22050ad
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c
@@ -0,0 +1,743 @@
+/** @file
+
+  The XHCI register operation routines.
+
+Copyright (c) 2011 - 2015, 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 "Xhci.h"
+
+/**
+  Read 1-byte width XHCI capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 1-byte width capability register.
+
+  @return The register content read.
+  @retval If err, return 0xFF.
+
+**/
+UINT8
+XhcReadCapReg8 (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT8                   Data;
+  EFI_STATUS              Status;
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint8,
+                             XHC_BAR_INDEX,
+                             (UINT64) Offset,
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadCapReg: Pci Io read error - %r at %d\n", Status, Offset));
+    Data = 0xFF;
+  }
+
+  return Data;
+}
+
+/**
+  Read 4-bytes width XHCI capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 4-bytes width capability register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFFFFFF.
+
+**/
+UINT32
+XhcReadCapReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) Offset,
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadCapReg: Pci Io read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Read 4-bytes width XHCI Operational register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 4-bytes width operational register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFFFFFF.
+
+**/
+UINT32
+XhcReadOpReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->CapLength != 0);
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->CapLength + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadOpReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Write the data to the 4-bytes width XHCI operational register.
+
+  @param  Xhc      The XHCI Instance.
+  @param  Offset   The offset of the 4-bytes width operational register.
+  @param  Data     The data to write.
+
+**/
+VOID
+XhcWriteOpReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->CapLength != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->CapLength + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteOpReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+/**
+  Write the data to the 2-bytes width XHCI operational register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 2-bytes width operational register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteOpReg16 (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT16               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->CapLength != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint16,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->CapLength + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteOpReg16: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+/**
+  Read XHCI door bell register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the door bell register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadDoorBellReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->DBOff != 0);
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->DBOff + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadDoorBellReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI door bell register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the door bell register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteDoorBellReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->DBOff != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->DBOff + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteOpReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadRuntimeReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->RTSOff != 0);
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->RTSOff + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadRuntimeReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteRuntimeReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->RTSOff != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->RTSOff + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteRuntimeReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+/**
+  Read XHCI extended capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the extended capability register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadExtCapReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->ExtCapRegBase != 0);
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->ExtCapRegBase + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadExtCapReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI extended capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the extended capability register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteExtCapReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->ExtCapRegBase != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->ExtCapRegBase + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteExtCapReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+
+/**
+  Set one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcSetRuntimeRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = XhcReadRuntimeReg (Xhc, Offset);
+  Data |= Bit;
+  XhcWriteRuntimeReg (Xhc, Offset, Data);
+}
+
+/**
+  Clear one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcClearRuntimeRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = XhcReadRuntimeReg (Xhc, Offset);
+  Data &= ~Bit;
+  XhcWriteRuntimeReg (Xhc, Offset, Data);
+}
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcSetOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = XhcReadOpReg (Xhc, Offset);
+  Data |= Bit;
+  XhcWriteOpReg (Xhc, Offset, Data);
+}
+
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to clear.
+
+**/
+VOID
+XhcClearOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = XhcReadOpReg (Xhc, Offset);
+  Data &= ~Bit;
+  XhcWriteOpReg (Xhc, Offset, Data);
+}
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to become set (or clear).
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operation register.
+  @param  Bit          The bit of the register to wait for.
+  @param  WaitToSet    Wait the bit to set or clear.
+  @param  Timeout      The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT  The time out occurred.
+
+**/
+EFI_STATUS
+XhcWaitOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit,
+  IN BOOLEAN              WaitToSet,
+  IN UINT32               Timeout
+  )
+{
+  UINT32                  Index;
+  UINT64                  Loop;
+
+  Loop   = Timeout * XHC_1_MILLISECOND;
+
+  for (Index = 0; Index < Loop; Index++) {
+    if (XHC_REG_BIT_IS_SET (Xhc, Offset, Bit) == WaitToSet) {
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (XHC_1_MICROSECOND);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Set Bios Ownership
+
+  @param  Xhc          The XHCI Instance.
+
+**/
+VOID
+XhcSetBiosOwnership (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  UINT32                    Buffer;
+
+  if (Xhc->UsbLegSupOffset == 0xFFFFFFFF) {
+    return;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcSetBiosOwnership: called to set BIOS ownership\n"));
+
+  Buffer = XhcReadExtCapReg (Xhc, Xhc->UsbLegSupOffset);
+  Buffer = ((Buffer & (~USBLEGSP_OS_SEMAPHORE)) | USBLEGSP_BIOS_SEMAPHORE);
+  XhcWriteExtCapReg (Xhc, Xhc->UsbLegSupOffset, Buffer);
+}
+
+/**
+  Clear Bios Ownership
+
+  @param  Xhc       The XHCI Instance.
+
+**/
+VOID
+XhcClearBiosOwnership (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  UINT32                    Buffer;
+
+  if (Xhc->UsbLegSupOffset == 0xFFFFFFFF) {
+    return;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcClearBiosOwnership: called to clear BIOS ownership\n"));
+
+  Buffer = XhcReadExtCapReg (Xhc, Xhc->UsbLegSupOffset);
+  Buffer = ((Buffer & (~USBLEGSP_BIOS_SEMAPHORE)) | USBLEGSP_OS_SEMAPHORE);
+  XhcWriteExtCapReg (Xhc, Xhc->UsbLegSupOffset, Buffer);
+}
+
+/**
+  Calculate the offset of the XHCI capability.
+
+  @param  Xhc     The XHCI Instance.
+  @param  CapId   The XHCI Capability ID.
+
+  @return The offset of XHCI legacy support capability register.
+
+**/
+UINT32
+XhcGetCapabilityAddr (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT8                CapId
+  )
+{
+  UINT32 ExtCapOffset;
+  UINT8  NextExtCapReg;
+  UINT32 Data;
+
+  ExtCapOffset = 0;
+
+  do {
+    //
+    // Check if the extended capability register's capability id is USB Legacy Support.
+    //
+    Data = XhcReadExtCapReg (Xhc, ExtCapOffset);
+    if ((Data & 0xFF) == CapId) {
+      return ExtCapOffset;
+    }
+    //
+    // If not, then traverse all of the ext capability registers till finding out it.
+    //
+    NextExtCapReg = (UINT8)((Data >> 8) & 0xFF);
+    ExtCapOffset += (NextExtCapReg << 2);
+  } while (NextExtCapReg != 0);
+
+  return 0xFFFFFFFF;
+}
+
+/**
+  Whether the XHCI host controller is halted.
+
+  @param  Xhc     The XHCI Instance.
+
+  @retval TRUE    The controller is halted.
+  @retval FALSE   It isn't halted.
+
+**/
+BOOLEAN
+XhcIsHalt (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  return XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT);
+}
+
+
+/**
+  Whether system error occurred.
+
+  @param  Xhc      The XHCI Instance.
+
+  @retval TRUE     System error happened.
+  @retval FALSE    No system error.
+
+**/
+BOOLEAN
+XhcIsSysError (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  return XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HSE);
+}
+
+/**
+  Reset the XHCI host controller.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The XHCI host controller is reset.
+  @return Others       Failed to reset the XHCI before Timeout.
+
+**/
+EFI_STATUS
+XhcResetHC (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EFI_SUCCESS;
+
+  DEBUG ((EFI_D_INFO, "XhcResetHC!\n"));
+  //
+  // Host can only be reset when it is halt. If not so, halt it
+  //
+  if (!XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT)) {
+    Status = XhcHaltHC (Xhc, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if ((Xhc->DebugCapSupOffset == 0xFFFFFFFF) || ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset) & 0xFF) != XHC_CAP_USB_DEBUG) ||
+      ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset + XHC_DC_DCCTRL) & BIT0) == 0)) {
+    XhcSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET);
+    Status = XhcWaitOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET, FALSE, Timeout);
+  }
+
+  return Status;
+}
+
+
+/**
+  Halt the XHCI host controller.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @return EFI_SUCCESS  The XHCI host controller is halt.
+  @return EFI_TIMEOUT  Failed to halt the XHCI before Timeout.
+
+**/
+EFI_STATUS
+XhcHaltHC (
+  IN USB_XHCI_INSTANCE   *Xhc,
+  IN UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  XhcClearOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RUN);
+  Status = XhcWaitOpRegBit (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT, TRUE, Timeout);
+  return Status;
+}
+
+
+/**
+  Set the XHCI host controller to run.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @return EFI_SUCCESS  The XHCI host controller is running.
+  @return EFI_TIMEOUT  Failed to set the XHCI to run before Timeout.
+
+**/
+EFI_STATUS
+XhcRunHC (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  XhcSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RUN);
+  Status = XhcWaitOpRegBit (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT, FALSE, Timeout);
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.h
new file mode 100644
index 0000000000..b748c8d397
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.h
@@ -0,0 +1,583 @@
+/** @file
+
+  This file contains the register definition of XHCI host controller.
+
+Copyright (c) 2011 - 2013, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_XHCI_REG_H_
+#define _EFI_XHCI_REG_H_
+
+#define PCI_IF_XHCI                 0x30
+
+//
+// PCI Configuration Registers
+//
+#define XHC_BAR_INDEX               0x00
+
+#define XHC_PCI_BAR_OFFSET          0x10       // Memory Bar Register Offset
+#define XHC_PCI_BAR_MASK            0xFFFF     // Memory Base Address Mask
+
+#define USB_HUB_CLASS_CODE          0x09
+#define USB_HUB_SUBCLASS_CODE       0x00
+
+#define XHC_CAP_USB_LEGACY          0x01
+#define XHC_CAP_USB_DEBUG           0x0A
+
+//============================================//
+//           XHCI register offset             //
+//============================================//
+
+//
+// Capability registers offset
+//
+#define XHC_CAPLENGTH_OFFSET               0x00 // Capability register length offset
+#define XHC_HCIVERSION_OFFSET              0x02 // Interface Version Number 02-03h
+#define XHC_HCSPARAMS1_OFFSET              0x04 // Structural Parameters 1
+#define XHC_HCSPARAMS2_OFFSET              0x08 // Structural Parameters 2
+#define XHC_HCSPARAMS3_OFFSET              0x0c // Structural Parameters 3
+#define XHC_HCCPARAMS_OFFSET               0x10 // Capability Parameters
+#define XHC_DBOFF_OFFSET                   0x14 // Doorbell Offset
+#define XHC_RTSOFF_OFFSET                  0x18 // Runtime Register Space Offset
+
+//
+// Operational registers offset
+//
+#define XHC_USBCMD_OFFSET                  0x0000 // USB Command Register Offset
+#define XHC_USBSTS_OFFSET                  0x0004 // USB Status Register Offset
+#define XHC_PAGESIZE_OFFSET                0x0008 // USB Page Size Register Offset
+#define XHC_DNCTRL_OFFSET                  0x0014 // Device Notification Control Register Offset
+#define XHC_CRCR_OFFSET                    0x0018 // Command Ring Control Register Offset
+#define XHC_DCBAAP_OFFSET                  0x0030 // Device Context Base Address Array Pointer Register Offset
+#define XHC_CONFIG_OFFSET                  0x0038 // Configure Register Offset
+#define XHC_PORTSC_OFFSET                  0x0400 // Port Status and Control Register Offset
+
+//
+// Runtime registers offset
+//
+#define XHC_MFINDEX_OFFSET                 0x00 // Microframe Index Register Offset
+#define XHC_IMAN_OFFSET                    0x20 // Interrupter X Management Register Offset
+#define XHC_IMOD_OFFSET                    0x24 // Interrupter X Moderation Register Offset
+#define XHC_ERSTSZ_OFFSET                  0x28 // Event Ring Segment Table Size Register Offset
+#define XHC_ERSTBA_OFFSET                  0x30 // Event Ring Segment Table Base Address Register Offset
+#define XHC_ERDP_OFFSET                    0x38 // Event Ring Dequeue Pointer Register Offset
+
+//
+// Debug registers offset
+//
+#define XHC_DC_DCCTRL                      0x20
+
+#define USBLEGSP_BIOS_SEMAPHORE            BIT16 // HC BIOS Owned Semaphore
+#define USBLEGSP_OS_SEMAPHORE              BIT24 // HC OS Owned Semaphore
+
+#pragma pack (1)
+typedef struct {
+  UINT8                   MaxSlots;       // Number of Device Slots
+  UINT16                  MaxIntrs:11;    // Number of Interrupters
+  UINT16                  Rsvd:5;
+  UINT8                   MaxPorts;       // Number of Ports
+} HCSPARAMS1;
+
+//
+// Structural Parameters 1 Register Bitmap Definition
+//
+typedef union {
+  UINT32                  Dword;
+  HCSPARAMS1              Data;
+} XHC_HCSPARAMS1;
+
+typedef struct {
+  UINT32                  Ist:4;          // Isochronous Scheduling Threshold
+  UINT32                  Erst:4;         // Event Ring Segment Table Max
+  UINT32                  Rsvd:13;
+  UINT32                  ScratchBufHi:5; // Max Scratchpad Buffers Hi
+  UINT32                  Spr:1;          // Scratchpad Restore
+  UINT32                  ScratchBufLo:5; // Max Scratchpad Buffers Lo
+} HCSPARAMS2;
+
+//
+// Structural Parameters 2 Register Bitmap Definition
+//
+typedef union {
+  UINT32                  Dword;
+  HCSPARAMS2              Data;
+} XHC_HCSPARAMS2;
+
+typedef struct {
+  UINT16                  Ac64:1;        // 64-bit Addressing Capability
+  UINT16                  Bnc:1;         // BW Negotiation Capability
+  UINT16                  Csz:1;         // Context Size
+  UINT16                  Ppc:1;         // Port Power Control
+  UINT16                  Pind:1;        // Port Indicators
+  UINT16                  Lhrc:1;        // Light HC Reset Capability
+  UINT16                  Ltc:1;         // Latency Tolerance Messaging Capability
+  UINT16                  Nss:1;         // No Secondary SID Support
+  UINT16                  Pae:1;         // Parse All Event Data
+  UINT16                  Rsvd:3;
+  UINT16                  MaxPsaSize:4;  // Maximum Primary Stream Array Size
+  UINT16                  ExtCapReg;     // xHCI Extended Capabilities Pointer
+} HCCPARAMS;
+
+//
+// Capability Parameters Register Bitmap Definition
+//
+typedef union {
+  UINT32                  Dword;
+  HCCPARAMS               Data;
+} XHC_HCCPARAMS;
+
+#pragma pack ()
+
+//
+// Register Bit Definition
+//
+#define XHC_USBCMD_RUN                     BIT0  // Run/Stop
+#define XHC_USBCMD_RESET                   BIT1  // Host Controller Reset
+#define XHC_USBCMD_INTE                    BIT2  // Interrupter Enable
+#define XHC_USBCMD_HSEE                    BIT3  // Host System Error Enable
+
+#define XHC_USBSTS_HALT                    BIT0  // Host Controller Halted
+#define XHC_USBSTS_HSE                     BIT2  // Host System Error
+#define XHC_USBSTS_EINT                    BIT3  // Event Interrupt
+#define XHC_USBSTS_PCD                     BIT4  // Port Change Detect
+#define XHC_USBSTS_SSS                     BIT8  // Save State Status
+#define XHC_USBSTS_RSS                     BIT9  // Restore State Status
+#define XHC_USBSTS_SRE                     BIT10 // Save/Restore Error
+#define XHC_USBSTS_CNR                     BIT11 // Host Controller Not Ready
+#define XHC_USBSTS_HCE                     BIT12 // Host Controller Error
+
+#define XHC_PAGESIZE_MASK                  0xFFFF // Page Size
+
+#define XHC_CRCR_RCS                       BIT0  // Ring Cycle State
+#define XHC_CRCR_CS                        BIT1  // Command Stop
+#define XHC_CRCR_CA                        BIT2  // Command Abort
+#define XHC_CRCR_CRR                       BIT3  // Command Ring Running
+
+#define XHC_CONFIG_MASK                    0xFF  // Command Ring Running
+
+#define XHC_PORTSC_CCS                     BIT0  // Current Connect Status
+#define XHC_PORTSC_PED                     BIT1  // Port Enabled/Disabled
+#define XHC_PORTSC_OCA                     BIT3  // Over-current Active
+#define XHC_PORTSC_RESET                   BIT4  // Port Reset
+#define XHC_PORTSC_PLS                     (BIT5|BIT6|BIT7|BIT8)     // Port Link State
+#define XHC_PORTSC_PP                      BIT9  // Port Power
+#define XHC_PORTSC_PS                      (BIT10|BIT11|BIT12)       // Port Speed
+#define XHC_PORTSC_LWS                     BIT16 // Port Link State Write Strobe
+#define XHC_PORTSC_CSC                     BIT17 // Connect Status Change
+#define XHC_PORTSC_PEC                     BIT18 // Port Enabled/Disabled Change
+#define XHC_PORTSC_WRC                     BIT19 // Warm Port Reset Change
+#define XHC_PORTSC_OCC                     BIT20 // Over-Current Change
+#define XHC_PORTSC_PRC                     BIT21 // Port Reset Change
+#define XHC_PORTSC_PLC                     BIT22 // Port Link State Change
+#define XHC_PORTSC_CEC                     BIT23 // Port Config Error Change
+#define XHC_PORTSC_CAS                     BIT24 // Cold Attach Status
+
+#define XHC_HUB_PORTSC_CCS                 BIT0  // Hub's Current Connect Status
+#define XHC_HUB_PORTSC_PED                 BIT1  // Hub's Port Enabled/Disabled
+#define XHC_HUB_PORTSC_OCA                 BIT3  // Hub's Over-current Active
+#define XHC_HUB_PORTSC_RESET               BIT4  // Hub's Port Reset
+#define XHC_HUB_PORTSC_PP                  BIT9  // Hub's Port Power
+#define XHC_HUB_PORTSC_CSC                 BIT16 // Hub's Connect Status Change
+#define XHC_HUB_PORTSC_PEC                 BIT17 // Hub's Port Enabled/Disabled Change
+#define XHC_HUB_PORTSC_OCC                 BIT19 // Hub's Over-Current Change
+#define XHC_HUB_PORTSC_PRC                 BIT20 // Hub's Port Reset Change
+#define XHC_HUB_PORTSC_BHRC                BIT21 // Hub's Port Warm Reset Change
+#define XHC_IMAN_IP                        BIT0  // Interrupt Pending
+#define XHC_IMAN_IE                        BIT1  // Interrupt Enable
+
+#define XHC_IMODI_MASK                     0x0000FFFF  // Interrupt Moderation Interval
+#define XHC_IMODC_MASK                     0xFFFF0000  // Interrupt Moderation Counter
+
+//
+//  Hub Class Feature Selector for Clear Port Feature Request
+//  It's the extension of hub class feature selector of USB 2.0 in USB 3.0 Spec.
+//  For more details, Please refer to USB 3.0 Spec Table 10-7.
+//
+typedef enum {
+  Usb3PortBHPortReset          = 28,
+  Usb3PortBHPortResetChange    = 29
+} XHC_PORT_FEATURE;
+
+//
+// Structure to map the hardware port states to the
+// UEFI's port states.
+//
+typedef struct {
+  UINT32                  HwState;
+  UINT16                  UefiState;
+} USB_PORT_STATE_MAP;
+
+//
+// Structure to map the hardware port states to feature selector for clear port feature request.
+//
+typedef struct {
+  UINT32                  HwState;
+  UINT16                  Selector;
+} USB_CLEAR_PORT_MAP;
+
+/**
+  Read 1-byte width XHCI capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 1-byte width capability register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFF.
+
+**/
+UINT8
+XhcReadCapReg8 (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Read 4-bytes width XHCI capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 4-bytes width capability register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFFFFFF.
+
+**/
+UINT32
+XhcReadCapReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Read 4-bytes width XHCI Operational register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 4-bytes width operational register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFFFFFF.
+
+**/
+UINT32
+XhcReadOpReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Write the data to the 4-bytes width XHCI operational register.
+
+  @param  Xhc      The XHCI Instance.
+  @param  Offset   The offset of the 4-bytes width operational register.
+  @param  Data     The data to write.
+
+**/
+VOID
+XhcWriteOpReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Write the data to the 2-bytes width XHCI operational register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 2-bytes width operational register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteOpReg16 (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT16               Data
+  );
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadRuntimeReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteRuntimeReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Read XHCI door bell register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the door bell register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadDoorBellReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Write the data to the XHCI door bell register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the door bell register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteDoorBellReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcSetOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to clear.
+
+**/
+VOID
+XhcClearOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to be set (or clear).
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit of the register to wait for.
+  @param  WaitToSet    Wait the bit to set or clear.
+  @param  Timeout      The time to wait before abort (in microsecond, us).
+
+  @retval EFI_SUCCESS  The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT  The time out occurred.
+
+**/
+EFI_STATUS
+XhcWaitOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit,
+  IN BOOLEAN              WaitToSet,
+  IN UINT32               Timeout
+  );
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadRuntimeReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteRuntimeReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Set one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcSetRuntimeRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Clear one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcClearRuntimeRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Read XHCI extended capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the extended capability register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadExtCapReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Whether the XHCI host controller is halted.
+
+  @param  Xhc     The XHCI Instance.
+
+  @retval TRUE    The controller is halted.
+  @retval FALSE   It isn't halted.
+
+**/
+BOOLEAN
+XhcIsHalt (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Whether system error occurred.
+
+  @param  Xhc      The XHCI Instance.
+
+  @retval TRUE     System error happened.
+  @retval FALSE    No system error.
+
+**/
+BOOLEAN
+XhcIsSysError (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Reset the XHCI host controller.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in microsecond, us).
+
+  @retval EFI_SUCCESS  The XHCI host controller is reset.
+  @return Others       Failed to reset the XHCI before Timeout.
+
+**/
+EFI_STATUS
+XhcResetHC (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Timeout
+  );
+
+/**
+  Halt the XHCI host controller.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in microsecond, us).
+
+  @return EFI_SUCCESS  The XHCI host controller is halt.
+  @return EFI_TIMEOUT  Failed to halt the XHCI before Timeout.
+
+**/
+EFI_STATUS
+XhcHaltHC (
+  IN USB_XHCI_INSTANCE   *Xhc,
+  IN UINT32              Timeout
+  );
+
+/**
+  Set the XHCI host controller to run.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in microsecond, us).
+
+  @return EFI_SUCCESS  The XHCI host controller is running.
+  @return EFI_TIMEOUT  Failed to set the XHCI to run before Timeout.
+
+**/
+EFI_STATUS
+XhcRunHC (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Timeout
+  );
+
+/**
+  Calculate the offset of the XHCI capability.
+
+  @param  Xhc     The XHCI Instance.
+  @param  CapId   The XHCI Capability ID.
+
+  @return The offset of XHCI legacy support capability register.
+
+**/
+UINT32
+XhcGetCapabilityAddr (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT8                CapId
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.c
new file mode 100644
index 0000000000..c25342dc1f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.c
@@ -0,0 +1,3838 @@
+/** @file
+
+  XHCI transfer scheduling routines.
+
+Copyright (c) 2011 - 2015, 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 "Xhci.h"
+
+/**
+  Create a command transfer TRB to support XHCI command interfaces.
+
+  @param  Xhc       The XHCI Instance.
+  @param  CmdTrb    The cmd TRB to be executed.
+
+  @return Created URB or NULL.
+
+**/
+URB*
+XhcCreateCmdTrb (
+  IN USB_XHCI_INSTANCE  *Xhc,
+  IN TRB_TEMPLATE       *CmdTrb
+  )
+{
+  URB    *Urb;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature  = XHC_URB_SIG;
+
+  Urb->Ring       = &Xhc->CmdRing;
+  XhcSyncTrsRing (Xhc, Urb->Ring);
+  Urb->TrbNum     = 1;
+  Urb->TrbStart   = Urb->Ring->RingEnqueue;
+  CopyMem (Urb->TrbStart, CmdTrb, sizeof (TRB_TEMPLATE));
+  Urb->TrbStart->CycleBit = Urb->Ring->RingPCS & BIT0;
+  Urb->TrbEnd             = Urb->TrbStart;
+
+  return Urb;
+}
+
+/**
+  Execute a XHCI cmd TRB pointed by CmdTrb.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  CmdTrb                The cmd TRB to be executed.
+  @param  Timeout               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+  @param  EvtTrb                The event TRB corresponding to the cmd TRB.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcCmdTransfer (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  TRB_TEMPLATE          *CmdTrb,
+  IN  UINTN                 Timeout,
+  OUT TRB_TEMPLATE          **EvtTrb
+  )
+{
+  EFI_STATUS      Status;
+  URB             *Urb;
+
+  //
+  // Validate the parameters
+  //
+  if ((Xhc == NULL) || (CmdTrb == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EFI_DEVICE_ERROR;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcCmdTransfer: HC is halted\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, then poll the execution status.
+  //
+  Urb = XhcCreateCmdTrb (Xhc, CmdTrb);
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcCmdTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status  = XhcExecTransfer (Xhc, TRUE, Urb, Timeout);
+  *EvtTrb = Urb->EvtTrb;
+
+  if (Urb->Result == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  XhcFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Create a new URB for a new transaction.
+
+  @param  Xhc       The XHCI Instance
+  @param  BusAddr   The logical device address assigned by UsbBus driver
+  @param  EpAddr    Endpoint addrress
+  @param  DevSpeed  The device speed
+  @param  MaxPacket The max packet length of the endpoint
+  @param  Type      The transaction type
+  @param  Request   The standard USB request for control transfer
+  @param  Data      The user data to transfer
+  @param  DataLen   The length of data buffer
+  @param  Callback  The function to call when data is transferred
+  @param  Context   The context to the callback
+
+  @return Created URB or NULL
+
+**/
+URB*
+XhcCreateUrb (
+  IN USB_XHCI_INSTANCE                  *Xhc,
+  IN UINT8                              BusAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINTN                              MaxPacket,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context
+  )
+{
+  USB_ENDPOINT                  *Ep;
+  EFI_STATUS                    Status;
+  URB                           *Urb;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature = XHC_URB_SIG;
+  InitializeListHead (&Urb->UrbList);
+
+  Ep            = &Urb->Ep;
+  Ep->BusAddr   = BusAddr;
+  Ep->EpAddr    = (UINT8)(EpAddr & 0x0F);
+  Ep->Direction = ((EpAddr & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut;
+  Ep->DevSpeed  = DevSpeed;
+  Ep->MaxPacket = MaxPacket;
+  Ep->Type      = Type;
+
+  Urb->Request  = Request;
+  Urb->Data     = Data;
+  Urb->DataLen  = DataLen;
+  Urb->Callback = Callback;
+  Urb->Context  = Context;
+
+  Status = XhcCreateTransferTrb (Xhc, Urb);
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcCreateUrb: XhcCreateTransferTrb Failed, Status = %r\n", Status));
+    FreePool (Urb);
+    Urb = NULL;
+  }
+
+  return Urb;
+}
+
+/**
+  Free an allocated URB.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The URB to free.
+
+**/
+VOID
+XhcFreeUrb (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN URB                  *Urb
+  )
+{
+  if ((Xhc == NULL) || (Urb == NULL)) {
+    return;
+  }
+  
+  if (Urb->DataMap != NULL) {
+    Xhc->PciIo->Unmap (Xhc->PciIo, Urb->DataMap);
+  }
+
+  FreePool (Urb);
+}
+
+/**
+  Create a transfer TRB.
+
+  @param  Xhc     The XHCI Instance
+  @param  Urb     The urb used to construct the transfer TRB.
+
+  @return Created TRB or NULL
+
+**/
+EFI_STATUS
+XhcCreateTransferTrb (
+  IN USB_XHCI_INSTANCE          *Xhc,
+  IN URB                        *Urb
+  )
+{
+  VOID                          *OutputContext;
+  TRANSFER_RING                 *EPRing;
+  UINT8                         EPType;
+  UINT8                         SlotId;
+  UINT8                         Dci;
+  TRB                           *TrbStart;
+  UINTN                         TotalLen;
+  UINTN                         Len;
+  UINTN                         TrbNum;
+  EFI_PCI_IO_PROTOCOL_OPERATION MapOp;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  VOID                          *Map;
+  EFI_STATUS                    Status;
+
+  SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Urb->Finished  = FALSE;
+  Urb->StartDone = FALSE;
+  Urb->EndDone   = FALSE;
+  Urb->Completed = 0;
+  Urb->Result    = EFI_USB_NOERROR;
+
+  Dci       = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  ASSERT (Dci < 32);
+  EPRing    = (TRANSFER_RING *)(UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1];
+  Urb->Ring = EPRing;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  if (Xhc->HcCParams.Data.Csz == 0) {
+    EPType  = (UINT8) ((DEVICE_CONTEXT *)OutputContext)->EP[Dci-1].EPType;
+  } else {
+    EPType  = (UINT8) ((DEVICE_CONTEXT_64 *)OutputContext)->EP[Dci-1].EPType;
+  }
+  
+  if (Urb->Data != NULL) {
+    if (((UINT8) (Urb->Ep.Direction)) == EfiUsbDataIn) {
+      MapOp = EfiPciIoOperationBusMasterWrite;
+    } else {
+      MapOp = EfiPciIoOperationBusMasterRead;
+    }
+    
+    Len = Urb->DataLen;
+    Status  = Xhc->PciIo->Map (Xhc->PciIo, MapOp, Urb->Data, &Len, &PhyAddr, &Map);
+    
+    if (EFI_ERROR (Status) || (Len != Urb->DataLen)) {
+      DEBUG ((EFI_D_ERROR, "XhcCreateTransferTrb: Fail to map Urb->Data.\n"));
+      return EFI_OUT_OF_RESOURCES;
+    }
+    
+    Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+    Urb->DataMap  = Map;
+  }
+
+  //
+  // Construct the TRB
+  //
+  XhcSyncTrsRing (Xhc, EPRing);
+  Urb->TrbStart = EPRing->RingEnqueue;
+  switch (EPType) {
+    case ED_CONTROL_BIDIR:
+      //
+      // For control transfer, create SETUP_STAGE_TRB first.
+      //
+      TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+      TrbStart->TrbCtrSetup.bmRequestType = Urb->Request->RequestType;
+      TrbStart->TrbCtrSetup.bRequest      = Urb->Request->Request;
+      TrbStart->TrbCtrSetup.wValue        = Urb->Request->Value;
+      TrbStart->TrbCtrSetup.wIndex        = Urb->Request->Index;
+      TrbStart->TrbCtrSetup.wLength       = Urb->Request->Length;
+      TrbStart->TrbCtrSetup.Length        = 8;
+      TrbStart->TrbCtrSetup.IntTarget     = 0;
+      TrbStart->TrbCtrSetup.IOC           = 1;
+      TrbStart->TrbCtrSetup.IDT           = 1;
+      TrbStart->TrbCtrSetup.Type          = TRB_TYPE_SETUP_STAGE;
+      if (Urb->Ep.Direction == EfiUsbDataIn) {
+        TrbStart->TrbCtrSetup.TRT = 3;
+      } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+        TrbStart->TrbCtrSetup.TRT = 2;
+      } else {
+        TrbStart->TrbCtrSetup.TRT = 0;
+      }
+      //
+      // Update the cycle bit
+      //
+      TrbStart->TrbCtrSetup.CycleBit = EPRing->RingPCS & BIT0;
+      Urb->TrbNum++;
+
+      //
+      // For control transfer, create DATA_STAGE_TRB.
+      //
+      if (Urb->DataLen > 0) {
+        XhcSyncTrsRing (Xhc, EPRing);
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbCtrData.TRBPtrLo  = XHC_LOW_32BIT(Urb->DataPhy);
+        TrbStart->TrbCtrData.TRBPtrHi  = XHC_HIGH_32BIT(Urb->DataPhy);
+        TrbStart->TrbCtrData.Length    = (UINT32) Urb->DataLen;
+        TrbStart->TrbCtrData.TDSize    = 0;
+        TrbStart->TrbCtrData.IntTarget = 0;
+        TrbStart->TrbCtrData.ISP       = 1;
+        TrbStart->TrbCtrData.IOC       = 1;
+        TrbStart->TrbCtrData.IDT       = 0;
+        TrbStart->TrbCtrData.CH        = 0;
+        TrbStart->TrbCtrData.Type      = TRB_TYPE_DATA_STAGE;
+        if (Urb->Ep.Direction == EfiUsbDataIn) {
+          TrbStart->TrbCtrData.DIR = 1;
+        } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+          TrbStart->TrbCtrData.DIR = 0;
+        } else {
+          TrbStart->TrbCtrData.DIR = 0;
+        }
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbCtrData.CycleBit = EPRing->RingPCS & BIT0;
+        Urb->TrbNum++;
+      }
+      //
+      // For control transfer, create STATUS_STAGE_TRB.
+      // Get the pointer to next TRB for status stage use
+      //
+      XhcSyncTrsRing (Xhc, EPRing);
+      TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+      TrbStart->TrbCtrStatus.IntTarget = 0;
+      TrbStart->TrbCtrStatus.IOC       = 1;
+      TrbStart->TrbCtrStatus.CH        = 0;
+      TrbStart->TrbCtrStatus.Type      = TRB_TYPE_STATUS_STAGE;
+      if (Urb->Ep.Direction == EfiUsbDataIn) {
+        TrbStart->TrbCtrStatus.DIR = 0;
+      } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+        TrbStart->TrbCtrStatus.DIR = 1;
+      } else {
+        TrbStart->TrbCtrStatus.DIR = 0;
+      }
+      //
+      // Update the cycle bit
+      //
+      TrbStart->TrbCtrStatus.CycleBit = EPRing->RingPCS & BIT0;
+      //
+      // Update the enqueue pointer
+      //
+      XhcSyncTrsRing (Xhc, EPRing);
+      Urb->TrbNum++;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+
+      break;
+
+    case ED_BULK_OUT:
+    case ED_BULK_IN:
+      TotalLen = 0;
+      Len      = 0;
+      TrbNum   = 0;
+      TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+      while (TotalLen < Urb->DataLen) {
+        if ((TotalLen + 0x10000) >= Urb->DataLen) {
+          Len = Urb->DataLen - TotalLen;
+        } else {
+          Len = 0x10000;
+        }
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbNormal.TRBPtrLo  = XHC_LOW_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.TRBPtrHi  = XHC_HIGH_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.Length    = (UINT32) Len;
+        TrbStart->TrbNormal.TDSize    = 0;
+        TrbStart->TrbNormal.IntTarget = 0;
+        TrbStart->TrbNormal.ISP       = 1;
+        TrbStart->TrbNormal.IOC       = 1;
+        TrbStart->TrbNormal.Type      = TRB_TYPE_NORMAL;
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbNormal.CycleBit = EPRing->RingPCS & BIT0;
+
+        XhcSyncTrsRing (Xhc, EPRing);
+        TrbNum++;
+        TotalLen += Len;
+      }
+
+      Urb->TrbNum = TrbNum;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+      break;
+
+    case ED_INTERRUPT_OUT:
+    case ED_INTERRUPT_IN:
+      TotalLen = 0;
+      Len      = 0;
+      TrbNum   = 0;
+      TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+      while (TotalLen < Urb->DataLen) {
+        if ((TotalLen + 0x10000) >= Urb->DataLen) {
+          Len = Urb->DataLen - TotalLen;
+        } else {
+          Len = 0x10000;
+        }
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbNormal.TRBPtrLo  = XHC_LOW_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.TRBPtrHi  = XHC_HIGH_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.Length    = (UINT32) Len;
+        TrbStart->TrbNormal.TDSize    = 0;
+        TrbStart->TrbNormal.IntTarget = 0;
+        TrbStart->TrbNormal.ISP       = 1;
+        TrbStart->TrbNormal.IOC       = 1;
+        TrbStart->TrbNormal.Type      = TRB_TYPE_NORMAL;
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbNormal.CycleBit = EPRing->RingPCS & BIT0;
+
+        XhcSyncTrsRing (Xhc, EPRing);
+        TrbNum++;
+        TotalLen += Len;
+      }
+
+      Urb->TrbNum = TrbNum;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+      break;
+
+    default:
+      DEBUG ((EFI_D_INFO, "Not supported EPType 0x%x!\n",EPType));
+      ASSERT (FALSE);
+      break;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialize the XHCI host controller for schedule.
+
+  @param  Xhc        The XHCI Instance to be initialized.
+
+**/
+VOID
+XhcInitSched (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  VOID                  *Dcbaa;
+  EFI_PHYSICAL_ADDRESS  DcbaaPhy;
+  UINT64                CmdRing;
+  EFI_PHYSICAL_ADDRESS  CmdRingPhy; 
+  UINTN                 Entries;
+  UINT32                MaxScratchpadBufs;
+  UINT64                *ScratchBuf;
+  EFI_PHYSICAL_ADDRESS  ScratchPhy;
+  UINT64                *ScratchEntry;
+  EFI_PHYSICAL_ADDRESS  ScratchEntryPhy;
+  UINT32                Index;
+  UINTN                 *ScratchEntryMap;
+  EFI_STATUS            Status;
+
+  //
+  // Initialize memory management.
+  //
+  Xhc->MemPool = UsbHcInitMemPool (Xhc->PciIo);
+  ASSERT (Xhc->MemPool != NULL);
+
+  //
+  // Program the Max Device Slots Enabled (MaxSlotsEn) field in the CONFIG register (5.4.7)
+  // to enable the device slots that system software is going to use.
+  //
+  Xhc->MaxSlotsEn = Xhc->HcSParams1.Data.MaxSlots;
+  ASSERT (Xhc->MaxSlotsEn >= 1 && Xhc->MaxSlotsEn <= 255);
+  XhcWriteOpReg (Xhc, XHC_CONFIG_OFFSET, Xhc->MaxSlotsEn);
+
+  //
+  // The Device Context Base Address Array entry associated with each allocated Device Slot
+  // shall contain a 64-bit pointer to the base of the associated Device Context.
+  // The Device Context Base Address Array shall contain MaxSlotsEn + 1 entries.
+  // Software shall set Device Context Base Address Array entries for unallocated Device Slots to '0'.
+  //
+  Entries = (Xhc->MaxSlotsEn + 1) * sizeof(UINT64);
+  Dcbaa = UsbHcAllocateMem (Xhc->MemPool, Entries);
+  ASSERT (Dcbaa != NULL);
+  ZeroMem (Dcbaa, Entries);
+
+  //
+  // A Scratchpad Buffer is a PAGESIZE block of system memory located on a PAGESIZE boundary.
+  // System software shall allocate the Scratchpad Buffer(s) before placing the xHC in to Run
+  // mode (Run/Stop(R/S) ='1').
+  //
+  MaxScratchpadBufs      = ((Xhc->HcSParams2.Data.ScratchBufHi) << 5) | (Xhc->HcSParams2.Data.ScratchBufLo);
+  Xhc->MaxScratchpadBufs = MaxScratchpadBufs;
+  ASSERT (MaxScratchpadBufs <= 1023);
+  if (MaxScratchpadBufs != 0) {
+    //
+    // Allocate the buffer to record the Mapping for each scratch buffer in order to Unmap them
+    //
+    ScratchEntryMap = AllocateZeroPool (sizeof (UINTN) * MaxScratchpadBufs);
+    ASSERT (ScratchEntryMap != NULL);
+    Xhc->ScratchEntryMap = ScratchEntryMap;
+    
+    //
+    // Allocate the buffer to record the host address for each entry
+    //
+    ScratchEntry = AllocateZeroPool (sizeof (UINT64) * MaxScratchpadBufs);
+    ASSERT (ScratchEntry != NULL);
+    Xhc->ScratchEntry = ScratchEntry;
+
+    ScratchPhy = 0;
+    Status = UsbHcAllocateAlignedPages (
+               Xhc->PciIo,
+               EFI_SIZE_TO_PAGES (MaxScratchpadBufs * sizeof (UINT64)),
+               Xhc->PageSize,
+               (VOID **) &ScratchBuf, 
+               &ScratchPhy,
+               &Xhc->ScratchMap
+               );
+    ASSERT_EFI_ERROR (Status);
+
+    ZeroMem (ScratchBuf, MaxScratchpadBufs * sizeof (UINT64));
+    Xhc->ScratchBuf = ScratchBuf;
+
+    //
+    // Allocate each scratch buffer
+    //
+    for (Index = 0; Index < MaxScratchpadBufs; Index++) {
+      ScratchEntryPhy = 0;
+      Status = UsbHcAllocateAlignedPages (
+                 Xhc->PciIo,
+                 EFI_SIZE_TO_PAGES (Xhc->PageSize),
+                 Xhc->PageSize,
+                 (VOID **) &ScratchEntry[Index],
+                 &ScratchEntryPhy,
+                 (VOID **) &ScratchEntryMap[Index]
+                 );
+      ASSERT_EFI_ERROR (Status);
+      ZeroMem ((VOID *)(UINTN)ScratchEntry[Index], Xhc->PageSize);
+      //
+      // Fill with the PCI device address
+      //
+      *ScratchBuf++ = ScratchEntryPhy;
+    }
+    //
+    // The Scratchpad Buffer Array contains pointers to the Scratchpad Buffers. Entry 0 of the
+    // Device Context Base Address Array points to the Scratchpad Buffer Array.
+    //
+    *(UINT64 *)Dcbaa = (UINT64)(UINTN) ScratchPhy;
+  }
+
+  //
+  // Program the Device Context Base Address Array Pointer (DCBAAP) register (5.4.6) with
+  // a 64-bit address pointing to where the Device Context Base Address Array is located.
+  //
+  Xhc->DCBAA = (UINT64 *)(UINTN)Dcbaa;
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  DcbaaPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Dcbaa, Entries);
+  XhcWriteOpReg (Xhc, XHC_DCBAAP_OFFSET, XHC_LOW_32BIT(DcbaaPhy));
+  XhcWriteOpReg (Xhc, XHC_DCBAAP_OFFSET + 4, XHC_HIGH_32BIT (DcbaaPhy));
+
+  DEBUG ((EFI_D_INFO, "XhcInitSched:DCBAA=0x%x\n", (UINT64)(UINTN)Xhc->DCBAA));
+
+  //
+  // Define the Command Ring Dequeue Pointer by programming the Command Ring Control Register
+  // (5.4.5) with a 64-bit address pointing to the starting address of the first TRB of the Command Ring.
+  // Note: The Command Ring is 64 byte aligned, so the low order 6 bits of the Command Ring Pointer shall
+  // always be '0'.
+  //
+  CreateTransferRing (Xhc, CMD_RING_TRB_NUMBER, &Xhc->CmdRing);
+  //
+  // The xHC uses the Enqueue Pointer to determine when a Transfer Ring is empty. As it fetches TRBs from a
+  // Transfer Ring it checks for a Cycle bit transition. If a transition detected, the ring is empty.
+  // So we set RCS as inverted PCS init value to let Command Ring empty
+  //
+  CmdRing  = (UINT64)(UINTN)Xhc->CmdRing.RingSeg0;
+  CmdRingPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, (VOID *)(UINTN) CmdRing, sizeof (TRB_TEMPLATE) * CMD_RING_TRB_NUMBER);
+  ASSERT ((CmdRingPhy & 0x3F) == 0);
+  CmdRingPhy |= XHC_CRCR_RCS;
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcWriteOpReg (Xhc, XHC_CRCR_OFFSET, XHC_LOW_32BIT(CmdRingPhy));
+  XhcWriteOpReg (Xhc, XHC_CRCR_OFFSET + 4, XHC_HIGH_32BIT (CmdRingPhy));
+
+  DEBUG ((EFI_D_INFO, "XhcInitSched:XHC_CRCR=0x%x\n", Xhc->CmdRing.RingSeg0));
+
+  //
+  // Disable the 'interrupter enable' bit in USB_CMD
+  // and clear IE & IP bit in all Interrupter X Management Registers.
+  //
+  XhcClearOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_INTE);
+  for (Index = 0; Index < (UINT16)(Xhc->HcSParams1.Data.MaxIntrs); Index++) {
+    XhcClearRuntimeRegBit (Xhc, XHC_IMAN_OFFSET + (Index * 32), XHC_IMAN_IE);
+    XhcSetRuntimeRegBit (Xhc, XHC_IMAN_OFFSET + (Index * 32), XHC_IMAN_IP);
+  }
+
+  //
+  // Allocate EventRing for Cmd, Ctrl, Bulk, Interrupt, AsynInterrupt transfer
+  //
+  CreateEventRing (Xhc, &Xhc->EventRing);
+  DEBUG ((EFI_D_INFO, "XhcInitSched:XHC_EVENTRING=0x%x\n", Xhc->EventRing.EventRingSeg0));
+}
+
+/**
+  System software shall use a Reset Endpoint Command (section 4.11.4.7) to remove the Halted
+  condition in the xHC. After the successful completion of the Reset Endpoint Command, the Endpoint
+  Context is transitioned from the Halted to the Stopped state and the Transfer Ring of the endpoint is
+  reenabled. The next write to the Doorbell of the Endpoint will transition the Endpoint Context from the
+  Stopped to the Running state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which makes the endpoint halted.
+
+  @retval EFI_SUCCESS           The recovery is successful.
+  @retval Others                Failed to recovery halted endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcRecoverHaltedEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  ASSERT (Dci < 32);
+  
+  DEBUG ((EFI_D_INFO, "Recovery Halted Slot = %x,Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Reset endpoint command to transit from halt to stop state
+  //
+  Status = XhcResetEndpoint(Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcRecoverHaltedEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2)Set dequeue pointer
+  //
+  Status = XhcSetTrDequeuePointer(Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcRecoverHaltedEndpoint: Set Transfer Ring Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3)Ring the doorbell to transit from stop to active
+  //
+  XhcRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDequeueTrbFromEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  ASSERT (Dci < 32);
+  
+  DEBUG ((EFI_D_INFO, "Stop Slot = %x,Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Stop endpoint command to stop xHC from executing of the TDs on the endpoint
+  //
+  Status = XhcStopEndpoint(Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDequeueTrbFromEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2)Set dequeue pointer
+  //
+  Status = XhcSetTrDequeuePointer(Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDequeueTrbFromEndpoint: Set Transfer Ring Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3)Ring the doorbell to transit from stop to active
+  //
+  XhcRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
+/**
+  Create XHCI event ring.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  EventRing           The created event ring.
+
+**/
+VOID
+CreateEventRing (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  OUT EVENT_RING            *EventRing
+  )
+{
+  VOID                        *Buf;
+  EVENT_RING_SEG_TABLE_ENTRY  *ERSTBase;
+  UINTN                       Size;
+  EFI_PHYSICAL_ADDRESS        ERSTPhy;
+  EFI_PHYSICAL_ADDRESS        DequeuePhy;
+
+  ASSERT (EventRing != NULL);
+
+  Size = sizeof (TRB_TEMPLATE) * EVENT_RING_TRB_NUMBER;
+  Buf = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, Size);
+
+  EventRing->EventRingSeg0    = Buf;
+  EventRing->TrbNumber        = EVENT_RING_TRB_NUMBER;
+  EventRing->EventRingDequeue = (TRB_TEMPLATE *) EventRing->EventRingSeg0;
+  EventRing->EventRingEnqueue = (TRB_TEMPLATE *) EventRing->EventRingSeg0;
+  
+  DequeuePhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, Size);
+  
+  //
+  // Software maintains an Event Ring Consumer Cycle State (CCS) bit, initializing it to '1'
+  // and toggling it every time the Event Ring Dequeue Pointer wraps back to the beginning of the Event Ring.
+  //
+  EventRing->EventRingCCS = 1;
+
+  Size = sizeof (EVENT_RING_SEG_TABLE_ENTRY) * ERST_NUMBER;
+  Buf = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, Size);
+
+  ERSTBase              = (EVENT_RING_SEG_TABLE_ENTRY *) Buf;
+  EventRing->ERSTBase   = ERSTBase;
+  ERSTBase->PtrLo       = XHC_LOW_32BIT (DequeuePhy);
+  ERSTBase->PtrHi       = XHC_HIGH_32BIT (DequeuePhy);
+  ERSTBase->RingTrbSize = EVENT_RING_TRB_NUMBER;
+
+  ERSTPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, ERSTBase, Size);
+
+  //
+  // Program the Interrupter Event Ring Segment Table Size (ERSTSZ) register (5.5.2.3.1)
+  //
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTSZ_OFFSET,
+    ERST_NUMBER
+    );
+  //
+  // Program the Interrupter Event Ring Dequeue Pointer (ERDP) register (5.5.2.3.3)
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERDP_OFFSET,
+    XHC_LOW_32BIT((UINT64)(UINTN)DequeuePhy)
+    );
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERDP_OFFSET + 4,
+    XHC_HIGH_32BIT((UINT64)(UINTN)DequeuePhy)
+    );
+  //
+  // Program the Interrupter Event Ring Segment Table Base Address (ERSTBA) register(5.5.2.3.2)
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTBA_OFFSET,
+    XHC_LOW_32BIT((UINT64)(UINTN)ERSTPhy)
+    );
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTBA_OFFSET + 4,
+    XHC_HIGH_32BIT((UINT64)(UINTN)ERSTPhy)
+    );
+  //
+  // Need set IMAN IE bit to enble the ring interrupt
+  //
+  XhcSetRuntimeRegBit (Xhc, XHC_IMAN_OFFSET, XHC_IMAN_IE);
+}
+
+/**
+  Create XHCI transfer ring.
+
+  @param  Xhc               The XHCI Instance.
+  @param  TrbNum            The number of TRB in the ring.
+  @param  TransferRing           The created transfer ring.
+
+**/
+VOID
+CreateTransferRing (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  UINTN                 TrbNum,
+  OUT TRANSFER_RING         *TransferRing
+  )
+{
+  VOID                  *Buf;
+  LINK_TRB              *EndTrb;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  Buf = UsbHcAllocateMem (Xhc->MemPool, sizeof (TRB_TEMPLATE) * TrbNum);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, sizeof (TRB_TEMPLATE) * TrbNum);
+
+  TransferRing->RingSeg0     = Buf;
+  TransferRing->TrbNumber    = TrbNum;
+  TransferRing->RingEnqueue  = (TRB_TEMPLATE *) TransferRing->RingSeg0;
+  TransferRing->RingDequeue  = (TRB_TEMPLATE *) TransferRing->RingSeg0;
+  TransferRing->RingPCS      = 1;
+  //
+  // 4.9.2 Transfer Ring Management
+  // To form a ring (or circular queue) a Link TRB may be inserted at the end of a ring to
+  // point to the first TRB in the ring.
+  //
+  EndTrb        = (LINK_TRB *) ((UINTN)Buf + sizeof (TRB_TEMPLATE) * (TrbNum - 1));
+  EndTrb->Type  = TRB_TYPE_LINK;
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, sizeof (TRB_TEMPLATE) * TrbNum);
+  EndTrb->PtrLo = XHC_LOW_32BIT (PhyAddr);
+  EndTrb->PtrHi = XHC_HIGH_32BIT (PhyAddr);
+  //
+  // Toggle Cycle (TC). When set to '1', the xHC shall toggle its interpretation of the Cycle bit.
+  //
+  EndTrb->TC    = 1;
+  //
+  // Set Cycle bit as other TRB PCS init value
+  //
+  EndTrb->CycleBit = 0;
+}
+
+/**
+  Free XHCI event ring.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  EventRing           The event ring to be freed.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcFreeEventRing (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  EVENT_RING          *EventRing
+)
+{
+  if(EventRing->EventRingSeg0 == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Free EventRing Segment 0
+  //
+  UsbHcFreeMem (Xhc->MemPool, EventRing->EventRingSeg0, sizeof (TRB_TEMPLATE) * EVENT_RING_TRB_NUMBER);
+
+  //
+  // Free ESRT table
+  //
+  UsbHcFreeMem (Xhc->MemPool, EventRing->ERSTBase, sizeof (EVENT_RING_SEG_TABLE_ENTRY) * ERST_NUMBER);
+  return EFI_SUCCESS;
+}
+
+/**
+  Free the resouce allocated at initializing schedule.
+
+  @param  Xhc        The XHCI Instance.
+
+**/
+VOID
+XhcFreeSched (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  UINT32                  Index;
+  UINT64                  *ScratchEntry;
+  
+  if (Xhc->ScratchBuf != NULL) {
+    ScratchEntry = Xhc->ScratchEntry;
+    for (Index = 0; Index < Xhc->MaxScratchpadBufs; Index++) {
+      //
+      // Free Scratchpad Buffers
+      //
+      UsbHcFreeAlignedPages (Xhc->PciIo, (VOID*)(UINTN)ScratchEntry[Index], EFI_SIZE_TO_PAGES (Xhc->PageSize), (VOID *) Xhc->ScratchEntryMap[Index]);
+    }
+    //
+    // Free Scratchpad Buffer Array
+    //
+    UsbHcFreeAlignedPages (Xhc->PciIo, Xhc->ScratchBuf, EFI_SIZE_TO_PAGES (Xhc->MaxScratchpadBufs * sizeof (UINT64)), Xhc->ScratchMap);
+    FreePool (Xhc->ScratchEntryMap);
+    FreePool (Xhc->ScratchEntry);
+  }
+
+  if (Xhc->CmdRing.RingSeg0 != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->CmdRing.RingSeg0, sizeof (TRB_TEMPLATE) * CMD_RING_TRB_NUMBER);
+    Xhc->CmdRing.RingSeg0 = NULL;
+  }
+  
+  XhcFreeEventRing (Xhc,&Xhc->EventRing);
+
+  if (Xhc->DCBAA != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->DCBAA, (Xhc->MaxSlotsEn + 1) * sizeof(UINT64));
+    Xhc->DCBAA = NULL;
+  }
+  
+  //
+  // Free memory pool at last
+  //
+  if (Xhc->MemPool != NULL) {
+    UsbHcFreeMemPool (Xhc->MemPool);
+    Xhc->MemPool = NULL;
+  }
+}
+
+/**
+  Check if the Trb is a transaction of the URBs in XHCI's asynchronous transfer list.
+
+  @param Xhc    The XHCI Instance.
+  @param Trb    The TRB to be checked.
+  @param Urb    The pointer to the matched Urb.
+
+  @retval TRUE  The Trb is matched with a transaction of the URBs in the async list.
+  @retval FALSE The Trb is not matched with any URBs in the async list.
+
+**/
+BOOLEAN
+IsAsyncIntTrb (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  TRB_TEMPLATE        *Trb,
+  OUT URB                 **Urb
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  TRB_TEMPLATE            *CheckedTrb;
+  URB                     *CheckedUrb;
+  UINTN                   Index;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Xhc->AsyncIntTransfers) {
+    CheckedUrb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+    CheckedTrb = CheckedUrb->TrbStart;
+    for (Index = 0; Index < CheckedUrb->TrbNum; Index++) {
+      if (Trb == CheckedTrb) {
+        *Urb = CheckedUrb;
+        return TRUE;
+      }
+      CheckedTrb++;
+      if ((UINTN)CheckedTrb >= ((UINTN) CheckedUrb->Ring->RingSeg0 + sizeof (TRB_TEMPLATE) * CheckedUrb->Ring->TrbNumber)) {
+        CheckedTrb = (TRB_TEMPLATE*) CheckedUrb->Ring->RingSeg0;
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Check if the Trb is a transaction of the URB.
+
+  @param Trb    The TRB to be checked
+  @param Urb    The transfer ring to be checked.
+
+  @retval TRUE  It is a transaction of the URB.
+  @retval FALSE It is not any transaction of the URB.
+
+**/
+BOOLEAN
+IsTransferRingTrb (
+  IN  TRB_TEMPLATE        *Trb,
+  IN  URB                 *Urb
+  )
+{
+  TRB_TEMPLATE  *CheckedTrb;
+  UINTN         Index;
+
+  CheckedTrb = Urb->Ring->RingSeg0;
+
+  ASSERT (Urb->Ring->TrbNumber == CMD_RING_TRB_NUMBER || Urb->Ring->TrbNumber == TR_RING_TRB_NUMBER);
+
+  for (Index = 0; Index < Urb->Ring->TrbNumber; Index++) {
+    if (Trb == CheckedTrb) {
+      return TRUE;
+    }
+    CheckedTrb++;
+  }
+
+  return FALSE;
+}
+
+/**
+  Check the URB's execution result and update the URB's
+  result accordingly.
+
+  @param  Xhc             The XHCI Instance.
+  @param  Urb             The URB to check result.
+
+  @return Whether the result of URB transfer is finialized.
+
+**/
+BOOLEAN
+XhcCheckUrbResult (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  EVT_TRB_TRANSFER        *EvtTrb;
+  TRB_TEMPLATE            *TRBPtr;
+  UINTN                   Index;
+  UINT8                   TRBType;
+  EFI_STATUS              Status;
+  URB                     *AsyncUrb;
+  URB                     *CheckedUrb;
+  UINT64                  XhcDequeue;
+  UINT32                  High;
+  UINT32                  Low;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+
+  ASSERT ((Xhc != NULL) && (Urb != NULL));
+
+  Status   = EFI_SUCCESS;
+  AsyncUrb = NULL;
+
+  if (Urb->Finished) {
+    goto EXIT;
+  }
+
+  EvtTrb = NULL;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    Urb->Result |= EFI_USB_ERR_SYSTEM;
+    goto EXIT;
+  }
+
+  //
+  // Traverse the event ring to find out all new events from the previous check.
+  //
+  XhcSyncEventRing (Xhc, &Xhc->EventRing);
+  for (Index = 0; Index < Xhc->EventRing.TrbNumber; Index++) {
+    Status = XhcCheckNewEvent (Xhc, &Xhc->EventRing, ((TRB_TEMPLATE **)&EvtTrb));
+    if (Status == EFI_NOT_READY) {
+      //
+      // All new events are handled, return directly.
+      //
+      goto EXIT;
+    }
+
+    //
+    // Only handle COMMAND_COMPLETETION_EVENT and TRANSFER_EVENT.
+    //
+    if ((EvtTrb->Type != TRB_TYPE_COMMAND_COMPLT_EVENT) && (EvtTrb->Type != TRB_TYPE_TRANS_EVENT)) {
+      continue;
+    }
+    
+    //
+    // Need convert pci device address to host address
+    //
+    PhyAddr = (EFI_PHYSICAL_ADDRESS)(EvtTrb->TRBPtrLo | LShiftU64 ((UINT64) EvtTrb->TRBPtrHi, 32));
+    TRBPtr = (TRB_TEMPLATE *)(UINTN) UsbHcGetHostAddrForPciAddr (Xhc->MemPool, (VOID *)(UINTN) PhyAddr, sizeof (TRB_TEMPLATE));
+
+    //
+    // Update the status of Urb according to the finished event regardless of whether
+    // the urb is current checked one or in the XHCI's async transfer list.
+    // This way is used to avoid that those completed async transfer events don't get
+    // handled in time and are flushed by newer coming events.
+    //
+    if (IsTransferRingTrb (TRBPtr, Urb)) {
+      CheckedUrb = Urb;
+    } else if (IsAsyncIntTrb (Xhc, TRBPtr, &AsyncUrb)) {    
+      CheckedUrb = AsyncUrb;
+    } else {
+      continue;
+    }
+  
+    switch (EvtTrb->Completecode) {
+      case TRB_COMPLETION_STALL_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_STALL;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: STALL_ERROR! Completecode = %x\n",EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_BABBLE_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_BABBLE;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: BABBLE_ERROR! Completecode = %x\n",EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_DATA_BUFFER_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_BUFFER;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: ERR_BUFFER! Completecode = %x\n",EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_USB_TRANSACTION_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_TIMEOUT;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: TRANSACTION_ERROR! Completecode = %x\n",EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_SHORT_PACKET:
+      case TRB_COMPLETION_SUCCESS:
+        if (EvtTrb->Completecode == TRB_COMPLETION_SHORT_PACKET) {
+          DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: short packet happens!\n"));
+        }
+
+        TRBType = (UINT8) (TRBPtr->Type);
+        if ((TRBType == TRB_TYPE_DATA_STAGE) ||
+            (TRBType == TRB_TYPE_NORMAL) ||
+            (TRBType == TRB_TYPE_ISOCH)) {
+          CheckedUrb->Completed += (((TRANSFER_TRB_NORMAL*)TRBPtr)->Length - EvtTrb->Length);
+        }
+
+        break;
+
+      default:
+        DEBUG ((EFI_D_ERROR, "Transfer Default Error Occur! Completecode = 0x%x!\n",EvtTrb->Completecode));
+        CheckedUrb->Result  |= EFI_USB_ERR_TIMEOUT;
+        CheckedUrb->Finished = TRUE;
+        goto EXIT;
+    }
+
+    //
+    // Only check first and end Trb event address
+    //
+    if (TRBPtr == CheckedUrb->TrbStart) {
+      CheckedUrb->StartDone = TRUE;
+    }
+
+    if (TRBPtr == CheckedUrb->TrbEnd) {
+      CheckedUrb->EndDone = TRUE;
+    }
+
+    if (CheckedUrb->StartDone && CheckedUrb->EndDone) {
+      CheckedUrb->Finished = TRUE;
+      CheckedUrb->EvtTrb   = (TRB_TEMPLATE *)EvtTrb;
+    }
+  }
+
+EXIT:
+
+  //
+  // Advance event ring to last available entry
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  Low  = XhcReadRuntimeReg (Xhc, XHC_ERDP_OFFSET);
+  High = XhcReadRuntimeReg (Xhc, XHC_ERDP_OFFSET + 4);
+  XhcDequeue = (UINT64)(LShiftU64((UINT64)High, 32) | Low);
+
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->EventRing.EventRingDequeue, sizeof (TRB_TEMPLATE));
+
+  if ((XhcDequeue & (~0x0F)) != (PhyAddr & (~0x0F))) {
+    //
+    // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+    // So divide it to two 32-bytes width register access.
+    //
+    XhcWriteRuntimeReg (Xhc, XHC_ERDP_OFFSET, XHC_LOW_32BIT (PhyAddr) | BIT3);
+    XhcWriteRuntimeReg (Xhc, XHC_ERDP_OFFSET + 4, XHC_HIGH_32BIT (PhyAddr));
+  }
+
+  return Urb->Finished;
+}
+
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Xhc               The XHCI Instance.
+  @param  CmdTransfer       The executed URB is for cmd transfer or not.
+  @param  Urb               The URB to execute.
+  @param  Timeout           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+XhcExecTransfer (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  BOOLEAN             CmdTransfer,
+  IN  URB                 *Urb,
+  IN  UINTN               Timeout
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   Index;
+  UINT64                  Loop;
+  UINT8                   SlotId;
+  UINT8                   Dci;
+  BOOLEAN                 Finished;
+
+  if (CmdTransfer) {
+    SlotId = 0;
+    Dci    = 0;
+  } else {
+    SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+    if (SlotId == 0) {
+      return EFI_DEVICE_ERROR;
+    }
+    Dci  = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+    ASSERT (Dci < 32);
+  }
+
+  Status = EFI_SUCCESS;
+  Loop   = Timeout * XHC_1_MILLISECOND;
+  if (Timeout == 0) {
+    Loop = 0xFFFFFFFF;
+  }
+
+  XhcRingDoorBell (Xhc, SlotId, Dci);
+
+  for (Index = 0; Index < Loop; Index++) {
+    Finished = XhcCheckUrbResult (Xhc, Urb);
+    if (Finished) {
+      break;
+    }
+    gBS->Stall (XHC_1_MICROSECOND);
+  }
+
+  if (Index == Loop) {
+    Urb->Result = EFI_USB_ERR_TIMEOUT;
+    Status      = EFI_TIMEOUT;
+  } else if (Urb->Result != EFI_USB_NOERROR) {
+    Status      = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  BusAddr               The logical device address assigned by UsbBus driver.
+  @param  EpNum                 The endpoint of the target.
+
+  @retval EFI_SUCCESS           An asynchronous transfer is removed.
+  @retval EFI_NOT_FOUND         No transfer for the device is found.
+
+**/
+EFI_STATUS
+XhciDelAsyncIntTransfer (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT8               BusAddr,
+  IN  UINT8               EpNum
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  URB                     *Urb;
+  EFI_USB_DATA_DIRECTION  Direction;
+
+  Direction = ((EpNum & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut;
+  EpNum    &= 0x0F;
+
+  Urb = NULL;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Xhc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+    if ((Urb->Ep.BusAddr == BusAddr) &&
+        (Urb->Ep.EpAddr == EpNum) &&
+        (Urb->Ep.Direction == Direction)) {
+      RemoveEntryList (&Urb->UrbList);
+      FreePool (Urb->Data);
+      XhcFreeUrb (Xhc, Urb);
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Remove all the asynchronous interrutp transfers.
+
+  @param  Xhc    The XHCI Instance.
+
+**/
+VOID
+XhciDelAllAsyncIntTransfers (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  URB                     *Urb;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Xhc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+    RemoveEntryList (&Urb->UrbList);
+    FreePool (Urb->Data);
+    XhcFreeUrb (Xhc, Urb);
+  }
+}
+
+/**
+  Update the queue head for next round of asynchronous transfer
+
+  @param  Xhc     The XHCI Instance.
+  @param  Urb     The URB to update
+
+**/
+VOID
+XhcUpdateAsyncRequest (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN URB                      *Urb
+  )
+{
+  EFI_STATUS    Status;
+
+  if (Urb->Result == EFI_USB_NOERROR) {
+    Status = XhcCreateTransferTrb (Xhc, Urb);
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+    Status = RingIntTransferDoorBell (Xhc, Urb);
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+  }
+}
+
+/**
+  Flush data from PCI controller specific address to mapped system
+  memory address.
+
+  @param  Xhc                The XHCI device.
+  @param  Urb                The URB to unmap.
+
+  @retval EFI_SUCCESS        Success to flush data to mapped system memory.
+  @retval EFI_DEVICE_ERROR   Fail to flush data to mapped system memory.
+
+**/
+EFI_STATUS
+XhcFlushAsyncIntMap (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  EFI_PCI_IO_PROTOCOL_OPERATION MapOp;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  UINTN                         Len;
+  VOID                          *Map;
+
+  PciIo = Xhc->PciIo;
+  Len   = Urb->DataLen;
+
+  if (Urb->Ep.Direction == EfiUsbDataIn) {
+    MapOp = EfiPciIoOperationBusMasterWrite;
+  } else {
+    MapOp = EfiPciIoOperationBusMasterRead;
+  }
+
+  if (Urb->DataMap != NULL) {
+    Status = PciIo->Unmap (PciIo, Urb->DataMap);
+    if (EFI_ERROR (Status)) {
+      goto ON_ERROR;
+    }
+  }
+
+  Urb->DataMap = NULL;
+
+  Status = PciIo->Map (PciIo, MapOp, Urb->Data, &Len, &PhyAddr, &Map);
+  if (EFI_ERROR (Status) || (Len != Urb->DataLen)) {
+    goto ON_ERROR;
+  }
+
+  Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+  Urb->DataMap  = Map;
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  return EFI_DEVICE_ERROR;
+}
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                 Interrupt event.
+  @param  Context               Pointer to USB_XHCI_INSTANCE.
+
+**/
+VOID
+EFIAPI
+XhcMonitorAsyncRequests (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  UINT8                   *ProcBuf;
+  URB                     *Urb;
+  UINT8                   SlotId;
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = (USB_XHCI_INSTANCE*) Context;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Xhc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+
+    //
+    // Make sure that the device is available before every check.
+    //
+    SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+    if (SlotId == 0) {
+      continue;
+    }
+
+    //
+    // Check the result of URB execution. If it is still
+    // active, check the next one.
+    //
+    XhcCheckUrbResult (Xhc, Urb);
+
+    if (!Urb->Finished) {
+      continue;
+    }
+
+    //
+    // Flush any PCI posted write transactions from a PCI host
+    // bridge to system memory.
+    //
+    Status = XhcFlushAsyncIntMap (Xhc, Urb);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcMonitorAsyncRequests: Fail to Flush AsyncInt Mapped Memeory\n"));
+    }
+
+    //
+    // Allocate a buffer then copy the transferred data for user.
+    // If failed to allocate the buffer, update the URB for next
+    // round of transfer. Ignore the data of this round.
+    //
+    ProcBuf = NULL;
+    if (Urb->Result == EFI_USB_NOERROR) {
+      ASSERT (Urb->Completed <= Urb->DataLen);
+
+      ProcBuf = AllocateZeroPool (Urb->Completed);
+
+      if (ProcBuf == NULL) {
+        XhcUpdateAsyncRequest (Xhc, Urb);
+        continue;
+      }
+
+      CopyMem (ProcBuf, Urb->Data, Urb->Completed);
+    }
+
+    //
+    // Leave error recovery to its related device driver. A
+    // common case of the error recovery is to re-submit the
+    // interrupt transfer which is linked to the head of the
+    // list. This function scans from head to tail. So the
+    // re-submitted interrupt transfer's callback function
+    // will not be called again in this round. Don't touch this
+    // URB after the callback, it may have been removed by the
+    // callback.
+    //
+    if (Urb->Callback != NULL) {
+      //
+      // Restore the old TPL, USB bus maybe connect device in
+      // his callback. Some drivers may has a lower TPL restriction.
+      //
+      gBS->RestoreTPL (OldTpl);
+      (Urb->Callback) (ProcBuf, Urb->Completed, Urb->Context, Urb->Result);
+      OldTpl = gBS->RaiseTPL (XHC_TPL);
+    }
+
+    if (ProcBuf != NULL) {
+      gBS->FreePool (ProcBuf);
+    }
+
+    XhcUpdateAsyncRequest (Xhc, Urb);
+  }
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Monitor the port status change. Enable/Disable device slot if there is a device attached/detached.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  ParentRouteChart      The route string pointed to the parent device if it exists.
+  @param  Port                  The port to be polled.
+  @param  PortState             The port state.
+
+  @retval EFI_SUCCESS           Successfully enable/disable device slot according to port state.
+  @retval Others                Should not appear.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPollPortStatusChange (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  USB_DEV_ROUTE         ParentRouteChart,
+  IN  UINT8                 Port,
+  IN  EFI_USB_PORT_STATUS   *PortState
+  )
+{
+  EFI_STATUS        Status;
+  UINT8             Speed;
+  UINT8             SlotId;
+  USB_DEV_ROUTE     RouteChart;
+
+  Status = EFI_SUCCESS;
+
+  if ((PortState->PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (ParentRouteChart.Dword == 0) {
+    RouteChart.Route.RouteString = 0;
+    RouteChart.Route.RootPortNum = Port + 1;
+    RouteChart.Route.TierNum     = 1;
+  } else {
+    if(Port < 14) {
+      RouteChart.Route.RouteString = ParentRouteChart.Route.RouteString | (Port << (4 * (ParentRouteChart.Route.TierNum - 1)));
+    } else {
+      RouteChart.Route.RouteString = ParentRouteChart.Route.RouteString | (15 << (4 * (ParentRouteChart.Route.TierNum - 1)));
+    }
+    RouteChart.Route.RootPortNum   = ParentRouteChart.Route.RootPortNum;
+    RouteChart.Route.TierNum       = ParentRouteChart.Route.TierNum + 1;
+  }
+
+  SlotId = XhcRouteStringToSlotId (Xhc, RouteChart);
+  if (SlotId != 0) {
+    if (Xhc->HcCParams.Data.Csz == 0) {
+      Status = XhcDisableSlotCmd (Xhc, SlotId);
+    } else {
+      Status = XhcDisableSlotCmd64 (Xhc, SlotId);
+    }
+  }
+
+  if (((PortState->PortStatus & USB_PORT_STAT_ENABLE) != 0) &&
+      ((PortState->PortStatus & USB_PORT_STAT_CONNECTION) != 0)) {
+    //
+    // Has a device attached, Identify device speed after port is enabled.
+    //
+    Speed = EFI_USB_SPEED_FULL;
+    if ((PortState->PortStatus & USB_PORT_STAT_LOW_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_LOW;
+    } else if ((PortState->PortStatus & USB_PORT_STAT_HIGH_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_HIGH;
+    } else if ((PortState->PortStatus & USB_PORT_STAT_SUPER_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_SUPER;
+    }
+    //
+    // Execute Enable_Slot cmd for attached device, initialize device context and assign device address.
+    //
+    SlotId = XhcRouteStringToSlotId (Xhc, RouteChart);
+    if ((SlotId == 0) && ((PortState->PortChangeStatus & USB_PORT_STAT_C_RESET) != 0)) {
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcInitializeDeviceSlot (Xhc, ParentRouteChart, Port, RouteChart, Speed);
+      } else {
+        Status = XhcInitializeDeviceSlot64 (Xhc, ParentRouteChart, Port, RouteChart, Speed);
+      }
+    }
+  } 
+
+  return Status;
+}
+
+
+/**
+  Calculate the device context index by endpoint address and direction.
+
+  @param  EpAddr              The target endpoint number.
+  @param  Direction           The direction of the target endpoint.
+
+  @return The device context index of endpoint.
+
+**/
+UINT8
+XhcEndpointToDci (
+  IN  UINT8                   EpAddr,
+  IN  UINT8                   Direction
+  )
+{
+  UINT8 Index;
+
+  if (EpAddr == 0) {
+    return 1;
+  } else {
+    Index = (UINT8) (2 * EpAddr);
+    if (Direction == EfiUsbDataIn) {
+      Index += 1;
+    }
+    return Index;
+  }
+}
+
+/**
+  Find out the actual device address according to the requested device address from UsbBus.
+
+  @param  Xhc             The XHCI Instance.
+  @param  BusDevAddr      The requested device address by UsbBus upper driver.
+
+  @return The actual device address assigned to the device.
+
+**/
+UINT8
+EFIAPI
+XhcBusDevAddrToSlotId (
+  IN  USB_XHCI_INSTANCE  *Xhc,
+  IN  UINT8              BusDevAddr
+  )
+{
+  UINT8  Index;
+
+  for (Index = 0; Index < 255; Index++) {
+    if (Xhc->UsbDevContext[Index + 1].Enabled &&
+        (Xhc->UsbDevContext[Index + 1].SlotId != 0) &&
+        (Xhc->UsbDevContext[Index + 1].BusDevAddr == BusDevAddr)) {
+      break;
+    }
+  }
+
+  if (Index == 255) {
+    return 0;
+  }
+
+  return Xhc->UsbDevContext[Index + 1].SlotId;
+}
+
+/**
+  Find out the slot id according to the device's route string.
+
+  @param  Xhc             The XHCI Instance.
+  @param  RouteString     The route string described the device location.
+
+  @return The slot id used by the device.
+
+**/
+UINT8
+EFIAPI
+XhcRouteStringToSlotId (
+  IN  USB_XHCI_INSTANCE  *Xhc,
+  IN  USB_DEV_ROUTE      RouteString
+  )
+{
+  UINT8  Index;
+
+  for (Index = 0; Index < 255; Index++) {
+    if (Xhc->UsbDevContext[Index + 1].Enabled &&
+        (Xhc->UsbDevContext[Index + 1].SlotId != 0) &&
+        (Xhc->UsbDevContext[Index + 1].RouteString.Dword == RouteString.Dword)) {
+      break;
+    }
+  }
+
+  if (Index == 255) {
+    return 0;
+  }
+
+  return Xhc->UsbDevContext[Index + 1].SlotId;
+}
+
+/**
+  Synchronize the specified event ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc         The XHCI Instance.
+  @param  EvtRing     The event ring to sync.
+
+  @retval EFI_SUCCESS The event ring is synchronized successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncEventRing (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN EVENT_RING           *EvtRing
+  )
+{
+  UINTN               Index;
+  TRB_TEMPLATE        *EvtTrb1;
+
+  ASSERT (EvtRing != NULL);
+
+  //
+  // Calculate the EventRingEnqueue and EventRingCCS.
+  // Note: only support single Segment
+  //
+  EvtTrb1 = EvtRing->EventRingDequeue;
+
+  for (Index = 0; Index < EvtRing->TrbNumber; Index++) {
+    if (EvtTrb1->CycleBit != EvtRing->EventRingCCS) {
+      break;
+    }
+
+    EvtTrb1++;
+
+    if ((UINTN)EvtTrb1 >= ((UINTN) EvtRing->EventRingSeg0 + sizeof (TRB_TEMPLATE) * EvtRing->TrbNumber)) {
+      EvtTrb1 = EvtRing->EventRingSeg0;
+      EvtRing->EventRingCCS = (EvtRing->EventRingCCS) ? 0 : 1;
+    }
+  }
+
+  if (Index < EvtRing->TrbNumber) {
+    EvtRing->EventRingEnqueue = EvtTrb1;
+  } else {
+    ASSERT (FALSE);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Synchronize the specified transfer ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc         The XHCI Instance.
+  @param  TrsRing     The transfer ring to sync.
+
+  @retval EFI_SUCCESS The transfer ring is synchronized successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncTrsRing (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN TRANSFER_RING        *TrsRing
+  )
+{
+  UINTN               Index;
+  TRB_TEMPLATE        *TrsTrb;
+
+  ASSERT (TrsRing != NULL);
+  //
+  // Calculate the latest RingEnqueue and RingPCS
+  //
+  TrsTrb = TrsRing->RingEnqueue;
+  ASSERT (TrsTrb != NULL);
+
+  for (Index = 0; Index < TrsRing->TrbNumber; Index++) {
+    if (TrsTrb->CycleBit != (TrsRing->RingPCS & BIT0)) {
+      break;
+    }
+    TrsTrb++;
+    if ((UINT8) TrsTrb->Type == TRB_TYPE_LINK) {
+      ASSERT (((LINK_TRB*)TrsTrb)->TC != 0);
+      //
+      // set cycle bit in Link TRB as normal
+      //
+      ((LINK_TRB*)TrsTrb)->CycleBit = TrsRing->RingPCS & BIT0;
+      //
+      // Toggle PCS maintained by software
+      //
+      TrsRing->RingPCS = (TrsRing->RingPCS & BIT0) ? 0 : 1;
+      TrsTrb = (TRB_TEMPLATE *) TrsRing->RingSeg0;  // Use host address
+    }
+  }
+
+  ASSERT (Index != TrsRing->TrbNumber);
+
+  if (TrsTrb != TrsRing->RingEnqueue) {
+    TrsRing->RingEnqueue = TrsTrb;
+  }
+
+  //
+  // Clear the Trb context for enqueue, but reserve the PCS bit
+  //
+  TrsTrb->Parameter1 = 0;
+  TrsTrb->Parameter2 = 0;
+  TrsTrb->Status     = 0;
+  TrsTrb->RsvdZ1     = 0;
+  TrsTrb->Type       = 0;
+  TrsTrb->Control    = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if there is a new generated event.
+
+  @param  Xhc           The XHCI Instance.
+  @param  EvtRing       The event ring to check.
+  @param  NewEvtTrb     The new event TRB found.
+
+  @retval EFI_SUCCESS   Found a new event TRB at the event ring.
+  @retval EFI_NOT_READY The event ring has no new event.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcCheckNewEvent (
+  IN  USB_XHCI_INSTANCE       *Xhc,
+  IN  EVENT_RING              *EvtRing,
+  OUT TRB_TEMPLATE            **NewEvtTrb
+  )
+{
+  ASSERT (EvtRing != NULL);
+
+  *NewEvtTrb = EvtRing->EventRingDequeue;
+
+  if (EvtRing->EventRingDequeue == EvtRing->EventRingEnqueue) {
+    return EFI_NOT_READY;
+  }
+
+  EvtRing->EventRingDequeue++;
+  //
+  // If the dequeue pointer is beyond the ring, then roll-back it to the begining of the ring.
+  //
+  if ((UINTN)EvtRing->EventRingDequeue >= ((UINTN) EvtRing->EventRingSeg0 + sizeof (TRB_TEMPLATE) * EvtRing->TrbNumber)) {
+    EvtRing->EventRingDequeue = EvtRing->EventRingSeg0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Successfully ring the door bell.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcRingDoorBell (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT8                SlotId,
+  IN UINT8                Dci
+  )
+{
+  if (SlotId == 0) {
+    XhcWriteDoorBellReg (Xhc, 0, 0);
+  } else {
+    XhcWriteDoorBellReg (Xhc, SlotId * sizeof (UINT32), Dci);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed through URB.
+
+  @param  Xhc           The XHCI Instance.
+  @param  Urb           The URB to be rung.
+
+  @retval EFI_SUCCESS   Successfully ring the door bell.
+
+**/
+EFI_STATUS
+RingIntTransferDoorBell (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  UINT8                SlotId;
+  UINT8                Dci;
+
+  SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  Dci    = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  XhcRingDoorBell (Xhc, SlotId, Dci);
+  return EFI_SUCCESS;
+}
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  ParentRouteChart    The route string pointed to the parent device.
+  @param  ParentPort          The port at which the device is located.
+  @param  RouteChart          The route string pointed to the device.
+  @param  DeviceSpeed         The device speed.
+
+  @retval EFI_SUCCESS   Successfully assign a slot to the device and assign an address to it.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcInitializeDeviceSlot (
+  IN  USB_XHCI_INSTANCE         *Xhc,
+  IN  USB_DEV_ROUTE             ParentRouteChart,
+  IN  UINT16                    ParentPort,
+  IN  USB_DEV_ROUTE             RouteChart,
+  IN  UINT8                     DeviceSpeed
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT               *InputContext;
+  DEVICE_CONTEXT              *OutputContext;
+  TRANSFER_RING               *EndpointTransferRing;
+  CMD_TRB_ADDRESS_DEVICE      CmdTrbAddr;
+  UINT8                       DeviceAddress;
+  CMD_TRB_ENABLE_SLOT         CmdTrb;
+  UINT8                       SlotId;
+  UINT8                       ParentSlotId;
+  DEVICE_CONTEXT              *ParentDeviceContext;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ZeroMem (&CmdTrb, sizeof (CMD_TRB_ENABLE_SLOT));
+  CmdTrb.CycleBit = 1;
+  CmdTrb.Type     = TRB_TYPE_EN_SLOT;
+
+  Status = XhcCmdTransfer (
+              Xhc,
+              (TRB_TEMPLATE *) (UINTN) &CmdTrb,
+              XHC_GENERIC_TIMEOUT,
+              (TRB_TEMPLATE **) (UINTN) &EvtTrb
+              );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcInitializeDeviceSlot: Enable Slot Failed, Status = %r\n", Status));
+    return Status;
+  }
+  ASSERT (EvtTrb->SlotId <= Xhc->MaxSlotsEn);
+  DEBUG ((EFI_D_INFO, "Enable Slot Successfully, The Slot ID = 0x%x\n", EvtTrb->SlotId));
+  SlotId = (UINT8)EvtTrb->SlotId;
+  ASSERT (SlotId != 0);
+
+  ZeroMem (&Xhc->UsbDevContext[SlotId], sizeof (USB_DEV_CONTEXT));
+  Xhc->UsbDevContext[SlotId].Enabled                 = TRUE;
+  Xhc->UsbDevContext[SlotId].SlotId                  = SlotId;
+  Xhc->UsbDevContext[SlotId].RouteString.Dword       = RouteChart.Dword;
+  Xhc->UsbDevContext[SlotId].ParentRouteString.Dword = ParentRouteChart.Dword;
+
+  //
+  // 4.3.3 Device Slot Initialization
+  // 1) Allocate an Input Context data structure (6.2.5) and initialize all fields to '0'.
+  //
+  InputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (INPUT_CONTEXT));
+  ASSERT (InputContext != NULL);
+  ASSERT (((UINTN) InputContext & 0x3F) == 0);
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  Xhc->UsbDevContext[SlotId].InputContext = (VOID *) InputContext;
+
+  //
+  // 2) Initialize the Input Control Context (6.2.5.1) of the Input Context by setting the A0 and A1
+  //    flags to '1'. These flags indicate that the Slot Context and the Endpoint 0 Context of the Input
+  //    Context are affected by the command.
+  //
+  InputContext->InputControlContext.Dword2 |= (BIT0 | BIT1);
+
+  //
+  // 3) Initialize the Input Slot Context data structure
+  //
+  InputContext->Slot.RouteString    = RouteChart.Route.RouteString;
+  InputContext->Slot.Speed          = DeviceSpeed + 1;
+  InputContext->Slot.ContextEntries = 1;
+  InputContext->Slot.RootHubPortNum = RouteChart.Route.RootPortNum;
+
+  if (RouteChart.Route.RouteString) {
+    //
+    // The device is behind of hub device.
+    //
+    ParentSlotId = XhcRouteStringToSlotId(Xhc, ParentRouteChart);
+    ASSERT (ParentSlotId != 0);
+    //
+    //if the Full/Low device attached to a High Speed Hub, Init the TTPortNum and TTHubSlotId field of slot context
+    //
+    ParentDeviceContext = (DEVICE_CONTEXT *)Xhc->UsbDevContext[ParentSlotId].OutputContext;
+    if ((ParentDeviceContext->Slot.TTPortNum == 0) &&
+        (ParentDeviceContext->Slot.TTHubSlotId == 0)) {
+      if ((ParentDeviceContext->Slot.Speed == (EFI_USB_SPEED_HIGH + 1)) && (DeviceSpeed < EFI_USB_SPEED_HIGH)) {
+        //
+        // Full/Low device attached to High speed hub port that isolates the high speed signaling
+        // environment from Full/Low speed signaling environment for a device
+        //
+        InputContext->Slot.TTPortNum   = ParentPort;
+        InputContext->Slot.TTHubSlotId = ParentSlotId;
+      }
+    } else {
+      //
+      // Inherit the TT parameters from parent device.
+      //
+      InputContext->Slot.TTPortNum   = ParentDeviceContext->Slot.TTPortNum;
+      InputContext->Slot.TTHubSlotId = ParentDeviceContext->Slot.TTHubSlotId;
+      //
+      // If the device is a High speed device then down the speed to be the same as its parent Hub
+      //
+      if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+        InputContext->Slot.Speed = ParentDeviceContext->Slot.Speed;
+      }
+    }
+  }
+
+  //
+  // 4) Allocate and initialize the Transfer Ring for the Default Control Endpoint.
+  //
+  EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+  Xhc->UsbDevContext[SlotId].EndpointTransferRing[0] = EndpointTransferRing;
+  CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[0]);
+  //
+  // 5) Initialize the Input default control Endpoint 0 Context (6.2.3).
+  //
+  InputContext->EP[0].EPType = ED_CONTROL_BIDIR;
+
+  if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    InputContext->EP[0].MaxPacketSize = 512;
+  } else if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    InputContext->EP[0].MaxPacketSize = 64;
+  } else {
+    InputContext->EP[0].MaxPacketSize = 8;
+  }
+  //
+  // Initial value of Average TRB Length for Control endpoints would be 8B, Interrupt endpoints
+  // 1KB, and Bulk and Isoch endpoints 3KB.
+  //
+  InputContext->EP[0].AverageTRBLength = 8;
+  InputContext->EP[0].MaxBurstSize     = 0;
+  InputContext->EP[0].Interval         = 0;
+  InputContext->EP[0].MaxPStreams      = 0;
+  InputContext->EP[0].Mult             = 0;
+  InputContext->EP[0].CErr             = 3;
+
+  //
+  // Init the DCS(dequeue cycle state) as the transfer ring's CCS
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (
+              Xhc->MemPool,
+              ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[0])->RingSeg0,
+              sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+              );
+  InputContext->EP[0].PtrLo = XHC_LOW_32BIT (PhyAddr) | BIT0;
+  InputContext->EP[0].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // 6) Allocate the Output Device Context data structure (6.2.1) and initialize it to '0'.
+  //
+  OutputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (DEVICE_CONTEXT));
+  ASSERT (OutputContext != NULL);
+  ASSERT (((UINTN) OutputContext & 0x3F) == 0);
+  ZeroMem (OutputContext, sizeof (DEVICE_CONTEXT));
+
+  Xhc->UsbDevContext[SlotId].OutputContext = OutputContext;
+  //
+  // 7) Load the appropriate (Device Slot ID) entry in the Device Context Base Address Array (5.4.6) with
+  //    a pointer to the Output Device Context data structure (6.2.1).
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, OutputContext, sizeof (DEVICE_CONTEXT));
+  //
+  // Fill DCBAA with PCI device address
+  //
+  Xhc->DCBAA[SlotId] = (UINT64) (UINTN) PhyAddr;
+
+  //
+  // 8) Issue an Address Device Command for the Device Slot, where the command points to the Input
+  //    Context data structure described above.
+  //
+  ZeroMem (&CmdTrbAddr, sizeof (CmdTrbAddr));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbAddr.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbAddr.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbAddr.CycleBit = 1;
+  CmdTrbAddr.Type     = TRB_TYPE_ADDRESS_DEV;
+  CmdTrbAddr.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbAddr,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (!EFI_ERROR (Status)) {
+    DeviceAddress = (UINT8) ((DEVICE_CONTEXT *) OutputContext)->Slot.DeviceAddress;
+    DEBUG ((EFI_D_INFO, "    Address %d assigned successfully\n", DeviceAddress));
+    Xhc->UsbDevContext[SlotId].XhciDevAddr = DeviceAddress;
+  }
+
+  return Status;
+}
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  ParentRouteChart    The route string pointed to the parent device.
+  @param  ParentPort          The port at which the device is located.
+  @param  RouteChart          The route string pointed to the device.
+  @param  DeviceSpeed         The device speed.
+
+  @retval EFI_SUCCESS   Successfully assign a slot to the device and assign an address to it.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcInitializeDeviceSlot64 (
+  IN  USB_XHCI_INSTANCE         *Xhc,
+  IN  USB_DEV_ROUTE             ParentRouteChart,
+  IN  UINT16                    ParentPort,
+  IN  USB_DEV_ROUTE             RouteChart,
+  IN  UINT8                     DeviceSpeed
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT_64            *InputContext;
+  DEVICE_CONTEXT_64           *OutputContext;
+  TRANSFER_RING               *EndpointTransferRing;
+  CMD_TRB_ADDRESS_DEVICE      CmdTrbAddr;
+  UINT8                       DeviceAddress;
+  CMD_TRB_ENABLE_SLOT         CmdTrb;
+  UINT8                       SlotId;
+  UINT8                       ParentSlotId;
+  DEVICE_CONTEXT_64           *ParentDeviceContext;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ZeroMem (&CmdTrb, sizeof (CMD_TRB_ENABLE_SLOT));
+  CmdTrb.CycleBit = 1;
+  CmdTrb.Type     = TRB_TYPE_EN_SLOT;
+
+  Status = XhcCmdTransfer (
+              Xhc,
+              (TRB_TEMPLATE *) (UINTN) &CmdTrb,
+              XHC_GENERIC_TIMEOUT,
+              (TRB_TEMPLATE **) (UINTN) &EvtTrb
+              );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcInitializeDeviceSlot64: Enable Slot Failed, Status = %r\n", Status));
+    return Status;
+  }
+  ASSERT (EvtTrb->SlotId <= Xhc->MaxSlotsEn);
+  DEBUG ((EFI_D_INFO, "Enable Slot Successfully, The Slot ID = 0x%x\n", EvtTrb->SlotId));
+  SlotId = (UINT8)EvtTrb->SlotId;
+  ASSERT (SlotId != 0);
+
+  ZeroMem (&Xhc->UsbDevContext[SlotId], sizeof (USB_DEV_CONTEXT));
+  Xhc->UsbDevContext[SlotId].Enabled                 = TRUE;
+  Xhc->UsbDevContext[SlotId].SlotId                  = SlotId;
+  Xhc->UsbDevContext[SlotId].RouteString.Dword       = RouteChart.Dword;
+  Xhc->UsbDevContext[SlotId].ParentRouteString.Dword = ParentRouteChart.Dword;
+
+  //
+  // 4.3.3 Device Slot Initialization
+  // 1) Allocate an Input Context data structure (6.2.5) and initialize all fields to '0'.
+  //
+  InputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (INPUT_CONTEXT_64));
+  ASSERT (InputContext != NULL);
+  ASSERT (((UINTN) InputContext & 0x3F) == 0);
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  Xhc->UsbDevContext[SlotId].InputContext = (VOID *) InputContext;
+
+  //
+  // 2) Initialize the Input Control Context (6.2.5.1) of the Input Context by setting the A0 and A1
+  //    flags to '1'. These flags indicate that the Slot Context and the Endpoint 0 Context of the Input
+  //    Context are affected by the command.
+  //
+  InputContext->InputControlContext.Dword2 |= (BIT0 | BIT1);
+
+  //
+  // 3) Initialize the Input Slot Context data structure
+  //
+  InputContext->Slot.RouteString    = RouteChart.Route.RouteString;
+  InputContext->Slot.Speed          = DeviceSpeed + 1;
+  InputContext->Slot.ContextEntries = 1;
+  InputContext->Slot.RootHubPortNum = RouteChart.Route.RootPortNum;
+
+  if (RouteChart.Route.RouteString) {
+    //
+    // The device is behind of hub device.
+    //
+    ParentSlotId = XhcRouteStringToSlotId(Xhc, ParentRouteChart);
+    ASSERT (ParentSlotId != 0);
+    //
+    //if the Full/Low device attached to a High Speed Hub, Init the TTPortNum and TTHubSlotId field of slot context
+    //
+    ParentDeviceContext = (DEVICE_CONTEXT_64 *)Xhc->UsbDevContext[ParentSlotId].OutputContext;
+    if ((ParentDeviceContext->Slot.TTPortNum == 0) &&
+        (ParentDeviceContext->Slot.TTHubSlotId == 0)) {
+      if ((ParentDeviceContext->Slot.Speed == (EFI_USB_SPEED_HIGH + 1)) && (DeviceSpeed < EFI_USB_SPEED_HIGH)) {
+        //
+        // Full/Low device attached to High speed hub port that isolates the high speed signaling
+        // environment from Full/Low speed signaling environment for a device
+        //
+        InputContext->Slot.TTPortNum   = ParentPort;
+        InputContext->Slot.TTHubSlotId = ParentSlotId;
+      }
+    } else {
+      //
+      // Inherit the TT parameters from parent device.
+      //
+      InputContext->Slot.TTPortNum   = ParentDeviceContext->Slot.TTPortNum;
+      InputContext->Slot.TTHubSlotId = ParentDeviceContext->Slot.TTHubSlotId;
+      //
+      // If the device is a High speed device then down the speed to be the same as its parent Hub
+      //
+      if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+        InputContext->Slot.Speed = ParentDeviceContext->Slot.Speed;
+      }
+    }
+  }
+
+  //
+  // 4) Allocate and initialize the Transfer Ring for the Default Control Endpoint.
+  //
+  EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+  Xhc->UsbDevContext[SlotId].EndpointTransferRing[0] = EndpointTransferRing;
+  CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[0]);
+  //
+  // 5) Initialize the Input default control Endpoint 0 Context (6.2.3).
+  //
+  InputContext->EP[0].EPType = ED_CONTROL_BIDIR;
+
+  if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    InputContext->EP[0].MaxPacketSize = 512;
+  } else if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    InputContext->EP[0].MaxPacketSize = 64;
+  } else {
+    InputContext->EP[0].MaxPacketSize = 8;
+  }
+  //
+  // Initial value of Average TRB Length for Control endpoints would be 8B, Interrupt endpoints
+  // 1KB, and Bulk and Isoch endpoints 3KB.
+  //
+  InputContext->EP[0].AverageTRBLength = 8;
+  InputContext->EP[0].MaxBurstSize     = 0;
+  InputContext->EP[0].Interval         = 0;
+  InputContext->EP[0].MaxPStreams      = 0;
+  InputContext->EP[0].Mult             = 0;
+  InputContext->EP[0].CErr             = 3;
+
+  //
+  // Init the DCS(dequeue cycle state) as the transfer ring's CCS
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (
+              Xhc->MemPool,
+              ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[0])->RingSeg0,
+              sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+              );
+  InputContext->EP[0].PtrLo = XHC_LOW_32BIT (PhyAddr) | BIT0;
+  InputContext->EP[0].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // 6) Allocate the Output Device Context data structure (6.2.1) and initialize it to '0'.
+  //
+  OutputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (DEVICE_CONTEXT_64));
+  ASSERT (OutputContext != NULL);
+  ASSERT (((UINTN) OutputContext & 0x3F) == 0);
+  ZeroMem (OutputContext, sizeof (DEVICE_CONTEXT_64));
+
+  Xhc->UsbDevContext[SlotId].OutputContext = OutputContext;
+  //
+  // 7) Load the appropriate (Device Slot ID) entry in the Device Context Base Address Array (5.4.6) with
+  //    a pointer to the Output Device Context data structure (6.2.1).
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, OutputContext, sizeof (DEVICE_CONTEXT_64));
+  //
+  // Fill DCBAA with PCI device address
+  //
+  Xhc->DCBAA[SlotId] = (UINT64) (UINTN) PhyAddr;
+
+  //
+  // 8) Issue an Address Device Command for the Device Slot, where the command points to the Input
+  //    Context data structure described above.
+  //
+  ZeroMem (&CmdTrbAddr, sizeof (CmdTrbAddr));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbAddr.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbAddr.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbAddr.CycleBit = 1;
+  CmdTrbAddr.Type     = TRB_TYPE_ADDRESS_DEV;
+  CmdTrbAddr.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbAddr,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (!EFI_ERROR (Status)) {
+    DeviceAddress = (UINT8) ((DEVICE_CONTEXT_64 *) OutputContext)->Slot.DeviceAddress;
+    DEBUG ((EFI_D_INFO, "    Address %d assigned successfully\n", DeviceAddress));
+    Xhc->UsbDevContext[SlotId].XhciDevAddr = DeviceAddress;
+  }
+  return Status;
+}
+
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDisableSlotCmd (
+  IN USB_XHCI_INSTANCE         *Xhc,
+  IN UINT8                     SlotId
+  )
+{
+  EFI_STATUS            Status;
+  TRB_TEMPLATE          *EvtTrb;
+  CMD_TRB_DISABLE_SLOT  CmdTrbDisSlot;
+  UINT8                 Index;
+  VOID                  *RingSeg;
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0) ||
+        (Xhc->UsbDevContext[Index + 1].ParentRouteString.Dword != Xhc->UsbDevContext[SlotId].RouteString.Dword)) {
+      continue;
+    }
+
+    Status = XhcDisableSlotCmd (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcDisableSlotCmd: failed to disable child, ignore error\n"));
+      Xhc->UsbDevContext[Index + 1].SlotId = 0;
+    }
+  }
+
+  //
+  // Construct the disable slot command
+  //
+  DEBUG ((EFI_D_INFO, "Disable device slot %d!\n", SlotId));
+
+  ZeroMem (&CmdTrbDisSlot, sizeof (CmdTrbDisSlot));
+  CmdTrbDisSlot.CycleBit = 1;
+  CmdTrbDisSlot.Type     = TRB_TYPE_DIS_SLOT;
+  CmdTrbDisSlot.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbDisSlot,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDisableSlotCmd: Disable Slot Command Failed, Status = %r\n", Status));
+    return Status;
+  }
+  //
+  // Free the slot's device context entry
+  //
+  Xhc->DCBAA[SlotId] = 0;
+
+  //
+  // Free the slot related data structure
+  //
+  for (Index = 0; Index < 31; Index++) {
+    if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] != NULL) {
+      RingSeg = ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index])->RingSeg0;
+      if (RingSeg != NULL) {
+        UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+      }
+      FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index]);
+      Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] = NULL;
+    }
+  }
+
+  for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+    if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] != NULL) {
+      FreePool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+    }
+  }
+
+  if (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting != NULL) {
+    FreePool (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting);
+  }
+
+  if (Xhc->UsbDevContext[SlotId].InputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT));
+  }
+
+  if (Xhc->UsbDevContext[SlotId].OutputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].OutputContext, sizeof (DEVICE_CONTEXT));
+  }
+  //
+  // Doesn't zero the entry because XhcAsyncInterruptTransfer() may be invoked to remove the established
+  // asynchronous interrupt pipe after the device is disabled. It needs the device address mapping info to
+  // remove urb from XHCI's asynchronous transfer list.
+  //
+  Xhc->UsbDevContext[SlotId].Enabled = FALSE;
+  Xhc->UsbDevContext[SlotId].SlotId  = 0;
+
+  return Status;
+}
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDisableSlotCmd64 (
+  IN USB_XHCI_INSTANCE         *Xhc,
+  IN UINT8                     SlotId
+  )
+{
+  EFI_STATUS            Status;
+  TRB_TEMPLATE          *EvtTrb;
+  CMD_TRB_DISABLE_SLOT  CmdTrbDisSlot;
+  UINT8                 Index;
+  VOID                  *RingSeg;
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0) ||
+        (Xhc->UsbDevContext[Index + 1].ParentRouteString.Dword != Xhc->UsbDevContext[SlotId].RouteString.Dword)) {
+      continue;
+    }
+
+    Status = XhcDisableSlotCmd64 (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcDisableSlotCmd: failed to disable child, ignore error\n"));
+      Xhc->UsbDevContext[Index + 1].SlotId = 0;
+    }
+  }
+
+  //
+  // Construct the disable slot command
+  //
+  DEBUG ((EFI_D_INFO, "Disable device slot %d!\n", SlotId));
+
+  ZeroMem (&CmdTrbDisSlot, sizeof (CmdTrbDisSlot));
+  CmdTrbDisSlot.CycleBit = 1;
+  CmdTrbDisSlot.Type     = TRB_TYPE_DIS_SLOT;
+  CmdTrbDisSlot.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbDisSlot,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDisableSlotCmd: Disable Slot Command Failed, Status = %r\n", Status));
+    return Status;
+  }
+  //
+  // Free the slot's device context entry
+  //
+  Xhc->DCBAA[SlotId] = 0;
+
+  //
+  // Free the slot related data structure
+  //
+  for (Index = 0; Index < 31; Index++) {
+    if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] != NULL) {
+      RingSeg = ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index])->RingSeg0;
+      if (RingSeg != NULL) {
+        UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+      }
+      FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index]);
+      Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] = NULL;
+    }
+  }
+
+  for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+    if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] != NULL) {
+      FreePool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+    }
+  }
+
+  if (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting != NULL) {
+    FreePool (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting);
+  }
+
+  if (Xhc->UsbDevContext[SlotId].InputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT_64));
+  }
+
+  if (Xhc->UsbDevContext[SlotId].OutputContext != NULL) {
+     UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].OutputContext, sizeof (DEVICE_CONTEXT_64));
+  }
+  //
+  // Doesn't zero the entry because XhcAsyncInterruptTransfer() may be invoked to remove the established
+  // asynchronous interrupt pipe after the device is disabled. It needs the device address mapping info to
+  // remove urb from XHCI's asynchronous transfer list.
+  //
+  Xhc->UsbDevContext[SlotId].Enabled = FALSE;
+  Xhc->UsbDevContext[SlotId].SlotId  = 0;
+
+  return Status;
+}
+
+/**
+  Initialize endpoint context in input context.
+
+  @param Xhc            The XHCI Instance.
+  @param SlotId         The slot id to be configured.
+  @param DeviceSpeed    The device's speed.
+  @param InputContext   The pointer to the input context.
+  @param IfDesc         The pointer to the usb device interface descriptor.
+
+  @return The maximum device context index of endpoint.
+
+**/
+UINT8
+EFIAPI
+XhcInitializeEndpointContext (
+  IN USB_XHCI_INSTANCE          *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN INPUT_CONTEXT              *InputContext,
+  IN USB_INTERFACE_DESCRIPTOR   *IfDesc
+  )
+{
+  USB_ENDPOINT_DESCRIPTOR       *EpDesc;
+  UINTN                         NumEp;
+  UINTN                         EpIndex;
+  UINT8                         EpAddr;
+  UINT8                         Direction;
+  UINT8                         Dci;
+  UINT8                         MaxDci;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  UINT8                         Interval;
+  TRANSFER_RING                 *EndpointTransferRing;
+
+  MaxDci = 0;
+
+  NumEp = IfDesc->NumEndpoints;
+
+  EpDesc = (USB_ENDPOINT_DESCRIPTOR *)(IfDesc + 1);
+  for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+    while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+    }
+
+    EpAddr    = (UINT8)(EpDesc->EndpointAddress & 0x0F);
+    Direction = (UINT8)((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+    Dci = XhcEndpointToDci (EpAddr, Direction);
+    ASSERT (Dci < 32);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    InputContext->InputControlContext.Dword2 |= (BIT0 << Dci);
+    InputContext->EP[Dci-1].MaxPacketSize     = EpDesc->MaxPacketSize;
+
+    if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+      //
+      // 6.2.3.4, shall be set to the value defined in the bMaxBurst field of the SuperSpeed Endpoint Companion Descriptor.
+      //
+      InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+    } else {
+      InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+    }
+
+    switch (EpDesc->Attributes & USB_ENDPOINT_TYPE_MASK) {
+      case USB_ENDPOINT_BULK:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_BULK_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_BULK_OUT;
+        }
+
+        InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+        if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+          EndpointTransferRing = AllocateZeroPool(sizeof (TRANSFER_RING));
+          Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+          CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+        }
+
+        break;
+      case USB_ENDPOINT_ISO:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 0;
+          InputContext->EP[Dci-1].EPType = ED_ISOCH_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 0;
+          InputContext->EP[Dci-1].EPType = ED_ISOCH_OUT;
+        }
+        //
+        // Do not support isochronous transfer now.
+        //
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext: Unsupport ISO EP found, Transfer ring is not allocated.\n"));
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+        continue;
+      case USB_ENDPOINT_INTERRUPT:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_INTERRUPT_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_INTERRUPT_OUT;
+        }
+        InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+        InputContext->EP[Dci-1].MaxESITPayload   = EpDesc->MaxPacketSize;
+        //
+        // Get the bInterval from descriptor and init the the interval field of endpoint context
+        //
+        if ((DeviceSpeed == EFI_USB_SPEED_FULL) || (DeviceSpeed == EFI_USB_SPEED_LOW)) {
+          Interval = EpDesc->Interval;
+          //
+          // Calculate through the bInterval field of Endpoint descriptor.
+          //
+          ASSERT (Interval != 0);
+          InputContext->EP[Dci-1].Interval = (UINT32)HighBitSet32((UINT32)Interval) + 3;
+        } else if ((DeviceSpeed == EFI_USB_SPEED_HIGH) || (DeviceSpeed == EFI_USB_SPEED_SUPER)) {
+          Interval = EpDesc->Interval;
+          ASSERT (Interval >= 1 && Interval <= 16);
+          //
+          // Refer to XHCI 1.0 spec section 6.2.3.6, table 61
+          //
+          InputContext->EP[Dci-1].Interval         = Interval - 1;
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          InputContext->EP[Dci-1].MaxESITPayload   = 0x0002;
+          InputContext->EP[Dci-1].MaxBurstSize     = 0x0;
+          InputContext->EP[Dci-1].CErr             = 3;
+        }
+
+        if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+          EndpointTransferRing = AllocateZeroPool(sizeof (TRANSFER_RING));
+          Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+          CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+        }
+        break;
+
+      case USB_ENDPOINT_CONTROL:
+        //
+        // Do not support control transfer now.
+        //
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext: Unsupport Control EP found, Transfer ring is not allocated.\n"));
+      default:
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext: Unknown EP found, Transfer ring is not allocated.\n"));
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+        continue;
+    }
+
+    PhyAddr = UsbHcGetPciAddrForHostAddr (
+                Xhc->MemPool,
+                ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingSeg0,
+                sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+                );
+    PhyAddr &= ~((EFI_PHYSICAL_ADDRESS)0x0F);
+    PhyAddr |= (EFI_PHYSICAL_ADDRESS)((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingPCS;
+    InputContext->EP[Dci-1].PtrLo = XHC_LOW_32BIT (PhyAddr);
+    InputContext->EP[Dci-1].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+  }
+
+  return MaxDci;
+}
+
+/**
+  Initialize endpoint context in input context.
+
+  @param Xhc            The XHCI Instance.
+  @param SlotId         The slot id to be configured.
+  @param DeviceSpeed    The device's speed.
+  @param InputContext   The pointer to the input context.
+  @param IfDesc         The pointer to the usb device interface descriptor.
+
+  @return The maximum device context index of endpoint.
+
+**/
+UINT8
+EFIAPI
+XhcInitializeEndpointContext64 (
+  IN USB_XHCI_INSTANCE          *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN INPUT_CONTEXT_64           *InputContext,
+  IN USB_INTERFACE_DESCRIPTOR   *IfDesc
+  )
+{
+  USB_ENDPOINT_DESCRIPTOR       *EpDesc;
+  UINTN                         NumEp;
+  UINTN                         EpIndex;
+  UINT8                         EpAddr;
+  UINT8                         Direction;
+  UINT8                         Dci;
+  UINT8                         MaxDci;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  UINT8                         Interval;
+  TRANSFER_RING                 *EndpointTransferRing;
+
+  MaxDci = 0;
+
+  NumEp = IfDesc->NumEndpoints;
+
+  EpDesc = (USB_ENDPOINT_DESCRIPTOR *)(IfDesc + 1);
+  for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+    while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+    }
+
+    EpAddr    = (UINT8)(EpDesc->EndpointAddress & 0x0F);
+    Direction = (UINT8)((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+    Dci = XhcEndpointToDci (EpAddr, Direction);
+    ASSERT (Dci < 32);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    InputContext->InputControlContext.Dword2 |= (BIT0 << Dci);
+    InputContext->EP[Dci-1].MaxPacketSize     = EpDesc->MaxPacketSize;
+
+    if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+      //
+      // 6.2.3.4, shall be set to the value defined in the bMaxBurst field of the SuperSpeed Endpoint Companion Descriptor.
+      //
+      InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+    } else {
+      InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+    }
+
+    switch (EpDesc->Attributes & USB_ENDPOINT_TYPE_MASK) {
+      case USB_ENDPOINT_BULK:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_BULK_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_BULK_OUT;
+        }
+
+        InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+        if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+          EndpointTransferRing = AllocateZeroPool(sizeof (TRANSFER_RING));
+          Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+          CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+        }
+
+        break;
+      case USB_ENDPOINT_ISO:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 0;
+          InputContext->EP[Dci-1].EPType = ED_ISOCH_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 0;
+          InputContext->EP[Dci-1].EPType = ED_ISOCH_OUT;
+        }
+        //
+        // Do not support isochronous transfer now.
+        //
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext64: Unsupport ISO EP found, Transfer ring is not allocated.\n"));
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+        continue;
+      case USB_ENDPOINT_INTERRUPT:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_INTERRUPT_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_INTERRUPT_OUT;
+        }
+        InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+        InputContext->EP[Dci-1].MaxESITPayload   = EpDesc->MaxPacketSize;
+        //
+        // Get the bInterval from descriptor and init the the interval field of endpoint context
+        //
+        if ((DeviceSpeed == EFI_USB_SPEED_FULL) || (DeviceSpeed == EFI_USB_SPEED_LOW)) {
+          Interval = EpDesc->Interval;
+          //
+          // Calculate through the bInterval field of Endpoint descriptor.
+          //
+          ASSERT (Interval != 0);
+          InputContext->EP[Dci-1].Interval = (UINT32)HighBitSet32((UINT32)Interval) + 3;
+        } else if ((DeviceSpeed == EFI_USB_SPEED_HIGH) || (DeviceSpeed == EFI_USB_SPEED_SUPER)) {
+          Interval = EpDesc->Interval;
+          ASSERT (Interval >= 1 && Interval <= 16);
+          //
+          // Refer to XHCI 1.0 spec section 6.2.3.6, table 61
+          //
+          InputContext->EP[Dci-1].Interval         = Interval - 1;
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          InputContext->EP[Dci-1].MaxESITPayload   = 0x0002;
+          InputContext->EP[Dci-1].MaxBurstSize     = 0x0;
+          InputContext->EP[Dci-1].CErr             = 3;
+        }
+
+        if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+          EndpointTransferRing = AllocateZeroPool(sizeof (TRANSFER_RING));
+          Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+          CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+        }
+        break;
+
+      case USB_ENDPOINT_CONTROL:
+        //
+        // Do not support control transfer now.
+        //
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext64: Unsupport Control EP found, Transfer ring is not allocated.\n"));
+      default:
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext64: Unknown EP found, Transfer ring is not allocated.\n"));
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+        continue;
+    }
+
+    PhyAddr = UsbHcGetPciAddrForHostAddr (
+                Xhc->MemPool,
+                ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingSeg0,
+                sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+                );
+    PhyAddr &= ~((EFI_PHYSICAL_ADDRESS)0x0F);
+    PhyAddr |= (EFI_PHYSICAL_ADDRESS)((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingPCS;
+    InputContext->EP[Dci-1].PtrLo = XHC_LOW_32BIT (PhyAddr);
+    InputContext->EP[Dci-1].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+  }
+
+  return MaxDci;
+}
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetConfigCmd (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc
+  )
+{
+  EFI_STATUS                  Status;
+  USB_INTERFACE_DESCRIPTOR    *IfDesc;
+  UINT8                       Index;
+  UINT8                       Dci;
+  UINT8                       MaxDci;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  INPUT_CONTEXT               *InputContext;
+  DEVICE_CONTEXT              *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  //
+  // 4.6.6 Configure Endpoint
+  //
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *)(ConfigDesc + 1);
+  for (Index = 0; Index < ConfigDesc->NumInterfaces; Index++) {
+    while ((IfDesc->DescriptorType != USB_DESC_TYPE_INTERFACE) || (IfDesc->AlternateSetting != 0)) {
+      IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+    }
+
+    Dci = XhcInitializeEndpointContext (Xhc, SlotId, DeviceSpeed, InputContext, IfDesc);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+  InputContext->Slot.ContextEntries         = MaxDci;
+  //
+  // configure endpoint
+  //
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Endpoint\n"));
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetConfigCmd: Config Endpoint Failed, Status = %r\n", Status));
+  } else {
+    Xhc->UsbDevContext[SlotId].ActiveConfiguration = ConfigDesc->ConfigurationValue;
+  }
+
+  return Status;
+}
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetConfigCmd64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc
+  )
+{
+  EFI_STATUS                  Status;
+  USB_INTERFACE_DESCRIPTOR    *IfDesc;
+  UINT8                       Index;
+  UINT8                       Dci;
+  UINT8                       MaxDci;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  INPUT_CONTEXT_64            *InputContext;
+  DEVICE_CONTEXT_64           *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  //
+  // 4.6.6 Configure Endpoint
+  //
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT_64));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *)(ConfigDesc + 1);
+  for (Index = 0; Index < ConfigDesc->NumInterfaces; Index++) {
+    while ((IfDesc->DescriptorType != USB_DESC_TYPE_INTERFACE) || (IfDesc->AlternateSetting != 0)) {
+      IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+    }
+
+    Dci = XhcInitializeEndpointContext64 (Xhc, SlotId, DeviceSpeed, InputContext, IfDesc);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+ 
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+  InputContext->Slot.ContextEntries         = MaxDci;
+  //
+  // configure endpoint
+  //
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr  = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Endpoint\n"));
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetConfigCmd64: Config Endpoint Failed, Status = %r\n", Status));
+  } else {
+    Xhc->UsbDevContext[SlotId].ActiveConfiguration = ConfigDesc->ConfigurationValue;
+  }
+
+  return Status;
+}
+
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Stop endpoint successfully.
+  @retval Others                Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcStopEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  CMD_TRB_STOP_ENDPOINT         CmdTrbStopED;
+
+  DEBUG ((EFI_D_INFO, "XhcStopEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbStopED, sizeof (CmdTrbStopED));
+  CmdTrbStopED.CycleBit = 1;
+  CmdTrbStopED.Type     = TRB_TYPE_STOP_ENDPOINT;
+  CmdTrbStopED.EDID     = Dci;
+  CmdTrbStopED.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbStopED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcStopEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Reset endpoint successfully.
+  @retval Others                Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcResetEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_TRB_RESET_ENDPOINT      CmdTrbResetED;
+
+  DEBUG ((EFI_D_INFO, "XhcResetEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbResetED, sizeof (CmdTrbResetED));
+  CmdTrbResetED.CycleBit = 1;
+  CmdTrbResetED.Type     = TRB_TYPE_RESET_ENDPOINT;
+  CmdTrbResetED.EDID     = Dci;
+  CmdTrbResetED.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbResetED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcResetEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+  @param  Urb                   The dequeue pointer of the transfer ring specified
+                                by the urb to be updated.
+
+  @retval EFI_SUCCESS           Set transfer ring dequeue pointer succeeds.
+  @retval Others                Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetTrDequeuePointer (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci,
+  IN URB                    *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_SET_TR_DEQ_POINTER      CmdSetTRDeq;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  DEBUG ((EFI_D_INFO, "XhcSetTrDequeuePointer: Slot = 0x%x, Dci = 0x%x, Urb = 0x%x\n", SlotId, Dci, Urb));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdSetTRDeq, sizeof (CmdSetTRDeq));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Urb->Ring->RingEnqueue, sizeof (CMD_SET_TR_DEQ_POINTER));
+  CmdSetTRDeq.PtrLo    = XHC_LOW_32BIT (PhyAddr) | Urb->Ring->RingPCS;
+  CmdSetTRDeq.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdSetTRDeq.CycleBit = 1;
+  CmdSetTRDeq.Type     = TRB_TYPE_SET_TR_DEQUE;
+  CmdSetTRDeq.Endpoint = Dci;
+  CmdSetTRDeq.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdSetTRDeq,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetTrDequeuePointer: Set TR Dequeue Pointer Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Set interface through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+  @param  Request       USB device request to send.
+
+  @retval EFI_SUCCESS   Successfully set interface.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetInterface (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc,
+  IN EFI_USB_DEVICE_REQUEST   *Request
+  )
+{
+  EFI_STATUS                  Status;
+  USB_INTERFACE_DESCRIPTOR    *IfDescActive;
+  USB_INTERFACE_DESCRIPTOR    *IfDescSet;
+  USB_INTERFACE_DESCRIPTOR    *IfDesc;
+  USB_ENDPOINT_DESCRIPTOR     *EpDesc;
+  UINTN                       NumEp;
+  UINTN                       EpIndex;
+  UINT8                       EpAddr;
+  UINT8                       Direction;
+  UINT8                       Dci;
+  UINT8                       MaxDci;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+  VOID                        *RingSeg;
+
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  INPUT_CONTEXT               *InputContext;
+  DEVICE_CONTEXT              *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+
+  Status = EFI_SUCCESS;
+
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  //
+  // XHCI 4.6.6 Configure Endpoint
+  // When this command is used to "Set an Alternate Interface on a device", software shall set the Drop
+  // Context and Add Context flags as follows:
+  // 1) If an endpoint is not modified by the Alternate Interface setting, then software shall set the Drop
+  // Context and Add Context flags to '0'.
+  //
+  // Except the interface indicated by Reqeust->Index, no impact to other interfaces.
+  // So the default Drop Context and Add Context flags can be '0' to cover 1).
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDescActive = NULL;
+  IfDescSet = NULL;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *)(ConfigDesc + 1);
+  while ((UINTN) IfDesc < ((UINTN) ConfigDesc + ConfigDesc->TotalLength)) {
+    if (IfDesc->DescriptorType == USB_DESC_TYPE_INTERFACE) {
+      if (IfDesc->InterfaceNumber == (UINT8) Request->Index) {
+        if (IfDesc->AlternateSetting == Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[IfDesc->InterfaceNumber]) {
+          //
+          // Find out the active interface descriptor.
+          //
+          IfDescActive = IfDesc;
+        } else if (IfDesc->AlternateSetting == (UINT8) Request->Value) {
+          //
+          // Find out the interface descriptor to set.
+          //
+          IfDescSet = IfDesc;
+        }
+      }
+    }
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  //
+  // XHCI 4.6.6 Configure Endpoint
+  // When this command is used to "Set an Alternate Interface on a device", software shall set the Drop
+  // Context and Add Context flags as follows:
+  // 2) If an endpoint previously disabled, is enabled by the Alternate Interface setting, then software shall set
+  // the Drop Context flag to '0' and Add Context flag to '1', and initialize the Input Endpoint Context.
+  // 3) If an endpoint previously enabled, is disabled by the Alternate Interface setting, then software shall set
+  // the Drop Context flag to '1' and Add Context flag to '0'.
+  // 4) If a parameter of an enabled endpoint is modified by an Alternate Interface setting, the Drop Context
+  // and Add Context flags shall be set to '1'.
+  //
+  // Below codes are to cover 2), 3) and 4).
+  //
+
+  if ((IfDescActive != NULL) && (IfDescSet != NULL)) {
+    NumEp = IfDescActive->NumEndpoints;
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *) (IfDescActive + 1);
+    for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+      while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+      }
+
+      EpAddr    = (UINT8) (EpDesc->EndpointAddress & 0x0F);
+      Direction = (UINT8) ((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+      Dci = XhcEndpointToDci (EpAddr, Direction);
+      ASSERT (Dci < 32);
+      if (Dci > MaxDci) {
+        MaxDci = Dci;
+      }
+      //
+      // XHCI 4.3.6 - Setting Alternate Interfaces
+      // 1) Stop any Running Transfer Rings affected by the Alternate Interface setting.
+      //
+      Status = XhcStopEndpoint (Xhc, SlotId, Dci);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      //
+      // XHCI 4.3.6 - Setting Alternate Interfaces
+      // 2) Free Transfer Rings of all endpoints that will be affected by the Alternate Interface setting.
+      //
+      if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1] != NULL) {
+        RingSeg = ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1])->RingSeg0;
+        if (RingSeg != NULL) {
+          UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+        }
+        FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1]);
+        Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1] = NULL;
+      }
+
+      //
+      // Set the Drop Context flag to '1'.
+      //
+      InputContext->InputControlContext.Dword1 |= (BIT0 << Dci);
+
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+    }
+
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 3) Clear all the Endpoint Context fields of each endpoint that will be disabled by the Alternate
+    // Interface setting, to '0'.
+    //
+    // The step 3) has been covered by the ZeroMem () to InputContext at the start of the function.
+    //
+
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 4) For each endpoint enabled by the Configure Endpoint Command:
+    //   a. Allocate a Transfer Ring.
+    //   b. Initialize the Transfer Ring Segment(s) by clearing all fields of all TRBs to '0'.
+    //   c. Initialize the Endpoint Context data structure.
+    //
+    Dci = XhcInitializeEndpointContext (Xhc, SlotId, DeviceSpeed, InputContext, IfDescSet);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    InputContext->InputControlContext.Dword2 |= BIT0;
+    InputContext->Slot.ContextEntries         = MaxDci;
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 5) Issue and successfully complete a Configure Endpoint Command.
+    //
+    ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+    PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+    CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+    CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+    CmdTrbCfgEP.CycleBit = 1;
+    CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+    CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+    DEBUG ((EFI_D_INFO, "SetInterface: Configure Endpoint\n"));
+    Status = XhcCmdTransfer (
+               Xhc,
+               (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+               XHC_GENERIC_TIMEOUT,
+               (TRB_TEMPLATE **) (UINTN) &EvtTrb
+               );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SetInterface: Config Endpoint Failed, Status = %r\n", Status));
+    } else {
+      //
+      // Update the active AlternateSetting.
+      //
+      Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[(UINT8) Request->Index] = (UINT8) Request->Value;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Set interface through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+  @param  Request       USB device request to send.
+
+  @retval EFI_SUCCESS   Successfully set interface.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetInterface64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc,
+  IN EFI_USB_DEVICE_REQUEST   *Request
+  )
+{
+  EFI_STATUS                  Status;
+  USB_INTERFACE_DESCRIPTOR    *IfDescActive;
+  USB_INTERFACE_DESCRIPTOR    *IfDescSet;
+  USB_INTERFACE_DESCRIPTOR    *IfDesc;
+  USB_ENDPOINT_DESCRIPTOR     *EpDesc;
+  UINTN                       NumEp;
+  UINTN                       EpIndex;
+  UINT8                       EpAddr;
+  UINT8                       Direction;
+  UINT8                       Dci;
+  UINT8                       MaxDci;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+  VOID                        *RingSeg;
+
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  INPUT_CONTEXT_64            *InputContext;
+  DEVICE_CONTEXT_64           *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+
+  Status = EFI_SUCCESS;
+
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  //
+  // XHCI 4.6.6 Configure Endpoint
+  // When this command is used to "Set an Alternate Interface on a device", software shall set the Drop
+  // Context and Add Context flags as follows:
+  // 1) If an endpoint is not modified by the Alternate Interface setting, then software shall set the Drop
+  // Context and Add Context flags to '0'.
+  //
+  // Except the interface indicated by Reqeust->Index, no impact to other interfaces.
+  // So the default Drop Context and Add Context flags can be '0' to cover 1).
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT_64));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDescActive = NULL;
+  IfDescSet = NULL;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *)(ConfigDesc + 1);
+  while ((UINTN) IfDesc < ((UINTN) ConfigDesc + ConfigDesc->TotalLength)) {
+    if (IfDesc->DescriptorType == USB_DESC_TYPE_INTERFACE) {
+      if (IfDesc->InterfaceNumber == (UINT8) Request->Index) {
+        if (IfDesc->AlternateSetting == Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[IfDesc->InterfaceNumber]) {
+          //
+          // Find out the active interface descriptor.
+          //
+          IfDescActive = IfDesc;
+        } else if (IfDesc->AlternateSetting == (UINT8) Request->Value) {
+          //
+          // Find out the interface descriptor to set.
+          //
+          IfDescSet = IfDesc;
+        }
+      }
+    }
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  //
+  // XHCI 4.6.6 Configure Endpoint
+  // When this command is used to "Set an Alternate Interface on a device", software shall set the Drop
+  // Context and Add Context flags as follows:
+  // 2) If an endpoint previously disabled, is enabled by the Alternate Interface setting, then software shall set
+  // the Drop Context flag to '0' and Add Context flag to '1', and initialize the Input Endpoint Context.
+  // 3) If an endpoint previously enabled, is disabled by the Alternate Interface setting, then software shall set
+  // the Drop Context flag to '1' and Add Context flag to '0'.
+  // 4) If a parameter of an enabled endpoint is modified by an Alternate Interface setting, the Drop Context
+  // and Add Context flags shall be set to '1'.
+  //
+  // Below codes are to cover 2), 3) and 4).
+  //
+
+  if ((IfDescActive != NULL) && (IfDescSet != NULL)) {
+    NumEp = IfDescActive->NumEndpoints;
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *) (IfDescActive + 1);
+    for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+      while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+      }
+
+      EpAddr    = (UINT8) (EpDesc->EndpointAddress & 0x0F);
+      Direction = (UINT8) ((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+      Dci = XhcEndpointToDci (EpAddr, Direction);
+      ASSERT (Dci < 32);
+      if (Dci > MaxDci) {
+        MaxDci = Dci;
+      }
+      //
+      // XHCI 4.3.6 - Setting Alternate Interfaces
+      // 1) Stop any Running Transfer Rings affected by the Alternate Interface setting.
+      //
+      Status = XhcStopEndpoint (Xhc, SlotId, Dci);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      //
+      // XHCI 4.3.6 - Setting Alternate Interfaces
+      // 2) Free Transfer Rings of all endpoints that will be affected by the Alternate Interface setting.
+      //
+      if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1] != NULL) {
+        RingSeg = ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1])->RingSeg0;
+        if (RingSeg != NULL) {
+          UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+        }
+        FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1]);
+        Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1] = NULL;
+      }
+
+      //
+      // Set the Drop Context flag to '1'.
+      //
+      InputContext->InputControlContext.Dword1 |= (BIT0 << Dci);
+
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+    }
+
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 3) Clear all the Endpoint Context fields of each endpoint that will be disabled by the Alternate
+    // Interface setting, to '0'.
+    //
+    // The step 3) has been covered by the ZeroMem () to InputContext at the start of the function.
+    //
+
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 4) For each endpoint enabled by the Configure Endpoint Command:
+    //   a. Allocate a Transfer Ring.
+    //   b. Initialize the Transfer Ring Segment(s) by clearing all fields of all TRBs to '0'.
+    //   c. Initialize the Endpoint Context data structure.
+    //
+    Dci = XhcInitializeEndpointContext64 (Xhc, SlotId, DeviceSpeed, InputContext, IfDescSet);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    InputContext->InputControlContext.Dword2 |= BIT0;
+    InputContext->Slot.ContextEntries         = MaxDci;
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 5) Issue and successfully complete a Configure Endpoint Command.
+    //
+    ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+    PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+    CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+    CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+    CmdTrbCfgEP.CycleBit = 1;
+    CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+    CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+    DEBUG ((EFI_D_INFO, "SetInterface64: Configure Endpoint\n"));
+    Status = XhcCmdTransfer (
+               Xhc,
+               (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+               XHC_GENERIC_TIMEOUT,
+               (TRB_TEMPLATE **) (UINTN) &EvtTrb
+               );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SetInterface64: Config Endpoint Failed, Status = %r\n", Status));
+    } else {
+      //
+      // Update the active AlternateSetting.
+      //
+      Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[(UINT8) Request->Index] = (UINT8) Request->Value;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcEvaluateContext (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT32                   MaxPacketSize
+  )
+{
+  EFI_STATUS                  Status;
+  CMD_TRB_EVALUATE_CONTEXT    CmdTrbEvalu;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT               *InputContext;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  InputContext = Xhc->UsbDevContext[SlotId].InputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  InputContext->InputControlContext.Dword2 |= BIT1;
+  InputContext->EP[0].MaxPacketSize         = MaxPacketSize;
+
+  ZeroMem (&CmdTrbEvalu, sizeof (CmdTrbEvalu));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbEvalu.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbEvalu.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbEvalu.CycleBit = 1;
+  CmdTrbEvalu.Type     = TRB_TYPE_EVALU_CONTXT;
+  CmdTrbEvalu.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Evaluate context\n"));
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbEvalu,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcEvaluateContext: Evaluate Context Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcEvaluateContext64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT32                   MaxPacketSize
+  )
+{
+  EFI_STATUS                  Status;
+  CMD_TRB_EVALUATE_CONTEXT    CmdTrbEvalu;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT_64            *InputContext;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  InputContext = Xhc->UsbDevContext[SlotId].InputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  InputContext->InputControlContext.Dword2 |= BIT1;
+  InputContext->EP[0].MaxPacketSize         = MaxPacketSize;
+
+  ZeroMem (&CmdTrbEvalu, sizeof (CmdTrbEvalu));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbEvalu.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbEvalu.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbEvalu.CycleBit = 1;
+  CmdTrbEvalu.Type     = TRB_TYPE_EVALU_CONTXT;
+  CmdTrbEvalu.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Evaluate context\n"));
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbEvalu,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcEvaluateContext64: Evaluate Context Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcConfigHubContext (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    PortNum,
+  IN UINT8                    TTT,
+  IN UINT8                    MTT
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT               *InputContext;
+  DEVICE_CONTEXT              *OutputContext;
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+
+  //
+  // Copy the slot context from OutputContext to Input context
+  //
+  CopyMem(&(InputContext->Slot), &(OutputContext->Slot), sizeof (SLOT_CONTEXT));
+  InputContext->Slot.Hub     = 1;
+  InputContext->Slot.PortNum = PortNum;
+  InputContext->Slot.TTT     = TTT;
+  InputContext->Slot.MTT     = MTT;
+
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Hub Slot Context\n"));
+  Status = XhcCmdTransfer (
+              Xhc,
+              (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+              XHC_GENERIC_TIMEOUT,
+              (TRB_TEMPLATE **) (UINTN) &EvtTrb
+              );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcConfigHubContext: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcConfigHubContext64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    PortNum,
+  IN UINT8                    TTT,
+  IN UINT8                    MTT
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT_64            *InputContext;
+  DEVICE_CONTEXT_64           *OutputContext;
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+
+  //
+  // Copy the slot context from OutputContext to Input context
+  //
+  CopyMem(&(InputContext->Slot), &(OutputContext->Slot), sizeof (SLOT_CONTEXT_64));
+  InputContext->Slot.Hub     = 1;
+  InputContext->Slot.PortNum = PortNum;
+  InputContext->Slot.TTT     = TTT;
+  InputContext->Slot.MTT     = MTT;
+
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Hub Slot Context\n"));
+  Status = XhcCmdTransfer (
+              Xhc,
+              (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+              XHC_GENERIC_TIMEOUT,
+              (TRB_TEMPLATE **) (UINTN) &EvtTrb
+              );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcConfigHubContext64: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.h
new file mode 100644
index 0000000000..931c7efa0c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.h
@@ -0,0 +1,1461 @@
+/** @file
+
+  This file contains the definition for XHCI host controller schedule routines.
+
+Copyright (c) 2011 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_XHCI_SCHED_H_
+#define _EFI_XHCI_SCHED_H_
+
+#define XHC_URB_SIG      SIGNATURE_32 ('U', 'S', 'B', 'R')
+
+//
+// Transfer types, used in URB to identify the transfer type
+//
+#define XHC_CTRL_TRANSFER                     0x01
+#define XHC_BULK_TRANSFER                     0x02
+#define XHC_INT_TRANSFER_SYNC                 0x04
+#define XHC_INT_TRANSFER_ASYNC                0x08
+#define XHC_INT_ONLY_TRANSFER_ASYNC           0x10
+
+//
+// 6.4.6 TRB Types
+//
+#define TRB_TYPE_NORMAL                       1
+#define TRB_TYPE_SETUP_STAGE                  2
+#define TRB_TYPE_DATA_STAGE                   3
+#define TRB_TYPE_STATUS_STAGE                 4
+#define TRB_TYPE_ISOCH                        5
+#define TRB_TYPE_LINK                         6
+#define TRB_TYPE_EVENT_DATA                   7
+#define TRB_TYPE_NO_OP                        8
+#define TRB_TYPE_EN_SLOT                      9
+#define TRB_TYPE_DIS_SLOT                     10
+#define TRB_TYPE_ADDRESS_DEV                  11
+#define TRB_TYPE_CON_ENDPOINT                 12
+#define TRB_TYPE_EVALU_CONTXT                 13
+#define TRB_TYPE_RESET_ENDPOINT               14
+#define TRB_TYPE_STOP_ENDPOINT                15
+#define TRB_TYPE_SET_TR_DEQUE                 16
+#define TRB_TYPE_RESET_DEV                    17
+#define TRB_TYPE_GET_PORT_BANW                21
+#define TRB_TYPE_FORCE_HEADER                 22
+#define TRB_TYPE_NO_OP_COMMAND                23
+#define TRB_TYPE_TRANS_EVENT                  32
+#define TRB_TYPE_COMMAND_COMPLT_EVENT         33
+#define TRB_TYPE_PORT_STATUS_CHANGE_EVENT     34
+#define TRB_TYPE_HOST_CONTROLLER_EVENT        37
+#define TRB_TYPE_DEVICE_NOTIFI_EVENT          38
+#define TRB_TYPE_MFINDEX_WRAP_EVENT           39
+
+//
+// Endpoint Type (EP Type).
+//
+#define ED_NOT_VALID                          0
+#define ED_ISOCH_OUT                          1
+#define ED_BULK_OUT                           2
+#define ED_INTERRUPT_OUT                      3
+#define ED_CONTROL_BIDIR                      4
+#define ED_ISOCH_IN                           5
+#define ED_BULK_IN                            6
+#define ED_INTERRUPT_IN                       7
+
+//
+// 6.4.5 TRB Completion Codes
+//
+#define TRB_COMPLETION_INVALID                0
+#define TRB_COMPLETION_SUCCESS                1
+#define TRB_COMPLETION_DATA_BUFFER_ERROR      2
+#define TRB_COMPLETION_BABBLE_ERROR           3
+#define TRB_COMPLETION_USB_TRANSACTION_ERROR  4
+#define TRB_COMPLETION_TRB_ERROR              5
+#define TRB_COMPLETION_STALL_ERROR            6
+#define TRB_COMPLETION_SHORT_PACKET           13
+
+//
+// The topology string used to present usb device location
+//
+typedef struct _USB_DEV_TOPOLOGY {
+  //
+  // The tier concatenation of down stream port.
+  //
+  UINT32 RouteString:20;
+  //
+  // The root port number of the chain.
+  //
+  UINT32 RootPortNum:8;
+  //
+  // The Tier the device reside.
+  //
+  UINT32 TierNum:4;
+} USB_DEV_TOPOLOGY;
+
+//
+// USB Device's RouteChart
+//
+typedef union _USB_DEV_ROUTE {
+  UINT32              Dword;
+  USB_DEV_TOPOLOGY    Route;
+} USB_DEV_ROUTE;
+
+//
+// Endpoint address and its capabilities
+//
+typedef struct _USB_ENDPOINT {
+  //
+  // Store logical device address assigned by UsbBus
+  // It's because some XHCI host controllers may assign the same physcial device
+  // address for those devices inserted at different root port.
+  //
+  UINT8                     BusAddr;
+  UINT8                     DevAddr;
+  UINT8                     EpAddr;
+  EFI_USB_DATA_DIRECTION    Direction;
+  UINT8                     DevSpeed;
+  UINTN                     MaxPacket;
+  UINTN                     Type;
+} USB_ENDPOINT;
+
+//
+// TRB Template
+//
+typedef struct _TRB_TEMPLATE {
+  UINT32                    Parameter1;
+
+  UINT32                    Parameter2;
+
+  UINT32                    Status;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ1:9;
+  UINT32                    Type:6;
+  UINT32                    Control:16;
+} TRB_TEMPLATE;
+
+typedef struct _TRANSFER_RING {
+  VOID                      *RingSeg0;
+  UINTN                     TrbNumber;
+  TRB_TEMPLATE              *RingEnqueue;
+  TRB_TEMPLATE              *RingDequeue;
+  UINT32                    RingPCS;
+} TRANSFER_RING;
+
+typedef struct _EVENT_RING {
+  VOID                      *ERSTBase;
+  VOID                      *EventRingSeg0;
+  UINTN                     TrbNumber;
+  TRB_TEMPLATE              *EventRingEnqueue;
+  TRB_TEMPLATE              *EventRingDequeue;
+  UINT32                    EventRingCCS;
+} EVENT_RING;
+
+//
+// URB (Usb Request Block) contains information for all kinds of
+// usb requests.
+//
+typedef struct _URB {
+  UINT32                          Signature;
+  LIST_ENTRY                      UrbList;
+  //
+  // Usb Device URB related information
+  //
+  USB_ENDPOINT                    Ep;
+  EFI_USB_DEVICE_REQUEST          *Request;
+  VOID                            *Data;
+  UINTN                           DataLen;
+  VOID                            *DataPhy;
+  VOID                            *DataMap;
+  EFI_ASYNC_USB_TRANSFER_CALLBACK Callback;
+  VOID                            *Context;
+  //
+  // Execute result
+  //
+  UINT32                          Result;
+  //
+  // completed data length
+  //
+  UINTN                           Completed;
+  //
+  // Command/Tranfer Ring info
+  //
+  TRANSFER_RING                   *Ring;
+  TRB_TEMPLATE                    *TrbStart;
+  TRB_TEMPLATE                    *TrbEnd;
+  UINTN                           TrbNum;
+  BOOLEAN                         StartDone;
+  BOOLEAN                         EndDone;
+  BOOLEAN                         Finished;
+
+  TRB_TEMPLATE                    *EvtTrb;
+} URB;
+
+//
+// 6.5 Event Ring Segment Table
+// The Event Ring Segment Table is used to define multi-segment Event Rings and to enable runtime
+// expansion and shrinking of the Event Ring. The location of the Event Ring Segment Table is defined by the
+// Event Ring Segment Table Base Address Register (5.5.2.3.2). The size of the Event Ring Segment Table
+// is defined by the Event Ring Segment Table Base Size Register (5.5.2.3.1).
+//
+typedef struct _EVENT_RING_SEG_TABLE_ENTRY {
+  UINT32                  PtrLo;
+  UINT32                  PtrHi;
+  UINT32                  RingTrbSize:16;
+  UINT32                  RsvdZ1:16;
+  UINT32                  RsvdZ2;
+} EVENT_RING_SEG_TABLE_ENTRY;
+
+//
+// 6.4.1.1 Normal TRB
+// A Normal TRB is used in several ways; exclusively on Bulk and Interrupt Transfer Rings for normal and
+// Scatter/Gather operations, to define additional data buffers for Scatter/Gather operations on Isoch Transfer
+// Rings, and to define the Data stage information for Control Transfer Rings.
+//
+typedef struct _TRANSFER_TRB_NORMAL {
+  UINT32                  TRBPtrLo;
+
+  UINT32                  TRBPtrHi;
+
+  UINT32                  Length:17;
+  UINT32                  TDSize:5;
+  UINT32                  IntTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  ENT:1;
+  UINT32                  ISP:1;
+  UINT32                  NS:1;
+  UINT32                  CH:1;
+  UINT32                  IOC:1;
+  UINT32                  IDT:1;
+  UINT32                  RsvdZ1:2;
+  UINT32                  BEI:1;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ2:16;
+} TRANSFER_TRB_NORMAL;
+
+//
+// 6.4.1.2.1 Setup Stage TRB
+// A Setup Stage TRB is created by system software to initiate a USB Setup packet on a control endpoint.
+//
+typedef struct _TRANSFER_TRB_CONTROL_SETUP {
+  UINT32                  bmRequestType:8;
+  UINT32                  bRequest:8;
+  UINT32                  wValue:16;
+
+  UINT32                  wIndex:16;
+  UINT32                  wLength:16;
+
+  UINT32                  Length:17;
+  UINT32                  RsvdZ1:5;
+  UINT32                  IntTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:4;
+  UINT32                  IOC:1;
+  UINT32                  IDT:1;
+  UINT32                  RsvdZ3:3;
+  UINT32                  Type:6;
+  UINT32                  TRT:2;
+  UINT32                  RsvdZ4:14;
+} TRANSFER_TRB_CONTROL_SETUP;
+
+//
+// 6.4.1.2.2 Data Stage TRB
+// A Data Stage TRB is used generate the Data stage transaction of a USB Control transfer.
+//
+typedef struct _TRANSFER_TRB_CONTROL_DATA {
+  UINT32                  TRBPtrLo;
+
+  UINT32                  TRBPtrHi;
+
+  UINT32                  Length:17;
+  UINT32                  TDSize:5;
+  UINT32                  IntTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  ENT:1;
+  UINT32                  ISP:1;
+  UINT32                  NS:1;
+  UINT32                  CH:1;
+  UINT32                  IOC:1;
+  UINT32                  IDT:1;
+  UINT32                  RsvdZ1:3;
+  UINT32                  Type:6;
+  UINT32                  DIR:1;
+  UINT32                  RsvdZ2:15;
+} TRANSFER_TRB_CONTROL_DATA;
+
+//
+// 6.4.1.2.2 Data Stage TRB
+// A Data Stage TRB is used generate the Data stage transaction of a USB Control transfer.
+//
+typedef struct _TRANSFER_TRB_CONTROL_STATUS {
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  RsvdZ3:22;
+  UINT32                  IntTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  ENT:1;
+  UINT32                  RsvdZ4:2;
+  UINT32                  CH:1;
+  UINT32                  IOC:1;
+  UINT32                  RsvdZ5:4;
+  UINT32                  Type:6;
+  UINT32                  DIR:1;
+  UINT32                  RsvdZ6:15;
+} TRANSFER_TRB_CONTROL_STATUS;
+
+//
+// 6.4.2.1 Transfer Event TRB
+// A Transfer Event provides the completion status associated with a Transfer TRB. Refer to section 4.11.3.1
+// for more information on the use and operation of Transfer Events.
+//
+typedef struct _EVT_TRB_TRANSFER {
+  UINT32                  TRBPtrLo;
+
+  UINT32                  TRBPtrHi;
+
+  UINT32                  Length:24;
+  UINT32                  Completecode:8;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ1:1;
+  UINT32                  ED:1;
+  UINT32                  RsvdZ2:7;
+  UINT32                  Type:6;
+  UINT32                  EndpointId:5;
+  UINT32                  RsvdZ3:3;
+  UINT32                  SlotId:8;
+} EVT_TRB_TRANSFER;
+
+//
+// 6.4.2.2 Command Completion Event TRB
+// A Command Completion Event TRB shall be generated by the xHC when a command completes on the
+// Command Ring. Refer to section 4.11.4 for more information on the use of Command Completion Events.
+//
+typedef struct _EVT_TRB_COMMAND_COMPLETION {
+  UINT32                  TRBPtrLo;
+
+  UINT32                  TRBPtrHi;
+
+  UINT32                  RsvdZ2:24;
+  UINT32                  Completecode:8;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  VFID:8;
+  UINT32                  SlotId:8;
+} EVT_TRB_COMMAND_COMPLETION;
+
+typedef union _TRB {
+  TRB_TEMPLATE                TrbTemplate;
+  TRANSFER_TRB_NORMAL         TrbNormal;
+  TRANSFER_TRB_CONTROL_SETUP  TrbCtrSetup;
+  TRANSFER_TRB_CONTROL_DATA   TrbCtrData;
+  TRANSFER_TRB_CONTROL_STATUS TrbCtrStatus;
+} TRB;
+
+//
+// 6.4.3.1 No Op Command TRB
+// The No Op Command TRB provides a simple means for verifying the operation of the Command Ring
+// mechanisms offered by the xHCI.
+//
+typedef struct _CMD_TRB_NO_OP {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ4:16;
+} CMD_TRB_NO_OP;
+
+//
+// 6.4.3.2 Enable Slot Command TRB
+// The Enable Slot Command TRB causes the xHC to select an available Device Slot and return the ID of the
+// selected slot to the host in a Command Completion Event.
+//
+typedef struct _CMD_TRB_ENABLE_SLOT {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ4:16;
+} CMD_TRB_ENABLE_SLOT;
+
+//
+// 6.4.3.3 Disable Slot Command TRB
+// The Disable Slot Command TRB releases any bandwidth assigned to the disabled slot and frees any
+// internal xHC resources assigned to the slot.
+//
+typedef struct _CMD_TRB_DISABLE_SLOT {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ4:8;
+  UINT32                  SlotId:8;
+} CMD_TRB_DISABLE_SLOT;
+
+//
+// 6.4.3.4 Address Device Command TRB
+// The Address Device Command TRB transitions the selected Device Context from the Default to the
+// Addressed state and causes the xHC to select an address for the USB device in the Default State and
+// issue a SET_ADDRESS request to the USB device.
+//
+typedef struct _CMD_TRB_ADDRESS_DEVICE {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:8;
+  UINT32                  BSR:1;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ3:8;
+  UINT32                  SlotId:8;
+} CMD_TRB_ADDRESS_DEVICE;
+
+//
+// 6.4.3.5 Configure Endpoint Command TRB
+// The Configure Endpoint Command TRB evaluates the bandwidth and resource requirements of the
+// endpoints selected by the command.
+//
+typedef struct _CMD_TRB_CONFIG_ENDPOINT {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:8;
+  UINT32                  DC:1;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ3:8;
+  UINT32                  SlotId:8;
+} CMD_TRB_CONFIG_ENDPOINT;
+
+//
+// 6.4.3.6 Evaluate Context Command TRB
+// The Evaluate Context Command TRB is used by system software to inform the xHC that the selected
+// Context data structures in the Device Context have been modified by system software and that the xHC
+// shall evaluate any changes
+//
+typedef struct _CMD_TRB_EVALUATE_CONTEXT {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:9;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ3:8;
+  UINT32                  SlotId:8;
+} CMD_TRB_EVALUATE_CONTEXT;
+
+//
+// 6.4.3.7 Reset Endpoint Command TRB
+// The Reset Endpoint Command TRB is used by system software to reset a specified Transfer Ring
+//
+typedef struct _CMD_TRB_RESET_ENDPOINT {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:8;
+  UINT32                  TSP:1;
+  UINT32                  Type:6;
+  UINT32                  EDID:5;
+  UINT32                  RsvdZ4:3;
+  UINT32                  SlotId:8;
+} CMD_TRB_RESET_ENDPOINT;
+
+//
+// 6.4.3.8 Stop Endpoint Command TRB
+// The Stop Endpoint Command TRB command allows software to stop the xHC execution of the TDs on a
+// Transfer Ring and temporarily take ownership of TDs that had previously been passed to the xHC.
+//
+typedef struct _CMD_TRB_STOP_ENDPOINT {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  EDID:5;
+  UINT32                  RsvdZ4:2;
+  UINT32                  SP:1;
+  UINT32                  SlotId:8;
+} CMD_TRB_STOP_ENDPOINT;
+
+//
+// 6.4.3.9 Set TR Dequeue Pointer Command TRB
+// The Set TR Dequeue Pointer Command TRB is used by system software to modify the TR Dequeue
+// Pointer and DCS fields of an Endpoint or Stream Context.
+//
+typedef struct _CMD_SET_TR_DEQ_POINTER {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1:16;
+  UINT32                  StreamID:16;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:9;
+  UINT32                  Type:6;
+  UINT32                  Endpoint:5;
+  UINT32                  RsvdZ3:3;
+  UINT32                  SlotId:8;
+} CMD_SET_TR_DEQ_POINTER;
+
+//
+// 6.4.4.1 Link TRB
+// A Link TRB provides support for non-contiguous TRB Rings.
+//
+typedef struct _LINK_TRB {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1:22;
+  UINT32                  InterTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  TC:1;
+  UINT32                  RsvdZ2:2;
+  UINT32                  CH:1;
+  UINT32                  IOC:1;
+  UINT32                  RsvdZ3:4;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ4:16;
+} LINK_TRB;
+
+//
+// 6.2.2 Slot Context
+//
+typedef struct _SLOT_CONTEXT {
+  UINT32                  RouteString:20;
+  UINT32                  Speed:4;
+  UINT32                  RsvdZ1:1;
+  UINT32                  MTT:1;
+  UINT32                  Hub:1;
+  UINT32                  ContextEntries:5;
+
+  UINT32                  MaxExitLatency:16;
+  UINT32                  RootHubPortNum:8;
+  UINT32                  PortNum:8;
+
+  UINT32                  TTHubSlotId:8;
+  UINT32                  TTPortNum:8;
+  UINT32                  TTT:2;
+  UINT32                  RsvdZ2:4;
+  UINT32                  InterTarget:10;
+
+  UINT32                  DeviceAddress:8;
+  UINT32                  RsvdZ3:19;
+  UINT32                  SlotState:5;
+
+  UINT32                  RsvdZ4;
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+} SLOT_CONTEXT;
+
+typedef struct _SLOT_CONTEXT_64 {
+  UINT32                  RouteString:20;
+  UINT32                  Speed:4;
+  UINT32                  RsvdZ1:1;
+  UINT32                  MTT:1;
+  UINT32                  Hub:1;
+  UINT32                  ContextEntries:5;
+
+  UINT32                  MaxExitLatency:16;
+  UINT32                  RootHubPortNum:8;
+  UINT32                  PortNum:8;
+
+  UINT32                  TTHubSlotId:8;
+  UINT32                  TTPortNum:8;
+  UINT32                  TTT:2;
+  UINT32                  RsvdZ2:4;
+  UINT32                  InterTarget:10;
+
+  UINT32                  DeviceAddress:8;
+  UINT32                  RsvdZ3:19;
+  UINT32                  SlotState:5;
+
+  UINT32                  RsvdZ4;
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+
+  UINT32                  RsvdZ8;
+  UINT32                  RsvdZ9;
+  UINT32                  RsvdZ10;
+  UINT32                  RsvdZ11;
+  
+  UINT32                  RsvdZ12;
+  UINT32                  RsvdZ13;
+  UINT32                  RsvdZ14;
+  UINT32                  RsvdZ15;
+
+} SLOT_CONTEXT_64;
+
+
+//
+// 6.2.3 Endpoint Context
+//
+typedef struct _ENDPOINT_CONTEXT {
+  UINT32                  EPState:3;
+  UINT32                  RsvdZ1:5;
+  UINT32                  Mult:2;
+  UINT32                  MaxPStreams:5;
+  UINT32                  LSA:1;
+  UINT32                  Interval:8;
+  UINT32                  RsvdZ2:8;
+
+  UINT32                  RsvdZ3:1;
+  UINT32                  CErr:2;
+  UINT32                  EPType:3;
+  UINT32                  RsvdZ4:1;
+  UINT32                  HID:1;
+  UINT32                  MaxBurstSize:8;
+  UINT32                  MaxPacketSize:16;
+
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  AverageTRBLength:16;
+  UINT32                  MaxESITPayload:16;
+
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+} ENDPOINT_CONTEXT;
+
+typedef struct _ENDPOINT_CONTEXT_64 {
+  UINT32                  EPState:3;
+  UINT32                  RsvdZ1:5;
+  UINT32                  Mult:2;
+  UINT32                  MaxPStreams:5;
+  UINT32                  LSA:1;
+  UINT32                  Interval:8;
+  UINT32                  RsvdZ2:8;
+
+  UINT32                  RsvdZ3:1;
+  UINT32                  CErr:2;
+  UINT32                  EPType:3;
+  UINT32                  RsvdZ4:1;
+  UINT32                  HID:1;
+  UINT32                  MaxBurstSize:8;
+  UINT32                  MaxPacketSize:16;
+
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  AverageTRBLength:16;
+  UINT32                  MaxESITPayload:16;
+
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+  
+  UINT32                  RsvdZ8;
+  UINT32                  RsvdZ9;
+  UINT32                  RsvdZ10;
+  UINT32                  RsvdZ11;
+  
+  UINT32                  RsvdZ12;
+  UINT32                  RsvdZ13;
+  UINT32                  RsvdZ14;
+  UINT32                  RsvdZ15;
+
+} ENDPOINT_CONTEXT_64;
+
+
+//
+// 6.2.5.1 Input Control Context
+//
+typedef struct _INPUT_CONTRL_CONTEXT {
+  UINT32                  Dword1;
+  UINT32                  Dword2;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+  UINT32                  RsvdZ3;
+  UINT32                  RsvdZ4;
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+} INPUT_CONTRL_CONTEXT;
+
+typedef struct _INPUT_CONTRL_CONTEXT_64 {
+  UINT32                  Dword1;
+  UINT32                  Dword2;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+  UINT32                  RsvdZ3;
+  UINT32                  RsvdZ4;
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+  UINT32                  RsvdZ8;
+  UINT32                  RsvdZ9;
+  UINT32                  RsvdZ10;
+  UINT32                  RsvdZ11;
+  UINT32                  RsvdZ12;
+  UINT32                  RsvdZ13;
+  UINT32                  RsvdZ14;
+} INPUT_CONTRL_CONTEXT_64;
+
+//
+// 6.2.1 Device Context
+//
+typedef struct _DEVICE_CONTEXT {
+  SLOT_CONTEXT            Slot;
+  ENDPOINT_CONTEXT        EP[31];
+} DEVICE_CONTEXT;
+
+typedef struct _DEVICE_CONTEXT_64 {
+  SLOT_CONTEXT_64         Slot;
+  ENDPOINT_CONTEXT_64     EP[31];
+} DEVICE_CONTEXT_64;
+
+//
+// 6.2.5 Input Context
+//
+typedef struct _INPUT_CONTEXT {
+  INPUT_CONTRL_CONTEXT    InputControlContext;
+  SLOT_CONTEXT            Slot;
+  ENDPOINT_CONTEXT        EP[31];
+} INPUT_CONTEXT;
+
+typedef struct _INPUT_CONTEXT_64 {
+  INPUT_CONTRL_CONTEXT_64 InputControlContext;
+  SLOT_CONTEXT_64         Slot;
+  ENDPOINT_CONTEXT_64     EP[31];
+} INPUT_CONTEXT_64;
+
+
+/**
+  Initialize the XHCI host controller for schedule.
+
+  @param  Xhc        The XHCI Instance to be initialized.
+
+**/
+VOID
+XhcInitSched (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Free the resouce allocated at initializing schedule.
+
+  @param  Xhc        The XHCI Instance.
+
+**/
+VOID
+XhcFreeSched (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed through URB.
+
+  @param  Xhc           The XHCI Instance.
+  @param  Urb           The URB to be rung.
+
+  @retval EFI_SUCCESS   Successfully ring the door bell.
+
+**/
+EFI_STATUS
+RingIntTransferDoorBell (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  );
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Xhc               The XHCI Instance.
+  @param  CmdTransfer       The executed URB is for cmd transfer or not.
+  @param  Urb               The URB to execute.
+  @param  Timeout           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+XhcExecTransfer (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  BOOLEAN             CmdTransfer,
+  IN  URB                 *Urb,
+  IN  UINTN               Timeout
+  );
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  BusAddr               The logical device address assigned by UsbBus driver.
+  @param  EpNum                 The endpoint of the target.
+
+  @retval EFI_SUCCESS           An asynchronous transfer is removed.
+  @retval EFI_NOT_FOUND         No transfer for the device is found.
+
+**/
+EFI_STATUS
+XhciDelAsyncIntTransfer (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT8               BusAddr,
+  IN  UINT8               EpNum
+  );
+
+/**
+  Remove all the asynchronous interrupt transfers.
+
+  @param  Xhc                   The XHCI Instance.
+
+**/
+VOID
+XhciDelAllAsyncIntTransfers (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Set Bios Ownership
+
+  @param  Xhc          The XHCI Instance.
+
+**/
+VOID
+XhcSetBiosOwnership (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Clear Bios Ownership
+
+  @param  Xhc       The XHCI Instance.
+
+**/
+VOID
+XhcClearBiosOwnership (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Find out the slot id according to the device's route string.
+
+  @param  Xhc             The XHCI Instance.
+  @param  RouteString     The route string described the device location.
+
+  @return The slot id used by the device.
+
+**/
+UINT8
+EFIAPI
+XhcRouteStringToSlotId (
+  IN  USB_XHCI_INSTANCE  *Xhc,
+  IN  USB_DEV_ROUTE      RouteString
+  );
+
+/**
+  Calculate the device context index by endpoint address and direction.
+
+  @param  EpAddr              The target endpoint number.
+  @param  Direction           The direction of the target endpoint.
+
+  @return The device context index of endpoint.
+
+**/
+UINT8
+XhcEndpointToDci (
+  IN  UINT8                   EpAddr,
+  IN  UINT8                   Direction
+  );
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Successfully ring the door bell.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcRingDoorBell (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT8                SlotId,
+  IN UINT8                Dci
+  );
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                 Interrupt event.
+  @param  Context               Pointer to USB_XHCI_INSTANCE.
+
+**/
+VOID
+EFIAPI
+XhcMonitorAsyncRequests (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+
+/**
+  Monitor the port status change. Enable/Disable device slot if there is a device attached/detached.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  ParentRouteChart      The route string pointed to the parent device if it exists.
+  @param  Port                  The port to be polled.
+  @param  PortState             The port state.
+
+  @retval EFI_SUCCESS           Successfully enable/disable device slot according to port state.
+  @retval Others                Should not appear.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPollPortStatusChange (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  USB_DEV_ROUTE         ParentRouteChart,
+  IN  UINT8                 Port,
+  IN  EFI_USB_PORT_STATUS   *PortState
+  );
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcConfigHubContext (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    PortNum,
+  IN UINT8                    TTT,
+  IN UINT8                    MTT
+  );
+
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcConfigHubContext64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    PortNum,
+  IN UINT8                    TTT,
+  IN UINT8                    MTT
+  );
+
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetConfigCmd (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc
+  );
+
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetConfigCmd64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc
+  );
+
+/**
+  Set interface through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+  @param  Request       USB device request to send.
+
+  @retval EFI_SUCCESS   Successfully set interface.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetInterface (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc,
+  IN EFI_USB_DEVICE_REQUEST   *Request
+  );
+
+/**
+  Set interface through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+  @param  Request       USB device request to send.
+
+  @retval EFI_SUCCESS   Successfully set interface.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetInterface64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc,
+  IN EFI_USB_DEVICE_REQUEST   *Request
+  );
+
+/**
+  Find out the actual device address according to the requested device address from UsbBus.
+
+  @param  Xhc             The XHCI Instance.
+  @param  BusDevAddr      The requested device address by UsbBus upper driver.
+
+  @return The actual device address assigned to the device.
+
+**/
+UINT8
+EFIAPI
+XhcBusDevAddrToSlotId (
+  IN  USB_XHCI_INSTANCE  *Xhc,
+  IN  UINT8              BusDevAddr
+  );
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  ParentRouteChart    The route string pointed to the parent device.
+  @param  ParentPort          The port at which the device is located.
+  @param  RouteChart          The route string pointed to the device.
+  @param  DeviceSpeed         The device speed.
+
+  @retval EFI_SUCCESS   Successfully assign a slot to the device and assign an address to it.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcInitializeDeviceSlot (
+  IN  USB_XHCI_INSTANCE         *Xhc,
+  IN  USB_DEV_ROUTE             ParentRouteChart,
+  IN  UINT16                    ParentPort,
+  IN  USB_DEV_ROUTE             RouteChart,
+  IN  UINT8                     DeviceSpeed
+  );
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  ParentRouteChart    The route string pointed to the parent device.
+  @param  ParentPort          The port at which the device is located.
+  @param  RouteChart          The route string pointed to the device.
+  @param  DeviceSpeed         The device speed.
+
+  @retval EFI_SUCCESS   Successfully assign a slot to the device and assign an address to it.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcInitializeDeviceSlot64 (
+  IN  USB_XHCI_INSTANCE         *Xhc,
+  IN  USB_DEV_ROUTE             ParentRouteChart,
+  IN  UINT16                    ParentPort,
+  IN  USB_DEV_ROUTE             RouteChart,
+  IN  UINT8                     DeviceSpeed
+  );
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcEvaluateContext (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT32                   MaxPacketSize
+  );
+
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcEvaluateContext64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT32                   MaxPacketSize
+  );
+
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDisableSlotCmd (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId
+  );
+
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDisableSlotCmd64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId
+  );
+
+
+/**
+  Synchronize the specified transfer ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc         The XHCI Instance.
+  @param  TrsRing     The transfer ring to sync.
+
+  @retval EFI_SUCCESS The transfer ring is synchronized successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncTrsRing (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  TRANSFER_RING           *TrsRing
+  );
+
+/**
+  Synchronize the specified event ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc         The XHCI Instance.
+  @param  EvtRing     The event ring to sync.
+
+  @retval EFI_SUCCESS The event ring is synchronized successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncEventRing (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  EVENT_RING              *EvtRing
+  );
+
+/**
+  Check if there is a new generated event.
+
+  @param  Xhc           The XHCI Instance.
+  @param  EvtRing       The event ring to check.
+  @param  NewEvtTrb     The new event TRB found.
+
+  @retval EFI_SUCCESS   Found a new event TRB at the event ring.
+  @retval EFI_NOT_READY The event ring has no new event.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcCheckNewEvent (
+  IN  USB_XHCI_INSTANCE       *Xhc,
+  IN  EVENT_RING              *EvtRing,
+  OUT TRB_TEMPLATE            **NewEvtTrb
+  );
+
+/**
+  Create XHCI transfer ring.
+
+  @param  Xhc               The XHCI Instance.
+  @param  TrbNum            The number of TRB in the ring.
+  @param  TransferRing           The created transfer ring.
+
+**/
+VOID
+CreateTransferRing (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  UINTN                 TrbNum,
+  OUT TRANSFER_RING         *TransferRing
+  );
+
+/**
+  Create XHCI event ring.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  EventRing           The created event ring.
+
+**/
+VOID
+CreateEventRing (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  OUT EVENT_RING            *EventRing
+  );
+
+/**
+  System software shall use a Reset Endpoint Command (section 4.11.4.7) to remove the Halted
+  condition in the xHC. After the successful completion of the Reset Endpoint Command, the Endpoint
+  Context is transitioned from the Halted to the Stopped state and the Transfer Ring of the endpoint is
+  reenabled. The next write to the Doorbell of the Endpoint will transition the Endpoint Context from the
+  Stopped to the Running state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which makes the endpoint halted.
+
+  @retval EFI_SUCCESS           The recovery is successful.
+  @retval Others                Failed to recovery halted endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcRecoverHaltedEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  );
+
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDequeueTrbFromEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  );
+
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Stop endpoint successfully.
+  @retval Others                Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcStopEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  );
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Reset endpoint successfully.
+  @retval Others                Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcResetEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  );
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+  @param  Urb                   The dequeue pointer of the transfer ring specified
+                                by the urb to be updated.
+
+  @retval EFI_SUCCESS           Set transfer ring dequeue pointer succeeds.
+  @retval Others                Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetTrDequeuePointer (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci,
+  IN URB                    *Urb
+  );
+
+/**
+  Create a new URB for a new transaction.
+
+  @param  Xhc       The XHCI Instance
+  @param  DevAddr   The device address
+  @param  EpAddr    Endpoint addrress
+  @param  DevSpeed  The device speed
+  @param  MaxPacket The max packet length of the endpoint
+  @param  Type      The transaction type
+  @param  Request   The standard USB request for control transfer
+  @param  Data      The user data to transfer
+  @param  DataLen   The length of data buffer
+  @param  Callback  The function to call when data is transferred
+  @param  Context   The context to the callback
+
+  @return Created URB or NULL
+
+**/
+URB*
+XhcCreateUrb (
+  IN USB_XHCI_INSTANCE                  *Xhc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINTN                              MaxPacket,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context
+  );
+
+/**
+  Free an allocated URB.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The URB to free.
+
+**/
+VOID
+XhcFreeUrb (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN URB                  *Urb
+  );
+
+/**
+  Create a transfer TRB.
+
+  @param  Xhc     The XHCI Instance
+  @param  Urb     The urb used to construct the transfer TRB.
+
+  @return Created TRB or NULL
+
+**/
+EFI_STATUS
+XhcCreateTransferTrb (
+  IN USB_XHCI_INSTANCE            *Xhc,
+  IN URB                          *Urb
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c
new file mode 100644
index 0000000000..663ba39d32
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c
@@ -0,0 +1,662 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2014 - 2015, 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 "XhcPeim.h"
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pages         How many pages to allocate.
+
+  @return Pointer to the allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN UINTN              Pages
+  )
+{
+  USBHC_MEM_BLOCK       *Block;
+  EFI_STATUS            Status;
+  UINTN                 PageNumber;
+  EFI_PHYSICAL_ADDRESS  TempPtr;
+
+  PageNumber = EFI_SIZE_TO_PAGES (sizeof (USBHC_MEM_BLOCK));
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             PageNumber,
+             &TempPtr
+             );
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (PageNumber));
+
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+
+  Block = (USBHC_MEM_BLOCK *) (UINTN) TempPtr;
+  Block->BufLen = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen = Block->BufLen / (USBHC_MEM_UNIT * 8);
+
+  PageNumber = EFI_SIZE_TO_PAGES (Block->BitsLen);
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             PageNumber,
+             &TempPtr
+             );
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (PageNumber));
+
+  Block->Bits = (UINT8 *) (UINTN) TempPtr;
+
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             Pages,
+             &TempPtr
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (Pages));
+
+  Block->BufHost = (UINT8 *) (UINTN) TempPtr;;
+  Block->Buf = (UINT8 *) (UINTN) TempPtr;
+  Block->Next = NULL;
+
+  return Block;
+}
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool          The memory pool to free the block from.
+  @param  Block         The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL     *Pool,
+  IN USBHC_MEM_BLOCK    *Block
+  )
+{
+  ASSERT ((Pool != NULL) && (Block != NULL));
+  //
+  // No free memory in PEI.
+  //
+}
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block         The memory block to allocate memory from.
+  @param  Units         Number of memory units to allocate.
+
+  @return The pointer to the allocated memory.
+          If couldn't allocate the needed memory, the return value is NULL.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN USBHC_MEM_BLOCK    *Block,
+  IN UINTN              Units
+  )
+{
+  UINTN                 Byte;
+  UINT8                 Bit;
+  UINTN                 StartByte;
+  UINT8                 StartBit;
+  UINTN                 Available;
+  UINTN                 Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | (UINT8) USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->BufHost + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  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
+UsbHcGetPciAddrForHostAddr (
+  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;
+}
+
+/**
+  Calculate the corresponding host address according to the pci address.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The pointer to pci memory.
+  @param  Size          The size of the memory region.
+
+  @return               The host memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetHostAddrForPciAddr (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  )
+{
+  USBHC_MEM_BLOCK       *Head;
+  USBHC_MEM_BLOCK       *Block;
+  UINTN                 AllocSize;
+  EFI_PHYSICAL_ADDRESS  HostAddr;
+  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->Buf <= (UINT8 *) Mem) && (((UINT8 *) Mem + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      break;
+    }
+  }
+
+  ASSERT ((Block != NULL));
+  //
+  // calculate the host memory address for pci memory address.
+  //
+  Offset = (UINT8 *) Mem - Block->Buf;
+  HostAddr = (EFI_PHYSICAL_ADDRESS) (UINTN) (Block->BufHost + Offset);
+  return HostAddr;
+}
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head          The head of the memory pool's block list.
+  @param  Block         The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK    *Head,
+  IN USBHC_MEM_BLOCK    *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+/**
+  Is the memory block empty?
+
+  @param  Block         The memory block to check.
+
+  @retval TRUE          The memory block is empty.
+  @retval FALSE         The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK    *Block
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head          The block list head of the memory's pool.
+  @param  BlockToUnlink The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK    *Head,
+  IN USBHC_MEM_BLOCK    *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK       *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @return Pointer to the allocated memory pool or NULL if failed.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  VOID
+  )
+{
+  USBHC_MEM_POOL        *Pool;
+  UINTN                 PageNumber;
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  TempPtr;
+
+  PageNumber = EFI_SIZE_TO_PAGES (sizeof (USBHC_MEM_POOL));
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             PageNumber,
+             &TempPtr
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (PageNumber));
+
+  Pool = (USBHC_MEM_POOL *) ((UINTN) TempPtr);
+  Pool->Head = UsbHcAllocMemBlock (USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    //
+    // No free memory in PEI.
+    //
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+/**
+  Release the memory management pool.
+
+  @param  Pool          The USB memory pool to free.
+
+**/
+VOID
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL     *Pool
+  )
+{
+  USBHC_MEM_BLOCK       *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UsbHcUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    //UsbHcUnlinkMemBlock (Pool->Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+}
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool          The host controller's memory pool.
+  @param  Size          Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN USBHC_MEM_POOL     *Pool,
+  IN UINTN              Size
+  )
+{
+  USBHC_MEM_BLOCK       *Head;
+  USBHC_MEM_BLOCK       *Block;
+  USBHC_MEM_BLOCK       *NewBlock;
+  VOID                  *Mem;
+  UINTN                 AllocSize;
+  UINTN                 Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize);
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+  NewBlock = UsbHcAllocMemBlock (Pages);
+
+  if (NewBlock == NULL) {
+    return NULL;
+  }
+
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The memory to free.
+  @param  Size          The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  )
+{
+  USBHC_MEM_BLOCK       *Head;
+  USBHC_MEM_BLOCK       *Block;
+  UINT8                 *ToFree;
+  UINTN                 AllocSize;
+  UINTN                 Byte;
+  UINTN                 Bit;
+  UINTN                 Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->BufHost <= ToFree) && ((ToFree + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory pointer
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    //UsbHcUnlinkMemBlock (Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+}
+
+/**
+  Allocates pages at a specified alignment.
+
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+
+  @param  Pages                 The number of pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to
+                                use to access the hosts HostAddress.
+
+  @retval EFI_SUCCESS           Success to allocate aligned pages.
+  @retval EFI_INVALID_PARAMETER Pages or Alignment is not valid.
+  @retval EFI_OUT_OF_RESOURCES  Do not have enough resources to allocate memory.
+
+**/
+EFI_STATUS
+UsbHcAllocateAlignedPages (
+  IN UINTN                      Pages,
+  IN UINTN                      Alignment,
+  OUT VOID                      **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS      *DeviceAddress
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  Memory;
+  UINTN                 AlignedMemory;
+  UINTN                 AlignmentMask;
+  UINTN                 RealPages;
+
+  //
+  // Alignment must be a power of two or zero.
+  //
+  ASSERT ((Alignment & (Alignment - 1)) == 0);
+
+  if ((Alignment & (Alignment - 1)) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Pages == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Alignment > EFI_PAGE_SIZE) {
+    //
+    // Calculate the total number of pages since alignment is larger than page size.
+    //
+    AlignmentMask  = Alignment - 1;
+    RealPages      = Pages + EFI_SIZE_TO_PAGES (Alignment);
+    //
+    // Make sure that Pages plus EFI_SIZE_TO_PAGES (Alignment) does not overflow.
+    //
+    ASSERT (RealPages > Pages);
+
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesData,
+               Pages,
+               &Memory
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    AlignedMemory = ((UINTN) Memory + AlignmentMask) & ~AlignmentMask;
+  } else {
+    //
+    // Do not over-allocate pages in this case.
+    //
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesData,
+               Pages,
+               &Memory
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    AlignedMemory = (UINTN) Memory;
+  }
+
+  *HostAddress = (VOID *) AlignedMemory;
+  *DeviceAddress = (EFI_PHYSICAL_ADDRESS) AlignedMemory;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Frees memory that was allocated with UsbHcAllocateAlignedPages().
+
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  Pages                 The number of pages to free.
+
+**/
+VOID
+UsbHcFreeAlignedPages (
+  IN VOID               *HostAddress,
+  IN UINTN              Pages
+  )
+{
+  ASSERT (Pages != 0);
+  //
+  // No free memory in PEI.
+  //
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.h
new file mode 100644
index 0000000000..c314e92004
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.h
@@ -0,0 +1,142 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2014, Intel Corporation. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PEI_XHCI_MEM_H_
+#define _EFI_PEI_XHCI_MEM_H_
+
+#include <Uefi.h>
+
+#define USBHC_MEM_DEFAULT_PAGES 16
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+
+struct _USBHC_MEM_BLOCK {
+  UINT8                 *Bits;  // Bit array to record which unit is allocated
+  UINTN                 BitsLen;
+  UINT8                 *Buf;
+  UINT8                 *BufHost;
+  UINTN                 BufLen; // Memory size in bytes
+  USBHC_MEM_BLOCK       *Next;
+};
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT          64
+
+#define USBHC_MEM_UNIT_MASK     (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_ROUND(Len)    (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+#define USB_HC_BIT(a)           ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)  \
+          do {               \
+            (Bit)++;         \
+            if ((Bit) > 7) { \
+              (Byte)++;      \
+              (Bit) = 0;     \
+            }                \
+          } while (0)
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB
+// host controller. XHCI requires the control memory and transfer
+// data to be on the same 4G memory.
+//
+typedef struct _USBHC_MEM_POOL {
+  BOOLEAN               Check4G;
+  UINT32                Which4G;
+  USBHC_MEM_BLOCK       *Head;
+} USBHC_MEM_POOL;
+
+/**
+  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
+UsbHcGetPciAddrForHostAddr (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  );
+
+/**
+  Calculate the corresponding host address according to the pci address.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The pointer to pci memory.
+  @param  Size          The size of the memory region.
+
+  @return               The host memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetHostAddrForPciAddr (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  );
+
+/**
+  Allocates pages at a specified alignment.
+
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+
+  @param  Pages                 The number of pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to
+                                use to access the hosts HostAddress.
+
+  @retval EFI_SUCCESS           Success to allocate aligned pages.
+  @retval EFI_INVALID_PARAMETER Pages or Alignment is not valid.
+  @retval EFI_OUT_OF_RESOURCES  Do not have enough resources to allocate memory.
+
+**/
+EFI_STATUS
+UsbHcAllocateAlignedPages (
+  IN UINTN                      Pages,
+  IN UINTN                      Alignment,
+  OUT VOID                      **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS      *DeviceAddress
+  );
+
+/**
+  Frees memory that was allocated with UsbHcAllocateAlignedPages().
+
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  Pages                 The number of pages to free.
+
+**/
+VOID
+UsbHcFreeAlignedPages (
+  IN VOID               *HostAddress,
+  IN UINTN              Pages
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.c b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.c
new file mode 100644
index 0000000000..eaea38d94d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.c
@@ -0,0 +1,1534 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2014 - 2015, 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 "XhcPeim.h"
+
+//
+// Two arrays used to translate the XHCI port state (change)
+// to the UEFI protocol's port state (change).
+//
+USB_PORT_STATE_MAP  mUsbPortStateMap[] = {
+  {XHC_PORTSC_CCS,   USB_PORT_STAT_CONNECTION},
+  {XHC_PORTSC_PED,   USB_PORT_STAT_ENABLE},
+  {XHC_PORTSC_OCA,   USB_PORT_STAT_OVERCURRENT},
+  {XHC_PORTSC_PP,    USB_PORT_STAT_POWER},
+  {XHC_PORTSC_RESET, USB_PORT_STAT_RESET}
+};
+
+USB_PORT_STATE_MAP  mUsbPortChangeMap[] = {
+  {XHC_PORTSC_CSC, USB_PORT_STAT_C_CONNECTION},
+  {XHC_PORTSC_PEC, USB_PORT_STAT_C_ENABLE},
+  {XHC_PORTSC_OCC, USB_PORT_STAT_C_OVERCURRENT},
+  {XHC_PORTSC_PRC, USB_PORT_STAT_C_RESET}
+};
+
+USB_CLEAR_PORT_MAP mUsbClearPortChangeMap[] = {
+  {XHC_PORTSC_CSC, EfiUsbPortConnectChange},
+  {XHC_PORTSC_PEC, EfiUsbPortEnableChange},
+  {XHC_PORTSC_OCC, EfiUsbPortOverCurrentChange},
+  {XHC_PORTSC_PRC, EfiUsbPortResetChange}
+};
+
+USB_PORT_STATE_MAP  mUsbHubPortStateMap[] = {
+  {XHC_HUB_PORTSC_CCS,   USB_PORT_STAT_CONNECTION},
+  {XHC_HUB_PORTSC_PED,   USB_PORT_STAT_ENABLE},
+  {XHC_HUB_PORTSC_OCA,   USB_PORT_STAT_OVERCURRENT},
+  {XHC_HUB_PORTSC_PP,    USB_PORT_STAT_POWER},
+  {XHC_HUB_PORTSC_RESET, USB_PORT_STAT_RESET}
+};
+
+USB_PORT_STATE_MAP  mUsbHubPortChangeMap[] = {
+  {XHC_HUB_PORTSC_CSC, USB_PORT_STAT_C_CONNECTION},
+  {XHC_HUB_PORTSC_PEC, USB_PORT_STAT_C_ENABLE},
+  {XHC_HUB_PORTSC_OCC, USB_PORT_STAT_C_OVERCURRENT},
+  {XHC_HUB_PORTSC_PRC, USB_PORT_STAT_C_RESET}
+};
+
+USB_CLEAR_PORT_MAP mUsbHubClearPortChangeMap[] = {
+  {XHC_HUB_PORTSC_CSC, EfiUsbPortConnectChange},
+  {XHC_HUB_PORTSC_PEC, EfiUsbPortEnableChange},
+  {XHC_HUB_PORTSC_OCC, EfiUsbPortOverCurrentChange},
+  {XHC_HUB_PORTSC_PRC, EfiUsbPortResetChange},
+  {XHC_HUB_PORTSC_BHRC, Usb3PortBHPortResetChange}
+};
+
+/**
+  Read XHCI Operation register.
+
+  @param Xhc            The XHCI device.
+  @param Offset         The operation register offset.
+
+  @retval the register content read.
+
+**/
+UINT32
+XhcPeiReadOpReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset
+  )
+{
+  UINT32                Data;
+
+  ASSERT (Xhc->CapLength != 0);
+
+  Data = MmioRead32 (Xhc->UsbHostControllerBaseAddress + Xhc->CapLength + Offset);
+  return Data;
+}
+
+/**
+  Write the data to the XHCI operation register.
+
+  @param Xhc            The XHCI device.
+  @param Offset         The operation register offset.
+  @param Data           The data to write.
+
+**/
+VOID
+XhcPeiWriteOpReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  )
+{
+  ASSERT (Xhc->CapLength != 0);
+
+  MmioWrite32 (Xhc->UsbHostControllerBaseAddress + Xhc->CapLength + Offset, Data);
+}
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiSetOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  )
+{
+  UINT32                Data;
+
+  Data  = XhcPeiReadOpReg (Xhc, Offset);
+  Data |= Bit;
+  XhcPeiWriteOpReg (Xhc, Offset, Data);
+}
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to clear.
+
+**/
+VOID
+XhcPeiClearOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  )
+{
+  UINT32                Data;
+
+  Data  = XhcPeiReadOpReg (Xhc, Offset);
+  Data &= ~Bit;
+  XhcPeiWriteOpReg (Xhc, Offset, Data);
+}
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to become set (or clear).
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to wait for.
+  @param  WaitToSet     Wait the bit to set or clear.
+  @param  Timeout       The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS   The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT   The time out occurred.
+
+**/
+EFI_STATUS
+XhcPeiWaitOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit,
+  IN BOOLEAN            WaitToSet,
+  IN UINT32             Timeout
+  )
+{
+  UINT64                Index;
+
+  for (Index = 0; Index < Timeout * XHC_1_MILLISECOND; Index++) {
+    if (XHC_REG_BIT_IS_SET (Xhc, Offset, Bit) == WaitToSet) {
+      return EFI_SUCCESS;
+    }
+
+    MicroSecondDelay (XHC_1_MICROSECOND);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Read XHCI capability register.
+
+  @param Xhc        The XHCI device.
+  @param Offset     Capability register address.
+
+  @retval the register content read.
+
+**/
+UINT32
+XhcPeiReadCapRegister (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset
+  )
+{
+  UINT32                Data;
+
+  Data = MmioRead32 (Xhc->UsbHostControllerBaseAddress + Offset);
+
+  return Data;
+}
+
+/**
+  Read XHCI door bell register.
+
+  @param  Xhc       The XHCI device.
+  @param  Offset    The offset of the door bell register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcPeiReadDoorBellReg (
+  IN  PEI_XHC_DEV       *Xhc,
+  IN  UINT32            Offset
+  )
+{
+  UINT32                  Data;
+
+  ASSERT (Xhc->DBOff != 0);
+
+  Data = MmioRead32 (Xhc->UsbHostControllerBaseAddress + Xhc->DBOff + Offset);
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI door bell register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the door bell register.
+  @param  Data          The data to write.
+
+**/
+VOID
+XhcPeiWriteDoorBellReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  )
+{
+  ASSERT (Xhc->DBOff != 0);
+
+  MmioWrite32 (Xhc->UsbHostControllerBaseAddress + Xhc->DBOff + Offset, Data);
+}
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcPeiReadRuntimeReg (
+  IN  PEI_XHC_DEV       *Xhc,
+  IN  UINT32            Offset
+  )
+{
+  UINT32                Data;
+
+  ASSERT (Xhc->RTSOff != 0);
+
+  Data = MmioRead32 (Xhc->UsbHostControllerBaseAddress + Xhc->RTSOff + Offset);
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc       The XHCI device.
+  @param  Offset    The offset of the runtime register.
+  @param  Data      The data to write.
+
+**/
+VOID
+XhcPeiWriteRuntimeReg (
+  IN PEI_XHC_DEV          *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  ASSERT (Xhc->RTSOff != 0);
+
+  MmioWrite32 (Xhc->UsbHostControllerBaseAddress + Xhc->RTSOff + Offset, Data);
+}
+
+/**
+  Set one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI device.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiSetRuntimeRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  )
+{
+  UINT32                Data;
+
+  Data  = XhcPeiReadRuntimeReg (Xhc, Offset);
+  Data |= Bit;
+  XhcPeiWriteRuntimeReg (Xhc, Offset, Data);
+}
+
+/**
+  Clear one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI device.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiClearRuntimeRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  )
+{
+  UINT32                Data;
+
+  Data  = XhcPeiReadRuntimeReg (Xhc, Offset);
+  Data &= ~Bit;
+  XhcPeiWriteRuntimeReg (Xhc, Offset, Data);
+}
+
+/**
+  Check whether Xhc is halted.
+
+  @param  Xhc           The XHCI device.
+
+  @retval TRUE          The controller is halted.
+  @retval FALSE         The controller isn't halted.
+
+**/
+BOOLEAN
+XhcPeiIsHalt (
+  IN PEI_XHC_DEV        *Xhc
+  )
+{
+  return XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT);
+}
+
+/**
+  Check whether system error occurred.
+
+  @param  Xhc           The XHCI device.
+
+  @retval TRUE          System error happened.
+  @retval FALSE         No system error.
+
+**/
+BOOLEAN
+XhcPeiIsSysError (
+  IN PEI_XHC_DEV        *Xhc
+  )
+{
+  return XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HSE);
+}
+
+/**
+  Reset the host controller.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT   The transfer failed due to time out.
+  @retval Others        Failed to reset the host.
+
+**/
+EFI_STATUS
+XhcPeiResetHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  )
+{
+  EFI_STATUS            Status;
+
+  //
+  // Host can only be reset when it is halt. If not so, halt it
+  //
+  if (!XhcPeiIsHalt (Xhc)) {
+    Status = XhcPeiHaltHC (Xhc, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  XhcPeiSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET);
+  Status = XhcPeiWaitOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET, FALSE, Timeout);
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcPeiResetHC: %r\n", Status));
+  return Status;
+}
+
+/**
+  Halt the host controller.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort.
+
+  @retval EFI_TIMEOUT   Failed to halt the controller before Timeout.
+  @retval EFI_SUCCESS   The XHCI is halt.
+
+**/
+EFI_STATUS
+XhcPeiHaltHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  )
+{
+  EFI_STATUS            Status;
+
+  XhcPeiClearOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RUN);
+  Status = XhcPeiWaitOpRegBit (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT, TRUE, Timeout);
+  DEBUG ((EFI_D_INFO, "XhcPeiHaltHC: %r\n", Status));
+  return Status;
+}
+
+/**
+  Set the XHCI to run.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort.
+
+  @retval EFI_SUCCESS   The XHCI is running.
+  @retval Others        Failed to set the XHCI to run.
+
+**/
+EFI_STATUS
+XhcPeiRunHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  )
+{
+  EFI_STATUS            Status;
+
+  XhcPeiSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RUN);
+  Status = XhcPeiWaitOpRegBit (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT, FALSE, Timeout);
+  DEBUG ((EFI_D_INFO, "XhcPeiRunHC: %r\n", Status));
+  return Status;
+}
+
+/**
+  Submits control transfer to a target USB device.
+
+  @param  PeiServices               The pointer of EFI_PEI_SERVICES.
+  @param  This                      The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress             The target device address.
+  @param  DeviceSpeed               Target device speed.
+  @param  MaximumPacketLength       Maximum packet size the default control transfer
+                                    endpoint is capable of sending or receiving.
+  @param  Request                   USB device request to send.
+  @param  TransferDirection         Specifies the data direction for the data stage.
+  @param  Data                      Data buffer to be transmitted or received from USB device.
+  @param  DataLength                The size (in bytes) of the data buffer.
+  @param  TimeOut                   Indicates the maximum timeout, in millisecond.
+                                    If Timeout is 0, then the caller must wait for the function
+                                    to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Translator                Transaction translator to be used by this device.
+  @param  TransferResult            Return the result of this control transfer.
+
+  @retval EFI_SUCCESS               Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES      The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER     Some parameters are invalid.
+  @retval EFI_TIMEOUT               Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR          Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiControlTransfer (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN UINT8                                  DeviceAddress,
+  IN UINT8                                  DeviceSpeed,
+  IN UINTN                                  MaximumPacketLength,
+  IN EFI_USB_DEVICE_REQUEST                 *Request,
+  IN EFI_USB_DATA_DIRECTION                 TransferDirection,
+  IN OUT VOID                               *Data,
+  IN OUT UINTN                              *DataLength,
+  IN UINTN                                  TimeOut,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR     *Translator,
+  OUT UINT32                                *TransferResult
+  )
+{
+  PEI_XHC_DEV                   *Xhc;
+  URB                           *Urb;
+  UINT8                         Endpoint;
+  UINT8                         Index;
+  UINT8                         DescriptorType;
+  UINT8                         SlotId;
+  UINT8                         TTT;
+  UINT8                         MTT;
+  UINT32                        MaxPacket0;
+  EFI_USB_HUB_DESCRIPTOR        *HubDesc;
+  EFI_STATUS                    Status;
+  EFI_STATUS                    RecoveryStatus;
+  UINTN                         MapSize;
+  EFI_USB_PORT_STATUS           PortStatus;
+  UINT32                        State;
+  EFI_USB_DEVICE_REQUEST        ClearPortRequest;
+  UINTN                         Len;
+
+  //
+  // Validate parameters
+  //
+  if ((Request == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbDataIn) &&
+      (TransferDirection != EfiUsbDataOut) &&
+      (TransferDirection != EfiUsbNoData)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection == EfiUsbNoData) &&
+      ((Data != NULL) || (*DataLength != 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) &&
+     ((Data == NULL) || (*DataLength == 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8)  && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64) &&
+      (MaximumPacketLength != 512)
+      ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_SUPER) && (MaximumPacketLength != 512)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Xhc             = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+
+  Status          = EFI_DEVICE_ERROR;
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Len             = 0;
+
+  if (XhcPeiIsHalt (Xhc) || XhcPeiIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: HC is halted or has system error\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Hook the Set_Address request from UsbBus.
+  // According to XHCI 1.0 spec, the Set_Address request is replaced by XHCI's Address_Device cmd.
+  //
+  if ((Request->Request     == USB_REQ_SET_ADDRESS) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE))) {
+    //
+    // Reset the BusDevAddr field of all disabled entries in UsbDevContext array firstly.
+    // This way is used to clean the history to avoid using wrong device address afterwards.
+    //
+    for (Index = 0; Index < 255; Index++) {
+      if (!Xhc->UsbDevContext[Index + 1].Enabled &&
+          (Xhc->UsbDevContext[Index + 1].SlotId == 0) &&
+          (Xhc->UsbDevContext[Index + 1].BusDevAddr == (UINT8) Request->Value)) {
+        Xhc->UsbDevContext[Index + 1].BusDevAddr = 0;
+      }
+    }
+
+    if (Xhc->UsbDevContext[SlotId].XhciDevAddr == 0) {
+      goto ON_EXIT;
+    }
+    //
+    // The actual device address has been assigned by XHCI during initializing the device slot.
+    // So we just need establish the mapping relationship between the device address requested from UsbBus
+    // and the actual device address assigned by XHCI. The following invocations through EFI_USB2_HC_PROTOCOL interface
+    // can find out the actual device address by it.
+    //
+    Xhc->UsbDevContext[SlotId].BusDevAddr = (UINT8) Request->Value;
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  // Note that we encode the direction in address although default control
+  // endpoint is bidirectional. XhcPeiCreateUrb expects this
+  // combination of Ep addr and its direction.
+  //
+  Endpoint = (UINT8) (0 | ((TransferDirection == EfiUsbDataIn) ? 0x80 : 0));
+  Urb = XhcPeiCreateUrb (
+          Xhc,
+          DeviceAddress,
+          Endpoint,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_CTRL_TRANSFER,
+          Request,
+          Data,
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: failed to create URB"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcPeiExecTransfer (Xhc, FALSE, Urb, TimeOut);
+
+  //
+  // Get the status from URB. The result is updated in XhcPeiCheckUrbResult
+  // which is called by XhcPeiExecTransfer
+  //
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcPeiDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcPeiControlTransfer: XhcPeiDequeueTrbFromEndpoint failed\n"));
+    }
+    goto FREE_URB;
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcPeiRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: XhcPeiRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+      goto FREE_URB;
+    } else {
+      goto FREE_URB;
+    }
+  }
+
+  //
+  // Hook Get_Descriptor request from UsbBus as we need evaluate context and configure endpoint.
+  // Hook Get_Status request form UsbBus as we need trace device attach/detach event happened at hub.
+  // Hook Set_Config request from UsbBus as we need configure device endpoint.
+  //
+  if ((Request->Request     == USB_REQ_GET_DESCRIPTOR) &&
+      ((Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE)) ||
+      ((Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_CLASS, USB_TARGET_DEVICE))))) {
+    DescriptorType = (UINT8) (Request->Value >> 8);
+    if ((DescriptorType == USB_DESC_TYPE_DEVICE) && ((*DataLength == sizeof (EFI_USB_DEVICE_DESCRIPTOR)) || ((DeviceSpeed == EFI_USB_SPEED_FULL) && (*DataLength == 8)))) {
+      ASSERT (Data != NULL);
+      //
+      // Store a copy of device scriptor as hub device need this info to configure endpoint.
+      //
+      CopyMem (&Xhc->UsbDevContext[SlotId].DevDesc, Data, *DataLength);
+      if (Xhc->UsbDevContext[SlotId].DevDesc.BcdUSB == 0x0300) {
+        //
+        // If it's a usb3.0 device, then its max packet size is a 2^n.
+        //
+        MaxPacket0 = 1 << Xhc->UsbDevContext[SlotId].DevDesc.MaxPacketSize0;
+      } else {
+        MaxPacket0 = Xhc->UsbDevContext[SlotId].DevDesc.MaxPacketSize0;
+      }
+      Xhc->UsbDevContext[SlotId].ConfDesc = AllocateZeroPool (Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations * sizeof (EFI_USB_CONFIG_DESCRIPTOR *));
+      if (Xhc->UsbDevContext[SlotId].ConfDesc == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto FREE_URB;
+      }
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcPeiEvaluateContext (Xhc, SlotId, MaxPacket0);
+      } else {
+        Status = XhcPeiEvaluateContext64 (Xhc, SlotId, MaxPacket0);
+      }
+    } else if (DescriptorType == USB_DESC_TYPE_CONFIG) {
+      ASSERT (Data != NULL);
+      if (*DataLength == ((UINT16 *) Data)[1]) {
+        //
+        // Get configuration value from request, store the configuration descriptor for Configure_Endpoint cmd.
+        //
+        Index = (UINT8) Request->Value;
+        ASSERT (Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations);
+        Xhc->UsbDevContext[SlotId].ConfDesc[Index] = AllocateZeroPool (*DataLength);
+        if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] == NULL) {
+          Status = EFI_OUT_OF_RESOURCES;
+          goto FREE_URB;
+        }
+        CopyMem (Xhc->UsbDevContext[SlotId].ConfDesc[Index], Data, *DataLength);
+      }
+    } else if (((DescriptorType == USB_DESC_TYPE_HUB) ||
+               (DescriptorType == USB_DESC_TYPE_HUB_SUPER_SPEED)) && (*DataLength > 2)) {
+      ASSERT (Data != NULL);
+      HubDesc = (EFI_USB_HUB_DESCRIPTOR *) Data;
+      ASSERT (HubDesc->NumPorts <= 15);
+      //
+      // The bit 5,6 of HubCharacter field of Hub Descriptor is TTT.
+      //
+      TTT = (UINT8) ((HubDesc->HubCharacter & (BIT5 | BIT6)) >> 5);
+      if (Xhc->UsbDevContext[SlotId].DevDesc.DeviceProtocol == 2) {
+        //
+        // Don't support multi-TT feature for super speed hub now.
+        //
+        MTT = 0;
+        DEBUG ((EFI_D_ERROR, "XHCI: Don't support multi-TT feature for Hub now. (force to disable MTT)\n"));
+      } else {
+        MTT = 0;
+      }
+
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcPeiConfigHubContext (Xhc, SlotId, HubDesc->NumPorts, TTT, MTT);
+      } else {
+        Status = XhcPeiConfigHubContext64 (Xhc, SlotId, HubDesc->NumPorts, TTT, MTT);
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_SET_CONFIG) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE))) {
+    //
+    // Hook Set_Config request from UsbBus as we need configure device endpoint.
+    //
+    for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+      if (Xhc->UsbDevContext[SlotId].ConfDesc[Index]->ConfigurationValue == (UINT8)Request->Value) {
+        if (Xhc->HcCParams.Data.Csz == 0) {
+          Status = XhcPeiSetConfigCmd (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+        } else {
+          Status = XhcPeiSetConfigCmd64 (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+        }
+        break;
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_GET_STATUS) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_CLASS, USB_TARGET_OTHER))) {
+    ASSERT (Data != NULL);
+    //
+    // Hook Get_Status request from UsbBus to keep track of the port status change.
+    //
+    State                       = *(UINT32 *) Data;
+    PortStatus.PortStatus       = 0;
+    PortStatus.PortChangeStatus = 0;
+
+    if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+      //
+      // For super speed hub, its bit10~12 presents the attached device speed.
+      //
+      if ((State & XHC_PORTSC_PS) >> 10 == 0) {
+        PortStatus.PortStatus |= USB_PORT_STAT_SUPER_SPEED;
+      }
+    } else {
+      //
+      // For high or full/low speed hub, its bit9~10 presents the attached device speed.
+      //
+      if (XHC_BIT_IS_SET (State, BIT9)) {
+        PortStatus.PortStatus |= USB_PORT_STAT_LOW_SPEED;
+      } else if (XHC_BIT_IS_SET (State, BIT10)) {
+        PortStatus.PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+      }
+    }
+
+    //
+    // Convert the XHCI port/port change state to UEFI status
+    //
+    MapSize = sizeof (mUsbHubPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubPortStateMap[Index].HwState)) {
+        PortStatus.PortStatus = (UINT16) (PortStatus.PortStatus | mUsbHubPortStateMap[Index].UefiState);
+      }
+    }
+
+    MapSize = sizeof (mUsbHubPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubPortChangeMap[Index].HwState)) {
+        PortStatus.PortChangeStatus = (UINT16) (PortStatus.PortChangeStatus | mUsbHubPortChangeMap[Index].UefiState);
+      }
+    }
+
+    MapSize = sizeof (mUsbHubClearPortChangeMap) / sizeof (USB_CLEAR_PORT_MAP);
+
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubClearPortChangeMap[Index].HwState)) {
+        ZeroMem (&ClearPortRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+        ClearPortRequest.RequestType  = USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_CLASS, USB_TARGET_OTHER);
+        ClearPortRequest.Request      = (UINT8) USB_REQ_CLEAR_FEATURE;
+        ClearPortRequest.Value        = mUsbHubClearPortChangeMap[Index].Selector;
+        ClearPortRequest.Index        = Request->Index;
+        ClearPortRequest.Length       = 0;
+
+        XhcPeiControlTransfer (
+          PeiServices,
+          This,
+          DeviceAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          &ClearPortRequest,
+          EfiUsbNoData,
+          NULL,
+          &Len,
+          TimeOut,
+          Translator,
+          TransferResult
+          );
+      }
+    }
+
+    XhcPeiPollPortStatusChange (Xhc, Xhc->UsbDevContext[SlotId].RouteString, (UINT8)Request->Index, &PortStatus);
+
+    *(UINT32 *) Data = *(UINT32 *) &PortStatus;
+  }
+
+FREE_URB:
+  XhcPeiFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support
+                                bulk transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of
+                                sending or receiving.
+  @param  Data                  Array of pointers to the buffers of data to transmit
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to use of
+                                the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+                                If Timeout is 0, then the caller must wait for the function
+                                to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiBulkTransfer (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN UINT8                                  DeviceAddress,
+  IN UINT8                                  EndPointAddress,
+  IN UINT8                                  DeviceSpeed,
+  IN UINTN                                  MaximumPacketLength,
+  IN OUT VOID                               *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN OUT UINTN                              *DataLength,
+  IN OUT UINT8                              *DataToggle,
+  IN UINTN                                  TimeOut,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR     *Translator,
+  OUT UINT32                                *TransferResult
+  )
+{
+  PEI_XHC_DEV                   *Xhc;
+  URB                           *Urb;
+  UINT8                         SlotId;
+  EFI_STATUS                    Status;
+  EFI_STATUS                    RecoveryStatus;
+
+  //
+  // Validate the parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (Data[0] == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 0) && (*DataToggle != 1)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((DeviceSpeed == EFI_USB_SPEED_HIGH) && (MaximumPacketLength > 512)) ||
+      ((DeviceSpeed == EFI_USB_SPEED_SUPER) && (MaximumPacketLength > 1024))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Xhc             = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (XhcPeiIsHalt (Xhc) || XhcPeiIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: HC is halted or has system error\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  Urb = XhcPeiCreateUrb (
+          Xhc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_BULK_TRANSFER,
+          NULL,
+          Data[0],
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcPeiExecTransfer (Xhc, FALSE, Urb, TimeOut);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcPeiDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcPeiBulkTransfer: XhcPeiDequeueTrbFromEndpoint failed\n"));
+    }
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcPeiRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: XhcPeiRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+  XhcPeiFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices           The pointer to the PEI Services Table.
+  @param[in]  This                  The pointer to this instance of the
+                                    PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber            The pointer to the number of the root hub ports.
+
+  @retval EFI_SUCCESS               The port number was retrieved successfully.
+  @retval EFI_INVALID_PARAMETER     PortNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiGetRootHubPortNumber (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  OUT UINT8                         *PortNumber
+  )
+{
+  PEI_XHC_DEV           *XhcDev;
+  XhcDev = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+
+  if (PortNumber == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *PortNumber = XhcDev->HcSParams1.Data.MaxPorts;
+  DEBUG ((EFI_D_INFO, "XhcPeiGetRootHubPortNumber: PortNumber = %x\n", *PortNumber));
+  return EFI_SUCCESS;
+}
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param  PeiServices               The pointer of EFI_PEI_SERVICES.
+  @param  This                      The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  PortNumber                Specifies the root hub port whose feature
+                                    is requested to be cleared.
+  @param  PortFeature               Indicates the feature selector associated with the
+                                    feature clear request.
+
+  @retval EFI_SUCCESS               The feature specified by PortFeature was cleared
+                                    for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER     PortNumber is invalid or PortFeature is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiClearRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  )
+{
+  PEI_XHC_DEV           *Xhc;
+  UINT32                Offset;
+  UINT32                State;
+  EFI_STATUS            Status;
+
+  Xhc = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+  Status = EFI_SUCCESS;
+
+  if (PortNumber >= Xhc->HcSParams1.Data.MaxPorts) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  State = XhcPeiReadOpReg (Xhc, Offset);
+  DEBUG ((EFI_D_INFO, "XhcPeiClearRootHubPortFeature: Port: %x State: %x\n", PortNumber, State));
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bits
+  //
+  State &= ~ (BIT1 | BIT17 | BIT18 | BIT19 | BIT20 | BIT21 | BIT22 | BIT23);
+
+  switch (PortFeature) {
+    case EfiUsbPortEnable:
+      //
+      // Ports may only be enabled by the xHC. Software cannot enable a port by writing a '1' to this flag.
+      // A port may be disabled by software writing a '1' to this flag.
+      //
+      State |= XHC_PORTSC_PED;
+      State &= ~XHC_PORTSC_RESET;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortSuspend:
+      State |= XHC_PORTSC_LWS;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      State &= ~XHC_PORTSC_PLS;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortReset:
+      //
+      // PORTSC_RESET BIT(4) bit is RW1S attribute, which means Write-1-to-set status:
+      // Register bits indicate status when read, a clear bit may be set by
+      // writing a '1'. Writing a '0' to RW1S bits has no effect.
+      //
+      break;
+
+    case EfiUsbPortPower:
+      if (Xhc->HcCParams.Data.Ppc) {
+        //
+        // Port Power Control supported
+        //
+        State &= ~XHC_PORTSC_PP;
+        XhcPeiWriteOpReg (Xhc, Offset, State);
+      }
+      break;
+
+    case EfiUsbPortOwner:
+      //
+      // XHCI root hub port don't has the owner bit, ignore the operation
+      //
+      break;
+
+    case EfiUsbPortConnectChange:
+      //
+      // Clear connect status change
+      //
+      State |= XHC_PORTSC_CSC;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortEnableChange:
+      //
+      // Clear enable status change
+      //
+      State |= XHC_PORTSC_PEC;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortOverCurrentChange:
+      //
+      // Clear PortOverCurrent change
+      //
+      State |= XHC_PORTSC_OCC;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortResetChange:
+      //
+      // Clear Port Reset change
+      //
+      State |= XHC_PORTSC_PRC;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortSuspendChange:
+      //
+      // Not supported or not related operation
+      //
+      break;
+
+    default:
+      Status = EFI_INVALID_PARAMETER;
+      break;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcPeiClearRootHubPortFeature: PortFeature: %x Status = %r\n", PortFeature, Status));
+  return Status;
+}
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param  PeiServices               The pointer of EFI_PEI_SERVICES
+  @param  This                      The pointer of PEI_USB2_HOST_CONTROLLER_PPI
+  @param  PortNumber                Root hub port to set.
+  @param  PortFeature               Feature to set.
+
+  @retval EFI_SUCCESS               The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER     PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_TIMEOUT               The time out occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiSetRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  )
+{
+  PEI_XHC_DEV           *Xhc;
+  UINT32                Offset;
+  UINT32                State;
+  EFI_STATUS            Status;
+
+  Xhc = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+  Status = EFI_SUCCESS;
+
+  if (PortNumber >= Xhc->HcSParams1.Data.MaxPorts) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  State = XhcPeiReadOpReg (Xhc, Offset);
+  DEBUG ((EFI_D_INFO, "XhcPeiSetRootHubPortFeature: Port: %x State: %x\n", PortNumber, State));
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bits
+  //
+  State &= ~ (BIT1 | BIT17 | BIT18 | BIT19 | BIT20 | BIT21 | BIT22 | BIT23);
+
+  switch (PortFeature) {
+    case EfiUsbPortEnable:
+      //
+      // Ports may only be enabled by the xHC. Software cannot enable a port by writing a '1' to this flag.
+      // A port may be disabled by software writing a '1' to this flag.
+      //
+      break;
+
+    case EfiUsbPortSuspend:
+      State |= XHC_PORTSC_LWS;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      State &= ~XHC_PORTSC_PLS;
+      State |= (3 << 5) ;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortReset:
+      //
+      // Make sure Host Controller not halt before reset it
+      //
+      if (XhcPeiIsHalt (Xhc)) {
+        Status = XhcPeiRunHC (Xhc, XHC_GENERIC_TIMEOUT);
+        if (EFI_ERROR (Status)) {
+          break;
+        }
+      }
+
+      //
+      // 4.3.1 Resetting a Root Hub Port
+      // 1) Write the PORTSC register with the Port Reset (PR) bit set to '1'.
+      // 2) Wait for a successful Port Status Change Event for the port, where the Port Reset Change (PRC)
+      //    bit in the PORTSC field is set to '1'.
+      //
+      State |= XHC_PORTSC_RESET;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      XhcPeiWaitOpRegBit(Xhc, Offset, XHC_PORTSC_PRC, TRUE, XHC_GENERIC_TIMEOUT);
+      break;
+
+    case EfiUsbPortPower:
+      if (Xhc->HcCParams.Data.Ppc) {
+        //
+        // Port Power Control supported
+        //
+        State |= XHC_PORTSC_PP;
+        XhcPeiWriteOpReg (Xhc, Offset, State);
+      }
+      break;
+
+    case EfiUsbPortOwner:
+      //
+      // XHCI root hub port don't has the owner bit, ignore the operation
+      //
+      break;
+
+    default:
+      Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcPeiSetRootHubPortFeature: PortFeature: %x Status = %r\n", PortFeature, Status));
+  return Status;
+}
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param  PeiServices               The pointer of EFI_PEI_SERVICES.
+  @param  This                      The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  PortNumber                The root hub port to retrieve the state from.
+  @param  PortStatus                Variable to receive the port state.
+
+  @retval EFI_SUCCESS               The status of the USB root hub port specified.
+                                    by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER     PortNumber is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiGetRootHubPortStatus (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  IN UINT8                          PortNumber,
+  OUT EFI_USB_PORT_STATUS           *PortStatus
+  )
+{
+  PEI_XHC_DEV               *Xhc;
+  UINT32                    Offset;
+  UINT32                    State;
+  UINTN                     Index;
+  UINTN                     MapSize;
+  USB_DEV_ROUTE             ParentRouteChart;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Xhc = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+
+  if (PortNumber >= Xhc->HcSParams1.Data.MaxPorts) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Clear port status.
+  //
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  Offset                        = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  State                         = XhcPeiReadOpReg (Xhc, Offset);
+  DEBUG ((EFI_D_INFO, "XhcPeiGetRootHubPortStatus: Port: %x State: %x\n", PortNumber, State));
+
+  //
+  // According to XHCI 1.0 spec, bit 10~13 of the root port status register identifies the speed of the attached device.
+  //
+  switch ((State & XHC_PORTSC_PS) >> 10) {
+    case 2:
+      PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+      break;
+
+    case 3:
+      PortStatus->PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+      break;
+
+    case 4:
+      PortStatus->PortStatus |= USB_PORT_STAT_SUPER_SPEED;
+      break;
+
+    default:
+      break;
+  }
+
+  //
+  // Convert the XHCI port/port change state to UEFI status
+  //
+  MapSize = sizeof (mUsbPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbPortStateMap[Index].HwState)) {
+      PortStatus->PortStatus = (UINT16) (PortStatus->PortStatus | mUsbPortStateMap[Index].UefiState);
+    }
+  }
+  //
+  // Bit5~8 reflects its current link state.
+  //
+  if ((State & XHC_PORTSC_PLS) >> 5 == 3) {
+    PortStatus->PortStatus |= USB_PORT_STAT_SUSPEND;
+  }
+
+  MapSize = sizeof (mUsbPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbPortChangeMap[Index].HwState)) {
+      PortStatus->PortChangeStatus = (UINT16) (PortStatus->PortChangeStatus | mUsbPortChangeMap[Index].UefiState);
+    }
+  }
+
+  MapSize = sizeof (mUsbClearPortChangeMap) / sizeof (USB_CLEAR_PORT_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbClearPortChangeMap[Index].HwState)) {
+      XhcPeiClearRootHubPortFeature (PeiServices, This, PortNumber, (EFI_USB_PORT_FEATURE)mUsbClearPortChangeMap[Index].Selector);
+    }
+  }
+
+  //
+  // Poll the root port status register to enable/disable corresponding device slot if there is a device attached/detached.
+  // For those devices behind hub, we get its attach/detach event by hooking Get_Port_Status request at control transfer for those hub.
+  //
+  ParentRouteChart.Dword = 0;
+  XhcPeiPollPortStatusChange (Xhc, ParentRouteChart, PortNumber, PortStatus);
+
+  DEBUG ((EFI_D_INFO, "XhcPeiGetRootHubPortStatus: PortChangeStatus: %x PortStatus: %x\n", PortStatus->PortChangeStatus, PortStatus->PortStatus));
+  return EFI_SUCCESS;
+}
+
+/**
+  @param FileHandle     Handle of the file being invoked.
+  @param PeiServices    Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS   PPI successfully installed.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeimEntry (
+  IN EFI_PEI_FILE_HANDLE    FileHandle,
+  IN CONST EFI_PEI_SERVICES **PeiServices
+  )
+{
+  PEI_USB_CONTROLLER_PPI      *UsbControllerPpi;
+  EFI_STATUS                  Status;
+  UINT8                       Index;
+  UINTN                       ControllerType;
+  UINTN                       BaseAddress;
+  UINTN                       MemPages;
+  PEI_XHC_DEV                 *XhcDev;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+  UINT32                      PageSize;
+
+  //
+  // Shadow this PEIM to run from memory.
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  Status = PeiServicesLocatePpi (
+             &gPeiUsbControllerPpiGuid,
+             0,
+             NULL,
+             (VOID **) &UsbControllerPpi
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Index = 0;
+  while (TRUE) {
+    Status = UsbControllerPpi->GetUsbController (
+                                 (EFI_PEI_SERVICES **) PeiServices,
+                                 UsbControllerPpi,
+                                 Index,
+                                 &ControllerType,
+                                 &BaseAddress
+                                 );
+    //
+    // When status is error, it means no controller is found.
+    //
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    //
+    // This PEIM is for XHC type controller.
+    //
+    if (ControllerType != PEI_XHCI_CONTROLLER) {
+      Index++;
+      continue;
+    }
+
+    MemPages = EFI_SIZE_TO_PAGES (sizeof (PEI_XHC_DEV));
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesData,
+               MemPages,
+               &TempPtr
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (MemPages));
+    XhcDev = (PEI_XHC_DEV *) ((UINTN) TempPtr);
+
+    XhcDev->Signature = USB_XHC_DEV_SIGNATURE;
+    XhcDev->UsbHostControllerBaseAddress = (UINT32) BaseAddress;
+    XhcDev->CapLength           = (UINT8) (XhcPeiReadCapRegister (XhcDev, XHC_CAPLENGTH_OFFSET) & 0x0FF);
+    XhcDev->HcSParams1.Dword    = XhcPeiReadCapRegister (XhcDev, XHC_HCSPARAMS1_OFFSET);
+    XhcDev->HcSParams2.Dword    = XhcPeiReadCapRegister (XhcDev, XHC_HCSPARAMS2_OFFSET);
+    XhcDev->HcCParams.Dword     = XhcPeiReadCapRegister (XhcDev, XHC_HCCPARAMS_OFFSET);
+    XhcDev->DBOff               = XhcPeiReadCapRegister (XhcDev, XHC_DBOFF_OFFSET);
+    XhcDev->RTSOff              = XhcPeiReadCapRegister (XhcDev, XHC_RTSOFF_OFFSET);
+
+    //
+    // This PageSize field defines the page size supported by the xHC implementation.
+    // This xHC supports a page size of 2^(n+12) if bit n is Set. For example,
+    // if bit 0 is Set, the xHC supports 4k byte page sizes.
+    //
+    PageSize         = XhcPeiReadOpReg (XhcDev, XHC_PAGESIZE_OFFSET) & XHC_PAGESIZE_MASK;
+    XhcDev->PageSize = 1 << (HighBitSet32 (PageSize) + 12);
+
+    DEBUG ((EFI_D_INFO, "XhciPei: UsbHostControllerBaseAddress: %x\n", XhcDev->UsbHostControllerBaseAddress));
+    DEBUG ((EFI_D_INFO, "XhciPei: CapLength:                    %x\n", XhcDev->CapLength));
+    DEBUG ((EFI_D_INFO, "XhciPei: HcSParams1:                   %x\n", XhcDev->HcSParams1.Dword));
+    DEBUG ((EFI_D_INFO, "XhciPei: HcSParams2:                   %x\n", XhcDev->HcSParams2.Dword));
+    DEBUG ((EFI_D_INFO, "XhciPei: HcCParams:                    %x\n", XhcDev->HcCParams.Dword));
+    DEBUG ((EFI_D_INFO, "XhciPei: DBOff:                        %x\n", XhcDev->DBOff));
+    DEBUG ((EFI_D_INFO, "XhciPei: RTSOff:                       %x\n", XhcDev->RTSOff));
+    DEBUG ((EFI_D_INFO, "XhciPei: PageSize:                     %x\n", XhcDev->PageSize));
+
+    XhcPeiResetHC (XhcDev, XHC_RESET_TIMEOUT);
+    ASSERT (XhcPeiIsHalt (XhcDev));
+
+    //
+    // Initialize the schedule
+    //
+    XhcPeiInitSched (XhcDev);
+
+    //
+    // Start the Host Controller
+    //
+    XhcPeiRunHC (XhcDev, XHC_GENERIC_TIMEOUT);
+
+    //
+    // Wait for root port state stable
+    //
+    MicroSecondDelay (XHC_ROOT_PORT_STATE_STABLE);
+
+    XhcDev->Usb2HostControllerPpi.ControlTransfer           = XhcPeiControlTransfer;
+    XhcDev->Usb2HostControllerPpi.BulkTransfer              = XhcPeiBulkTransfer;
+    XhcDev->Usb2HostControllerPpi.GetRootHubPortNumber      = XhcPeiGetRootHubPortNumber;
+    XhcDev->Usb2HostControllerPpi.GetRootHubPortStatus      = XhcPeiGetRootHubPortStatus;
+    XhcDev->Usb2HostControllerPpi.SetRootHubPortFeature     = XhcPeiSetRootHubPortFeature;
+    XhcDev->Usb2HostControllerPpi.ClearRootHubPortFeature   = XhcPeiClearRootHubPortFeature;
+
+    XhcDev->PpiDescriptor.Flags = (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+    XhcDev->PpiDescriptor.Guid = &gPeiUsb2HostControllerPpiGuid;
+    XhcDev->PpiDescriptor.Ppi = &XhcDev->Usb2HostControllerPpi;
+
+    PeiServicesInstallPpi (&XhcDev->PpiDescriptor);
+
+    Index++;
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.h b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.h
new file mode 100644
index 0000000000..ccf4dc26e5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.h
@@ -0,0 +1,239 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2014 - 2015, Intel Corporation. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _RECOVERY_XHC_H_
+#define _RECOVERY_XHC_H_
+
+#include <PiPei.h>
+
+#include <Ppi/UsbController.h>
+#include <Ppi/Usb2HostController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/IoLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+typedef struct _PEI_XHC_DEV PEI_XHC_DEV;
+typedef struct _USB_DEV_CONTEXT USB_DEV_CONTEXT;
+
+#include "UsbHcMem.h"
+#include "XhciReg.h"
+#include "XhciSched.h"
+
+#define CMD_RING_TRB_NUMBER         0x100
+#define TR_RING_TRB_NUMBER          0x100
+#define ERST_NUMBER                 0x01
+#define EVENT_RING_TRB_NUMBER       0x200
+
+#define XHC_1_MICROSECOND           1
+#define XHC_1_MILLISECOND           (1000 * XHC_1_MICROSECOND)
+#define XHC_1_SECOND                (1000 * XHC_1_MILLISECOND)
+
+//
+// XHC reset timeout experience values.
+// The unit is millisecond, setting it as 1s.
+//
+#define XHC_RESET_TIMEOUT           (1000)
+
+//
+// Wait for root port state stable.
+//
+#define XHC_ROOT_PORT_STATE_STABLE  (200 * XHC_1_MILLISECOND)
+
+//
+// XHC generic timeout experience values.
+// The unit is millisecond, setting it as 10s.
+//
+#define XHC_GENERIC_TIMEOUT         (10 * 1000)
+
+#define XHC_LOW_32BIT(Addr64)       ((UINT32)(((UINTN)(Addr64)) & 0XFFFFFFFF))
+#define XHC_HIGH_32BIT(Addr64)      ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+#define XHC_BIT_IS_SET(Data, Bit)   ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define XHC_REG_BIT_IS_SET(XHC, Offset, Bit) \
+          (XHC_BIT_IS_SET(XhcPeiReadOpReg ((XHC), (Offset)), (Bit)))
+
+#define USB_DESC_TYPE_HUB              0x29
+#define USB_DESC_TYPE_HUB_SUPER_SPEED  0x2a
+
+//
+// The RequestType in EFI_USB_DEVICE_REQUEST is composed of
+// three fields: One bit direction, 2 bit type, and 5 bit
+// target.
+//
+#define USB_REQUEST_TYPE(Dir, Type, Target) \
+          ((UINT8)((((Dir) == EfiUsbDataIn ? 0x01 : 0) << 7) | (Type) | (Target)))
+
+struct _USB_DEV_CONTEXT {
+  //
+  // Whether this entry in UsbDevContext array is used or not.
+  //
+  BOOLEAN                           Enabled;
+  //
+  // The slot id assigned to the new device through XHCI's Enable_Slot cmd.
+  //
+  UINT8                             SlotId;
+  //
+  // The route string presented an attached usb device.
+  //
+  USB_DEV_ROUTE                     RouteString;
+  //
+  // The route string of parent device if it exists. Otherwise it's zero.
+  //
+  USB_DEV_ROUTE                     ParentRouteString;
+  //
+  // The actual device address assigned by XHCI through Address_Device command.
+  //
+  UINT8                             XhciDevAddr;
+  //
+  // The requested device address from UsbBus driver through Set_Address standard usb request.
+  // As XHCI spec replaces this request with Address_Device command, we have to record the
+  // requested device address and establish a mapping relationship with the actual device address.
+  // Then UsbBus driver just need to be aware of the requested device address to access usb device
+  // through EFI_USB2_HC_PROTOCOL. Xhci driver would be responsible for translating it to actual
+  // device address and access the actual device.
+  //
+  UINT8                             BusDevAddr;
+  //
+  // The pointer to the input device context.
+  //
+  VOID                              *InputContext;
+  //
+  // The pointer to the output device context.
+  //
+  VOID                              *OutputContext;
+  //
+  // The transfer queue for every endpoint.
+  //
+  VOID                              *EndpointTransferRing[31];
+  //
+  // The device descriptor which is stored to support XHCI's Evaluate_Context cmd.
+  //
+  EFI_USB_DEVICE_DESCRIPTOR         DevDesc;
+  //
+  // As a usb device may include multiple configuration descriptors, we dynamically allocate an array
+  // to store them.
+  // Note that every configuration descriptor stored here includes those lower level descriptors,
+  // such as Interface descriptor, Endpoint descriptor, and so on.
+  // These information is used to support XHCI's Config_Endpoint cmd.
+  //
+  EFI_USB_CONFIG_DESCRIPTOR         **ConfDesc;
+};
+
+#define USB_XHC_DEV_SIGNATURE       SIGNATURE_32 ('x', 'h', 'c', 'i')
+
+struct _PEI_XHC_DEV {
+  UINTN                             Signature;
+  PEI_USB2_HOST_CONTROLLER_PPI      Usb2HostControllerPpi;
+  EFI_PEI_PPI_DESCRIPTOR            PpiDescriptor;
+  UINT32                            UsbHostControllerBaseAddress;
+  USBHC_MEM_POOL                    *MemPool;
+
+  //
+  // XHCI configuration data
+  //
+  UINT8                             CapLength;    ///< Capability Register Length
+  XHC_HCSPARAMS1                    HcSParams1;   ///< Structural Parameters 1
+  XHC_HCSPARAMS2                    HcSParams2;   ///< Structural Parameters 2
+  XHC_HCCPARAMS                     HcCParams;    ///< Capability Parameters
+  UINT32                            DBOff;        ///< Doorbell Offset
+  UINT32                            RTSOff;       ///< Runtime Register Space Offset
+  UINT32                            PageSize;
+  UINT32                            MaxScratchpadBufs;
+  UINT64                            *ScratchBuf;
+  UINT64                            *ScratchEntry;
+  UINT64                            *DCBAA;
+  UINT32                            MaxSlotsEn;
+  //
+  // Cmd Transfer Ring
+  //
+  TRANSFER_RING                     CmdRing;
+  //
+  // EventRing
+  //
+  EVENT_RING                        EventRing;
+
+  //
+  // Store device contexts managed by XHCI device
+  // The array supports up to 255 devices, entry 0 is reserved and should not be used.
+  //
+  USB_DEV_CONTEXT                   UsbDevContext[256];
+};
+
+#define PEI_RECOVERY_USB_XHC_DEV_FROM_THIS(a) CR (a, PEI_XHC_DEV, Usb2HostControllerPpi, USB_XHC_DEV_SIGNATURE)
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @return Pointer to the allocated memory pool or NULL if failed.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  VOID
+  )
+;
+
+/**
+  Release the memory management pool.
+
+  @param  Pool          The USB memory pool to free.
+
+**/
+VOID
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL     *Pool
+  )
+;
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool          The host controller's memory pool.
+  @param  Size          Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN USBHC_MEM_POOL     *Pool,
+  IN UINTN              Size
+  )
+;
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The memory to free.
+  @param  Size          The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  )
+;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.inf b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.inf
new file mode 100644
index 0000000000..dc65f283bc
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.inf
@@ -0,0 +1,64 @@
+## @file
+# The XhcPeim driver is responsible for managing the behavior of XHCI controller at PEI phase.
+#
+# It produces gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+# which is used to enable recovery function from USB Drivers.
+#
+# Copyright (c) 2014, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions
+# of the BSD License which accompanies this distribution.  The
+# full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = XhciPei
+  MODULE_UNI_FILE                = XhciPei.uni
+  FILE_GUID                      = 65E5746E-9C14-467d-B5B3-932A66D59F79
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = XhcPeimEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  XhcPeim.c
+  XhcPeim.h
+  XhciSched.c
+  UsbHcMem.c
+  XhciReg.h
+  XhciSched.h
+  UsbHcMem.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  IoLib
+  TimerLib
+  BaseMemoryLib
+  PeimEntryPoint
+  PeiServicesLib
+  MemoryAllocationLib
+
+[Ppis]
+  gPeiUsb2HostControllerPpiGuid                 ## PRODUCES
+  gPeiUsbControllerPpiGuid                      ## CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gPeiUsbControllerPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  XhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.uni b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.uni
new file mode 100644
index 0000000000..451007efb6
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPeiExtra.uni
new file mode 100644
index 0000000000..d977f2f8c2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciReg.h b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciReg.h
new file mode 100644
index 0000000000..1a62560665
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciReg.h
@@ -0,0 +1,471 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2014, Intel Corporation. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PEI_XHCI_REG_H_
+#define _EFI_PEI_XHCI_REG_H_
+
+//
+// Capability registers offset
+//
+#define XHC_CAPLENGTH_OFFSET            0x00    // Capability register length offset
+#define XHC_HCIVERSION_OFFSET           0x02    // Interface Version Number 02-03h
+#define XHC_HCSPARAMS1_OFFSET           0x04    // Structural Parameters 1
+#define XHC_HCSPARAMS2_OFFSET           0x08    // Structural Parameters 2
+#define XHC_HCSPARAMS3_OFFSET           0x0c    // Structural Parameters 3
+#define XHC_HCCPARAMS_OFFSET            0x10    // Capability Parameters
+#define XHC_DBOFF_OFFSET                0x14    // Doorbell Offset
+#define XHC_RTSOFF_OFFSET               0x18    // Runtime Register Space Offset
+
+//
+// Operational registers offset
+//
+#define XHC_USBCMD_OFFSET               0x0000  // USB Command Register Offset
+#define XHC_USBSTS_OFFSET               0x0004  // USB Status Register Offset
+#define XHC_PAGESIZE_OFFSET             0x0008  // USB Page Size Register Offset
+#define XHC_DNCTRL_OFFSET               0x0014  // Device Notification Control Register Offset
+#define XHC_CRCR_OFFSET                 0x0018  // Command Ring Control Register Offset
+#define XHC_DCBAAP_OFFSET               0x0030  // Device Context Base Address Array Pointer Register Offset
+#define XHC_CONFIG_OFFSET               0x0038  // Configure Register Offset
+#define XHC_PORTSC_OFFSET               0x0400  // Port Status and Control Register Offset
+
+//
+// Runtime registers offset
+//
+#define XHC_MFINDEX_OFFSET              0x00    // Microframe Index Register Offset
+#define XHC_IMAN_OFFSET                 0x20    // Interrupter X Management Register Offset
+#define XHC_IMOD_OFFSET                 0x24    // Interrupter X Moderation Register Offset
+#define XHC_ERSTSZ_OFFSET               0x28    // Event Ring Segment Table Size Register Offset
+#define XHC_ERSTBA_OFFSET               0x30    // Event Ring Segment Table Base Address Register Offset
+#define XHC_ERDP_OFFSET                 0x38    // Event Ring Dequeue Pointer Register Offset
+
+//
+// Register Bit Definition
+//
+#define XHC_USBCMD_RUN                  BIT0    // Run/Stop
+#define XHC_USBCMD_RESET                BIT1    // Host Controller Reset
+#define XHC_USBCMD_INTE                 BIT2    // Interrupter Enable
+#define XHC_USBCMD_HSEE                 BIT3    // Host System Error Enable
+
+#define XHC_USBSTS_HALT                 BIT0    // Host Controller Halted
+#define XHC_USBSTS_HSE                  BIT2    // Host System Error
+#define XHC_USBSTS_EINT                 BIT3    // Event Interrupt
+#define XHC_USBSTS_PCD                  BIT4    // Port Change Detect
+#define XHC_USBSTS_SSS                  BIT8    // Save State Status
+#define XHC_USBSTS_RSS                  BIT9    // Restore State Status
+#define XHC_USBSTS_SRE                  BIT10   // Save/Restore Error
+#define XHC_USBSTS_CNR                  BIT11   // Host Controller Not Ready
+#define XHC_USBSTS_HCE                  BIT12   // Host Controller Error
+
+#define XHC_PAGESIZE_MASK               0xFFFF  // Page Size
+
+#define XHC_CRCR_RCS                    BIT0    // Ring Cycle State
+#define XHC_CRCR_CS                     BIT1    // Command Stop
+#define XHC_CRCR_CA                     BIT2    // Command Abort
+#define XHC_CRCR_CRR                    BIT3    // Command Ring Running
+
+#define XHC_CONFIG_MASK                 0xFF    // Max Device Slots Enabled
+
+#define XHC_PORTSC_CCS                  BIT0    // Current Connect Status
+#define XHC_PORTSC_PED                  BIT1    // Port Enabled/Disabled
+#define XHC_PORTSC_OCA                  BIT3    // Over-current Active
+#define XHC_PORTSC_RESET                BIT4    // Port Reset
+#define XHC_PORTSC_PLS                  (BIT5|BIT6|BIT7|BIT8)   // Port Link State
+#define XHC_PORTSC_PP                   BIT9    // Port Power
+#define XHC_PORTSC_PS                   (BIT10|BIT11|BIT12)     // Port Speed
+#define XHC_PORTSC_LWS                  BIT16   // Port Link State Write Strobe
+#define XHC_PORTSC_CSC                  BIT17   // Connect Status Change
+#define XHC_PORTSC_PEC                  BIT18   // Port Enabled/Disabled Change
+#define XHC_PORTSC_WRC                  BIT19   // Warm Port Reset Change
+#define XHC_PORTSC_OCC                  BIT20   // Over-Current Change
+#define XHC_PORTSC_PRC                  BIT21   // Port Reset Change
+#define XHC_PORTSC_PLC                  BIT22   // Port Link State Change
+#define XHC_PORTSC_CEC                  BIT23   // Port Config Error Change
+#define XHC_PORTSC_CAS                  BIT24   // Cold Attach Status
+
+#define XHC_HUB_PORTSC_CCS              BIT0    // Hub's Current Connect Status
+#define XHC_HUB_PORTSC_PED              BIT1    // Hub's Port Enabled/Disabled
+#define XHC_HUB_PORTSC_OCA              BIT3    // Hub's Over-current Active
+#define XHC_HUB_PORTSC_RESET            BIT4    // Hub's Port Reset
+#define XHC_HUB_PORTSC_PP               BIT9    // Hub's Port Power
+#define XHC_HUB_PORTSC_CSC              BIT16   // Hub's Connect Status Change
+#define XHC_HUB_PORTSC_PEC              BIT17   // Hub's Port Enabled/Disabled Change
+#define XHC_HUB_PORTSC_OCC              BIT19   // Hub's Over-Current Change
+#define XHC_HUB_PORTSC_PRC              BIT20   // Hub's Port Reset Change
+#define XHC_HUB_PORTSC_BHRC             BIT21   // Hub's Port Warm Reset Change
+
+#define XHC_IMAN_IP                     BIT0    // Interrupt Pending
+#define XHC_IMAN_IE                     BIT1    // Interrupt Enable
+
+#define XHC_IMODI_MASK                  0x0000FFFF  // Interrupt Moderation Interval
+#define XHC_IMODC_MASK                  0xFFFF0000  // Interrupt Moderation Counter
+
+
+#pragma pack (1)
+typedef struct {
+  UINT8                 MaxSlots;       // Number of Device Slots
+  UINT16                MaxIntrs:11;    // Number of Interrupters
+  UINT16                Rsvd:5;
+  UINT8                 MaxPorts;       // Number of Ports
+} HCSPARAMS1;
+
+//
+// Structural Parameters 1 Register Bitmap Definition
+//
+typedef union {
+  UINT32                Dword;
+  HCSPARAMS1            Data;
+} XHC_HCSPARAMS1;
+
+typedef struct {
+  UINT32                Ist:4;          // Isochronous Scheduling Threshold
+  UINT32                Erst:4;         // Event Ring Segment Table Max
+  UINT32                Rsvd:13;
+  UINT32                ScratchBufHi:5; // Max Scratchpad Buffers Hi
+  UINT32                Spr:1;          // Scratchpad Restore
+  UINT32                ScratchBufLo:5; // Max Scratchpad Buffers Lo
+} HCSPARAMS2;
+
+//
+// Structural Parameters 2 Register Bitmap Definition
+//
+typedef union {
+  UINT32                Dword;
+  HCSPARAMS2            Data;
+} XHC_HCSPARAMS2;
+
+typedef struct {
+  UINT16                Ac64:1;        // 64-bit Addressing Capability
+  UINT16                Bnc:1;         // BW Negotiation Capability
+  UINT16                Csz:1;         // Context Size
+  UINT16                Ppc:1;         // Port Power Control
+  UINT16                Pind:1;        // Port Indicators
+  UINT16                Lhrc:1;        // Light HC Reset Capability
+  UINT16                Ltc:1;         // Latency Tolerance Messaging Capability
+  UINT16                Nss:1;         // No Secondary SID Support
+  UINT16                Pae:1;         // Parse All Event Data
+  UINT16                Rsvd:3;
+  UINT16                MaxPsaSize:4;  // Maximum Primary Stream Array Size
+  UINT16                ExtCapReg;     // xHCI Extended Capabilities Pointer
+} HCCPARAMS;
+
+//
+// Capability Parameters Register Bitmap Definition
+//
+typedef union {
+  UINT32                Dword;
+  HCCPARAMS             Data;
+} XHC_HCCPARAMS;
+
+#pragma pack ()
+
+//
+// XHCi Data and Ctrl Structures
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Pi;
+  UINT8                   SubClassCode;
+  UINT8                   BaseCode;
+} USB_CLASSC;
+
+typedef struct {
+  UINT8                     Length;
+  UINT8                     DescType;
+  UINT8                     NumPorts;
+  UINT16                    HubCharacter;
+  UINT8                     PwrOn2PwrGood;
+  UINT8                     HubContrCurrent;
+  UINT8                     Filler[16];
+} EFI_USB_HUB_DESCRIPTOR;
+#pragma pack()
+
+//
+//  Hub Class Feature Selector for Clear Port Feature Request
+//  It's the extension of hub class feature selector of USB 2.0 in USB 3.0 Spec.
+//  For more details, Please refer to USB 3.0 Spec Table 10-7.
+//
+typedef enum {
+  Usb3PortBHPortReset          = 28,
+  Usb3PortBHPortResetChange    = 29
+} XHC_PORT_FEATURE;
+
+//
+// Structure to map the hardware port states to the
+// UEFI's port states.
+//
+typedef struct {
+  UINT32                  HwState;
+  UINT16                  UefiState;
+} USB_PORT_STATE_MAP;
+
+//
+// Structure to map the hardware port states to feature selector for clear port feature request.
+//
+typedef struct {
+  UINT32                  HwState;
+  UINT16                  Selector;
+} USB_CLEAR_PORT_MAP;
+
+/**
+  Read XHCI Operation register.
+
+  @param Xhc            The XHCI device.
+  @param Offset         The operation register offset.
+
+  @retval the register content read.
+
+**/
+UINT32
+XhcPeiReadOpReg (
+  IN  PEI_XHC_DEV       *Xhc,
+  IN  UINT32            Offset
+  );
+
+/**
+  Write the data to the XHCI operation register.
+
+  @param Xhc            The XHCI device.
+  @param Offset         The operation register offset.
+  @param Data           The data to write.
+
+**/
+VOID
+XhcPeiWriteOpReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  );
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiSetOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  );
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to clear.
+
+**/
+VOID
+XhcPeiClearOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  );
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to be set (or clear).
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit of the register to wait for.
+  @param  WaitToSet     Wait the bit to set or clear.
+  @param  Timeout       The time to wait before abort (in microsecond, us).
+
+  @retval EFI_SUCCESS   The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT   The time out occurred.
+
+**/
+EFI_STATUS
+XhcPeiWaitOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit,
+  IN BOOLEAN            WaitToSet,
+  IN UINT32             Timeout
+  );
+
+/**
+  Read XHCI door bell register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the door bell register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcPeiReadDoorBellReg (
+  IN  PEI_XHC_DEV       *Xhc,
+  IN  UINT32            Offset
+  );
+
+/**
+  Write the data to the XHCI door bell register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the door bell register.
+  @param  Data          The data to write.
+
+**/
+VOID
+XhcPeiWriteDoorBellReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  );
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcPeiReadRuntimeReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN  UINT32            Offset
+  );
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+  @param  Data          The data to write.
+
+**/
+VOID
+XhcPeiWriteRuntimeReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  );
+
+/**
+  Set one bit of the runtime register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+  @param  Bit           The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiSetRuntimeRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  );
+
+/**
+  Clear one bit of the runtime register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+  @param  Bit           The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiClearRuntimeRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  );
+
+/**
+  Check whether Xhc is halted.
+
+  @param  Xhc           The XHCI device.
+
+  @retval TRUE          The controller is halted.
+  @retval FALSE         The controller isn't halted.
+
+**/
+BOOLEAN
+XhcPeiIsHalt (
+  IN PEI_XHC_DEV        *Xhc
+  );
+
+/**
+  Check whether system error occurred.
+
+  @param  Xhc           The XHCI device.
+
+  @retval TRUE          System error happened.
+  @retval FALSE         No system error.
+
+**/
+BOOLEAN
+XhcPeiIsSysError (
+  IN PEI_XHC_DEV        *Xhc
+  );
+
+/**
+  Reset the host controller.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT   The transfer failed due to time out.
+  @retval Others        Failed to reset the host.
+
+**/
+EFI_STATUS
+XhcPeiResetHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  );
+
+/**
+  Halt the host controller.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort.
+
+  @retval EFI_TIMEOUT   Failed to halt the controller before Timeout.
+  @retval EFI_SUCCESS   The XHCI is halt.
+
+**/
+EFI_STATUS
+XhcPeiHaltHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  );
+
+/**
+  Set the XHCI to run.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort.
+
+  @retval EFI_SUCCESS   The XHCI is running.
+  @retval Others        Failed to set the XHCI to run.
+
+**/
+EFI_STATUS
+XhcPeiRunHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.c b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.c
new file mode 100644
index 0000000000..3632e8a769
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.c
@@ -0,0 +1,2963 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2014 - 2015, 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 "XhcPeim.h"
+
+/**
+  Create a command transfer TRB to support XHCI command interfaces.
+
+  @param  Xhc       The XHCI device.
+  @param  CmdTrb    The cmd TRB to be executed.
+
+  @return Created URB or NULL.
+
+**/
+URB*
+XhcPeiCreateCmdTrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN TRB_TEMPLATE   *CmdTrb
+  )
+{
+  URB   *Urb;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature  = XHC_URB_SIG;
+
+  Urb->Ring       = &Xhc->CmdRing;
+  XhcPeiSyncTrsRing (Xhc, Urb->Ring);
+  Urb->TrbNum     = 1;
+  Urb->TrbStart   = Urb->Ring->RingEnqueue;
+  CopyMem (Urb->TrbStart, CmdTrb, sizeof (TRB_TEMPLATE));
+  Urb->TrbStart->CycleBit = Urb->Ring->RingPCS & BIT0;
+  Urb->TrbEnd             = Urb->TrbStart;
+
+  return Urb;
+}
+
+/**
+  Execute a XHCI cmd TRB pointed by CmdTrb.
+
+  @param  Xhc                   The XHCI device.
+  @param  CmdTrb                The cmd TRB to be executed.
+  @param  Timeout               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+  @param  EvtTrb                The event TRB corresponding to the cmd TRB.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+XhcPeiCmdTransfer (
+  IN PEI_XHC_DEV                *Xhc,
+  IN TRB_TEMPLATE               *CmdTrb,
+  IN UINTN                      Timeout,
+  OUT TRB_TEMPLATE              **EvtTrb
+  )
+{
+  EFI_STATUS    Status;
+  URB           *Urb;
+
+  //
+  // Validate the parameters
+  //
+  if ((Xhc == NULL) || (CmdTrb == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EFI_DEVICE_ERROR;
+
+  if (XhcPeiIsHalt (Xhc) || XhcPeiIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiCmdTransfer: HC is halted or has system error\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, then poll the execution status.
+  //
+  Urb = XhcPeiCreateCmdTrb (Xhc, CmdTrb);
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiCmdTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status  = XhcPeiExecTransfer (Xhc, TRUE, Urb, Timeout);
+  *EvtTrb = Urb->EvtTrb;
+
+  if (Urb->Result == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  XhcPeiFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Create a new URB for a new transaction.
+
+  @param  Xhc       The XHCI device
+  @param  BusAddr   The logical device address assigned by UsbBus driver
+  @param  EpAddr    Endpoint addrress
+  @param  DevSpeed  The device speed
+  @param  MaxPacket The max packet length of the endpoint
+  @param  Type      The transaction type
+  @param  Request   The standard USB request for control transfer
+  @param  Data      The user data to transfer
+  @param  DataLen   The length of data buffer
+  @param  Callback  The function to call when data is transferred
+  @param  Context   The context to the callback
+
+  @return Created URB or NULL
+
+**/
+URB*
+XhcPeiCreateUrb (
+  IN PEI_XHC_DEV                        *Xhc,
+  IN UINT8                              BusAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINTN                              MaxPacket,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context
+  )
+{
+  USB_ENDPOINT      *Ep;
+  EFI_STATUS        Status;
+  URB               *Urb;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature = XHC_URB_SIG;
+
+  Ep            = &Urb->Ep;
+  Ep->BusAddr   = BusAddr;
+  Ep->EpAddr    = (UINT8) (EpAddr & 0x0F);
+  Ep->Direction = ((EpAddr & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut;
+  Ep->DevSpeed  = DevSpeed;
+  Ep->MaxPacket = MaxPacket;
+  Ep->Type      = Type;
+
+  Urb->Request  = Request;
+  Urb->Data     = Data;
+  Urb->DataLen  = DataLen;
+  Urb->Callback = Callback;
+  Urb->Context  = Context;
+
+  Status = XhcPeiCreateTransferTrb (Xhc, Urb);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiCreateUrb: XhcPeiCreateTransferTrb Failed, Status = %r\n", Status));
+    FreePool (Urb);
+    Urb = NULL;
+  }
+
+  return Urb;
+}
+
+/**
+  Free an allocated URB.
+
+  @param  Xhc       The XHCI device.
+  @param  Urb       The URB to free.
+
+**/
+VOID
+XhcPeiFreeUrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN URB            *Urb
+  )
+{
+  if ((Xhc == NULL) || (Urb == NULL)) {
+    return;
+  }
+
+  FreePool (Urb);
+}
+
+/**
+  Create a transfer TRB.
+
+  @param  Xhc       The XHCI device
+  @param  Urb       The urb used to construct the transfer TRB.
+
+  @return Created TRB or NULL
+
+**/
+EFI_STATUS
+XhcPeiCreateTransferTrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN URB            *Urb
+  )
+{
+  VOID                          *OutputContext;
+  TRANSFER_RING                 *EPRing;
+  UINT8                         EPType;
+  UINT8                         SlotId;
+  UINT8                         Dci;
+  TRB                           *TrbStart;
+  UINTN                         TotalLen;
+  UINTN                         Len;
+  UINTN                         TrbNum;
+
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Urb->Finished  = FALSE;
+  Urb->StartDone = FALSE;
+  Urb->EndDone   = FALSE;
+  Urb->Completed = 0;
+  Urb->Result    = EFI_USB_NOERROR;
+
+  Dci       = XhcPeiEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  EPRing    = (TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1];
+  Urb->Ring = EPRing;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  if (Xhc->HcCParams.Data.Csz == 0) {
+    EPType  = (UINT8) ((DEVICE_CONTEXT *)OutputContext)->EP[Dci-1].EPType;
+  } else {
+    EPType  = (UINT8) ((DEVICE_CONTEXT_64 *)OutputContext)->EP[Dci-1].EPType;
+  }
+
+  Urb->DataPhy = Urb->Data;
+
+  //
+  // Construct the TRB
+  //
+  XhcPeiSyncTrsRing (Xhc, EPRing);
+  Urb->TrbStart = EPRing->RingEnqueue;
+  switch (EPType) {
+    case ED_CONTROL_BIDIR:
+      //
+      // For control transfer, create SETUP_STAGE_TRB first.
+      //
+      TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+      TrbStart->TrbCtrSetup.bmRequestType = Urb->Request->RequestType;
+      TrbStart->TrbCtrSetup.bRequest      = Urb->Request->Request;
+      TrbStart->TrbCtrSetup.wValue        = Urb->Request->Value;
+      TrbStart->TrbCtrSetup.wIndex        = Urb->Request->Index;
+      TrbStart->TrbCtrSetup.wLength       = Urb->Request->Length;
+      TrbStart->TrbCtrSetup.Length        = 8;
+      TrbStart->TrbCtrSetup.IntTarget     = 0;
+      TrbStart->TrbCtrSetup.IOC           = 1;
+      TrbStart->TrbCtrSetup.IDT           = 1;
+      TrbStart->TrbCtrSetup.Type          = TRB_TYPE_SETUP_STAGE;
+      if (Urb->Ep.Direction == EfiUsbDataIn) {
+        TrbStart->TrbCtrSetup.TRT = 3;
+      } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+        TrbStart->TrbCtrSetup.TRT = 2;
+      } else {
+        TrbStart->TrbCtrSetup.TRT = 0;
+      }
+      //
+      // Update the cycle bit
+      //
+      TrbStart->TrbCtrSetup.CycleBit = EPRing->RingPCS & BIT0;
+      Urb->TrbNum++;
+
+      //
+      // For control transfer, create DATA_STAGE_TRB.
+      //
+      if (Urb->DataLen > 0) {
+        XhcPeiSyncTrsRing (Xhc, EPRing);
+        TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+        TrbStart->TrbCtrData.TRBPtrLo  = XHC_LOW_32BIT (Urb->DataPhy);
+        TrbStart->TrbCtrData.TRBPtrHi  = XHC_HIGH_32BIT (Urb->DataPhy);
+        TrbStart->TrbCtrData.Length    = (UINT32) Urb->DataLen;
+        TrbStart->TrbCtrData.TDSize    = 0;
+        TrbStart->TrbCtrData.IntTarget = 0;
+        TrbStart->TrbCtrData.ISP       = 1;
+        TrbStart->TrbCtrData.IOC       = 1;
+        TrbStart->TrbCtrData.IDT       = 0;
+        TrbStart->TrbCtrData.CH        = 0;
+        TrbStart->TrbCtrData.Type      = TRB_TYPE_DATA_STAGE;
+        if (Urb->Ep.Direction == EfiUsbDataIn) {
+          TrbStart->TrbCtrData.DIR = 1;
+        } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+          TrbStart->TrbCtrData.DIR = 0;
+        } else {
+          TrbStart->TrbCtrData.DIR = 0;
+        }
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbCtrData.CycleBit = EPRing->RingPCS & BIT0;
+        Urb->TrbNum++;
+      }
+      //
+      // For control transfer, create STATUS_STAGE_TRB.
+      // Get the pointer to next TRB for status stage use
+      //
+      XhcPeiSyncTrsRing (Xhc, EPRing);
+      TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+      TrbStart->TrbCtrStatus.IntTarget = 0;
+      TrbStart->TrbCtrStatus.IOC       = 1;
+      TrbStart->TrbCtrStatus.CH        = 0;
+      TrbStart->TrbCtrStatus.Type      = TRB_TYPE_STATUS_STAGE;
+      if (Urb->Ep.Direction == EfiUsbDataIn) {
+        TrbStart->TrbCtrStatus.DIR = 0;
+      } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+        TrbStart->TrbCtrStatus.DIR = 1;
+      } else {
+        TrbStart->TrbCtrStatus.DIR = 0;
+      }
+      //
+      // Update the cycle bit
+      //
+      TrbStart->TrbCtrStatus.CycleBit = EPRing->RingPCS & BIT0;
+      //
+      // Update the enqueue pointer
+      //
+      XhcPeiSyncTrsRing (Xhc, EPRing);
+      Urb->TrbNum++;
+      Urb->TrbEnd = (TRB_TEMPLATE *) (UINTN) TrbStart;
+
+      break;
+
+    case ED_BULK_OUT:
+    case ED_BULK_IN:
+      TotalLen = 0;
+      Len      = 0;
+      TrbNum   = 0;
+      TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+      while (TotalLen < Urb->DataLen) {
+        if ((TotalLen + 0x10000) >= Urb->DataLen) {
+          Len = Urb->DataLen - TotalLen;
+        } else {
+          Len = 0x10000;
+        }
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbNormal.TRBPtrLo  = XHC_LOW_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.TRBPtrHi  = XHC_HIGH_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.Length    = (UINT32) Len;
+        TrbStart->TrbNormal.TDSize    = 0;
+        TrbStart->TrbNormal.IntTarget = 0;
+        TrbStart->TrbNormal.ISP       = 1;
+        TrbStart->TrbNormal.IOC       = 1;
+        TrbStart->TrbNormal.Type      = TRB_TYPE_NORMAL;
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbNormal.CycleBit = EPRing->RingPCS & BIT0;
+
+        XhcPeiSyncTrsRing (Xhc, EPRing);
+        TrbNum++;
+        TotalLen += Len;
+      }
+
+      Urb->TrbNum = TrbNum;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+      break;
+
+    case ED_INTERRUPT_OUT:
+    case ED_INTERRUPT_IN:
+      TotalLen = 0;
+      Len      = 0;
+      TrbNum   = 0;
+      TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+      while (TotalLen < Urb->DataLen) {
+        if ((TotalLen + 0x10000) >= Urb->DataLen) {
+          Len = Urb->DataLen - TotalLen;
+        } else {
+          Len = 0x10000;
+        }
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbNormal.TRBPtrLo  = XHC_LOW_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.TRBPtrHi  = XHC_HIGH_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.Length    = (UINT32) Len;
+        TrbStart->TrbNormal.TDSize    = 0;
+        TrbStart->TrbNormal.IntTarget = 0;
+        TrbStart->TrbNormal.ISP       = 1;
+        TrbStart->TrbNormal.IOC       = 1;
+        TrbStart->TrbNormal.Type      = TRB_TYPE_NORMAL;
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbNormal.CycleBit = EPRing->RingPCS & BIT0;
+
+        XhcPeiSyncTrsRing (Xhc, EPRing);
+        TrbNum++;
+        TotalLen += Len;
+      }
+
+      Urb->TrbNum = TrbNum;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+      break;
+
+    default:
+      DEBUG ((EFI_D_INFO, "Not supported EPType 0x%x!\n",EPType));
+      ASSERT (FALSE);
+      break;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  System software shall use a Reset Endpoint Command (section 4.11.4.7) to remove the Halted
+  condition in the xHC. After the successful completion of the Reset Endpoint Command, the Endpoint
+  Context is transitioned from the Halted to the Stopped state and the Transfer Ring of the endpoint is
+  reenabled. The next write to the Doorbell of the Endpoint will transition the Endpoint Context from the
+  Stopped to the Running state.
+
+  @param  Xhc           The XHCI device.
+  @param  Urb           The urb which makes the endpoint halted.
+
+  @retval EFI_SUCCESS   The recovery is successful.
+  @retval Others        Failed to recovery halted endpoint.
+
+**/
+EFI_STATUS
+XhcPeiRecoverHaltedEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcPeiEndpointToDci (Urb->Ep.EpAddr, (UINT8) (Urb->Ep.Direction));
+
+  DEBUG ((EFI_D_INFO, "XhcPeiRecoverHaltedEndpoint: Recovery Halted Slot = %x, Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Reset endpoint command to transit from halt to stop state
+  //
+  Status = XhcPeiResetEndpoint (Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiRecoverHaltedEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2) Set dequeue pointer
+  //
+  Status = XhcPeiSetTrDequeuePointer (Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiRecoverHaltedEndpoint: Set Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3) Ring the doorbell to transit from stop to active
+  //
+  XhcPeiRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+XhcPeiDequeueTrbFromEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcPeiEndpointToDci (Urb->Ep.EpAddr, (UINT8) (Urb->Ep.Direction));
+
+  DEBUG ((EFI_D_INFO, "XhcPeiDequeueTrbFromEndpoint: Stop Slot = %x, Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Stop endpoint command to stop endpoint.
+  //
+  Status = XhcPeiStopEndpoint (Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDequeueTrbFromEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2) Set dequeue pointer
+  //
+  Status = XhcPeiSetTrDequeuePointer (Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDequeueTrbFromEndpoint: Set Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3) Ring the doorbell to transit from stop to active
+  //
+  XhcPeiRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
+/**
+  Check if the Trb is a transaction of the URB.
+
+  @param Trb        The TRB to be checked
+  @param Urb        The transfer ring to be checked.
+
+  @retval TRUE      It is a transaction of the URB.
+  @retval FALSE     It is not any transaction of the URB.
+
+**/
+BOOLEAN
+XhcPeiIsTransferRingTrb (
+  IN TRB_TEMPLATE   *Trb,
+  IN URB            *Urb
+  )
+{
+  TRB_TEMPLATE  *CheckedTrb;
+  UINTN         Index;
+
+  CheckedTrb = Urb->Ring->RingSeg0;
+
+  ASSERT (Urb->Ring->TrbNumber == CMD_RING_TRB_NUMBER || Urb->Ring->TrbNumber == TR_RING_TRB_NUMBER);
+
+  for (Index = 0; Index < Urb->Ring->TrbNumber; Index++) {
+    if (Trb == CheckedTrb) {
+      return TRUE;
+    }
+    CheckedTrb++;
+  }
+
+  return FALSE;
+}
+
+/**
+  Check the URB's execution result and update the URB's
+  result accordingly.
+
+  @param  Xhc               The XHCI device.
+  @param  Urb               The URB to check result.
+
+  @return Whether the result of URB transfer is finialized.
+
+**/
+BOOLEAN
+XhcPeiCheckUrbResult (
+  IN PEI_XHC_DEV            *Xhc,
+  IN URB                    *Urb
+  )
+{
+  EVT_TRB_TRANSFER          *EvtTrb;
+  TRB_TEMPLATE              *TRBPtr;
+  UINTN                     Index;
+  UINT8                     TRBType;
+  EFI_STATUS                Status;
+  URB                       *CheckedUrb;
+  UINT64                    XhcDequeue;
+  UINT32                    High;
+  UINT32                    Low;
+  EFI_PHYSICAL_ADDRESS      PhyAddr;
+
+  ASSERT ((Xhc != NULL) && (Urb != NULL));
+
+  Status = EFI_SUCCESS;
+
+  if (Urb->Finished) {
+    goto EXIT;
+  }
+
+  EvtTrb = NULL;
+
+  if (XhcPeiIsHalt (Xhc) || XhcPeiIsSysError (Xhc)) {
+    Urb->Result |= EFI_USB_ERR_SYSTEM;
+    goto EXIT;
+  }
+
+  //
+  // Traverse the event ring to find out all new events from the previous check.
+  //
+  XhcPeiSyncEventRing (Xhc, &Xhc->EventRing);
+  for (Index = 0; Index < Xhc->EventRing.TrbNumber; Index++) {
+    Status = XhcPeiCheckNewEvent (Xhc, &Xhc->EventRing, ((TRB_TEMPLATE **) &EvtTrb));
+    if (Status == EFI_NOT_READY) {
+      //
+      // All new events are handled, return directly.
+      //
+      goto EXIT;
+    }
+
+    //
+    // Only handle COMMAND_COMPLETETION_EVENT and TRANSFER_EVENT.
+    //
+    if ((EvtTrb->Type != TRB_TYPE_COMMAND_COMPLT_EVENT) && (EvtTrb->Type != TRB_TYPE_TRANS_EVENT)) {
+      continue;
+    }
+
+    //
+    // Need convert pci device address to host address
+    //
+    PhyAddr = (EFI_PHYSICAL_ADDRESS) (EvtTrb->TRBPtrLo | LShiftU64 ((UINT64) EvtTrb->TRBPtrHi, 32));
+    TRBPtr = (TRB_TEMPLATE *) (UINTN) UsbHcGetHostAddrForPciAddr (Xhc->MemPool, (VOID *) (UINTN) PhyAddr, sizeof (TRB_TEMPLATE));
+
+    //
+    // Update the status of Urb according to the finished event regardless of whether
+    // the urb is current checked one or in the XHCI's async transfer list.
+    // This way is used to avoid that those completed async transfer events don't get
+    // handled in time and are flushed by newer coming events.
+    //
+    if (XhcPeiIsTransferRingTrb (TRBPtr, Urb)) {
+      CheckedUrb = Urb;
+    } else {
+      continue;
+    }
+
+    switch (EvtTrb->Completecode) {
+      case TRB_COMPLETION_STALL_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_STALL;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: STALL_ERROR! Completecode = %x\n", EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_BABBLE_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_BABBLE;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: BABBLE_ERROR! Completecode = %x\n", EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_DATA_BUFFER_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_BUFFER;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: ERR_BUFFER! Completecode = %x\n", EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_USB_TRANSACTION_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_TIMEOUT;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: TRANSACTION_ERROR! Completecode = %x\n", EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_SHORT_PACKET:
+      case TRB_COMPLETION_SUCCESS:
+        if (EvtTrb->Completecode == TRB_COMPLETION_SHORT_PACKET) {
+          DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: short packet happens!\n"));
+        }
+
+        TRBType = (UINT8) (TRBPtr->Type);
+        if ((TRBType == TRB_TYPE_DATA_STAGE) ||
+            (TRBType == TRB_TYPE_NORMAL) ||
+            (TRBType == TRB_TYPE_ISOCH)) {
+          CheckedUrb->Completed += (((TRANSFER_TRB_NORMAL*)TRBPtr)->Length - EvtTrb->Length);
+        }
+
+        break;
+
+      default:
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: Transfer Default Error Occur! Completecode = 0x%x!\n", EvtTrb->Completecode));
+        CheckedUrb->Result  |= EFI_USB_ERR_TIMEOUT;
+        CheckedUrb->Finished = TRUE;
+        goto EXIT;
+    }
+
+    //
+    // Only check first and end Trb event address
+    //
+    if (TRBPtr == CheckedUrb->TrbStart) {
+      CheckedUrb->StartDone = TRUE;
+    }
+
+    if (TRBPtr == CheckedUrb->TrbEnd) {
+      CheckedUrb->EndDone = TRUE;
+    }
+
+    if (CheckedUrb->StartDone && CheckedUrb->EndDone) {
+      CheckedUrb->Finished = TRUE;
+      CheckedUrb->EvtTrb   = (TRB_TEMPLATE *) EvtTrb;
+    }
+  }
+
+EXIT:
+
+  //
+  // Advance event ring to last available entry
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  Low  = XhcPeiReadRuntimeReg (Xhc, XHC_ERDP_OFFSET);
+  High = XhcPeiReadRuntimeReg (Xhc, XHC_ERDP_OFFSET + 4);
+  XhcDequeue = (UINT64) (LShiftU64((UINT64) High, 32) | Low);
+
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->EventRing.EventRingDequeue, sizeof (TRB_TEMPLATE));
+
+  if ((XhcDequeue & (~0x0F)) != (PhyAddr & (~0x0F))) {
+    //
+    // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+    // So divide it to two 32-bytes width register access.
+    //
+    XhcPeiWriteRuntimeReg (Xhc, XHC_ERDP_OFFSET, XHC_LOW_32BIT (PhyAddr) | BIT3);
+    XhcPeiWriteRuntimeReg (Xhc, XHC_ERDP_OFFSET + 4, XHC_HIGH_32BIT (PhyAddr));
+  }
+
+  return Urb->Finished;
+}
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Xhc               The XHCI device.
+  @param  CmdTransfer       The executed URB is for cmd transfer or not.
+  @param  Urb               The URB to execute.
+  @param  Timeout           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+XhcPeiExecTransfer (
+  IN PEI_XHC_DEV            *Xhc,
+  IN BOOLEAN                CmdTransfer,
+  IN URB                    *Urb,
+  IN UINTN                  Timeout
+  )
+{
+  EFI_STATUS    Status;
+  UINTN         Index;
+  UINT64        Loop;
+  UINT8         SlotId;
+  UINT8         Dci;
+  BOOLEAN       Finished;
+
+  if (CmdTransfer) {
+    SlotId = 0;
+    Dci    = 0;
+  } else {
+    SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+    if (SlotId == 0) {
+      return EFI_DEVICE_ERROR;
+    }
+    Dci  = XhcPeiEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  }
+
+  Status = EFI_SUCCESS;
+  Loop   = Timeout * XHC_1_MILLISECOND;
+  if (Timeout == 0) {
+    Loop = 0xFFFFFFFF;
+  }
+
+  XhcPeiRingDoorBell (Xhc, SlotId, Dci);
+
+  for (Index = 0; Index < Loop; Index++) {
+    Finished = XhcPeiCheckUrbResult (Xhc, Urb);
+    if (Finished) {
+      break;
+    }
+    MicroSecondDelay (XHC_1_MICROSECOND);
+  }
+
+  if (Index == Loop) {
+    Urb->Result = EFI_USB_ERR_TIMEOUT;
+    Status      = EFI_TIMEOUT;
+  } else if (Urb->Result != EFI_USB_NOERROR) {
+    Status      = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+/**
+  Monitor the port status change. Enable/Disable device slot if there is a device attached/detached.
+
+  @param  Xhc               The XHCI device.
+  @param  ParentRouteChart  The route string pointed to the parent device if it exists.
+  @param  Port              The port to be polled.
+  @param  PortState         The port state.
+
+  @retval EFI_SUCCESS       Successfully enable/disable device slot according to port state.
+  @retval Others            Should not appear.
+
+**/
+EFI_STATUS
+XhcPeiPollPortStatusChange (
+  IN PEI_XHC_DEV            *Xhc,
+  IN USB_DEV_ROUTE          ParentRouteChart,
+  IN UINT8                  Port,
+  IN EFI_USB_PORT_STATUS    *PortState
+  )
+{
+  EFI_STATUS        Status;
+  UINT8             Speed;
+  UINT8             SlotId;
+  USB_DEV_ROUTE     RouteChart;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiPollPortStatusChange: PortChangeStatus: %x PortStatus: %x\n", PortState->PortChangeStatus, PortState->PortStatus));
+
+  Status = EFI_SUCCESS;
+
+  if ((PortState->PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (ParentRouteChart.Dword == 0) {
+    RouteChart.Route.RouteString = 0;
+    RouteChart.Route.RootPortNum = Port + 1;
+    RouteChart.Route.TierNum     = 1;
+  } else {
+    if(Port < 14) {
+      RouteChart.Route.RouteString = ParentRouteChart.Route.RouteString | (Port << (4 * (ParentRouteChart.Route.TierNum - 1)));
+    } else {
+      RouteChart.Route.RouteString = ParentRouteChart.Route.RouteString | (15 << (4 * (ParentRouteChart.Route.TierNum - 1)));
+    }
+    RouteChart.Route.RootPortNum   = ParentRouteChart.Route.RootPortNum;
+    RouteChart.Route.TierNum       = ParentRouteChart.Route.TierNum + 1;
+  }
+
+  SlotId = XhcPeiRouteStringToSlotId (Xhc, RouteChart);
+  if (SlotId != 0) {
+    if (Xhc->HcCParams.Data.Csz == 0) {
+      Status = XhcPeiDisableSlotCmd (Xhc, SlotId);
+    } else {
+      Status = XhcPeiDisableSlotCmd64 (Xhc, SlotId);
+    }
+  }
+
+  if (((PortState->PortStatus & USB_PORT_STAT_ENABLE) != 0) &&
+      ((PortState->PortStatus & USB_PORT_STAT_CONNECTION) != 0)) {
+    //
+    // Has a device attached, Identify device speed after port is enabled.
+    //
+    Speed = EFI_USB_SPEED_FULL;
+    if ((PortState->PortStatus & USB_PORT_STAT_LOW_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_LOW;
+    } else if ((PortState->PortStatus & USB_PORT_STAT_HIGH_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_HIGH;
+    } else if ((PortState->PortStatus & USB_PORT_STAT_SUPER_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_SUPER;
+    }
+    //
+    // Execute Enable_Slot cmd for attached device, initialize device context and assign device address.
+    //
+    SlotId = XhcPeiRouteStringToSlotId (Xhc, RouteChart);
+    if ((SlotId == 0) && ((PortState->PortChangeStatus & USB_PORT_STAT_C_RESET) != 0)) {
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcPeiInitializeDeviceSlot (Xhc, ParentRouteChart, Port, RouteChart, Speed);
+      } else {
+        Status = XhcPeiInitializeDeviceSlot64 (Xhc, ParentRouteChart, Port, RouteChart, Speed);
+      }
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Calculate the device context index by endpoint address and direction.
+
+  @param  EpAddr        The target endpoint number.
+  @param  Direction     The direction of the target endpoint.
+
+  @return The device context index of endpoint.
+
+**/
+UINT8
+XhcPeiEndpointToDci (
+  IN UINT8                      EpAddr,
+  IN EFI_USB_DATA_DIRECTION     Direction
+  )
+{
+  UINT8 Index;
+
+  ASSERT (EpAddr <= 15);
+
+  if (EpAddr == 0) {
+    return 1;
+  } else {
+    Index = (UINT8) (2 * EpAddr);
+    if (Direction == EfiUsbDataIn) {
+      Index += 1;
+    }
+    return Index;
+  }
+}
+
+/**
+  Find out the actual device address according to the requested device address from UsbBus.
+
+  @param  Xhc           The XHCI device.
+  @param  BusDevAddr    The requested device address by UsbBus upper driver.
+
+  @return The actual device address assigned to the device.
+
+**/
+UINT8
+XhcPeiBusDevAddrToSlotId (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              BusDevAddr
+  )
+{
+  UINT8   Index;
+
+  for (Index = 0; Index < 255; Index++) {
+    if (Xhc->UsbDevContext[Index + 1].Enabled &&
+        (Xhc->UsbDevContext[Index + 1].SlotId != 0) &&
+        (Xhc->UsbDevContext[Index + 1].BusDevAddr == BusDevAddr)) {
+      break;
+    }
+  }
+
+  if (Index == 255) {
+    return 0;
+  }
+
+  return Xhc->UsbDevContext[Index + 1].SlotId;
+}
+
+/**
+  Find out the slot id according to the device's route string.
+
+  @param  Xhc           The XHCI device.
+  @param  RouteString   The route string described the device location.
+
+  @return The slot id used by the device.
+
+**/
+UINT8
+XhcPeiRouteStringToSlotId (
+  IN PEI_XHC_DEV        *Xhc,
+  IN USB_DEV_ROUTE      RouteString
+  )
+{
+  UINT8   Index;
+
+  for (Index = 0; Index < 255; Index++) {
+    if (Xhc->UsbDevContext[Index + 1].Enabled &&
+        (Xhc->UsbDevContext[Index + 1].SlotId != 0) &&
+        (Xhc->UsbDevContext[Index + 1].RouteString.Dword == RouteString.Dword)) {
+      break;
+    }
+  }
+
+  if (Index == 255) {
+    return 0;
+  }
+
+  return Xhc->UsbDevContext[Index + 1].SlotId;
+}
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+**/
+VOID
+XhcPeiRingDoorBell (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  )
+{
+  if (SlotId == 0) {
+    XhcPeiWriteDoorBellReg (Xhc, 0, 0);
+  } else {
+    XhcPeiWriteDoorBellReg (Xhc, SlotId * sizeof (UINT32), Dci);
+  }
+}
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                   The XHCI device.
+  @param  ParentRouteChart      The route string pointed to the parent device.
+  @param  ParentPort            The port at which the device is located.
+  @param  RouteChart            The route string pointed to the device.
+  @param  DeviceSpeed           The device speed.
+
+  @retval EFI_SUCCESS           Successfully assign a slot to the device and assign an address to it.
+  @retval Others                Fail to initialize device slot.
+
+**/
+EFI_STATUS
+XhcPeiInitializeDeviceSlot (
+  IN PEI_XHC_DEV                *Xhc,
+  IN USB_DEV_ROUTE              ParentRouteChart,
+  IN UINT16                     ParentPort,
+  IN USB_DEV_ROUTE              RouteChart,
+  IN UINT8                      DeviceSpeed
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT                 *InputContext;
+  DEVICE_CONTEXT                *OutputContext;
+  TRANSFER_RING                 *EndpointTransferRing;
+  CMD_TRB_ADDRESS_DEVICE        CmdTrbAddr;
+  UINT8                         DeviceAddress;
+  CMD_TRB_ENABLE_SLOT           CmdTrb;
+  UINT8                         SlotId;
+  UINT8                         ParentSlotId;
+  DEVICE_CONTEXT                *ParentDeviceContext;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ZeroMem (&CmdTrb, sizeof (CMD_TRB_ENABLE_SLOT));
+  CmdTrb.CycleBit = 1;
+  CmdTrb.Type     = TRB_TYPE_EN_SLOT;
+
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrb,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiInitializeDeviceSlot: Enable Slot Failed, Status = %r\n", Status));
+    return Status;
+  }
+  ASSERT (EvtTrb->SlotId <= Xhc->MaxSlotsEn);
+  DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot: Enable Slot Successfully, The Slot ID = 0x%x\n", EvtTrb->SlotId));
+  SlotId = (UINT8) EvtTrb->SlotId;
+  ASSERT (SlotId != 0);
+
+  ZeroMem (&Xhc->UsbDevContext[SlotId], sizeof (USB_DEV_CONTEXT));
+  Xhc->UsbDevContext[SlotId].Enabled                 = TRUE;
+  Xhc->UsbDevContext[SlotId].SlotId                  = SlotId;
+  Xhc->UsbDevContext[SlotId].RouteString.Dword       = RouteChart.Dword;
+  Xhc->UsbDevContext[SlotId].ParentRouteString.Dword = ParentRouteChart.Dword;
+
+  //
+  // 4.3.3 Device Slot Initialization
+  // 1) Allocate an Input Context data structure (6.2.5) and initialize all fields to '0'.
+  //
+  InputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (INPUT_CONTEXT));
+  ASSERT (InputContext != NULL);
+  ASSERT (((UINTN) InputContext & 0x3F) == 0);
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  Xhc->UsbDevContext[SlotId].InputContext = (VOID *) InputContext;
+
+  //
+  // 2) Initialize the Input Control Context (6.2.5.1) of the Input Context by setting the A0 and A1
+  //    flags to '1'. These flags indicate that the Slot Context and the Endpoint 0 Context of the Input
+  //    Context are affected by the command.
+  //
+  InputContext->InputControlContext.Dword2 |= (BIT0 | BIT1);
+
+  //
+  // 3) Initialize the Input Slot Context data structure
+  //
+  InputContext->Slot.RouteString    = RouteChart.Route.RouteString;
+  InputContext->Slot.Speed          = DeviceSpeed + 1;
+  InputContext->Slot.ContextEntries = 1;
+  InputContext->Slot.RootHubPortNum = RouteChart.Route.RootPortNum;
+
+  if (RouteChart.Route.RouteString != 0) {
+    //
+    // The device is behind of hub device.
+    //
+    ParentSlotId = XhcPeiRouteStringToSlotId (Xhc, ParentRouteChart);
+    ASSERT (ParentSlotId != 0);
+    //
+    // If the Full/Low device attached to a High Speed Hub, init the TTPortNum and TTHubSlotId field of slot context
+    //
+    ParentDeviceContext = (DEVICE_CONTEXT *) Xhc->UsbDevContext[ParentSlotId].OutputContext;
+    if ((ParentDeviceContext->Slot.TTPortNum == 0) &&
+        (ParentDeviceContext->Slot.TTHubSlotId == 0)) {
+      if ((ParentDeviceContext->Slot.Speed == (EFI_USB_SPEED_HIGH + 1)) && (DeviceSpeed < EFI_USB_SPEED_HIGH)) {
+        //
+        // Full/Low device attached to High speed hub port that isolates the high speed signaling
+        // environment from Full/Low speed signaling environment for a device
+        //
+        InputContext->Slot.TTPortNum   = ParentPort;
+        InputContext->Slot.TTHubSlotId = ParentSlotId;
+      }
+    } else {
+      //
+      // Inherit the TT parameters from parent device.
+      //
+      InputContext->Slot.TTPortNum   = ParentDeviceContext->Slot.TTPortNum;
+      InputContext->Slot.TTHubSlotId = ParentDeviceContext->Slot.TTHubSlotId;
+      //
+      // If the device is a High speed device then down the speed to be the same as its parent Hub
+      //
+      if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+        InputContext->Slot.Speed = ParentDeviceContext->Slot.Speed;
+      }
+    }
+  }
+
+  //
+  // 4) Allocate and initialize the Transfer Ring for the Default Control Endpoint.
+  //
+  EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+  Xhc->UsbDevContext[SlotId].EndpointTransferRing[0] = EndpointTransferRing;
+  XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[0]);
+  //
+  // 5) Initialize the Input default control Endpoint 0 Context (6.2.3).
+  //
+  InputContext->EP[0].EPType = ED_CONTROL_BIDIR;
+
+  if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    InputContext->EP[0].MaxPacketSize = 512;
+  } else if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    InputContext->EP[0].MaxPacketSize = 64;
+  } else {
+    InputContext->EP[0].MaxPacketSize = 8;
+  }
+  //
+  // Initial value of Average TRB Length for Control endpoints would be 8B, Interrupt endpoints
+  // 1KB, and Bulk and Isoch endpoints 3KB.
+  //
+  InputContext->EP[0].AverageTRBLength = 8;
+  InputContext->EP[0].MaxBurstSize     = 0;
+  InputContext->EP[0].Interval         = 0;
+  InputContext->EP[0].MaxPStreams      = 0;
+  InputContext->EP[0].Mult             = 0;
+  InputContext->EP[0].CErr             = 3;
+
+  //
+  // Init the DCS(dequeue cycle state) as the transfer ring's CCS
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (
+              Xhc->MemPool,
+              ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[0])->RingSeg0,
+              sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+              );
+  InputContext->EP[0].PtrLo = XHC_LOW_32BIT (PhyAddr) | BIT0;
+  InputContext->EP[0].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // 6) Allocate the Output Device Context data structure (6.2.1) and initialize it to '0'.
+  //
+  OutputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (DEVICE_CONTEXT));
+  ASSERT (OutputContext != NULL);
+  ASSERT (((UINTN) OutputContext & 0x3F) == 0);
+  ZeroMem (OutputContext, sizeof (DEVICE_CONTEXT));
+
+  Xhc->UsbDevContext[SlotId].OutputContext = OutputContext;
+  //
+  // 7) Load the appropriate (Device Slot ID) entry in the Device Context Base Address Array (5.4.6) with
+  //    a pointer to the Output Device Context data structure (6.2.1).
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, OutputContext, sizeof (DEVICE_CONTEXT));
+  //
+  // Fill DCBAA with PCI device address
+  //
+  Xhc->DCBAA[SlotId] = (UINT64) (UINTN) PhyAddr;
+
+  //
+  // 8) Issue an Address Device Command for the Device Slot, where the command points to the Input
+  //    Context data structure described above.
+  //
+  ZeroMem (&CmdTrbAddr, sizeof (CmdTrbAddr));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbAddr.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbAddr.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbAddr.CycleBit = 1;
+  CmdTrbAddr.Type     = TRB_TYPE_ADDRESS_DEV;
+  CmdTrbAddr.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbAddr,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (!EFI_ERROR (Status)) {
+    DeviceAddress = (UINT8) OutputContext->Slot.DeviceAddress;
+    DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot: Address %d assigned successfully\n", DeviceAddress));
+    Xhc->UsbDevContext[SlotId].XhciDevAddr = DeviceAddress;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot: Enable Slot, Status = %r\n", Status));
+  return Status;
+}
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                   The XHCI device.
+  @param  ParentRouteChart      The route string pointed to the parent device.
+  @param  ParentPort            The port at which the device is located.
+  @param  RouteChart            The route string pointed to the device.
+  @param  DeviceSpeed           The device speed.
+
+  @retval EFI_SUCCESS           Successfully assign a slot to the device and assign an address to it.
+  @retval Others                Fail to initialize device slot.
+
+**/
+EFI_STATUS
+XhcPeiInitializeDeviceSlot64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN USB_DEV_ROUTE              ParentRouteChart,
+  IN UINT16                     ParentPort,
+  IN USB_DEV_ROUTE              RouteChart,
+  IN UINT8                      DeviceSpeed
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT_64              *InputContext;
+  DEVICE_CONTEXT_64             *OutputContext;
+  TRANSFER_RING                 *EndpointTransferRing;
+  CMD_TRB_ADDRESS_DEVICE        CmdTrbAddr;
+  UINT8                         DeviceAddress;
+  CMD_TRB_ENABLE_SLOT           CmdTrb;
+  UINT8                         SlotId;
+  UINT8                         ParentSlotId;
+  DEVICE_CONTEXT_64             *ParentDeviceContext;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ZeroMem (&CmdTrb, sizeof (CMD_TRB_ENABLE_SLOT));
+  CmdTrb.CycleBit = 1;
+  CmdTrb.Type     = TRB_TYPE_EN_SLOT;
+
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrb,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiInitializeDeviceSlot64: Enable Slot Failed, Status = %r\n", Status));
+    return Status;
+  }
+  ASSERT (EvtTrb->SlotId <= Xhc->MaxSlotsEn);
+  DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot64: Enable Slot Successfully, The Slot ID = 0x%x\n", EvtTrb->SlotId));
+  SlotId = (UINT8)EvtTrb->SlotId;
+  ASSERT (SlotId != 0);
+
+  ZeroMem (&Xhc->UsbDevContext[SlotId], sizeof (USB_DEV_CONTEXT));
+  Xhc->UsbDevContext[SlotId].Enabled                 = TRUE;
+  Xhc->UsbDevContext[SlotId].SlotId                  = SlotId;
+  Xhc->UsbDevContext[SlotId].RouteString.Dword       = RouteChart.Dword;
+  Xhc->UsbDevContext[SlotId].ParentRouteString.Dword = ParentRouteChart.Dword;
+
+  //
+  // 4.3.3 Device Slot Initialization
+  // 1) Allocate an Input Context data structure (6.2.5) and initialize all fields to '0'.
+  //
+  InputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (INPUT_CONTEXT_64));
+  ASSERT (InputContext != NULL);
+  ASSERT (((UINTN) InputContext & 0x3F) == 0);
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  Xhc->UsbDevContext[SlotId].InputContext = (VOID *) InputContext;
+
+  //
+  // 2) Initialize the Input Control Context (6.2.5.1) of the Input Context by setting the A0 and A1
+  //    flags to '1'. These flags indicate that the Slot Context and the Endpoint 0 Context of the Input
+  //    Context are affected by the command.
+  //
+  InputContext->InputControlContext.Dword2 |= (BIT0 | BIT1);
+
+  //
+  // 3) Initialize the Input Slot Context data structure
+  //
+  InputContext->Slot.RouteString    = RouteChart.Route.RouteString;
+  InputContext->Slot.Speed          = DeviceSpeed + 1;
+  InputContext->Slot.ContextEntries = 1;
+  InputContext->Slot.RootHubPortNum = RouteChart.Route.RootPortNum;
+
+  if (RouteChart.Route.RouteString != 0) {
+    //
+    // The device is behind of hub device.
+    //
+    ParentSlotId = XhcPeiRouteStringToSlotId (Xhc, ParentRouteChart);
+    ASSERT (ParentSlotId != 0);
+    //
+    //if the Full/Low device attached to a High Speed Hub, Init the TTPortNum and TTHubSlotId field of slot context
+    //
+    ParentDeviceContext = (DEVICE_CONTEXT_64 *) Xhc->UsbDevContext[ParentSlotId].OutputContext;
+    if ((ParentDeviceContext->Slot.TTPortNum == 0) &&
+        (ParentDeviceContext->Slot.TTHubSlotId == 0)) {
+      if ((ParentDeviceContext->Slot.Speed == (EFI_USB_SPEED_HIGH + 1)) && (DeviceSpeed < EFI_USB_SPEED_HIGH)) {
+        //
+        // Full/Low device attached to High speed hub port that isolates the high speed signaling
+        // environment from Full/Low speed signaling environment for a device
+        //
+        InputContext->Slot.TTPortNum   = ParentPort;
+        InputContext->Slot.TTHubSlotId = ParentSlotId;
+      }
+    } else {
+      //
+      // Inherit the TT parameters from parent device.
+      //
+      InputContext->Slot.TTPortNum   = ParentDeviceContext->Slot.TTPortNum;
+      InputContext->Slot.TTHubSlotId = ParentDeviceContext->Slot.TTHubSlotId;
+      //
+      // If the device is a High speed device then down the speed to be the same as its parent Hub
+      //
+      if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+        InputContext->Slot.Speed = ParentDeviceContext->Slot.Speed;
+      }
+    }
+  }
+
+  //
+  // 4) Allocate and initialize the Transfer Ring for the Default Control Endpoint.
+  //
+  EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+  Xhc->UsbDevContext[SlotId].EndpointTransferRing[0] = EndpointTransferRing;
+  XhcPeiCreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[0]);
+  //
+  // 5) Initialize the Input default control Endpoint 0 Context (6.2.3).
+  //
+  InputContext->EP[0].EPType = ED_CONTROL_BIDIR;
+
+  if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    InputContext->EP[0].MaxPacketSize = 512;
+  } else if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    InputContext->EP[0].MaxPacketSize = 64;
+  } else {
+    InputContext->EP[0].MaxPacketSize = 8;
+  }
+  //
+  // Initial value of Average TRB Length for Control endpoints would be 8B, Interrupt endpoints
+  // 1KB, and Bulk and Isoch endpoints 3KB.
+  //
+  InputContext->EP[0].AverageTRBLength = 8;
+  InputContext->EP[0].MaxBurstSize     = 0;
+  InputContext->EP[0].Interval         = 0;
+  InputContext->EP[0].MaxPStreams      = 0;
+  InputContext->EP[0].Mult             = 0;
+  InputContext->EP[0].CErr             = 3;
+
+  //
+  // Init the DCS(dequeue cycle state) as the transfer ring's CCS
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (
+              Xhc->MemPool,
+              ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[0])->RingSeg0,
+              sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+              );
+  InputContext->EP[0].PtrLo = XHC_LOW_32BIT (PhyAddr) | BIT0;
+  InputContext->EP[0].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // 6) Allocate the Output Device Context data structure (6.2.1) and initialize it to '0'.
+  //
+  OutputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (DEVICE_CONTEXT_64));
+  ASSERT (OutputContext != NULL);
+  ASSERT (((UINTN) OutputContext & 0x3F) == 0);
+  ZeroMem (OutputContext, sizeof (DEVICE_CONTEXT_64));
+
+  Xhc->UsbDevContext[SlotId].OutputContext = OutputContext;
+  //
+  // 7) Load the appropriate (Device Slot ID) entry in the Device Context Base Address Array (5.4.6) with
+  //    a pointer to the Output Device Context data structure (6.2.1).
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, OutputContext, sizeof (DEVICE_CONTEXT_64));
+  //
+  // Fill DCBAA with PCI device address
+  //
+  Xhc->DCBAA[SlotId] = (UINT64) (UINTN) PhyAddr;
+
+  //
+  // 8) Issue an Address Device Command for the Device Slot, where the command points to the Input
+  //    Context data structure described above.
+  //
+  ZeroMem (&CmdTrbAddr, sizeof (CmdTrbAddr));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbAddr.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbAddr.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbAddr.CycleBit = 1;
+  CmdTrbAddr.Type     = TRB_TYPE_ADDRESS_DEV;
+  CmdTrbAddr.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbAddr,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (!EFI_ERROR (Status)) {
+    DeviceAddress = (UINT8) OutputContext->Slot.DeviceAddress;
+    DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot64: Address %d assigned successfully\n", DeviceAddress));
+    Xhc->UsbDevContext[SlotId].XhciDevAddr = DeviceAddress;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot64: Enable Slot, Status = %r\n", Status));
+  return Status;
+}
+
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+XhcPeiDisableSlotCmd (
+  IN PEI_XHC_DEV               *Xhc,
+  IN UINT8                     SlotId
+  )
+{
+  EFI_STATUS            Status;
+  TRB_TEMPLATE          *EvtTrb;
+  CMD_TRB_DISABLE_SLOT  CmdTrbDisSlot;
+  UINT8                 Index;
+  VOID                  *RingSeg;
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0) ||
+        (Xhc->UsbDevContext[Index + 1].ParentRouteString.Dword != Xhc->UsbDevContext[SlotId].RouteString.Dword)) {
+      continue;
+    }
+
+    Status = XhcPeiDisableSlotCmd (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcPeiDisableSlotCmd: failed to disable child, ignore error\n"));
+      Xhc->UsbDevContext[Index + 1].SlotId = 0;
+    }
+  }
+
+  //
+  // Construct the disable slot command
+  //
+  DEBUG ((EFI_D_INFO, "XhcPeiDisableSlotCmd: Disable device slot %d!\n", SlotId));
+
+  ZeroMem (&CmdTrbDisSlot, sizeof (CmdTrbDisSlot));
+  CmdTrbDisSlot.CycleBit = 1;
+  CmdTrbDisSlot.Type     = TRB_TYPE_DIS_SLOT;
+  CmdTrbDisSlot.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbDisSlot,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDisableSlotCmd: Disable Slot Command Failed, Status = %r\n", Status));
+    return Status;
+  }
+  //
+  // Free the slot's device context entry
+  //
+  Xhc->DCBAA[SlotId] = 0;
+
+  //
+  // Free the slot related data structure
+  //
+  for (Index = 0; Index < 31; Index++) {
+    if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] != NULL) {
+      RingSeg = ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index])->RingSeg0;
+      if (RingSeg != NULL) {
+        UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+      }
+      FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index]);
+      Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] = NULL;
+    }
+  }
+
+  for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+    if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] != NULL) {
+      FreePool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+    }
+  }
+
+  if (Xhc->UsbDevContext[SlotId].InputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT));
+  }
+
+  if (Xhc->UsbDevContext[SlotId].OutputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].OutputContext, sizeof (DEVICE_CONTEXT));
+  }
+  //
+  // Doesn't zero the entry because XhcAsyncInterruptTransfer() may be invoked to remove the established
+  // asynchronous interrupt pipe after the device is disabled. It needs the device address mapping info to
+  // remove urb from XHCI's asynchronous transfer list.
+  //
+  Xhc->UsbDevContext[SlotId].Enabled = FALSE;
+  Xhc->UsbDevContext[SlotId].SlotId  = 0;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiDisableSlotCmd: Disable Slot Command, Status = %r\n", Status));
+  return Status;
+}
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+XhcPeiDisableSlotCmd64 (
+  IN PEI_XHC_DEV               *Xhc,
+  IN UINT8                     SlotId
+  )
+{
+  EFI_STATUS            Status;
+  TRB_TEMPLATE          *EvtTrb;
+  CMD_TRB_DISABLE_SLOT  CmdTrbDisSlot;
+  UINT8                 Index;
+  VOID                  *RingSeg;
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0) ||
+        (Xhc->UsbDevContext[Index + 1].ParentRouteString.Dword != Xhc->UsbDevContext[SlotId].RouteString.Dword)) {
+      continue;
+    }
+
+    Status = XhcPeiDisableSlotCmd64 (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcPeiDisableSlotCmd64: failed to disable child, ignore error\n"));
+      Xhc->UsbDevContext[Index + 1].SlotId = 0;
+    }
+  }
+
+  //
+  // Construct the disable slot command
+  //
+  DEBUG ((EFI_D_INFO, "XhcPeiDisableSlotCmd64: Disable device slot %d!\n", SlotId));
+
+  ZeroMem (&CmdTrbDisSlot, sizeof (CmdTrbDisSlot));
+  CmdTrbDisSlot.CycleBit = 1;
+  CmdTrbDisSlot.Type     = TRB_TYPE_DIS_SLOT;
+  CmdTrbDisSlot.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbDisSlot,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDisableSlotCmd64: Disable Slot Command Failed, Status = %r\n", Status));
+    return Status;
+  }
+  //
+  // Free the slot's device context entry
+  //
+  Xhc->DCBAA[SlotId] = 0;
+
+  //
+  // Free the slot related data structure
+  //
+  for (Index = 0; Index < 31; Index++) {
+    if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] != NULL) {
+      RingSeg = ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index])->RingSeg0;
+      if (RingSeg != NULL) {
+        UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+      }
+      FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index]);
+      Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] = NULL;
+    }
+  }
+
+  for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+    if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] != NULL) {
+      FreePool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+    }
+  }
+
+  if (Xhc->UsbDevContext[SlotId].InputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT_64));
+  }
+
+  if (Xhc->UsbDevContext[SlotId].OutputContext != NULL) {
+     UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].OutputContext, sizeof (DEVICE_CONTEXT_64));
+  }
+  //
+  // Doesn't zero the entry because XhcAsyncInterruptTransfer() may be invoked to remove the established
+  // asynchronous interrupt pipe after the device is disabled. It needs the device address mapping info to
+  // remove urb from XHCI's asynchronous transfer list.
+  //
+  Xhc->UsbDevContext[SlotId].Enabled = FALSE;
+  Xhc->UsbDevContext[SlotId].SlotId  = 0;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiDisableSlotCmd64: Disable Slot Command, Status = %r\n", Status));
+  return Status;
+}
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+XhcPeiSetConfigCmd (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR      *ConfigDesc
+  )
+{
+  EFI_STATUS                    Status;
+  USB_INTERFACE_DESCRIPTOR      *IfDesc;
+  USB_ENDPOINT_DESCRIPTOR       *EpDesc;
+  UINT8                         Index;
+  UINTN                         NumEp;
+  UINTN                         EpIndex;
+  UINT8                         EpAddr;
+  EFI_USB_DATA_DIRECTION        Direction;
+  UINT8                         Dci;
+  UINT8                         MaxDci;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  UINT8                         Interval;
+
+  TRANSFER_RING                 *EndpointTransferRing;
+  CMD_TRB_CONFIG_ENDPOINT       CmdTrbCfgEP;
+  INPUT_CONTEXT                 *InputContext;
+  DEVICE_CONTEXT                *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  //
+  // 4.6.6 Configure Endpoint
+  //
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *) (ConfigDesc + 1);
+  for (Index = 0; Index < ConfigDesc->NumInterfaces; Index++) {
+    while ((IfDesc->DescriptorType != USB_DESC_TYPE_INTERFACE) || (IfDesc->AlternateSetting != 0)) {
+      IfDesc = (USB_INTERFACE_DESCRIPTOR *) ((UINTN) IfDesc + IfDesc->Length);
+    }
+
+    NumEp = IfDesc->NumEndpoints;
+
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *) (IfDesc + 1);
+    for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+      while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *) ((UINTN) EpDesc + EpDesc->Length);
+      }
+
+      EpAddr    = (UINT8) (EpDesc->EndpointAddress & 0x0F);
+      Direction = (UINT8) ((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+      Dci = XhcPeiEndpointToDci (EpAddr, Direction);
+      if (Dci > MaxDci) {
+        MaxDci = Dci;
+      }
+
+      InputContext->InputControlContext.Dword2 |= (BIT0 << Dci);
+      InputContext->EP[Dci-1].MaxPacketSize     = EpDesc->MaxPacketSize;
+
+      if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+        //
+        // 6.2.3.4, shall be set to the value defined in the bMaxBurst field of the SuperSpeed Endpoint Companion Descriptor.
+        //
+        InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+      } else {
+        InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+      }
+
+      switch (EpDesc->Attributes & USB_ENDPOINT_TYPE_MASK) {
+        case USB_ENDPOINT_BULK:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_BULK_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_BULK_OUT;
+          }
+
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+            EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+            Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+            XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+          }
+
+          break;
+        case USB_ENDPOINT_ISO:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 0;
+            InputContext->EP[Dci-1].EPType = ED_ISOCH_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 0;
+            InputContext->EP[Dci-1].EPType = ED_ISOCH_OUT;
+          }
+          //
+          // Do not support isochronous transfer now.
+          //
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd: Unsupport ISO EP found, Transfer ring is not allocated.\n"));
+          EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+          continue;
+        case USB_ENDPOINT_INTERRUPT:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_INTERRUPT_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_INTERRUPT_OUT;
+          }
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          InputContext->EP[Dci-1].MaxESITPayload   = EpDesc->MaxPacketSize;
+          //
+          // Get the bInterval from descriptor and init the interval field of endpoint context
+          //
+          if ((DeviceSpeed == EFI_USB_SPEED_FULL) || (DeviceSpeed == EFI_USB_SPEED_LOW)) {
+            Interval = EpDesc->Interval;
+            //
+            // Calculate through the bInterval field of Endpoint descriptor.
+            //
+            ASSERT (Interval != 0);
+            InputContext->EP[Dci-1].Interval = (UINT32) HighBitSet32 ((UINT32) Interval) + 3;
+          } else if ((DeviceSpeed == EFI_USB_SPEED_HIGH) || (DeviceSpeed == EFI_USB_SPEED_SUPER)) {
+            Interval = EpDesc->Interval;
+            ASSERT (Interval >= 1 && Interval <= 16);
+            //
+            // Refer to XHCI 1.0 spec section 6.2.3.6, table 61
+            //
+            InputContext->EP[Dci-1].Interval = Interval - 1;
+          }
+
+          if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+            EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+            Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+            XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+          }
+          break;
+
+        case USB_ENDPOINT_CONTROL:
+          //
+          // Do not support control transfer now.
+          //
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd: Unsupport Control EP found, Transfer ring is not allocated.\n"));
+        default:
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd: Unknown EP found, Transfer ring is not allocated.\n"));
+          EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+          continue;
+      }
+
+      PhyAddr = UsbHcGetPciAddrForHostAddr (
+                  Xhc->MemPool,
+                  ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingSeg0,
+                  sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+                  );
+      PhyAddr &= ~((EFI_PHYSICAL_ADDRESS)0x0F);
+      PhyAddr |= (EFI_PHYSICAL_ADDRESS)((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingPCS;
+      InputContext->EP[Dci-1].PtrLo = XHC_LOW_32BIT (PhyAddr);
+      InputContext->EP[Dci-1].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *) ((UINTN) EpDesc + EpDesc->Length);
+    }
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *) ((UINTN) IfDesc + IfDesc->Length);
+  }
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+  InputContext->Slot.ContextEntries         = MaxDci;
+  //
+  // configure endpoint
+  //
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "XhcSetConfigCmd: Configure Endpoint\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetConfigCmd: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+XhcPeiSetConfigCmd64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR      *ConfigDesc
+  )
+{
+  EFI_STATUS                    Status;
+  USB_INTERFACE_DESCRIPTOR      *IfDesc;
+  USB_ENDPOINT_DESCRIPTOR       *EpDesc;
+  UINT8                         Index;
+  UINTN                         NumEp;
+  UINTN                         EpIndex;
+  UINT8                         EpAddr;
+  EFI_USB_DATA_DIRECTION        Direction;
+  UINT8                         Dci;
+  UINT8                         MaxDci;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  UINT8                         Interval;
+
+  TRANSFER_RING                 *EndpointTransferRing;
+  CMD_TRB_CONFIG_ENDPOINT       CmdTrbCfgEP;
+  INPUT_CONTEXT_64              *InputContext;
+  DEVICE_CONTEXT_64             *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  //
+  // 4.6.6 Configure Endpoint
+  //
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT_64));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *) (ConfigDesc + 1);
+  for (Index = 0; Index < ConfigDesc->NumInterfaces; Index++) {
+    while ((IfDesc->DescriptorType != USB_DESC_TYPE_INTERFACE) || (IfDesc->AlternateSetting != 0)) {
+      IfDesc = (USB_INTERFACE_DESCRIPTOR *) ((UINTN) IfDesc + IfDesc->Length);
+    }
+
+    NumEp = IfDesc->NumEndpoints;
+
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *) (IfDesc + 1);
+    for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+      while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *) ((UINTN) EpDesc + EpDesc->Length);
+      }
+
+      EpAddr    = (UINT8) (EpDesc->EndpointAddress & 0x0F);
+      Direction = (UINT8) ((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+      Dci = XhcPeiEndpointToDci (EpAddr, Direction);
+      ASSERT (Dci < 32);
+      if (Dci > MaxDci) {
+        MaxDci = Dci;
+      }
+
+      InputContext->InputControlContext.Dword2 |= (BIT0 << Dci);
+      InputContext->EP[Dci-1].MaxPacketSize     = EpDesc->MaxPacketSize;
+
+      if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+        //
+        // 6.2.3.4, shall be set to the value defined in the bMaxBurst field of the SuperSpeed Endpoint Companion Descriptor.
+        //
+        InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+      } else {
+        InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+      }
+
+      switch (EpDesc->Attributes & USB_ENDPOINT_TYPE_MASK) {
+        case USB_ENDPOINT_BULK:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_BULK_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_BULK_OUT;
+          }
+
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+            EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+            Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+            XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+          }
+
+          break;
+        case USB_ENDPOINT_ISO:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 0;
+            InputContext->EP[Dci-1].EPType = ED_ISOCH_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 0;
+            InputContext->EP[Dci-1].EPType = ED_ISOCH_OUT;
+          }
+          //
+          // Do not support isochronous transfer now.
+          //
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd64: Unsupport ISO EP found, Transfer ring is not allocated.\n"));
+          EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+          continue;
+        case USB_ENDPOINT_INTERRUPT:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_INTERRUPT_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_INTERRUPT_OUT;
+          }
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          InputContext->EP[Dci-1].MaxESITPayload   = EpDesc->MaxPacketSize;
+          //
+          // Get the bInterval from descriptor and init the the interval field of endpoint context
+          //
+          if ((DeviceSpeed == EFI_USB_SPEED_FULL) || (DeviceSpeed == EFI_USB_SPEED_LOW)) {
+            Interval = EpDesc->Interval;
+            //
+            // Calculate through the bInterval field of Endpoint descriptor.
+            //
+            ASSERT (Interval != 0);
+            InputContext->EP[Dci-1].Interval = (UINT32) HighBitSet32( (UINT32) Interval) + 3;
+          } else if ((DeviceSpeed == EFI_USB_SPEED_HIGH) || (DeviceSpeed == EFI_USB_SPEED_SUPER)) {
+            Interval = EpDesc->Interval;
+            ASSERT (Interval >= 1 && Interval <= 16);
+            //
+            // Refer to XHCI 1.0 spec section 6.2.3.6, table 61
+            //
+            InputContext->EP[Dci-1].Interval = Interval - 1;
+          }
+
+          if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+            EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+            Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+            XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+          }
+          break;
+
+        case USB_ENDPOINT_CONTROL:
+          //
+          // Do not support control transfer now.
+          //
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd64: Unsupport Control EP found, Transfer ring is not allocated.\n"));
+        default:
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd64: Unknown EP found, Transfer ring is not allocated.\n"));
+          EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+          continue;
+      }
+
+      PhyAddr = UsbHcGetPciAddrForHostAddr (
+                  Xhc->MemPool,
+                  ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingSeg0,
+                  sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+                  );
+
+      PhyAddr &= ~((EFI_PHYSICAL_ADDRESS)0x0F);
+      PhyAddr |= (EFI_PHYSICAL_ADDRESS)((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingPCS;
+
+      InputContext->EP[Dci-1].PtrLo = XHC_LOW_32BIT (PhyAddr);
+      InputContext->EP[Dci-1].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *) ((UINTN)EpDesc + EpDesc->Length);
+    }
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *) ((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+  InputContext->Slot.ContextEntries         = MaxDci;
+  //
+  // configure endpoint
+  //
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr  = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "XhcSetConfigCmd64: Configure Endpoint\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetConfigCmd64: Config Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+XhcPeiEvaluateContext (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT32                     MaxPacketSize
+  )
+{
+  EFI_STATUS                    Status;
+  CMD_TRB_EVALUATE_CONTEXT      CmdTrbEvalu;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT                 *InputContext;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  InputContext = Xhc->UsbDevContext[SlotId].InputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  InputContext->InputControlContext.Dword2 |= BIT1;
+  InputContext->EP[0].MaxPacketSize         = MaxPacketSize;
+
+  ZeroMem (&CmdTrbEvalu, sizeof (CmdTrbEvalu));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbEvalu.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbEvalu.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbEvalu.CycleBit = 1;
+  CmdTrbEvalu.Type     = TRB_TYPE_EVALU_CONTXT;
+  CmdTrbEvalu.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "XhcEvaluateContext: Evaluate context\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbEvalu,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcEvaluateContext: Evaluate Context Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+XhcPeiEvaluateContext64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT32                     MaxPacketSize
+  )
+{
+  EFI_STATUS                    Status;
+  CMD_TRB_EVALUATE_CONTEXT      CmdTrbEvalu;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT_64              *InputContext;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  InputContext = Xhc->UsbDevContext[SlotId].InputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  InputContext->InputControlContext.Dword2 |= BIT1;
+  InputContext->EP[0].MaxPacketSize         = MaxPacketSize;
+
+  ZeroMem (&CmdTrbEvalu, sizeof (CmdTrbEvalu));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbEvalu.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbEvalu.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbEvalu.CycleBit = 1;
+  CmdTrbEvalu.Type     = TRB_TYPE_EVALU_CONTXT;
+  CmdTrbEvalu.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "XhcEvaluateContext64: Evaluate context 64\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbEvalu,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcEvaluateContext64: Evaluate Context Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcPeiConfigHubContext (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      PortNum,
+  IN UINT8                      TTT,
+  IN UINT8                      MTT
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT                 *InputContext;
+  DEVICE_CONTEXT                *OutputContext;
+  CMD_TRB_CONFIG_ENDPOINT       CmdTrbCfgEP;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+
+  //
+  // Copy the slot context from OutputContext to Input context
+  //
+  CopyMem(&(InputContext->Slot), &(OutputContext->Slot), sizeof (SLOT_CONTEXT));
+  InputContext->Slot.Hub     = 1;
+  InputContext->Slot.PortNum = PortNum;
+  InputContext->Slot.TTT     = TTT;
+  InputContext->Slot.MTT     = MTT;
+
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Hub Slot Context\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcConfigHubContext: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcPeiConfigHubContext64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      PortNum,
+  IN UINT8                      TTT,
+  IN UINT8                      MTT
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT_64              *InputContext;
+  DEVICE_CONTEXT_64             *OutputContext;
+  CMD_TRB_CONFIG_ENDPOINT       CmdTrbCfgEP;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+
+  //
+  // Copy the slot context from OutputContext to Input context
+  //
+  CopyMem(&(InputContext->Slot), &(OutputContext->Slot), sizeof (SLOT_CONTEXT_64));
+  InputContext->Slot.Hub     = 1;
+  InputContext->Slot.PortNum = PortNum;
+  InputContext->Slot.TTT     = TTT;
+  InputContext->Slot.MTT     = MTT;
+
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Hub Slot Context 64\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcConfigHubContext64: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Stop endpoint successfully.
+  @retval Others        Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiStopEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  CMD_TRB_STOP_ENDPOINT         CmdTrbStopED;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiStopEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbStopED, sizeof (CmdTrbStopED));
+  CmdTrbStopED.CycleBit = 1;
+  CmdTrbStopED.Type     = TRB_TYPE_STOP_ENDPOINT;
+  CmdTrbStopED.EDID     = Dci;
+  CmdTrbStopED.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbStopED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiStopEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Reset endpoint successfully.
+  @retval Others        Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiResetEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_TRB_RESET_ENDPOINT      CmdTrbResetED;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiResetEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbResetED, sizeof (CmdTrbResetED));
+  CmdTrbResetED.CycleBit = 1;
+  CmdTrbResetED.Type     = TRB_TYPE_RESET_ENDPOINT;
+  CmdTrbResetED.EDID     = Dci;
+  CmdTrbResetED.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbResetED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiResetEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+  @param  Urb           The dequeue pointer of the transfer ring specified
+                        by the urb to be updated.
+
+  @retval EFI_SUCCESS   Set transfer ring dequeue pointer succeeds.
+  @retval Others        Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiSetTrDequeuePointer (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci,
+  IN URB                *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_SET_TR_DEQ_POINTER      CmdSetTRDeq;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiSetTrDequeuePointer: Slot = 0x%x, Dci = 0x%x, Urb = 0x%x\n", SlotId, Dci, Urb));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdSetTRDeq, sizeof (CmdSetTRDeq));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Urb->Ring->RingEnqueue, sizeof (CMD_SET_TR_DEQ_POINTER));
+  CmdSetTRDeq.PtrLo    = XHC_LOW_32BIT (PhyAddr) | Urb->Ring->RingPCS;
+  CmdSetTRDeq.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdSetTRDeq.CycleBit = 1;
+  CmdSetTRDeq.Type     = TRB_TYPE_SET_TR_DEQUE;
+  CmdSetTRDeq.Endpoint = Dci;
+  CmdSetTRDeq.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdSetTRDeq,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiSetTrDequeuePointer: Set TR Dequeue Pointer Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Check if there is a new generated event.
+
+  @param  Xhc           The XHCI device.
+  @param  EvtRing       The event ring to check.
+  @param  NewEvtTrb     The new event TRB found.
+
+  @retval EFI_SUCCESS   Found a new event TRB at the event ring.
+  @retval EFI_NOT_READY The event ring has no new event.
+
+**/
+EFI_STATUS
+XhcPeiCheckNewEvent (
+  IN PEI_XHC_DEV        *Xhc,
+  IN EVENT_RING         *EvtRing,
+  OUT TRB_TEMPLATE      **NewEvtTrb
+  )
+{
+  ASSERT (EvtRing != NULL);
+
+  *NewEvtTrb = EvtRing->EventRingDequeue;
+
+  if (EvtRing->EventRingDequeue == EvtRing->EventRingEnqueue) {
+    return EFI_NOT_READY;
+  }
+
+  EvtRing->EventRingDequeue++;
+  //
+  // If the dequeue pointer is beyond the ring, then roll-back it to the begining of the ring.
+  //
+  if ((UINTN) EvtRing->EventRingDequeue >= ((UINTN) EvtRing->EventRingSeg0 + sizeof (TRB_TEMPLATE) * EvtRing->TrbNumber)) {
+    EvtRing->EventRingDequeue = EvtRing->EventRingSeg0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Synchronize the specified event ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc       The XHCI device.
+  @param  EvtRing   The event ring to sync.
+
+  @retval EFI_SUCCESS The event ring is synchronized successfully.
+
+**/
+EFI_STATUS
+XhcPeiSyncEventRing (
+  IN PEI_XHC_DEV    *Xhc,
+  IN EVENT_RING     *EvtRing
+  )
+{
+  UINTN             Index;
+  TRB_TEMPLATE      *EvtTrb;
+
+  ASSERT (EvtRing != NULL);
+
+  //
+  // Calculate the EventRingEnqueue and EventRingCCS.
+  // Note: only support single Segment
+  //
+  EvtTrb = EvtRing->EventRingDequeue;
+
+  for (Index = 0; Index < EvtRing->TrbNumber; Index++) {
+    if (EvtTrb->CycleBit != EvtRing->EventRingCCS) {
+      break;
+    }
+
+    EvtTrb++;
+
+    if ((UINTN) EvtTrb >= ((UINTN) EvtRing->EventRingSeg0 + sizeof (TRB_TEMPLATE) * EvtRing->TrbNumber)) {
+      EvtTrb = EvtRing->EventRingSeg0;
+      EvtRing->EventRingCCS = (EvtRing->EventRingCCS) ? 0 : 1;
+    }
+  }
+
+  if (Index < EvtRing->TrbNumber) {
+    EvtRing->EventRingEnqueue = EvtTrb;
+  } else {
+    ASSERT (FALSE);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Free XHCI event ring.
+
+  @param  Xhc           The XHCI device.
+  @param  EventRing     The event ring to be freed.
+
+**/
+VOID
+XhcPeiFreeEventRing (
+  IN PEI_XHC_DEV        *Xhc,
+  IN EVENT_RING         *EventRing
+  )
+{
+  if(EventRing->EventRingSeg0 == NULL) {
+    return;
+  }
+
+  //
+  // Free EventRing Segment 0
+  //
+  UsbHcFreeMem (Xhc->MemPool, EventRing->EventRingSeg0, sizeof (TRB_TEMPLATE) * EVENT_RING_TRB_NUMBER);
+
+  //
+  // Free ERST table
+  //
+  UsbHcFreeMem (Xhc->MemPool, EventRing->ERSTBase, sizeof (EVENT_RING_SEG_TABLE_ENTRY) * ERST_NUMBER);
+}
+
+/**
+  Create XHCI event ring.
+
+  @param  Xhc           The XHCI device.
+  @param  EventRing     The created event ring.
+
+**/
+VOID
+XhcPeiCreateEventRing (
+  IN PEI_XHC_DEV        *Xhc,
+  OUT EVENT_RING        *EventRing
+  )
+{
+  VOID                          *Buf;
+  EVENT_RING_SEG_TABLE_ENTRY    *ERSTBase;
+  UINTN                         Size;
+  EFI_PHYSICAL_ADDRESS          ERSTPhy;
+  EFI_PHYSICAL_ADDRESS          DequeuePhy;
+
+  ASSERT (EventRing != NULL);
+
+  Size = sizeof (TRB_TEMPLATE) * EVENT_RING_TRB_NUMBER;
+  Buf = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, Size);
+
+  DequeuePhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, Size);
+
+  EventRing->EventRingSeg0      = Buf;
+  EventRing->TrbNumber          = EVENT_RING_TRB_NUMBER;
+  EventRing->EventRingDequeue   = (TRB_TEMPLATE *) EventRing->EventRingSeg0;
+  EventRing->EventRingEnqueue   = (TRB_TEMPLATE *) EventRing->EventRingSeg0;
+
+  //
+  // Software maintains an Event Ring Consumer Cycle State (CCS) bit, initializing it to '1'
+  // and toggling it every time the Event Ring Dequeue Pointer wraps back to the beginning of the Event Ring.
+  //
+  EventRing->EventRingCCS = 1;
+
+  Size = sizeof (EVENT_RING_SEG_TABLE_ENTRY) * ERST_NUMBER;
+  Buf = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, Size);
+
+  ERSTBase              = (EVENT_RING_SEG_TABLE_ENTRY *) Buf;
+  EventRing->ERSTBase   = ERSTBase;
+  ERSTBase->PtrLo       = XHC_LOW_32BIT (DequeuePhy);
+  ERSTBase->PtrHi       = XHC_HIGH_32BIT (DequeuePhy);
+  ERSTBase->RingTrbSize = EVENT_RING_TRB_NUMBER;
+
+  ERSTPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, Size);
+
+  //
+  // Program the Interrupter Event Ring Segment Table Size (ERSTSZ) register (5.5.2.3.1)
+  //
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTSZ_OFFSET,
+    ERST_NUMBER
+    );
+  //
+  // Program the Interrupter Event Ring Dequeue Pointer (ERDP) register (5.5.2.3.3)
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERDP_OFFSET,
+    XHC_LOW_32BIT ((UINT64) (UINTN) DequeuePhy)
+    );
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERDP_OFFSET + 4,
+    XHC_HIGH_32BIT ((UINT64) (UINTN) DequeuePhy)
+    );
+  //
+  // Program the Interrupter Event Ring Segment Table Base Address (ERSTBA) register (5.5.2.3.2)
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTBA_OFFSET,
+    XHC_LOW_32BIT ((UINT64) (UINTN) ERSTPhy)
+    );
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTBA_OFFSET + 4,
+    XHC_HIGH_32BIT ((UINT64) (UINTN) ERSTPhy)
+    );
+  //
+  // Need set IMAN IE bit to enable the ring interrupt
+  //
+  XhcPeiSetRuntimeRegBit (Xhc, XHC_IMAN_OFFSET, XHC_IMAN_IE);
+}
+
+/**
+  Synchronize the specified transfer ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc       The XHCI device.
+  @param  TrsRing   The transfer ring to sync.
+
+  @retval EFI_SUCCESS The transfer ring is synchronized successfully.
+
+**/
+EFI_STATUS
+XhcPeiSyncTrsRing (
+  IN PEI_XHC_DEV    *Xhc,
+  IN TRANSFER_RING  *TrsRing
+  )
+{
+  UINTN             Index;
+  TRB_TEMPLATE      *TrsTrb;
+
+  ASSERT (TrsRing != NULL);
+  //
+  // Calculate the latest RingEnqueue and RingPCS
+  //
+  TrsTrb = TrsRing->RingEnqueue;
+  ASSERT (TrsTrb != NULL);
+
+  for (Index = 0; Index < TrsRing->TrbNumber; Index++) {
+    if (TrsTrb->CycleBit != (TrsRing->RingPCS & BIT0)) {
+      break;
+    }
+    TrsTrb++;
+    if ((UINT8) TrsTrb->Type == TRB_TYPE_LINK) {
+      ASSERT (((LINK_TRB *) TrsTrb)->TC != 0);
+      //
+      // set cycle bit in Link TRB as normal
+      //
+      ((LINK_TRB*)TrsTrb)->CycleBit = TrsRing->RingPCS & BIT0;
+      //
+      // Toggle PCS maintained by software
+      //
+      TrsRing->RingPCS = (TrsRing->RingPCS & BIT0) ? 0 : 1;
+      TrsTrb = (TRB_TEMPLATE *) TrsRing->RingSeg0;  // Use host address
+    }
+  }
+
+  ASSERT (Index != TrsRing->TrbNumber);
+
+  if (TrsTrb != TrsRing->RingEnqueue) {
+    TrsRing->RingEnqueue = TrsTrb;
+  }
+
+  //
+  // Clear the Trb context for enqueue, but reserve the PCS bit
+  //
+  TrsTrb->Parameter1 = 0;
+  TrsTrb->Parameter2 = 0;
+  TrsTrb->Status     = 0;
+  TrsTrb->RsvdZ1     = 0;
+  TrsTrb->Type       = 0;
+  TrsTrb->Control    = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create XHCI transfer ring.
+
+  @param  Xhc               The XHCI Device.
+  @param  TrbNum            The number of TRB in the ring.
+  @param  TransferRing      The created transfer ring.
+
+**/
+VOID
+XhcPeiCreateTransferRing (
+  IN PEI_XHC_DEV            *Xhc,
+  IN UINTN                  TrbNum,
+  OUT TRANSFER_RING         *TransferRing
+  )
+{
+  VOID                  *Buf;
+  LINK_TRB              *EndTrb;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  Buf = UsbHcAllocateMem (Xhc->MemPool, sizeof (TRB_TEMPLATE) * TrbNum);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, sizeof (TRB_TEMPLATE) * TrbNum);
+
+  TransferRing->RingSeg0     = Buf;
+  TransferRing->TrbNumber    = TrbNum;
+  TransferRing->RingEnqueue  = (TRB_TEMPLATE *) TransferRing->RingSeg0;
+  TransferRing->RingDequeue  = (TRB_TEMPLATE *) TransferRing->RingSeg0;
+  TransferRing->RingPCS      = 1;
+  //
+  // 4.9.2 Transfer Ring Management
+  // To form a ring (or circular queue) a Link TRB may be inserted at the end of a ring to
+  // point to the first TRB in the ring.
+  //
+  EndTrb        = (LINK_TRB *) ((UINTN) Buf + sizeof (TRB_TEMPLATE) * (TrbNum - 1));
+  EndTrb->Type  = TRB_TYPE_LINK;
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, sizeof (TRB_TEMPLATE) * TrbNum);
+  EndTrb->PtrLo = XHC_LOW_32BIT (PhyAddr);
+  EndTrb->PtrHi = XHC_HIGH_32BIT (PhyAddr);
+  //
+  // Toggle Cycle (TC). When set to '1', the xHC shall toggle its interpretation of the Cycle bit.
+  //
+  EndTrb->TC    = 1;
+  //
+  // Set Cycle bit as other TRB PCS init value
+  //
+  EndTrb->CycleBit = 0;
+}
+
+/**
+  Initialize the XHCI host controller for schedule.
+
+  @param  Xhc       The XHCI device to be initialized.
+
+**/
+VOID
+XhcPeiInitSched (
+  IN PEI_XHC_DEV        *Xhc
+  )
+{
+  VOID                  *Dcbaa;
+  EFI_PHYSICAL_ADDRESS  DcbaaPhy;
+  UINTN                 Size;
+  EFI_PHYSICAL_ADDRESS  CmdRingPhy;
+  UINT32                MaxScratchpadBufs;
+  UINT64                *ScratchBuf;
+  EFI_PHYSICAL_ADDRESS  ScratchPhy;
+  UINT64                *ScratchEntry;
+  EFI_PHYSICAL_ADDRESS  ScratchEntryPhy;
+  UINT32                Index;
+  EFI_STATUS            Status;
+
+  //
+  // Initialize memory management.
+  //
+  Xhc->MemPool = UsbHcInitMemPool ();
+  ASSERT (Xhc->MemPool != NULL);
+
+  //
+  // Program the Max Device Slots Enabled (MaxSlotsEn) field in the CONFIG register (5.4.7)
+  // to enable the device slots that system software is going to use.
+  //
+  Xhc->MaxSlotsEn = Xhc->HcSParams1.Data.MaxSlots;
+  ASSERT (Xhc->MaxSlotsEn >= 1 && Xhc->MaxSlotsEn <= 255);
+  XhcPeiWriteOpReg (Xhc, XHC_CONFIG_OFFSET, (XhcPeiReadOpReg (Xhc, XHC_CONFIG_OFFSET) & ~XHC_CONFIG_MASK) | Xhc->MaxSlotsEn);
+
+  //
+  // The Device Context Base Address Array entry associated with each allocated Device Slot
+  // shall contain a 64-bit pointer to the base of the associated Device Context.
+  // The Device Context Base Address Array shall contain MaxSlotsEn + 1 entries.
+  // Software shall set Device Context Base Address Array entries for unallocated Device Slots to '0'.
+  //
+  Size = (Xhc->MaxSlotsEn + 1) * sizeof (UINT64);
+  Dcbaa = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Dcbaa != NULL);
+
+  //
+  // A Scratchpad Buffer is a PAGESIZE block of system memory located on a PAGESIZE boundary.
+  // System software shall allocate the Scratchpad Buffer(s) before placing the xHC in to Run
+  // mode (Run/Stop(R/S) ='1').
+  //
+  MaxScratchpadBufs      = ((Xhc->HcSParams2.Data.ScratchBufHi) << 5) | (Xhc->HcSParams2.Data.ScratchBufLo);
+  Xhc->MaxScratchpadBufs = MaxScratchpadBufs;
+  ASSERT (MaxScratchpadBufs <= 1023);
+  if (MaxScratchpadBufs != 0) {
+    //
+    // Allocate the buffer to record the host address for each entry
+    //
+    ScratchEntry = AllocateZeroPool (sizeof (UINT64) * MaxScratchpadBufs);
+    ASSERT (ScratchEntry != NULL);
+    Xhc->ScratchEntry = ScratchEntry;
+
+    ScratchPhy = 0;
+    Status = UsbHcAllocateAlignedPages (
+               EFI_SIZE_TO_PAGES (MaxScratchpadBufs * sizeof (UINT64)),
+               Xhc->PageSize,
+               (VOID **) &ScratchBuf,
+               &ScratchPhy
+               );
+    ASSERT_EFI_ERROR (Status);
+
+    ZeroMem (ScratchBuf, MaxScratchpadBufs * sizeof (UINT64));
+    Xhc->ScratchBuf = ScratchBuf;
+
+    //
+    // Allocate each scratch buffer
+    //
+    for (Index = 0; Index < MaxScratchpadBufs; Index++) {
+      ScratchEntryPhy = 0;
+      Status = UsbHcAllocateAlignedPages (
+                 EFI_SIZE_TO_PAGES (Xhc->PageSize),
+                 Xhc->PageSize,
+                 (VOID **) &ScratchEntry[Index],
+                 &ScratchEntryPhy
+                 );
+      ASSERT_EFI_ERROR (Status);
+      ZeroMem ((VOID *) (UINTN) ScratchEntry[Index], Xhc->PageSize);
+      //
+      // Fill with the PCI device address
+      //
+      *ScratchBuf++ = ScratchEntryPhy;
+    }
+    //
+    // The Scratchpad Buffer Array contains pointers to the Scratchpad Buffers. Entry 0 of the
+    // Device Context Base Address Array points to the Scratchpad Buffer Array.
+    //
+    *(UINT64 *) Dcbaa = (UINT64) (UINTN) ScratchPhy;
+  }
+
+  //
+  // Program the Device Context Base Address Array Pointer (DCBAAP) register (5.4.6) with
+  // a 64-bit address pointing to where the Device Context Base Address Array is located.
+  //
+  Xhc->DCBAA = (UINT64 *) (UINTN) Dcbaa;
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  DcbaaPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Dcbaa, Size);
+  XhcPeiWriteOpReg (Xhc, XHC_DCBAAP_OFFSET, XHC_LOW_32BIT (DcbaaPhy));
+  XhcPeiWriteOpReg (Xhc, XHC_DCBAAP_OFFSET + 4, XHC_HIGH_32BIT (DcbaaPhy));
+
+  DEBUG ((EFI_D_INFO, "XhcPeiInitSched:DCBAA=0x%x\n", Xhc->DCBAA));
+
+  //
+  // Define the Command Ring Dequeue Pointer by programming the Command Ring Control Register
+  // (5.4.5) with a 64-bit address pointing to the starting address of the first TRB of the Command Ring.
+  // Note: The Command Ring is 64 byte aligned, so the low order 6 bits of the Command Ring Pointer shall
+  // always be '0'.
+  //
+  XhcPeiCreateTransferRing (Xhc, CMD_RING_TRB_NUMBER, &Xhc->CmdRing);
+  //
+  // The xHC uses the Enqueue Pointer to determine when a Transfer Ring is empty. As it fetches TRBs from a
+  // Transfer Ring it checks for a Cycle bit transition. If a transition detected, the ring is empty.
+  // So we set RCS as inverted PCS init value to let Command Ring empty
+  //
+  CmdRingPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->CmdRing.RingSeg0, sizeof (TRB_TEMPLATE) * CMD_RING_TRB_NUMBER);
+  ASSERT ((CmdRingPhy & 0x3F) == 0);
+  CmdRingPhy |= XHC_CRCR_RCS;
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcPeiWriteOpReg (Xhc, XHC_CRCR_OFFSET, XHC_LOW_32BIT (CmdRingPhy));
+  XhcPeiWriteOpReg (Xhc, XHC_CRCR_OFFSET + 4, XHC_HIGH_32BIT (CmdRingPhy));
+
+  DEBUG ((EFI_D_INFO, "XhcPeiInitSched:XHC_CRCR=0x%x\n", Xhc->CmdRing.RingSeg0));
+
+  //
+  // Disable the 'interrupter enable' bit in USB_CMD
+  // and clear IE & IP bit in all Interrupter X Management Registers.
+  //
+  XhcPeiClearOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_INTE);
+  for (Index = 0; Index < (UINT16)(Xhc->HcSParams1.Data.MaxIntrs); Index++) {
+    XhcPeiClearRuntimeRegBit (Xhc, XHC_IMAN_OFFSET + (Index * 32), XHC_IMAN_IE);
+    XhcPeiSetRuntimeRegBit (Xhc, XHC_IMAN_OFFSET + (Index * 32), XHC_IMAN_IP);
+  }
+
+  //
+  // Allocate EventRing for Cmd, Ctrl, Bulk, Interrupt, AsynInterrupt transfer
+  //
+  XhcPeiCreateEventRing (Xhc, &Xhc->EventRing);
+  DEBUG ((EFI_D_INFO, "XhcPeiInitSched:XHC_EVENTRING=0x%x\n", Xhc->EventRing.EventRingSeg0));
+}
+
+/**
+  Free the resouce allocated at initializing schedule.
+
+  @param  Xhc       The XHCI device.
+
+**/
+VOID
+XhcPeiFreeSched (
+  IN PEI_XHC_DEV    *Xhc
+  )
+{
+  UINT32                  Index;
+  UINT64                  *ScratchEntry;
+
+  if (Xhc->ScratchBuf != NULL) {
+    ScratchEntry = Xhc->ScratchEntry;
+    for (Index = 0; Index < Xhc->MaxScratchpadBufs; Index++) {
+      //
+      // Free Scratchpad Buffers
+      //
+      UsbHcFreeAlignedPages ((VOID*) (UINTN) ScratchEntry[Index], EFI_SIZE_TO_PAGES (Xhc->PageSize));
+    }
+    //
+    // Free Scratchpad Buffer Array
+    //
+    UsbHcFreeAlignedPages (Xhc->ScratchBuf, EFI_SIZE_TO_PAGES (Xhc->MaxScratchpadBufs * sizeof (UINT64)));
+    FreePool (Xhc->ScratchEntry);
+  }
+
+  if (Xhc->CmdRing.RingSeg0 != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->CmdRing.RingSeg0, sizeof (TRB_TEMPLATE) * CMD_RING_TRB_NUMBER);
+    Xhc->CmdRing.RingSeg0 = NULL;
+  }
+
+  XhcPeiFreeEventRing (Xhc,&Xhc->EventRing);
+
+  if (Xhc->DCBAA != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->DCBAA, (Xhc->MaxSlotsEn + 1) * sizeof (UINT64));
+    Xhc->DCBAA = NULL;
+  }
+
+  //
+  // Free memory pool at last
+  //
+  if (Xhc->MemPool != NULL) {
+    UsbHcFreeMemPool (Xhc->MemPool);
+    Xhc->MemPool = NULL;
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.h b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.h
new file mode 100644
index 0000000000..b3d4c45614
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.h
@@ -0,0 +1,1307 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2014 - 2015, Intel Corporation. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_PEI_XHCI_SCHED_H_
+#define _EFI_PEI_XHCI_SCHED_H_
+
+//
+// Transfer types, used in URB to identify the transfer type
+//
+#define XHC_CTRL_TRANSFER                       0x01
+#define XHC_BULK_TRANSFER                       0x02
+
+//
+// 6.4.6 TRB Types
+//
+#define TRB_TYPE_NORMAL                         1
+#define TRB_TYPE_SETUP_STAGE                    2
+#define TRB_TYPE_DATA_STAGE                     3
+#define TRB_TYPE_STATUS_STAGE                   4
+#define TRB_TYPE_ISOCH                          5
+#define TRB_TYPE_LINK                           6
+#define TRB_TYPE_EVENT_DATA                     7
+#define TRB_TYPE_NO_OP                          8
+#define TRB_TYPE_EN_SLOT                        9
+#define TRB_TYPE_DIS_SLOT                       10
+#define TRB_TYPE_ADDRESS_DEV                    11
+#define TRB_TYPE_CON_ENDPOINT                   12
+#define TRB_TYPE_EVALU_CONTXT                   13
+#define TRB_TYPE_RESET_ENDPOINT                 14
+#define TRB_TYPE_STOP_ENDPOINT                  15
+#define TRB_TYPE_SET_TR_DEQUE                   16
+#define TRB_TYPE_RESET_DEV                      17
+#define TRB_TYPE_GET_PORT_BANW                  21
+#define TRB_TYPE_FORCE_HEADER                   22
+#define TRB_TYPE_NO_OP_COMMAND                  23
+#define TRB_TYPE_TRANS_EVENT                    32
+#define TRB_TYPE_COMMAND_COMPLT_EVENT           33
+#define TRB_TYPE_PORT_STATUS_CHANGE_EVENT       34
+#define TRB_TYPE_HOST_CONTROLLER_EVENT          37
+#define TRB_TYPE_DEVICE_NOTIFI_EVENT            38
+#define TRB_TYPE_MFINDEX_WRAP_EVENT             39
+
+//
+// Endpoint Type (EP Type).
+//
+#define ED_NOT_VALID                            0
+#define ED_ISOCH_OUT                            1
+#define ED_BULK_OUT                             2
+#define ED_INTERRUPT_OUT                        3
+#define ED_CONTROL_BIDIR                        4
+#define ED_ISOCH_IN                             5
+#define ED_BULK_IN                              6
+#define ED_INTERRUPT_IN                         7
+
+//
+// 6.4.5 TRB Completion Codes
+//
+#define TRB_COMPLETION_INVALID                  0
+#define TRB_COMPLETION_SUCCESS                  1
+#define TRB_COMPLETION_DATA_BUFFER_ERROR        2
+#define TRB_COMPLETION_BABBLE_ERROR             3
+#define TRB_COMPLETION_USB_TRANSACTION_ERROR    4
+#define TRB_COMPLETION_TRB_ERROR                5
+#define TRB_COMPLETION_STALL_ERROR              6
+#define TRB_COMPLETION_SHORT_PACKET             13
+
+//
+// The topology string used to present usb device location
+//
+typedef struct _USB_DEV_TOPOLOGY {
+  //
+  // The tier concatenation of down stream port.
+  //
+  UINT32 RouteString:20;
+  //
+  // The root port number of the chain.
+  //
+  UINT32 RootPortNum:8;
+  //
+  // The Tier the device reside.
+  //
+  UINT32 TierNum:4;
+} USB_DEV_TOPOLOGY;
+
+//
+// USB Device's RouteChart
+//
+typedef union _USB_DEV_ROUTE {
+  UINT32                    Dword;
+  USB_DEV_TOPOLOGY          Route;
+} USB_DEV_ROUTE;
+
+//
+// Endpoint address and its capabilities
+//
+typedef struct _USB_ENDPOINT {
+  //
+  // Store logical device address assigned by UsbBus
+  // It's because some XHCI host controllers may assign the same physcial device
+  // address for those devices inserted at different root port.
+  //
+  UINT8                     BusAddr;
+  UINT8                     DevAddr;
+  UINT8                     EpAddr;
+  EFI_USB_DATA_DIRECTION    Direction;
+  UINT8                     DevSpeed;
+  UINTN                     MaxPacket;
+  UINTN                     Type;
+} USB_ENDPOINT;
+
+//
+// TRB Template
+//
+typedef struct _TRB_TEMPLATE {
+  UINT32                    Parameter1;
+
+  UINT32                    Parameter2;
+
+  UINT32                    Status;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ1:9;
+  UINT32                    Type:6;
+  UINT32                    Control:16;
+} TRB_TEMPLATE;
+
+typedef struct _TRANSFER_RING {
+  VOID                      *RingSeg0;
+  UINTN                     TrbNumber;
+  TRB_TEMPLATE              *RingEnqueue;
+  TRB_TEMPLATE              *RingDequeue;
+  UINT32                    RingPCS;
+} TRANSFER_RING;
+
+typedef struct _EVENT_RING {
+  VOID                      *ERSTBase;
+  VOID                      *EventRingSeg0;
+  UINTN                     TrbNumber;
+  TRB_TEMPLATE              *EventRingEnqueue;
+  TRB_TEMPLATE              *EventRingDequeue;
+  UINT32                    EventRingCCS;
+} EVENT_RING;
+
+#define XHC_URB_SIG                 SIGNATURE_32 ('U', 'S', 'B', 'R')
+
+//
+// URB (Usb Request Block) contains information for all kinds of
+// usb requests.
+//
+typedef struct _URB {
+  UINT32                            Signature;
+  //
+  // Usb Device URB related information
+  //
+  USB_ENDPOINT                      Ep;
+  EFI_USB_DEVICE_REQUEST            *Request;
+  VOID                              *Data;
+  UINTN                             DataLen;
+  VOID                              *DataPhy;
+  EFI_ASYNC_USB_TRANSFER_CALLBACK   Callback;
+  VOID                              *Context;
+  //
+  // Execute result
+  //
+  UINT32                            Result;
+  //
+  // completed data length
+  //
+  UINTN                             Completed;
+  //
+  // Command/Tranfer Ring info
+  //
+  TRANSFER_RING                     *Ring;
+  TRB_TEMPLATE                      *TrbStart;
+  TRB_TEMPLATE                      *TrbEnd;
+  UINTN                             TrbNum;
+  BOOLEAN                           StartDone;
+  BOOLEAN                           EndDone;
+  BOOLEAN                           Finished;
+
+  TRB_TEMPLATE                      *EvtTrb;
+} URB;
+
+//
+// 6.5 Event Ring Segment Table
+// The Event Ring Segment Table is used to define multi-segment Event Rings and to enable runtime
+// expansion and shrinking of the Event Ring. The location of the Event Ring Segment Table is defined by the
+// Event Ring Segment Table Base Address Register (5.5.2.3.2). The size of the Event Ring Segment Table
+// is defined by the Event Ring Segment Table Base Size Register (5.5.2.3.1).
+//
+typedef struct _EVENT_RING_SEG_TABLE_ENTRY {
+  UINT32                    PtrLo;
+  UINT32                    PtrHi;
+  UINT32                    RingTrbSize:16;
+  UINT32                    RsvdZ1:16;
+  UINT32                    RsvdZ2;
+} EVENT_RING_SEG_TABLE_ENTRY;
+
+//
+// 6.4.1.1 Normal TRB
+// A Normal TRB is used in several ways; exclusively on Bulk and Interrupt Transfer Rings for normal and
+// Scatter/Gather operations, to define additional data buffers for Scatter/Gather operations on Isoch Transfer
+// Rings, and to define the Data stage information for Control Transfer Rings.
+//
+typedef struct _TRANSFER_TRB_NORMAL {
+  UINT32                    TRBPtrLo;
+
+  UINT32                    TRBPtrHi;
+
+  UINT32                    Length:17;
+  UINT32                    TDSize:5;
+  UINT32                    IntTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    ENT:1;
+  UINT32                    ISP:1;
+  UINT32                    NS:1;
+  UINT32                    CH:1;
+  UINT32                    IOC:1;
+  UINT32                    IDT:1;
+  UINT32                    RsvdZ1:2;
+  UINT32                    BEI:1;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ2:16;
+} TRANSFER_TRB_NORMAL;
+
+//
+// 6.4.1.2.1 Setup Stage TRB
+// A Setup Stage TRB is created by system software to initiate a USB Setup packet on a control endpoint.
+//
+typedef struct _TRANSFER_TRB_CONTROL_SETUP {
+  UINT32                    bmRequestType:8;
+  UINT32                    bRequest:8;
+  UINT32                    wValue:16;
+
+  UINT32                    wIndex:16;
+  UINT32                    wLength:16;
+
+  UINT32                    Length:17;
+  UINT32                    RsvdZ1:5;
+  UINT32                    IntTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:4;
+  UINT32                    IOC:1;
+  UINT32                    IDT:1;
+  UINT32                    RsvdZ3:3;
+  UINT32                    Type:6;
+  UINT32                    TRT:2;
+  UINT32                    RsvdZ4:14;
+} TRANSFER_TRB_CONTROL_SETUP;
+
+//
+// 6.4.1.2.2 Data Stage TRB
+// A Data Stage TRB is used generate the Data stage transaction of a USB Control transfer.
+//
+typedef struct _TRANSFER_TRB_CONTROL_DATA {
+  UINT32                    TRBPtrLo;
+
+  UINT32                    TRBPtrHi;
+
+  UINT32                    Length:17;
+  UINT32                    TDSize:5;
+  UINT32                    IntTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    ENT:1;
+  UINT32                    ISP:1;
+  UINT32                    NS:1;
+  UINT32                    CH:1;
+  UINT32                    IOC:1;
+  UINT32                    IDT:1;
+  UINT32                    RsvdZ1:3;
+  UINT32                    Type:6;
+  UINT32                    DIR:1;
+  UINT32                    RsvdZ2:15;
+} TRANSFER_TRB_CONTROL_DATA;
+
+//
+// 6.4.1.2.2 Data Stage TRB
+// A Data Stage TRB is used generate the Data stage transaction of a USB Control transfer.
+//
+typedef struct _TRANSFER_TRB_CONTROL_STATUS {
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    RsvdZ3:22;
+  UINT32                    IntTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    ENT:1;
+  UINT32                    RsvdZ4:2;
+  UINT32                    CH:1;
+  UINT32                    IOC:1;
+  UINT32                    RsvdZ5:4;
+  UINT32                    Type:6;
+  UINT32                    DIR:1;
+  UINT32                    RsvdZ6:15;
+} TRANSFER_TRB_CONTROL_STATUS;
+
+//
+// 6.4.2.1 Transfer Event TRB
+// A Transfer Event provides the completion status associated with a Transfer TRB. Refer to section 4.11.3.1
+// for more information on the use and operation of Transfer Events.
+//
+typedef struct _EVT_TRB_TRANSFER {
+  UINT32                    TRBPtrLo;
+
+  UINT32                    TRBPtrHi;
+
+  UINT32                    Length:24;
+  UINT32                    Completecode:8;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ1:1;
+  UINT32                    ED:1;
+  UINT32                    RsvdZ2:7;
+  UINT32                    Type:6;
+  UINT32                    EndpointId:5;
+  UINT32                    RsvdZ3:3;
+  UINT32                    SlotId:8;
+} EVT_TRB_TRANSFER;
+
+//
+// 6.4.2.2 Command Completion Event TRB
+// A Command Completion Event TRB shall be generated by the xHC when a command completes on the
+// Command Ring. Refer to section 4.11.4 for more information on the use of Command Completion Events.
+//
+typedef struct _EVT_TRB_COMMAND_COMPLETION {
+  UINT32                    TRBPtrLo;
+
+  UINT32                    TRBPtrHi;
+
+  UINT32                    RsvdZ2:24;
+  UINT32                    Completecode:8;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    VFID:8;
+  UINT32                    SlotId:8;
+} EVT_TRB_COMMAND_COMPLETION;
+
+typedef union _TRB {
+  TRB_TEMPLATE                  TrbTemplate;
+  TRANSFER_TRB_NORMAL           TrbNormal;
+  TRANSFER_TRB_CONTROL_SETUP    TrbCtrSetup;
+  TRANSFER_TRB_CONTROL_DATA     TrbCtrData;
+  TRANSFER_TRB_CONTROL_STATUS   TrbCtrStatus;
+} TRB;
+
+//
+// 6.4.3.1 No Op Command TRB
+// The No Op Command TRB provides a simple means for verifying the operation of the Command Ring
+// mechanisms offered by the xHCI.
+//
+typedef struct _CMD_TRB_NO_OP {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ4:16;
+} CMD_TRB_NO_OP;
+
+//
+// 6.4.3.2 Enable Slot Command TRB
+// The Enable Slot Command TRB causes the xHC to select an available Device Slot and return the ID of the
+// selected slot to the host in a Command Completion Event.
+//
+typedef struct _CMD_TRB_ENABLE_SLOT {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ4:16;
+} CMD_TRB_ENABLE_SLOT;
+
+//
+// 6.4.3.3 Disable Slot Command TRB
+// The Disable Slot Command TRB releases any bandwidth assigned to the disabled slot and frees any
+// internal xHC resources assigned to the slot.
+//
+typedef struct _CMD_TRB_DISABLE_SLOT {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ4:8;
+  UINT32                    SlotId:8;
+} CMD_TRB_DISABLE_SLOT;
+
+//
+// 6.4.3.4 Address Device Command TRB
+// The Address Device Command TRB transitions the selected Device Context from the Default to the
+// Addressed state and causes the xHC to select an address for the USB device in the Default State and
+// issue a SET_ADDRESS request to the USB device.
+//
+typedef struct _CMD_TRB_ADDRESS_DEVICE {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:8;
+  UINT32                    BSR:1;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ3:8;
+  UINT32                    SlotId:8;
+} CMD_TRB_ADDRESS_DEVICE;
+
+//
+// 6.4.3.5 Configure Endpoint Command TRB
+// The Configure Endpoint Command TRB evaluates the bandwidth and resource requirements of the
+// endpoints selected by the command.
+//
+typedef struct _CMD_TRB_CONFIG_ENDPOINT {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:8;
+  UINT32                    DC:1;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ3:8;
+  UINT32                    SlotId:8;
+} CMD_TRB_CONFIG_ENDPOINT;
+
+//
+// 6.4.3.6 Evaluate Context Command TRB
+// The Evaluate Context Command TRB is used by system software to inform the xHC that the selected
+// Context data structures in the Device Context have been modified by system software and that the xHC
+// shall evaluate any changes
+//
+typedef struct _CMD_TRB_EVALUATE_CONTEXT {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:9;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ3:8;
+  UINT32                    SlotId:8;
+} CMD_TRB_EVALUATE_CONTEXT;
+
+//
+// 6.4.3.7 Reset Endpoint Command TRB
+// The Reset Endpoint Command TRB is used by system software to reset a specified Transfer Ring
+//
+typedef struct _CMD_TRB_RESET_ENDPOINT {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:8;
+  UINT32                    TSP:1;
+  UINT32                    Type:6;
+  UINT32                    EDID:5;
+  UINT32                    RsvdZ4:3;
+  UINT32                    SlotId:8;
+} CMD_TRB_RESET_ENDPOINT;
+
+//
+// 6.4.3.8 Stop Endpoint Command TRB
+// The Stop Endpoint Command TRB command allows software to stop the xHC execution of the TDs on a
+// Transfer Ring and temporarily take ownership of TDs that had previously been passed to the xHC.
+//
+typedef struct _CMD_TRB_STOP_ENDPOINT {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    EDID:5;
+  UINT32                    RsvdZ4:2;
+  UINT32                    SP:1;
+  UINT32                    SlotId:8;
+} CMD_TRB_STOP_ENDPOINT;
+
+//
+// 6.4.3.9 Set TR Dequeue Pointer Command TRB
+// The Set TR Dequeue Pointer Command TRB is used by system software to modify the TR Dequeue
+// Pointer and DCS fields of an Endpoint or Stream Context.
+//
+typedef struct _CMD_SET_TR_DEQ_POINTER {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1:16;
+  UINT32                    StreamID:16;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:9;
+  UINT32                    Type:6;
+  UINT32                    Endpoint:5;
+  UINT32                    RsvdZ3:3;
+  UINT32                    SlotId:8;
+} CMD_SET_TR_DEQ_POINTER;
+
+//
+// 6.4.4.1 Link TRB
+// A Link TRB provides support for non-contiguous TRB Rings.
+//
+typedef struct _LINK_TRB {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1:22;
+  UINT32                    InterTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    TC:1;
+  UINT32                    RsvdZ2:2;
+  UINT32                    CH:1;
+  UINT32                    IOC:1;
+  UINT32                    RsvdZ3:4;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ4:16;
+} LINK_TRB;
+
+//
+// 6.2.2 Slot Context
+//
+typedef struct _SLOT_CONTEXT {
+  UINT32                    RouteString:20;
+  UINT32                    Speed:4;
+  UINT32                    RsvdZ1:1;
+  UINT32                    MTT:1;
+  UINT32                    Hub:1;
+  UINT32                    ContextEntries:5;
+
+  UINT32                    MaxExitLatency:16;
+  UINT32                    RootHubPortNum:8;
+  UINT32                    PortNum:8;
+
+  UINT32                    TTHubSlotId:8;
+  UINT32                    TTPortNum:8;
+  UINT32                    TTT:2;
+  UINT32                    RsvdZ2:4;
+  UINT32                    InterTarget:10;
+
+  UINT32                    DeviceAddress:8;
+  UINT32                    RsvdZ3:19;
+  UINT32                    SlotState:5;
+
+  UINT32                    RsvdZ4;
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+} SLOT_CONTEXT;
+
+typedef struct _SLOT_CONTEXT_64 {
+  UINT32                    RouteString:20;
+  UINT32                    Speed:4;
+  UINT32                    RsvdZ1:1;
+  UINT32                    MTT:1;
+  UINT32                    Hub:1;
+  UINT32                    ContextEntries:5;
+
+  UINT32                    MaxExitLatency:16;
+  UINT32                    RootHubPortNum:8;
+  UINT32                    PortNum:8;
+
+  UINT32                    TTHubSlotId:8;
+  UINT32                    TTPortNum:8;
+  UINT32                    TTT:2;
+  UINT32                    RsvdZ2:4;
+  UINT32                    InterTarget:10;
+
+  UINT32                    DeviceAddress:8;
+  UINT32                    RsvdZ3:19;
+  UINT32                    SlotState:5;
+
+  UINT32                    RsvdZ4;
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+
+  UINT32                    RsvdZ8;
+  UINT32                    RsvdZ9;
+  UINT32                    RsvdZ10;
+  UINT32                    RsvdZ11;
+
+  UINT32                    RsvdZ12;
+  UINT32                    RsvdZ13;
+  UINT32                    RsvdZ14;
+  UINT32                    RsvdZ15;
+
+} SLOT_CONTEXT_64;
+
+
+//
+// 6.2.3 Endpoint Context
+//
+typedef struct _ENDPOINT_CONTEXT {
+  UINT32                    EPState:3;
+  UINT32                    RsvdZ1:5;
+  UINT32                    Mult:2;
+  UINT32                    MaxPStreams:5;
+  UINT32                    LSA:1;
+  UINT32                    Interval:8;
+  UINT32                    RsvdZ2:8;
+
+  UINT32                    RsvdZ3:1;
+  UINT32                    CErr:2;
+  UINT32                    EPType:3;
+  UINT32                    RsvdZ4:1;
+  UINT32                    HID:1;
+  UINT32                    MaxBurstSize:8;
+  UINT32                    MaxPacketSize:16;
+
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    AverageTRBLength:16;
+  UINT32                    MaxESITPayload:16;
+
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+} ENDPOINT_CONTEXT;
+
+typedef struct _ENDPOINT_CONTEXT_64 {
+  UINT32                    EPState:3;
+  UINT32                    RsvdZ1:5;
+  UINT32                    Mult:2;
+  UINT32                    MaxPStreams:5;
+  UINT32                    LSA:1;
+  UINT32                    Interval:8;
+  UINT32                    RsvdZ2:8;
+
+  UINT32                    RsvdZ3:1;
+  UINT32                    CErr:2;
+  UINT32                    EPType:3;
+  UINT32                    RsvdZ4:1;
+  UINT32                    HID:1;
+  UINT32                    MaxBurstSize:8;
+  UINT32                    MaxPacketSize:16;
+
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    AverageTRBLength:16;
+  UINT32                    MaxESITPayload:16;
+
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+
+  UINT32                    RsvdZ8;
+  UINT32                    RsvdZ9;
+  UINT32                    RsvdZ10;
+  UINT32                    RsvdZ11;
+
+  UINT32                    RsvdZ12;
+  UINT32                    RsvdZ13;
+  UINT32                    RsvdZ14;
+  UINT32                    RsvdZ15;
+
+} ENDPOINT_CONTEXT_64;
+
+
+//
+// 6.2.5.1 Input Control Context
+//
+typedef struct _INPUT_CONTRL_CONTEXT {
+  UINT32                    Dword1;
+  UINT32                    Dword2;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+  UINT32                    RsvdZ3;
+  UINT32                    RsvdZ4;
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+} INPUT_CONTRL_CONTEXT;
+
+typedef struct _INPUT_CONTRL_CONTEXT_64 {
+  UINT32                    Dword1;
+  UINT32                    Dword2;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+  UINT32                    RsvdZ3;
+  UINT32                    RsvdZ4;
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+  UINT32                    RsvdZ8;
+  UINT32                    RsvdZ9;
+  UINT32                    RsvdZ10;
+  UINT32                    RsvdZ11;
+  UINT32                    RsvdZ12;
+  UINT32                    RsvdZ13;
+  UINT32                    RsvdZ14;
+} INPUT_CONTRL_CONTEXT_64;
+
+//
+// 6.2.1 Device Context
+//
+typedef struct _DEVICE_CONTEXT {
+  SLOT_CONTEXT              Slot;
+  ENDPOINT_CONTEXT          EP[31];
+} DEVICE_CONTEXT;
+
+typedef struct _DEVICE_CONTEXT_64 {
+  SLOT_CONTEXT_64           Slot;
+  ENDPOINT_CONTEXT_64       EP[31];
+} DEVICE_CONTEXT_64;
+
+//
+// 6.2.5 Input Context
+//
+typedef struct _INPUT_CONTEXT {
+  INPUT_CONTRL_CONTEXT      InputControlContext;
+  SLOT_CONTEXT              Slot;
+  ENDPOINT_CONTEXT          EP[31];
+} INPUT_CONTEXT;
+
+typedef struct _INPUT_CONTEXT_64 {
+  INPUT_CONTRL_CONTEXT_64   InputControlContext;
+  SLOT_CONTEXT_64           Slot;
+  ENDPOINT_CONTEXT_64       EP[31];
+} INPUT_CONTEXT_64;
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Xhc               The XHCI device.
+  @param  CmdTransfer       The executed URB is for cmd transfer or not.
+  @param  Urb               The URB to execute.
+  @param  Timeout           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+XhcPeiExecTransfer (
+  IN PEI_XHC_DEV            *Xhc,
+  IN BOOLEAN                CmdTransfer,
+  IN URB                    *Urb,
+  IN UINTN                  Timeout
+  );
+
+/**
+  Find out the actual device address according to the requested device address from UsbBus.
+
+  @param  Xhc           The XHCI device.
+  @param  BusDevAddr    The requested device address by UsbBus upper driver.
+
+  @return The actual device address assigned to the device.
+
+**/
+UINT8
+XhcPeiBusDevAddrToSlotId (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              BusDevAddr
+  );
+
+/**
+  Find out the slot id according to the device's route string.
+
+  @param  Xhc           The XHCI device.
+  @param  RouteString   The route string described the device location.
+
+  @return The slot id used by the device.
+
+**/
+UINT8
+XhcPeiRouteStringToSlotId (
+  IN PEI_XHC_DEV        *Xhc,
+  IN USB_DEV_ROUTE      RouteString
+  );
+
+/**
+  Calculate the device context index by endpoint address and direction.
+
+  @param  EpAddr        The target endpoint number.
+  @param  Direction     The direction of the target endpoint.
+
+  @return The device context index of endpoint.
+
+**/
+UINT8
+XhcPeiEndpointToDci (
+  IN UINT8                      EpAddr,
+  IN EFI_USB_DATA_DIRECTION     Direction
+  );
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+**/
+VOID
+XhcPeiRingDoorBell (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  );
+
+/**
+  Monitor the port status change. Enable/Disable device slot if there is a device attached/detached.
+
+  @param  Xhc               The XHCI device.
+  @param  ParentRouteChart  The route string pointed to the parent device if it exists.
+  @param  Port              The port to be polled.
+  @param  PortState         The port state.
+
+  @retval EFI_SUCCESS       Successfully enable/disable device slot according to port state.
+  @retval Others            Should not appear.
+
+**/
+EFI_STATUS
+XhcPeiPollPortStatusChange (
+  IN PEI_XHC_DEV            *Xhc,
+  IN USB_DEV_ROUTE          ParentRouteChart,
+  IN UINT8                  Port,
+  IN EFI_USB_PORT_STATUS    *PortState
+  );
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcPeiConfigHubContext (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      PortNum,
+  IN UINT8                      TTT,
+  IN UINT8                      MTT
+  );
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcPeiConfigHubContext64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      PortNum,
+  IN UINT8                      TTT,
+  IN UINT8                      MTT
+  );
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+XhcPeiSetConfigCmd (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR      *ConfigDesc
+  );
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+XhcPeiSetConfigCmd64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR      *ConfigDesc
+  );
+
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Stop endpoint successfully.
+  @retval Others        Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiStopEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  );
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Reset endpoint successfully.
+  @retval Others        Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiResetEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  );
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+  @param  Urb           The dequeue pointer of the transfer ring specified
+                        by the urb to be updated.
+
+  @retval EFI_SUCCESS   Set transfer ring dequeue pointer succeeds.
+  @retval Others        Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiSetTrDequeuePointer (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci,
+  IN URB                *Urb
+  );
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                   The XHCI device.
+  @param  ParentRouteChart      The route string pointed to the parent device.
+  @param  ParentPort            The port at which the device is located.
+  @param  RouteChart            The route string pointed to the device.
+  @param  DeviceSpeed           The device speed.
+
+  @retval EFI_SUCCESS           Successfully assign a slot to the device and assign an address to it.
+  @retval Others                Fail to initialize device slot.
+
+**/
+EFI_STATUS
+XhcPeiInitializeDeviceSlot (
+  IN PEI_XHC_DEV                *Xhc,
+  IN USB_DEV_ROUTE              ParentRouteChart,
+  IN UINT16                     ParentPort,
+  IN USB_DEV_ROUTE              RouteChart,
+  IN UINT8                      DeviceSpeed
+  );
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                   The XHCI device.
+  @param  ParentRouteChart      The route string pointed to the parent device.
+  @param  ParentPort            The port at which the device is located.
+  @param  RouteChart            The route string pointed to the device.
+  @param  DeviceSpeed           The device speed.
+
+  @retval EFI_SUCCESS           Successfully assign a slot to the device and assign an address to it.
+  @retval Others                Fail to initialize device slot.
+
+**/
+EFI_STATUS
+XhcPeiInitializeDeviceSlot64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN USB_DEV_ROUTE              ParentRouteChart,
+  IN UINT16                     ParentPort,
+  IN USB_DEV_ROUTE              RouteChart,
+  IN UINT8                      DeviceSpeed
+  );
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+XhcPeiEvaluateContext (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT32                     MaxPacketSize
+  );
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+XhcPeiEvaluateContext64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT32                     MaxPacketSize
+  );
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+XhcPeiDisableSlotCmd (
+  IN PEI_XHC_DEV              *Xhc,
+  IN UINT8                    SlotId
+  );
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+XhcPeiDisableSlotCmd64 (
+  IN PEI_XHC_DEV              *Xhc,
+  IN UINT8                    SlotId
+  );
+
+/**
+  System software shall use a Reset Endpoint Command (section 4.11.4.7) to remove the Halted
+  condition in the xHC. After the successful completion of the Reset Endpoint Command, the Endpoint
+  Context is transitioned from the Halted to the Stopped state and the Transfer Ring of the endpoint is
+  reenabled. The next write to the Doorbell of the Endpoint will transition the Endpoint Context from the
+  Stopped to the Running state.
+
+  @param  Xhc           The XHCI device.
+  @param  Urb           The urb which makes the endpoint halted.
+
+  @retval EFI_SUCCESS   The recovery is successful.
+  @retval Others        Failed to recovery halted endpoint.
+
+**/
+EFI_STATUS
+XhcPeiRecoverHaltedEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  );
+
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+XhcPeiDequeueTrbFromEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  );
+
+/**
+  Create a new URB for a new transaction.
+
+  @param  Xhc       The XHCI device
+  @param  DevAddr   The device address
+  @param  EpAddr    Endpoint addrress
+  @param  DevSpeed  The device speed
+  @param  MaxPacket The max packet length of the endpoint
+  @param  Type      The transaction type
+  @param  Request   The standard USB request for control transfer
+  @param  Data      The user data to transfer
+  @param  DataLen   The length of data buffer
+  @param  Callback  The function to call when data is transferred
+  @param  Context   The context to the callback
+
+  @return Created URB or NULL
+
+**/
+URB*
+XhcPeiCreateUrb (
+  IN PEI_XHC_DEV                        *Xhc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINTN                              MaxPacket,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context
+  );
+
+/**
+  Free an allocated URB.
+
+  @param  Xhc       The XHCI device.
+  @param  Urb       The URB to free.
+
+**/
+VOID
+XhcPeiFreeUrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN URB            *Urb
+  );
+
+/**
+  Create a transfer TRB.
+
+  @param  Xhc       The XHCI device
+  @param  Urb       The urb used to construct the transfer TRB.
+
+  @return Created TRB or NULL
+
+**/
+EFI_STATUS
+XhcPeiCreateTransferTrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN URB            *Urb
+  );
+
+/**
+  Synchronize the specified transfer ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc       The XHCI device.
+  @param  TrsRing   The transfer ring to sync.
+
+  @retval EFI_SUCCESS The transfer ring is synchronized successfully.
+
+**/
+EFI_STATUS
+XhcPeiSyncTrsRing (
+  IN PEI_XHC_DEV    *Xhc,
+  IN TRANSFER_RING  *TrsRing
+  );
+
+/**
+  Create XHCI transfer ring.
+
+  @param  Xhc               The XHCI Device.
+  @param  TrbNum            The number of TRB in the ring.
+  @param  TransferRing      The created transfer ring.
+
+**/
+VOID
+XhcPeiCreateTransferRing (
+  IN PEI_XHC_DEV            *Xhc,
+  IN UINTN                  TrbNum,
+  OUT TRANSFER_RING         *TransferRing
+  );
+
+/**
+  Check if there is a new generated event.
+
+  @param  Xhc           The XHCI device.
+  @param  EvtRing       The event ring to check.
+  @param  NewEvtTrb     The new event TRB found.
+
+  @retval EFI_SUCCESS   Found a new event TRB at the event ring.
+  @retval EFI_NOT_READY The event ring has no new event.
+
+**/
+EFI_STATUS
+XhcPeiCheckNewEvent (
+  IN PEI_XHC_DEV        *Xhc,
+  IN EVENT_RING         *EvtRing,
+  OUT TRB_TEMPLATE      **NewEvtTrb
+  );
+
+/**
+  Synchronize the specified event ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc       The XHCI device.
+  @param  EvtRing   The event ring to sync.
+
+  @retval EFI_SUCCESS The event ring is synchronized successfully.
+
+**/
+EFI_STATUS
+XhcPeiSyncEventRing (
+  IN PEI_XHC_DEV    *Xhc,
+  IN EVENT_RING     *EvtRing
+  );
+
+/**
+  Create XHCI event ring.
+
+  @param  Xhc           The XHCI device.
+  @param  EventRing     The created event ring.
+
+**/
+VOID
+XhcPeiCreateEventRing (
+  IN PEI_XHC_DEV        *Xhc,
+  OUT EVENT_RING        *EventRing
+  );
+
+/**
+  Initialize the XHCI host controller for schedule.
+
+  @param  Xhc       The XHCI device to be initialized.
+
+**/
+VOID
+XhcPeiInitSched (
+  IN PEI_XHC_DEV        *Xhc
+  );
+
+/**
+  Free the resouce allocated at initializing schedule.
+
+  @param  Xhc       The XHCI device.
+
+**/
+VOID
+XhcPeiFreeSched (
+  IN PEI_XHC_DEV    *Xhc
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ComponentName.c
new file mode 100644
index 0000000000..e8ba85a9e7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ComponentName.c
@@ -0,0 +1,177 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for SCSI bus driver.
+
+Copyright (c) 2006 - 2011, 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 "ScsiBus.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gScsiBusComponentName = {
+  ScsiBusComponentNameGetDriverName,
+  ScsiBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gScsiBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ScsiBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ScsiBusComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mScsiBusDriverNameTable[] = {
+  { "eng;en", (CHAR16 *) L"SCSI Bus Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mScsiBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gScsiBusComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.c b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.c
new file mode 100644
index 0000000000..0802b61726
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.c
@@ -0,0 +1,1512 @@
+/** @file
+  SCSI Bus driver that layers on every SCSI Pass Thru and
+  Extended SCSI Pass Thru protocol in the system.
+
+Copyright (c) 2006 - 2015, 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 "ScsiBus.h"
+
+
+EFI_DRIVER_BINDING_PROTOCOL gSCSIBusDriverBinding = {
+  SCSIBusDriverBindingSupported,
+  SCSIBusDriverBindingStart,
+  SCSIBusDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+VOID  *mWorkingBuffer;
+
+/**
+  Convert EFI_SCSI_IO_SCSI_REQUEST_PACKET packet to EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET packet.
+
+  @param  Packet         The pointer of EFI_SCSI_IO_SCSI_REQUEST_PACKET
+  @param  CommandPacket  The pointer of EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiioToPassThruPacket (
+  IN      EFI_SCSI_IO_SCSI_REQUEST_PACKET         *Packet,
+  OUT     EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *CommandPacket
+  );
+
+/**
+  Convert EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET packet to EFI_SCSI_IO_SCSI_REQUEST_PACKET packet.
+
+  @param  ScsiPacket  The pointer of EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET
+  @param  Packet      The pointer of EFI_SCSI_IO_SCSI_REQUEST_PACKET
+
+**/
+EFI_STATUS
+EFIAPI
+PassThruToScsiioPacket (
+  IN     EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *ScsiPacket,
+  OUT    EFI_SCSI_IO_SCSI_REQUEST_PACKET         *Packet
+  );
+
+/**
+  Notify Function in which convert EFI1.0 PassThru Packet back to UEF2.0
+  SCSI IO Packet.
+
+  @param  Event    The instance of EFI_EVENT.
+  @param  Context  The parameter passed in.
+
+**/
+VOID
+EFIAPI
+NotifyFunction (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  );
+
+/**
+  Allocates an aligned buffer for SCSI device.
+
+  This function allocates an aligned buffer for the SCSI device to perform
+  SCSI pass through operations. The alignment requirement is from SCSI pass
+  through interface.
+
+  @param  ScsiIoDevice      The SCSI child device involved for the operation.
+  @param  BufferSize        The request buffer size.
+
+  @return A pointer to the aligned buffer or NULL if the allocation fails.
+
+**/
+VOID *
+AllocateAlignedBuffer (
+  IN SCSI_IO_DEV              *ScsiIoDevice,
+  IN UINTN                    BufferSize
+  )
+{
+  return AllocateAlignedPages (EFI_SIZE_TO_PAGES (BufferSize), ScsiIoDevice->ScsiIo.IoAlign);
+}
+
+/**
+  Frees an aligned buffer for SCSI device.
+
+  This function frees an aligned buffer for the SCSI device to perform
+  SCSI pass through operations.
+
+  @param  Buffer            The aligned buffer to be freed.
+  @param  BufferSize        The request buffer size.
+
+**/
+VOID
+FreeAlignedBuffer (
+  IN VOID                     *Buffer,
+  IN UINTN                    BufferSize
+  )
+{
+  if (Buffer != NULL) {
+    FreeAlignedPages (Buffer, EFI_SIZE_TO_PAGES (BufferSize));
+  }
+}
+
+/**
+  The user Entry Point for module ScsiBus. The user code starts with this function.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+  @param  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval other          Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeScsiBus(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gSCSIBusDriverBinding,
+             ImageHandle,
+             &gScsiBusComponentName,
+             &gScsiBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Supported() it must also follow these calling
+  restrictions.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_SCSI_PASS_THRU_PROTOCOL     *PassThru;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL *ExtPassThru;
+  UINT64                          Lun;
+  UINT8                           *TargetId;
+  SCSI_TARGET_ID                  ScsiTargetId;
+
+  TargetId = &ScsiTargetId.ScsiId.ExtScsi[0];
+  SetMem (TargetId, TARGET_MAX_BYTES, 0xFF);
+
+  //
+  // To keep backward compatibility, UEFI ExtPassThru Protocol is supported as well as 
+  // EFI PassThru Protocol. From priority perspective, ExtPassThru Protocol is firstly
+  // tried to open on host controller handle. If fails, then PassThru Protocol is tried instead.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  (VOID **)&ExtPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  } else if (!EFI_ERROR(Status)) {
+    //
+    // Check if RemainingDevicePath is NULL or the End of Device Path Node,
+    // if yes, return EFI_SUCCESS.
+    //
+    if ((RemainingDevicePath == NULL) || IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // Close protocol regardless of RemainingDevicePath validation
+      //
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiExtScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );      
+      return EFI_SUCCESS;
+    } else {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node, check its validation
+      //
+      Status = ExtPassThru->GetTargetLun (ExtPassThru, RemainingDevicePath, &TargetId, &Lun);
+      //
+      // Close protocol regardless of RemainingDevicePath validation
+      //
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiExtScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );      
+      if (!EFI_ERROR(Status)) {
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  //
+  // Come here in 2 condition: 
+  // 1. ExtPassThru doesn't exist.
+  // 2. ExtPassThru exists but RemainingDevicePath is invalid.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiScsiPassThruProtocolGuid,
+                  (VOID **)&PassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+  
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // Test RemainingDevicePath is valid or not.
+  //
+  if ((RemainingDevicePath != NULL) && !IsDevicePathEnd (RemainingDevicePath)) {
+    Status = PassThru->GetTargetLun (PassThru, RemainingDevicePath, &ScsiTargetId.ScsiId.Scsi, &Lun);
+  }
+  
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiScsiPassThruProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Start() it must also follow these calling
+  restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  UINT64                                Lun;
+  UINT8                                 *TargetId;
+  BOOLEAN                               ScanOtherPuns;
+  BOOLEAN                               FromFirstTarget;
+  BOOLEAN                               ExtScsiSupport;
+  EFI_STATUS                            Status;
+  EFI_STATUS                            DevicePathStatus;
+  EFI_STATUS                            PassThruStatus;
+  SCSI_BUS_DEVICE                       *ScsiBusDev;
+  SCSI_TARGET_ID                        ScsiTargetId;
+  EFI_DEVICE_PATH_PROTOCOL              *ParentDevicePath;
+  EFI_SCSI_PASS_THRU_PROTOCOL           *ScsiInterface;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL       *ExtScsiInterface;
+  EFI_SCSI_BUS_PROTOCOL                 *BusIdentify;
+
+  TargetId        = NULL;
+  ScanOtherPuns   = TRUE;
+  FromFirstTarget = FALSE;
+  ExtScsiSupport  = FALSE;
+  PassThruStatus  = EFI_SUCCESS;
+  
+  TargetId = &ScsiTargetId.ScsiId.ExtScsi[0];
+  SetMem (TargetId, TARGET_MAX_BYTES, 0xFF);
+  
+  DevicePathStatus = gBS->OpenProtocol (
+                            Controller,
+                            &gEfiDevicePathProtocolGuid,
+                            (VOID **) &ParentDevicePath,
+                            This->DriverBindingHandle,
+                            Controller,
+                            EFI_OPEN_PROTOCOL_BY_DRIVER
+                            );
+  if (EFI_ERROR (DevicePathStatus) && (DevicePathStatus != EFI_ALREADY_STARTED)) {
+    return DevicePathStatus;
+  }
+
+  //
+  // Report Status Code to indicate SCSI bus starts
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_SCSI | EFI_IOB_PC_INIT),
+    ParentDevicePath
+    );  
+
+  //
+  // To keep backward compatibility, UEFI ExtPassThru Protocol is supported as well as 
+  // EFI PassThru Protocol. From priority perspective, ExtPassThru Protocol is firstly
+  // tried to open on host controller handle. If fails, then PassThru Protocol is tried instead.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  (VOID **) &ExtScsiInterface,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  //
+  // Fail to open UEFI ExtendPassThru Protocol, then try to open EFI PassThru Protocol instead.
+  //
+  if (EFI_ERROR(Status) && (Status != EFI_ALREADY_STARTED)) {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiScsiPassThruProtocolGuid,
+                    (VOID **) &ScsiInterface,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+    //
+    // Fail to open EFI PassThru Protocol, Close the DevicePathProtocol if it is opened by this time.
+    //
+    if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+      if (!EFI_ERROR(DevicePathStatus)) {
+        gBS->CloseProtocol (
+               Controller,
+               &gEfiDevicePathProtocolGuid,
+               This->DriverBindingHandle,
+               Controller
+               );
+      } 
+      return Status;
+    } 
+  } else {
+    //
+    // Succeed to open ExtPassThru Protocol, and meanwhile open PassThru Protocol 
+    // with BY_DRIVER if it is also present on the handle. The intent is to prevent 
+    // another SCSI Bus Driver to work on the same host handle.
+    //
+    ExtScsiSupport = TRUE;
+    PassThruStatus = gBS->OpenProtocol (
+                            Controller,
+                            &gEfiScsiPassThruProtocolGuid,
+                            (VOID **) &ScsiInterface,
+                            This->DriverBindingHandle,
+                            Controller,
+                            EFI_OPEN_PROTOCOL_BY_DRIVER
+                            );
+  }
+    
+  if (Status != EFI_ALREADY_STARTED) {
+    //
+    // Go through here means either ExtPassThru or PassThru Protocol is successfully opened
+    // on this handle for this time. Then construct Host controller private data.
+    //
+    ScsiBusDev = NULL;
+    ScsiBusDev = AllocateZeroPool(sizeof(SCSI_BUS_DEVICE));
+    if (ScsiBusDev == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ErrorExit;
+    }
+    ScsiBusDev->Signature        = SCSI_BUS_DEVICE_SIGNATURE;
+    ScsiBusDev->ExtScsiSupport   = ExtScsiSupport;
+    ScsiBusDev->DevicePath       = ParentDevicePath;
+    if (ScsiBusDev->ExtScsiSupport) {
+      ScsiBusDev->ExtScsiInterface = ExtScsiInterface;
+    } else {
+      ScsiBusDev->ScsiInterface    = ScsiInterface;    
+    }
+
+    //
+    // Install EFI_SCSI_BUS_PROTOCOL to the controller handle, So ScsiBusDev could be
+    // retrieved on this controller handle. With ScsiBusDev, we can know which PassThru
+    // Protocol is present on the handle, UEFI ExtPassThru Protocol or EFI PassThru Protocol.
+    // 
+    Status = gBS->InstallProtocolInterface (
+                    &Controller,
+                    &gEfiCallerIdGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &ScsiBusDev->BusIdentify
+                    );
+    if (EFI_ERROR (Status)) {
+      goto ErrorExit;
+    }
+  } else {
+    //
+    // Go through here means Start() is re-invoked again, nothing special is required to do except
+    // picking up Host controller private information.
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &BusIdentify,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    ScsiBusDev = SCSI_BUS_CONTROLLER_DEVICE_FROM_THIS (BusIdentify);
+  }
+
+  //
+  // Report Status Code to indicate detecting devices on bus
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_SCSI | EFI_IOB_PC_DETECT),
+    ParentDevicePath
+    );
+
+  Lun  = 0;
+  if (RemainingDevicePath == NULL) {
+    //
+    // If RemainingDevicePath is NULL, 
+    // must enumerate all SCSI devices anyway
+    //
+    FromFirstTarget = TRUE;
+  } else if (!IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // If RemainingDevicePath isn't the End of Device Path Node, 
+    // only scan the specified device by RemainingDevicePath
+    //
+    if (ScsiBusDev->ExtScsiSupport) {
+      Status = ScsiBusDev->ExtScsiInterface->GetTargetLun (ScsiBusDev->ExtScsiInterface, RemainingDevicePath, &TargetId, &Lun);  
+    } else {
+      Status = ScsiBusDev->ScsiInterface->GetTargetLun (ScsiBusDev->ScsiInterface, RemainingDevicePath, &ScsiTargetId.ScsiId.Scsi, &Lun);
+    }
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  } else {
+    //
+    // If RemainingDevicePath is the End of Device Path Node,
+    // skip enumerate any device and return EFI_SUCESSS
+    // 
+    ScanOtherPuns = FALSE;
+  }
+
+  while(ScanOtherPuns) {
+    if (FromFirstTarget) {
+      //
+      // Remaining Device Path is NULL, scan all the possible Puns in the
+      // SCSI Channel.
+      //
+      if (ScsiBusDev->ExtScsiSupport) {
+        Status = ScsiBusDev->ExtScsiInterface->GetNextTargetLun (ScsiBusDev->ExtScsiInterface, &TargetId, &Lun);
+      } else {
+        Status = ScsiBusDev->ScsiInterface->GetNextDevice (ScsiBusDev->ScsiInterface, &ScsiTargetId.ScsiId.Scsi, &Lun);
+      }
+      if (EFI_ERROR (Status)) {
+        //
+        // no legal Pun and Lun found any more
+        //
+        break;
+      }
+    } else {
+      ScanOtherPuns = FALSE;
+    }
+    //
+    // Avoid creating handle for the host adapter.
+    //
+    if (ScsiBusDev->ExtScsiSupport) {
+      if ((ScsiTargetId.ScsiId.Scsi) == ScsiBusDev->ExtScsiInterface->Mode->AdapterId) {
+        continue;
+      }
+    } else {
+      if ((ScsiTargetId.ScsiId.Scsi) == ScsiBusDev->ScsiInterface->Mode->AdapterId) {
+        continue;
+      }
+    }
+    //
+    // Scan for the scsi device, if it attaches to the scsi bus,
+    // then create handle and install scsi i/o protocol.
+    //
+    Status = ScsiScanCreateDevice (This, Controller, &ScsiTargetId, Lun, ScsiBusDev);
+  }
+  return EFI_SUCCESS;
+
+ErrorExit:
+  
+  if (ScsiBusDev != NULL) {
+    FreePool (ScsiBusDev);
+  }
+  
+  if (ExtScsiSupport) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiExtScsiPassThruProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    if (!EFI_ERROR (PassThruStatus)) {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+    }
+  } else {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiScsiPassThruProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+  return Status;
+}
+
+/**
+  Stop this driver on ControllerHandle.
+
+  This service is called by the EFI boot service DisconnectController().
+  In order to make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController() must follow these
+  calling restrictions. If any other agent wishes to call Stop() it must also
+  follow these calling restrictions.
+  
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  BOOLEAN                     AllChildrenStopped;
+  UINTN                       Index;
+  EFI_SCSI_IO_PROTOCOL        *ScsiIo;
+  SCSI_IO_DEV                 *ScsiIoDevice;
+  VOID                        *ScsiPassThru;
+  EFI_SCSI_BUS_PROTOCOL       *Scsidentifier;
+  SCSI_BUS_DEVICE             *ScsiBusDev;
+
+  if (NumberOfChildren == 0) {
+    //
+    // Get the SCSI_BUS_DEVICE
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &Scsidentifier,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    ScsiBusDev = SCSI_BUS_CONTROLLER_DEVICE_FROM_THIS (Scsidentifier);
+
+    //
+    // Uninstall SCSI Bus Protocol
+    //
+    gBS->UninstallProtocolInterface (
+           Controller,
+           &gEfiCallerIdGuid,
+           &ScsiBusDev->BusIdentify
+           );
+
+    //
+    // Close the bus driver
+    //
+    if (ScsiBusDev->ExtScsiSupport) {
+      //
+      // Close ExtPassThru Protocol from this controller handle
+      //
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiExtScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      //
+      // When Start() succeeds to open ExtPassThru, it always tries to open PassThru BY_DRIVER.
+      // Its intent is to prevent another SCSI Bus Driver from woking on the same host handle. 
+      // So Stop() needs to try to close PassThru if present here.
+      //
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+    } else {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+    }
+
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiDevicePathProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    FreePool (ScsiBusDev);
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = gBS->OpenProtocol (
+                    ChildHandleBuffer[Index],
+                    &gEfiScsiIoProtocolGuid,
+                    (VOID **) &ScsiIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+      continue;
+    }
+
+    ScsiIoDevice = SCSI_IO_DEV_FROM_THIS (ScsiIo);
+    //
+    // Close the child handle
+    //
+    if (ScsiIoDevice->ExtScsiSupport) {
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      &gEfiExtScsiPassThruProtocolGuid,
+                      This->DriverBindingHandle,
+                      ChildHandleBuffer[Index]
+                      );
+
+    } else {
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      &gEfiScsiPassThruProtocolGuid,
+                      This->DriverBindingHandle,
+                      ChildHandleBuffer[Index]
+                      );
+    }
+
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    ChildHandleBuffer[Index],
+                    &gEfiDevicePathProtocolGuid,
+                    ScsiIoDevice->DevicePath,
+                    &gEfiScsiIoProtocolGuid,
+                    &ScsiIoDevice->ScsiIo,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+      if (ScsiIoDevice->ExtScsiSupport) {
+        gBS->OpenProtocol (
+               Controller,
+               &gEfiExtScsiPassThruProtocolGuid,
+               &ScsiPassThru,
+               This->DriverBindingHandle,
+               ChildHandleBuffer[Index],
+               EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+               );
+      } else {
+        gBS->OpenProtocol (
+               Controller,
+               &gEfiScsiPassThruProtocolGuid,
+               &ScsiPassThru,
+               This->DriverBindingHandle,
+               ChildHandleBuffer[Index],
+               EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+               );
+      }
+    } else {
+      FreePool (ScsiIoDevice);
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieves the device type information of the SCSI Controller.
+
+  @param  This          Protocol instance pointer.
+  @param  DeviceType    A pointer to the device type information retrieved from
+                        the SCSI Controller. 
+
+  @retval EFI_SUCCESS             Retrieves the device type information successfully.
+  @retval EFI_INVALID_PARAMETER   The DeviceType is NULL.
+  
+**/
+EFI_STATUS
+EFIAPI
+ScsiGetDeviceType (
+  IN  EFI_SCSI_IO_PROTOCOL     *This,
+  OUT UINT8                    *DeviceType
+  )
+{
+  SCSI_IO_DEV *ScsiIoDevice;
+
+  if (DeviceType == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ScsiIoDevice  = SCSI_IO_DEV_FROM_THIS (This);
+  *DeviceType   = ScsiIoDevice->ScsiDeviceType;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieves the device location in the SCSI channel.
+
+  @param  This   Protocol instance pointer.
+  @param  Target A pointer to the Target ID of a SCSI device
+                 on the SCSI channel.
+  @param  Lun    A pointer to the LUN of the SCSI device on
+                 the SCSI channel.
+
+  @retval EFI_SUCCESS           Retrieves the device location successfully.
+  @retval EFI_INVALID_PARAMETER The Target or Lun is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiGetDeviceLocation (
+  IN  EFI_SCSI_IO_PROTOCOL    *This,
+  IN OUT UINT8                **Target,
+  OUT UINT64                  *Lun
+  )
+{
+  SCSI_IO_DEV *ScsiIoDevice;
+
+  if (Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ScsiIoDevice = SCSI_IO_DEV_FROM_THIS (This);
+
+  CopyMem (*Target,&ScsiIoDevice->Pun, TARGET_MAX_BYTES);
+
+  *Lun         = ScsiIoDevice->Lun;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets the SCSI Bus that the SCSI Controller is attached to.
+
+  @param  This  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS       The SCSI bus is reset successfully.
+  @retval  EFI_DEVICE_ERROR  Errors encountered when resetting the SCSI bus.
+  @retval  EFI_UNSUPPORTED   The bus reset operation is not supported by the
+                             SCSI Host Controller.
+  @retval  EFI_TIMEOUT       A timeout occurred while attempting to reset 
+                             the SCSI bus.
+**/
+EFI_STATUS
+EFIAPI
+ScsiResetBus (
+  IN  EFI_SCSI_IO_PROTOCOL     *This
+  )
+{
+  SCSI_IO_DEV *ScsiIoDevice;
+
+  ScsiIoDevice = SCSI_IO_DEV_FROM_THIS (This);
+
+  //
+  // Report Status Code to indicate reset happens
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_RESET),
+    ScsiIoDevice->ScsiBusDeviceData->DevicePath
+    );
+
+  if (ScsiIoDevice->ExtScsiSupport){
+    return ScsiIoDevice->ExtScsiPassThru->ResetChannel (ScsiIoDevice->ExtScsiPassThru);
+  } else {
+    return ScsiIoDevice->ScsiPassThru->ResetChannel (ScsiIoDevice->ScsiPassThru);
+  }
+}
+
+
+/**
+  Resets the SCSI Controller that the device handle specifies.
+
+  @param  This  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS       Reset the SCSI controller successfully.
+  @retval  EFI_DEVICE_ERROR  Errors are encountered when resetting the SCSI Controller.
+  @retval  EFI_UNSUPPORTED   The SCSI bus does not support a device reset operation.
+  @retval  EFI_TIMEOUT       A timeout occurred while attempting to reset the
+                             SCSI Controller.
+**/
+EFI_STATUS
+EFIAPI
+ScsiResetDevice (
+  IN  EFI_SCSI_IO_PROTOCOL     *This
+  )
+{
+  SCSI_IO_DEV  *ScsiIoDevice;
+  UINT8        Target[TARGET_MAX_BYTES];
+
+  ScsiIoDevice = SCSI_IO_DEV_FROM_THIS (This);
+
+  //
+  // Report Status Code to indicate reset happens
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_RESET),
+    ScsiIoDevice->ScsiBusDeviceData->DevicePath
+    );
+  
+  CopyMem (Target,&ScsiIoDevice->Pun, TARGET_MAX_BYTES);
+
+
+  if (ScsiIoDevice->ExtScsiSupport) {
+    return ScsiIoDevice->ExtScsiPassThru->ResetTargetLun (
+                                        ScsiIoDevice->ExtScsiPassThru,
+                                        Target,
+                                        ScsiIoDevice->Lun
+                                          );
+  } else {
+    return ScsiIoDevice->ScsiPassThru->ResetTarget (
+                                          ScsiIoDevice->ScsiPassThru,
+                                          ScsiIoDevice->Pun.ScsiId.Scsi,
+                                          ScsiIoDevice->Lun
+                                            );
+  }
+}
+
+
+/**
+  Sends a SCSI Request Packet to the SCSI Controller for execution.
+
+  @param  This            Protocol instance pointer.
+  @param  CommandPacket   The SCSI request packet to send to the SCSI 
+                          Controller specified by the device handle.
+  @param  Event           If the SCSI bus where the SCSI device is attached
+                          does not support non-blocking I/O, then Event is 
+                          ignored, and blocking I/O is performed.  
+                          If Event is NULL, then blocking I/O is performed.
+                          If Event is not NULL and non-blocking I/O is 
+                          supported, then non-blocking I/O is performed,
+                          and Event will be signaled when the SCSI Request
+                          Packet completes.
+
+  @retval EFI_SUCCESS         The SCSI Request Packet was sent by the host 
+                              successfully, and TransferLength bytes were 
+                              transferred to/from DataBuffer.See 
+                              HostAdapterStatus, TargetStatus, 
+                              SenseDataLength, and SenseData in that order
+                              for additional status information.
+  @retval EFI_BAD_BUFFER_SIZE The SCSI Request Packet was executed, 
+                              but the entire DataBuffer could not be transferred.
+                              The actual number of bytes transferred is returned
+                              in TransferLength. See HostAdapterStatus, 
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+  @retval EFI_NOT_READY       The SCSI Request Packet could not be sent because 
+                              there are too many SCSI Command Packets already 
+                              queued.The caller may retry again later.
+  @retval EFI_DEVICE_ERROR    A device error occurred while attempting to send 
+                              the SCSI Request Packet. See HostAdapterStatus, 
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+  @retval EFI_INVALID_PARAMETER  The contents of CommandPacket are invalid.  
+                                 The SCSI Request Packet was not sent, so no 
+                                 additional status information is available.
+  @retval EFI_UNSUPPORTED     The command described by the SCSI Request Packet
+                              is not supported by the SCSI initiator(i.e., SCSI 
+                              Host Controller). The SCSI Request Packet was not
+                              sent, so no additional status information is 
+                              available.
+  @retval EFI_TIMEOUT         A timeout occurred while waiting for the SCSI 
+                              Request Packet to execute. See HostAdapterStatus,
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+**/
+EFI_STATUS
+EFIAPI
+ScsiExecuteSCSICommand (
+  IN     EFI_SCSI_IO_PROTOCOL                     *This,
+  IN OUT EFI_SCSI_IO_SCSI_REQUEST_PACKET          *Packet,
+  IN     EFI_EVENT                                Event  OPTIONAL
+  )
+{
+  SCSI_IO_DEV                                 *ScsiIoDevice;
+  EFI_STATUS                                  Status;
+  UINT8                                       Target[TARGET_MAX_BYTES];
+  EFI_EVENT                                   PacketEvent;
+  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *ExtRequestPacket;
+  SCSI_EVENT_DATA                             EventData;                                     
+
+  PacketEvent = NULL;
+  
+  if (Packet == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ScsiIoDevice  = SCSI_IO_DEV_FROM_THIS (This);
+  CopyMem (Target,&ScsiIoDevice->Pun, TARGET_MAX_BYTES);
+
+  if (ScsiIoDevice->ExtScsiSupport) {
+    ExtRequestPacket = (EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET *) Packet;
+
+    if (((ScsiIoDevice->ExtScsiPassThru->Mode->Attributes & EFI_SCSI_PASS_THRU_ATTRIBUTES_NONBLOCKIO) != 0) && (Event !=  NULL)) {
+      Status = ScsiIoDevice->ExtScsiPassThru->PassThru (
+                                                ScsiIoDevice->ExtScsiPassThru,
+                                                Target,
+                                                ScsiIoDevice->Lun,
+                                                ExtRequestPacket,
+                                                Event
+                                                );
+    } else {
+      //
+      // If there's no event or the SCSI Device doesn't support NON-BLOCKING,
+      // let the 'Event' parameter for PassThru() be NULL.
+      //
+      Status = ScsiIoDevice->ExtScsiPassThru->PassThru (
+                                                ScsiIoDevice->ExtScsiPassThru,
+                                                Target,
+                                                ScsiIoDevice->Lun,
+                                                ExtRequestPacket,
+                                                NULL
+                                                );
+      if ((!EFI_ERROR(Status)) && (Event != NULL)) {
+        //
+        // Signal Event to tell caller to pick up the SCSI IO packet if the
+        // PassThru() succeeds.
+        //
+        gBS->SignalEvent (Event);
+      }
+    }
+  } else {
+
+    mWorkingBuffer = AllocatePool (sizeof(EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET));
+
+    if (mWorkingBuffer == NULL) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    //
+    // Convert package into EFI1.0, EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET.
+    //
+    Status = ScsiioToPassThruPacket(Packet, (EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET*)mWorkingBuffer);
+    if (EFI_ERROR(Status)) {
+      FreePool(mWorkingBuffer);
+      return Status;
+    }
+
+    if (((ScsiIoDevice->ScsiPassThru->Mode->Attributes & EFI_SCSI_PASS_THRU_ATTRIBUTES_NONBLOCKIO) != 0) && (Event !=  NULL)) {
+      EventData.Data1 = (VOID*)Packet;
+      EventData.Data2 = Event;
+      //
+      // Create Event
+      //
+      Status = gBS->CreateEvent (
+                       EVT_NOTIFY_SIGNAL,
+                       TPL_NOTIFY,
+                       NotifyFunction,
+                       &EventData,
+                       &PacketEvent
+                       );
+      if (EFI_ERROR(Status)) {
+        FreePool(mWorkingBuffer);
+        return Status;
+      }
+
+      Status = ScsiIoDevice->ScsiPassThru->PassThru (
+                                          ScsiIoDevice->ScsiPassThru,
+                                          ScsiIoDevice->Pun.ScsiId.Scsi,
+                                          ScsiIoDevice->Lun,
+                                          mWorkingBuffer,
+                                          PacketEvent
+                                          );
+
+      if (EFI_ERROR(Status)) {
+        FreePool(mWorkingBuffer);
+        gBS->CloseEvent(PacketEvent);
+        return Status;
+      }
+
+    } else {
+      //
+      // If there's no event or SCSI Device doesn't support NON-BLOCKING, just convert
+      // EFI1.0 PassThru packet back to UEFI2.0 SCSI IO Packet.
+      //
+      Status = ScsiIoDevice->ScsiPassThru->PassThru (
+                                          ScsiIoDevice->ScsiPassThru,
+                                          ScsiIoDevice->Pun.ScsiId.Scsi,
+                                          ScsiIoDevice->Lun,
+                                          mWorkingBuffer,
+                                          NULL
+                                          );
+      if (EFI_ERROR(Status)) {
+        FreePool(mWorkingBuffer);
+        return Status;
+      }
+
+      PassThruToScsiioPacket((EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET*)mWorkingBuffer,Packet);
+      //
+      // After converting EFI1.0 PassThru Packet back to UEFI2.0 SCSI IO Packet,
+      // free mWorkingBuffer.
+      //
+      FreePool(mWorkingBuffer);
+
+      //
+      // Signal Event to tell caller to pick up the SCSI IO Packet.
+      //
+      if (Event != NULL) {
+        gBS->SignalEvent (Event);
+      }
+    }
+  }
+  return Status;
+}
+
+
+/**
+  Scan SCSI Bus to discover the device, and attach ScsiIoProtocol to it.
+
+  @param  This           Protocol instance pointer
+  @param  Controller     Controller handle
+  @param  TargetId       Tartget to be scanned
+  @param  Lun            The Lun of the SCSI device on the SCSI channel.
+  @param  ScsiBusDev     The pointer of SCSI_BUS_DEVICE
+
+  @retval EFI_SUCCESS           Successfully to discover the device and attach
+                                ScsiIoProtocol to it.
+  @retval EFI_OUT_OF_RESOURCES  Fail to discover the device.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiScanCreateDevice (
+  IN     EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN     EFI_HANDLE                    Controller,
+  IN     SCSI_TARGET_ID                *TargetId,
+  IN     UINT64                        Lun,
+  IN OUT SCSI_BUS_DEVICE               *ScsiBusDev
+  )
+{
+  EFI_STATUS                Status;
+  SCSI_IO_DEV               *ScsiIoDevice;
+  EFI_DEVICE_PATH_PROTOCOL  *ScsiDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath;
+  EFI_HANDLE                 DeviceHandle;
+
+  DevicePath          = NULL;
+  RemainingDevicePath = NULL;
+  ScsiDevicePath      = NULL;
+  ScsiIoDevice        = NULL;
+
+  //
+  // Build Device Path
+  //
+  if (ScsiBusDev->ExtScsiSupport){
+    Status = ScsiBusDev->ExtScsiInterface->BuildDevicePath (
+                                             ScsiBusDev->ExtScsiInterface,
+                                             &TargetId->ScsiId.ExtScsi[0],
+                                             Lun,
+                                             &ScsiDevicePath
+                                             );
+  } else {
+    Status = ScsiBusDev->ScsiInterface->BuildDevicePath (
+                                          ScsiBusDev->ScsiInterface,
+                                          TargetId->ScsiId.Scsi,
+                                          Lun,
+                                          &ScsiDevicePath
+                                          );
+  }
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DevicePath = AppendDevicePathNode (
+                 ScsiBusDev->DevicePath,
+                 ScsiDevicePath
+                 );
+
+  if (DevicePath == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  DeviceHandle = NULL;
+  RemainingDevicePath = DevicePath;
+  Status = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &RemainingDevicePath, &DeviceHandle);
+  if (!EFI_ERROR (Status) && (DeviceHandle != NULL) && IsDevicePathEnd(RemainingDevicePath)) {
+    //
+    // The device has been started, directly return to fast boot.
+    //
+    Status = EFI_ALREADY_STARTED;
+    goto ErrorExit;
+  }
+
+  ScsiIoDevice = AllocateZeroPool (sizeof (SCSI_IO_DEV));
+  if (ScsiIoDevice == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  ScsiIoDevice->Signature                 = SCSI_IO_DEV_SIGNATURE;
+  ScsiIoDevice->ScsiBusDeviceData         = ScsiBusDev;
+  CopyMem(&ScsiIoDevice->Pun, TargetId, TARGET_MAX_BYTES);
+  ScsiIoDevice->Lun                       = Lun;
+
+  if (ScsiBusDev->ExtScsiSupport) {
+    ScsiIoDevice->ExtScsiPassThru         = ScsiBusDev->ExtScsiInterface;
+    ScsiIoDevice->ExtScsiSupport          = TRUE;
+    ScsiIoDevice->ScsiIo.IoAlign          = ScsiIoDevice->ExtScsiPassThru->Mode->IoAlign;
+
+  } else {
+    ScsiIoDevice->ScsiPassThru            = ScsiBusDev->ScsiInterface;
+    ScsiIoDevice->ExtScsiSupport          = FALSE;
+    ScsiIoDevice->ScsiIo.IoAlign          = ScsiIoDevice->ScsiPassThru->Mode->IoAlign;
+  }
+
+  ScsiIoDevice->ScsiIo.GetDeviceType      = ScsiGetDeviceType;
+  ScsiIoDevice->ScsiIo.GetDeviceLocation  = ScsiGetDeviceLocation;
+  ScsiIoDevice->ScsiIo.ResetBus           = ScsiResetBus;
+  ScsiIoDevice->ScsiIo.ResetDevice        = ScsiResetDevice;
+  ScsiIoDevice->ScsiIo.ExecuteScsiCommand = ScsiExecuteSCSICommand;
+
+  //
+  // Report Status Code here since the new SCSI device will be discovered
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_SCSI | EFI_IOB_PC_ENABLE),
+    ScsiBusDev->DevicePath
+    );
+
+  if (!DiscoverScsiDevice (ScsiIoDevice)) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  ScsiIoDevice->DevicePath = DevicePath;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ScsiIoDevice->Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  ScsiIoDevice->DevicePath,
+                  &gEfiScsiIoProtocolGuid,
+                  &ScsiIoDevice->ScsiIo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  } else {
+    if (ScsiBusDev->ExtScsiSupport) {
+      gBS->OpenProtocol (
+             Controller,
+             &gEfiExtScsiPassThruProtocolGuid,
+             (VOID **) &(ScsiBusDev->ExtScsiInterface),
+             This->DriverBindingHandle,
+             ScsiIoDevice->Handle,
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+     } else {
+      gBS->OpenProtocol (
+             Controller,
+             &gEfiScsiPassThruProtocolGuid,
+             (VOID **) &(ScsiBusDev->ScsiInterface),
+             This->DriverBindingHandle,
+             ScsiIoDevice->Handle,
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+     }
+  }
+  return EFI_SUCCESS;
+
+ErrorExit:
+  
+  //
+  // The memory space for ScsiDevicePath is allocated in
+  // ScsiPassThru->BuildDevicePath() function; It is no longer used
+  // after AppendDevicePathNode,so free the memory it occupies.
+  //
+  FreePool (ScsiDevicePath);
+
+  if (DevicePath != NULL) {
+    FreePool (DevicePath);
+  }
+
+  if (ScsiIoDevice != NULL) {
+    FreePool (ScsiIoDevice);
+  }
+
+  return Status;
+}
+
+
+/**
+  Discovery SCSI Device
+
+  @param  ScsiIoDevice    The pointer of SCSI_IO_DEV
+
+  @retval  TRUE   Find SCSI Device and verify it.
+  @retval  FALSE  Unable to find SCSI Device.
+
+**/
+BOOLEAN
+DiscoverScsiDevice (
+  IN OUT  SCSI_IO_DEV   *ScsiIoDevice
+  )
+{
+  EFI_STATUS            Status;
+  UINT32                InquiryDataLength;
+  UINT8                 SenseDataLength;
+  UINT8                 HostAdapterStatus;
+  UINT8                 TargetStatus;
+  EFI_SCSI_INQUIRY_DATA *InquiryData;
+  UINT8                 MaxRetry;
+  UINT8                 Index;
+  BOOLEAN               ScsiDeviceFound;
+
+  HostAdapterStatus = 0;
+  TargetStatus      = 0;
+
+  InquiryData = AllocateAlignedBuffer (ScsiIoDevice, sizeof (EFI_SCSI_INQUIRY_DATA));
+  if (InquiryData == NULL) {
+    ScsiDeviceFound = FALSE;
+    goto Done;
+  }
+
+  //
+  // Using Inquiry command to scan for the device
+  //
+  InquiryDataLength = sizeof (EFI_SCSI_INQUIRY_DATA);
+  SenseDataLength   = 0;
+  ZeroMem (InquiryData, InquiryDataLength);
+
+  MaxRetry = 2;
+  for (Index = 0; Index < MaxRetry; Index++) {
+    Status = ScsiInquiryCommand (
+              &ScsiIoDevice->ScsiIo,
+              SCSI_BUS_TIMEOUT,
+              NULL,
+              &SenseDataLength,
+              &HostAdapterStatus,
+              &TargetStatus,
+              (VOID *) InquiryData,
+              &InquiryDataLength,
+              FALSE
+              );
+    if (!EFI_ERROR (Status)) {
+      break;
+    } else if ((Status == EFI_BAD_BUFFER_SIZE) || 
+               (Status == EFI_INVALID_PARAMETER) ||
+               (Status == EFI_UNSUPPORTED)) {
+      ScsiDeviceFound = FALSE;
+      goto Done;
+    }
+  }
+
+  if (Index == MaxRetry) {
+    ScsiDeviceFound = FALSE;
+    goto Done;
+  }
+  
+  //
+  // Retrieved inquiry data successfully
+  //
+  if ((InquiryData->Peripheral_Qualifier != 0) &&
+      (InquiryData->Peripheral_Qualifier != 3)) {
+    ScsiDeviceFound = FALSE;
+    goto Done;
+  }
+
+  if (InquiryData->Peripheral_Qualifier == 3) {
+    if (InquiryData->Peripheral_Type != 0x1f) {
+      ScsiDeviceFound = FALSE;
+      goto Done;
+    }
+  }
+
+  if (0x1e >= InquiryData->Peripheral_Type && InquiryData->Peripheral_Type >= 0xa) {
+    ScsiDeviceFound = FALSE;
+    goto Done;
+  }
+
+  //
+  // valid device type and peripheral qualifier combination.
+  //
+  ScsiIoDevice->ScsiDeviceType  = InquiryData->Peripheral_Type;
+  ScsiIoDevice->RemovableDevice = InquiryData->Rmb;
+  if (InquiryData->Version == 0) {
+    ScsiIoDevice->ScsiVersion = 0;
+  } else {
+    //
+    // ANSI-approved version
+    //
+    ScsiIoDevice->ScsiVersion = (UINT8) (InquiryData->Version & 0x07);
+  }
+
+  ScsiDeviceFound = TRUE;
+
+Done:
+  FreeAlignedBuffer (InquiryData, sizeof (EFI_SCSI_INQUIRY_DATA));
+
+  return ScsiDeviceFound;
+}
+
+
+/**
+  Convert EFI_SCSI_IO_SCSI_REQUEST_PACKET packet to EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET packet.
+
+  @param  Packet         The pointer of EFI_SCSI_IO_SCSI_REQUEST_PACKET
+  @param  CommandPacket  The pointer of EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiioToPassThruPacket (
+  IN      EFI_SCSI_IO_SCSI_REQUEST_PACKET         *Packet,
+  OUT     EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *CommandPacket
+  )
+{
+  //
+  //EFI 1.10 doesn't support Bi-Direction Command.
+  //
+  if (Packet->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_BIDIRECTIONAL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  ZeroMem (CommandPacket, sizeof (EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET));
+
+  CommandPacket->Timeout           = Packet->Timeout;
+  CommandPacket->Cdb               = Packet->Cdb;
+  CommandPacket->CdbLength         = Packet->CdbLength;
+  CommandPacket->DataDirection     = Packet->DataDirection;
+  CommandPacket->HostAdapterStatus = Packet->HostAdapterStatus;
+  CommandPacket->TargetStatus      = Packet->TargetStatus;
+  CommandPacket->SenseData         = Packet->SenseData;
+  CommandPacket->SenseDataLength   = Packet->SenseDataLength;
+
+  if (Packet->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_READ) {
+    CommandPacket->DataBuffer = Packet->InDataBuffer;
+    CommandPacket->TransferLength = Packet->InTransferLength;
+  } else if (Packet->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_WRITE) {
+    CommandPacket->DataBuffer = Packet->OutDataBuffer;
+    CommandPacket->TransferLength = Packet->OutTransferLength;
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Convert EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET packet to EFI_SCSI_IO_SCSI_REQUEST_PACKET packet.
+
+  @param  ScsiPacket  The pointer of EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET
+  @param  Packet      The pointer of EFI_SCSI_IO_SCSI_REQUEST_PACKET
+
+**/
+EFI_STATUS
+EFIAPI
+PassThruToScsiioPacket (
+  IN     EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *ScsiPacket,
+  OUT    EFI_SCSI_IO_SCSI_REQUEST_PACKET         *Packet
+  )
+{
+  Packet->Timeout           = ScsiPacket->Timeout;
+  Packet->Cdb               = ScsiPacket->Cdb;
+  Packet->CdbLength         = ScsiPacket->CdbLength;
+  Packet->DataDirection     = ScsiPacket->DataDirection;
+  Packet->HostAdapterStatus = ScsiPacket->HostAdapterStatus;
+  Packet->TargetStatus      = ScsiPacket->TargetStatus;
+  Packet->SenseData         = ScsiPacket->SenseData;
+  Packet->SenseDataLength   = ScsiPacket->SenseDataLength;
+
+  if (ScsiPacket->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_READ) {
+    Packet->InDataBuffer = ScsiPacket->DataBuffer;
+    Packet->InTransferLength = ScsiPacket->TransferLength;
+  } else if (Packet->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_WRITE) {
+    Packet->OutDataBuffer = ScsiPacket->DataBuffer;
+    Packet->OutTransferLength = ScsiPacket->TransferLength;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Notify Function in which convert EFI1.0 PassThru Packet back to UEF2.0
+  SCSI IO Packet.
+
+  @param  Event    The instance of EFI_EVENT.
+  @param  Context  The parameter passed in.
+
+**/
+VOID
+EFIAPI
+NotifyFunction (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  )
+{
+  EFI_SCSI_IO_SCSI_REQUEST_PACKET          *Packet;
+  EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET   *ScsiPacket;
+  EFI_EVENT                                CallerEvent;
+  SCSI_EVENT_DATA                          *PassData;
+
+  PassData = (SCSI_EVENT_DATA*)Context;
+  Packet  = (EFI_SCSI_IO_SCSI_REQUEST_PACKET *)PassData->Data1;
+  ScsiPacket =  (EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET*)mWorkingBuffer;
+
+  //
+  // Convert EFI1.0 PassThru packet to UEFI2.0 SCSI IO Packet.
+  //
+  PassThruToScsiioPacket(ScsiPacket, Packet);
+
+  //
+  // After converting EFI1.0 PassThru Packet back to UEFI2.0 SCSI IO Packet,
+  // free mWorkingBuffer.
+  //
+  gBS->FreePool(mWorkingBuffer);
+
+  //
+  // Signal Event to tell caller to pick up UEFI2.0 SCSI IO Packet.
+  //
+  CallerEvent = PassData->Data2;
+  gBS->CloseEvent(Event);
+  gBS->SignalEvent(CallerEvent);
+}
+
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.h b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.h
new file mode 100644
index 0000000000..babf86f2c3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.h
@@ -0,0 +1,492 @@
+/** @file
+  Header file for SCSI Bus Driver.
+
+Copyright (c) 2006 - 2013, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _SCSI_BUS_H_
+#define _SCSI_BUS_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/ScsiPassThru.h>
+#include <Protocol/ScsiPassThruExt.h>
+#include <Protocol/ScsiIo.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiScsiLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Scsi.h>
+
+#define SCSI_IO_DEV_SIGNATURE SIGNATURE_32 ('s', 'c', 'i', 'o')
+
+typedef union {
+  UINT32  Scsi;
+  UINT8   ExtScsi[4];   
+} SCSI_ID;
+
+typedef struct _SCSI_TARGET_ID {
+  SCSI_ID ScsiId;
+  UINT8   ExtScsiId[12];
+}SCSI_TARGET_ID;
+
+
+typedef struct {
+   VOID   *Data1;
+   VOID   *Data2;
+} SCSI_EVENT_DATA;
+
+//
+// SCSI Bus Controller device strcuture
+//
+#define SCSI_BUS_DEVICE_SIGNATURE  SIGNATURE_32 ('s', 'c', 's', 'i')
+
+//
+// SCSI Bus Timeout Experience Value
+//
+#define SCSI_BUS_TIMEOUT           EFI_TIMER_PERIOD_SECONDS (3)
+
+//
+// The ScsiBusProtocol is just used to locate ScsiBusDev
+// structure in the SCSIBusDriverBindingStop(). Then we can
+// Close all opened protocols and release this structure.
+// ScsiBusProtocol is the private protocol.
+// gEfiCallerIdGuid will be used as its protocol guid.
+//
+typedef struct _EFI_SCSI_BUS_PROTOCOL {
+  UINT64  Reserved;
+} EFI_SCSI_BUS_PROTOCOL;
+
+typedef struct _SCSI_BUS_DEVICE {
+  UINTN                                 Signature;
+  EFI_SCSI_BUS_PROTOCOL                 BusIdentify;
+  BOOLEAN                               ExtScsiSupport; 
+  EFI_SCSI_PASS_THRU_PROTOCOL           *ScsiInterface;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL       *ExtScsiInterface;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+} SCSI_BUS_DEVICE;
+
+#define SCSI_BUS_CONTROLLER_DEVICE_FROM_THIS(a)  CR (a, SCSI_BUS_DEVICE, BusIdentify, SCSI_BUS_DEVICE_SIGNATURE)
+
+typedef struct {
+  UINT32                             Signature;
+  EFI_HANDLE                         Handle;
+  EFI_SCSI_IO_PROTOCOL               ScsiIo;
+  EFI_DEVICE_PATH_PROTOCOL           *DevicePath;
+  BOOLEAN                            ExtScsiSupport; 
+  EFI_SCSI_PASS_THRU_PROTOCOL        *ScsiPassThru;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *ExtScsiPassThru;
+  SCSI_BUS_DEVICE                    *ScsiBusDeviceData;
+  SCSI_TARGET_ID                     Pun;
+  UINT64                             Lun;
+  UINT8                              ScsiDeviceType;
+  UINT8                              ScsiVersion;
+  BOOLEAN                            RemovableDevice;
+} SCSI_IO_DEV;
+
+#define SCSI_IO_DEV_FROM_THIS(a)  CR (a, SCSI_IO_DEV, ScsiIo, SCSI_IO_DEV_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gScsiBusDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gScsiBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gScsiBusComponentName2;
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Supported() it must also follow these calling
+  restrictions.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Start this driver on ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Start() it must also follow these calling
+  restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle.
+
+  This service is called by the EFI boot service DisconnectController().
+  In order to make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController() must follow these
+  calling restrictions. If any other agent wishes to call Stop() it must also
+  follow these calling restrictions.
+  
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Retrieves the device type information of the SCSI Controller.
+
+  @param  This          Protocol instance pointer.
+  @param  DeviceType    A pointer to the device type information retrieved from
+                        the SCSI Controller. 
+
+  @retval EFI_SUCCESS             Retrieves the device type information successfully.
+  @retval EFI_INVALID_PARAMETER   The DeviceType is NULL.
+  
+**/
+EFI_STATUS
+EFIAPI
+ScsiGetDeviceType (
+  IN  EFI_SCSI_IO_PROTOCOL     *This,
+  OUT UINT8                    *DeviceType
+  );
+
+/**
+  Retrieves the device location in the SCSI channel.
+
+  @param  This   Protocol instance pointer.
+  @param  Target A pointer to the Target ID of a SCSI device
+                 on the SCSI channel.
+  @param  Lun    A pointer to the LUN of the SCSI device on
+                 the SCSI channel.
+
+  @retval EFI_SUCCESS           Retrieves the device location successfully.
+  @retval EFI_INVALID_PARAMETER The Target or Lun is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiGetDeviceLocation (
+  IN  EFI_SCSI_IO_PROTOCOL    *This,
+  IN OUT UINT8                **Target,
+  OUT UINT64                  *Lun
+  );
+
+/**
+  Resets the SCSI Bus that the SCSI Controller is attached to.
+
+  @param  This  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS       The SCSI bus is reset successfully.
+  @retval  EFI_DEVICE_ERROR  Errors encountered when resetting the SCSI bus.
+  @retval  EFI_UNSUPPORTED   The bus reset operation is not supported by the
+                             SCSI Host Controller.
+  @retval  EFI_TIMEOUT       A timeout occurred while attempting to reset 
+                             the SCSI bus.
+**/
+EFI_STATUS
+EFIAPI
+ScsiResetBus (
+  IN  EFI_SCSI_IO_PROTOCOL     *This
+  );
+
+/**
+  Resets the SCSI Controller that the device handle specifies.
+
+  @param  This  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS       Reset the SCSI controller successfully.
+  @retval  EFI_DEVICE_ERROR  Errors are encountered when resetting the SCSI Controller.
+  @retval  EFI_UNSUPPORTED   The SCSI bus does not support a device reset operation.
+  @retval  EFI_TIMEOUT       A timeout occurred while attempting to reset the
+                             SCSI Controller.
+**/
+EFI_STATUS
+EFIAPI
+ScsiResetDevice (
+  IN  EFI_SCSI_IO_PROTOCOL     *This
+  );
+
+/**
+  Sends a SCSI Request Packet to the SCSI Controller for execution.
+
+  @param  This            Protocol instance pointer.
+  @param  CommandPacket   The SCSI request packet to send to the SCSI 
+                          Controller specified by the device handle.
+  @param  Event           If the SCSI bus where the SCSI device is attached
+                          does not support non-blocking I/O, then Event is 
+                          ignored, and blocking I/O is performed.  
+                          If Event is NULL, then blocking I/O is performed.
+                          If Event is not NULL and non-blocking I/O is 
+                          supported, then non-blocking I/O is performed,
+                          and Event will be signaled when the SCSI Request
+                          Packet completes.
+
+  @retval EFI_SUCCESS         The SCSI Request Packet was sent by the host 
+                              successfully, and TransferLength bytes were 
+                              transferred to/from DataBuffer.See 
+                              HostAdapterStatus, TargetStatus, 
+                              SenseDataLength, and SenseData in that order
+                              for additional status information.
+  @retval EFI_BAD_BUFFER_SIZE The SCSI Request Packet was executed, 
+                              but the entire DataBuffer could not be transferred.
+                              The actual number of bytes transferred is returned
+                              in TransferLength. See HostAdapterStatus, 
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+  @retval EFI_NOT_READY       The SCSI Request Packet could not be sent because 
+                              there are too many SCSI Command Packets already 
+                              queued.The caller may retry again later.
+  @retval EFI_DEVICE_ERROR    A device error occurred while attempting to send 
+                              the SCSI Request Packet. See HostAdapterStatus, 
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+  @retval EFI_INVALID_PARAMETER  The contents of CommandPacket are invalid.  
+                                 The SCSI Request Packet was not sent, so no 
+                                 additional status information is available.
+  @retval EFI_UNSUPPORTED     The command described by the SCSI Request Packet
+                              is not supported by the SCSI initiator(i.e., SCSI 
+                              Host Controller). The SCSI Request Packet was not
+                              sent, so no additional status information is 
+                              available.
+  @retval EFI_TIMEOUT         A timeout occurred while waiting for the SCSI 
+                              Request Packet to execute. See HostAdapterStatus,
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+**/
+EFI_STATUS
+EFIAPI
+ScsiExecuteSCSICommand (
+  IN  EFI_SCSI_IO_PROTOCOL                 *This,
+  IN OUT  EFI_SCSI_IO_SCSI_REQUEST_PACKET  *CommandPacket,
+  IN  EFI_EVENT                            Event  OPTIONAL
+  );
+
+/**
+  Scan SCSI Bus to discover the device, and attach ScsiIoProtocol to it.
+
+  @param  This           Protocol instance pointer
+  @param  Controller     Controller handle
+  @param  TargetId       Tartget to be scanned
+  @param  Lun            The Lun of the SCSI device on the SCSI channel.
+  @param  ScsiBusDev     The pointer of SCSI_BUS_DEVICE
+
+  @retval EFI_SUCCESS           Successfully to discover the device and attach
+                                ScsiIoProtocol to it.
+  @retval EFI_OUT_OF_RESOURCES  Fail to discover the device.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiScanCreateDevice (
+  IN     EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN     EFI_HANDLE                    Controller,
+  IN     SCSI_TARGET_ID                *TargetId,
+  IN     UINT64                        Lun,
+  IN OUT SCSI_BUS_DEVICE               *ScsiBusDev
+  );
+
+/**
+  Discovery SCSI Device
+
+  @param  ScsiIoDevice    The pointer of SCSI_IO_DEV
+
+  @retval  TRUE   Find SCSI Device and verify it.
+  @retval  FALSE  Unable to find SCSI Device.
+
+**/
+BOOLEAN
+DiscoverScsiDevice (
+  IN  OUT  SCSI_IO_DEV   *ScsiIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.uni b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.uni
new file mode 100644
index 0000000000..cce1cfdbe5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusDxe.inf b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusDxe.inf
new file mode 100644
index 0000000000..18ffd88d89
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusDxe.inf
@@ -0,0 +1,70 @@
+## @file
+#  The SCSI bus driver scans all SCSI devices and creates a device handle for each of them.
+#  Note that the driver will install the Device Path Protocol and SCSI I/O Protocol on
+#  these handles.
+#  
+#  Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution.  The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ScsiBus
+  MODULE_UNI_FILE                = ScsiBus.uni
+  FILE_GUID                      = 0167CCC4-D0F7-4f21-A3EF-9E64B7CDCE8B
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeScsiBus
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES              =  IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gSCSIBusDriverBinding                        
+#  COMPONENT_NAME                =  gScsiBusComponentName
+#  COMPONENT_NAME2               =  gScsiBusComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  ScsiBus.c
+  ScsiBus.h
+
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  DevicePathLib
+  UefiBootServicesTableLib
+  UefiScsiLib
+  BaseMemoryLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  MemoryAllocationLib
+  ReportStatusCodeLib
+
+
+[Protocols]
+  gEfiScsiIoProtocolGuid                        ## BY_START
+  ## TO_START
+  ## BY_START
+  gEfiDevicePathProtocolGuid
+  gEfiScsiPassThruProtocolGuid                  ## TO_START
+  gEfiExtScsiPassThruProtocolGuid               ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ScsiBusExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusExtra.uni b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusExtra.uni
new file mode 100644
index 0000000000..420965e83d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ComponentName.c
new file mode 100644
index 0000000000..08b71d08f3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ComponentName.c
@@ -0,0 +1,224 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for SCSI disk driver.
+
+Copyright (c) 2004 - 2015, 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 "ScsiDisk.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gScsiDiskComponentName = {
+  ScsiDiskComponentNameGetDriverName,
+  ScsiDiskComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gScsiDiskComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ScsiDiskComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ScsiDiskComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mScsiDiskDriverNameTable[] = {
+  { "eng;en", (CHAR16 *) L"Scsi Disk Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mScsiDiskDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gScsiDiskComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS            Status;
+  SCSI_DISK_DEV         *ScsiDiskDevice;
+  EFI_BLOCK_IO_PROTOCOL *BlockIo;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gScsiDiskDriverBinding.DriverBindingHandle,
+             &gEfiScsiIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlockIo,
+                  gScsiDiskDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (BlockIo);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ScsiDiskDevice->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gScsiDiskComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.c b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.c
new file mode 100644
index 0000000000..6cd9a18701
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.c
@@ -0,0 +1,4994 @@
+/** @file
+  SCSI disk driver that layers on every SCSI IO protocol in the system.
+
+Copyright (c) 2006 - 2016, 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 "ScsiDisk.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gScsiDiskDriverBinding = {
+  ScsiDiskDriverBindingSupported,
+  ScsiDiskDriverBindingStart,
+  ScsiDiskDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+EFI_DISK_INFO_PROTOCOL gScsiDiskInfoProtocolTemplate = {
+  EFI_DISK_INFO_SCSI_INTERFACE_GUID,
+  ScsiDiskInfoInquiry,
+  ScsiDiskInfoIdentify,
+  ScsiDiskInfoSenseData,
+  ScsiDiskInfoWhichIde
+};
+
+/**
+  Allocates an aligned buffer for SCSI disk.
+
+  This function allocates an aligned buffer for the SCSI disk to perform
+  SCSI IO operations. The alignment requirement is from SCSI IO interface.
+
+  @param  ScsiDiskDevice    The SCSI disk involved for the operation.
+  @param  BufferSize        The request buffer size.
+
+  @return A pointer to the aligned buffer or NULL if the allocation fails.
+
+**/
+VOID *
+AllocateAlignedBuffer (
+  IN SCSI_DISK_DEV            *ScsiDiskDevice,
+  IN UINTN                    BufferSize
+  )
+{
+  return AllocateAlignedPages (EFI_SIZE_TO_PAGES (BufferSize), ScsiDiskDevice->ScsiIo->IoAlign);
+}
+
+/**
+  Frees an aligned buffer for SCSI disk.
+
+  This function frees an aligned buffer for the SCSI disk to perform
+  SCSI IO operations.
+
+  @param  Buffer            The aligned buffer to be freed.
+  @param  BufferSize        The request buffer size.
+
+**/
+VOID
+FreeAlignedBuffer (
+  IN VOID                     *Buffer,
+  IN UINTN                    BufferSize
+  )
+{
+  if (Buffer != NULL) {
+    FreeAlignedPages (Buffer, EFI_SIZE_TO_PAGES (BufferSize));
+  }
+}
+
+/**
+  The user Entry Point for module ScsiDisk.
+
+  The user code starts with this function.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.  
+  @param  SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeScsiDisk(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gScsiDiskDriverBinding,
+             ImageHandle,
+             &gScsiDiskComponentName,
+             &gScsiDiskComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+
+  return Status;
+}
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions.
+  If any other agent wishes to call Supported() it must also follow these
+  calling restrictions.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath   OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  EFI_SCSI_IO_PROTOCOL  *ScsiIo;
+  UINT8                 DeviceType;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiScsiIoProtocolGuid,
+                  (VOID **) &ScsiIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = ScsiIo->GetDeviceType (ScsiIo, &DeviceType);
+  if (!EFI_ERROR (Status)) {
+    if ((DeviceType == EFI_SCSI_TYPE_DISK) || (DeviceType == EFI_SCSI_TYPE_CDROM)) {
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_UNSUPPORTED;
+    }
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiScsiIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Start() it must also follow these calling
+  restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath   OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  EFI_SCSI_IO_PROTOCOL  *ScsiIo;
+  SCSI_DISK_DEV         *ScsiDiskDevice;
+  BOOLEAN               Temp;
+  UINT8                 Index;
+  UINT8                 MaxRetry;
+  BOOLEAN               NeedRetry;
+  BOOLEAN               MustReadCapacity;
+
+  MustReadCapacity = TRUE;
+
+  ScsiDiskDevice = (SCSI_DISK_DEV *) AllocateZeroPool (sizeof (SCSI_DISK_DEV));
+  if (ScsiDiskDevice == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiScsiIoProtocolGuid,
+                  (VOID **) &ScsiIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (ScsiDiskDevice);
+    return Status;
+  }
+
+  ScsiDiskDevice->Signature            = SCSI_DISK_DEV_SIGNATURE;
+  ScsiDiskDevice->ScsiIo               = ScsiIo;
+  ScsiDiskDevice->BlkIo.Revision       = EFI_BLOCK_IO_PROTOCOL_REVISION3;
+  ScsiDiskDevice->BlkIo.Media          = &ScsiDiskDevice->BlkIoMedia;
+  ScsiDiskDevice->BlkIo.Media->IoAlign = ScsiIo->IoAlign;
+  ScsiDiskDevice->BlkIo.Reset          = ScsiDiskReset;
+  ScsiDiskDevice->BlkIo.ReadBlocks     = ScsiDiskReadBlocks;
+  ScsiDiskDevice->BlkIo.WriteBlocks    = ScsiDiskWriteBlocks;
+  ScsiDiskDevice->BlkIo.FlushBlocks    = ScsiDiskFlushBlocks;
+  ScsiDiskDevice->BlkIo2.Media         = &ScsiDiskDevice->BlkIoMedia;
+  ScsiDiskDevice->BlkIo2.Reset         = ScsiDiskResetEx;
+  ScsiDiskDevice->BlkIo2.ReadBlocksEx  = ScsiDiskReadBlocksEx;
+  ScsiDiskDevice->BlkIo2.WriteBlocksEx = ScsiDiskWriteBlocksEx;
+  ScsiDiskDevice->BlkIo2.FlushBlocksEx = ScsiDiskFlushBlocksEx;
+  ScsiDiskDevice->Handle               = Controller;
+  InitializeListHead (&ScsiDiskDevice->BlkIo2Queue);
+
+  ScsiIo->GetDeviceType (ScsiIo, &(ScsiDiskDevice->DeviceType));
+  switch (ScsiDiskDevice->DeviceType) {
+  case EFI_SCSI_TYPE_DISK:
+    ScsiDiskDevice->BlkIo.Media->BlockSize = 0x200;
+    MustReadCapacity = TRUE;
+    break;
+
+  case EFI_SCSI_TYPE_CDROM:
+    ScsiDiskDevice->BlkIo.Media->BlockSize = 0x800;
+    ScsiDiskDevice->BlkIo.Media->ReadOnly  = TRUE;
+    MustReadCapacity = FALSE;
+    break;
+  }
+  //
+  // The Sense Data Array's initial size is 6
+  //
+  ScsiDiskDevice->SenseDataNumber = 6;
+  ScsiDiskDevice->SenseData = (EFI_SCSI_SENSE_DATA *) AllocateZeroPool (
+                                 sizeof (EFI_SCSI_SENSE_DATA) * ScsiDiskDevice->SenseDataNumber
+                                 );
+  if (ScsiDiskDevice->SenseData == NULL) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiScsiIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    FreePool (ScsiDiskDevice);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Retrieve device information
+  //
+  MaxRetry = 2;
+  for (Index = 0; Index < MaxRetry; Index++) {
+    Status = ScsiDiskInquiryDevice (ScsiDiskDevice, &NeedRetry);
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+
+    if (!NeedRetry) {
+      FreePool (ScsiDiskDevice->SenseData);
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiScsiIoProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      FreePool (ScsiDiskDevice);
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  //
+  // The second parameter "TRUE" means must
+  // retrieve media capacity
+  //
+  Status = ScsiDiskDetectMedia (ScsiDiskDevice, MustReadCapacity, &Temp);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Determine if Block IO & Block IO2 should be produced on this controller
+    // handle
+    //
+    if (DetermineInstallBlockIo(Controller)) {
+      InitializeInstallDiskInfo(ScsiDiskDevice, Controller);
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &Controller,
+                      &gEfiBlockIoProtocolGuid,
+                      &ScsiDiskDevice->BlkIo,
+                      &gEfiBlockIo2ProtocolGuid,
+                      &ScsiDiskDevice->BlkIo2,
+                      &gEfiDiskInfoProtocolGuid,
+                      &ScsiDiskDevice->DiskInfo,
+                      NULL
+                      );
+      if (!EFI_ERROR(Status)) {
+        ScsiDiskDevice->ControllerNameTable = NULL;
+        AddUnicodeString2 (
+          "eng",
+          gScsiDiskComponentName.SupportedLanguages,
+          &ScsiDiskDevice->ControllerNameTable,
+          L"SCSI Disk Device",
+          TRUE
+          );
+        AddUnicodeString2 (
+          "en",
+          gScsiDiskComponentName2.SupportedLanguages,
+          &ScsiDiskDevice->ControllerNameTable,
+          L"SCSI Disk Device",
+          FALSE
+          );
+        return EFI_SUCCESS;
+      }
+    } 
+  }
+
+  gBS->FreePool (ScsiDiskDevice->SenseData);
+  gBS->FreePool (ScsiDiskDevice);
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiScsiIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  return Status;
+  
+}
+
+
+/**
+  Stop this driver on ControllerHandle.
+
+  This service is called by the EFI boot service DisconnectController().
+  In order to make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController() must follow these
+  calling restrictions. If any other agent wishes to call Stop() it must
+  also follow these calling restrictions.
+  
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer   OPTIONAL
+  )
+{
+  EFI_BLOCK_IO_PROTOCOL *BlkIo;
+  SCSI_DISK_DEV         *ScsiDiskDevice;
+  EFI_STATUS            Status;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlkIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (BlkIo);
+
+  //
+  // Wait for the BlockIo2 requests queue to become empty
+  //
+  while (!IsListEmpty (&ScsiDiskDevice->BlkIo2Queue));
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Controller,
+                  &gEfiBlockIoProtocolGuid,
+                  &ScsiDiskDevice->BlkIo,
+                  &gEfiBlockIo2ProtocolGuid,
+                  &ScsiDiskDevice->BlkIo2,
+                  &gEfiDiskInfoProtocolGuid,
+                  &ScsiDiskDevice->DiskInfo,
+                  NULL
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiScsiIoProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+
+    ReleaseScsiDiskDeviceResources (ScsiDiskDevice);
+
+    return EFI_SUCCESS;
+  }
+  //
+  // errors met
+  //
+  return Status;
+}
+
+/**
+  Reset SCSI Disk.
+
+
+  @param  This                 The pointer of EFI_BLOCK_IO_PROTOCOL
+  @param  ExtendedVerification The flag about if extend verificate
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+  @return EFI_STATUS is returned from EFI_SCSI_IO_PROTOCOL.ResetDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_TPL       OldTpl;
+  SCSI_DISK_DEV *ScsiDiskDevice;
+  EFI_STATUS    Status;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_BLKIO (This);
+
+  Status          = ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_UNSUPPORTED) {
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+  }
+
+  if (!ExtendedVerification) {
+    goto Done;
+  }
+
+  Status = ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Done;
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The function is to Read Block from SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected
+  @param  Lba        The logic block address
+  @param  BufferSize The size of Buffer
+  @param  Buffer     The buffer to fill the read out data
+
+  @retval EFI_SUCCESS           Successfully to read out block.
+  @retval EFI_DEVICE_ERROR      Fail to detect media.
+  @retval EFI_NO_MEDIA          Media is not present.
+  @retval EFI_MEDIA_CHANGED     Media has changed.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER Invalid parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  UINTN               BlockSize;
+  UINTN               NumberOfBlocks;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+  //
+  // Get the intrinsic block size
+  //
+  Media           = ScsiDiskDevice->BlkIo.Media;
+  BlockSize       = Media->BlockSize;
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto Done;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (BufferSize == 0) {
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto Done;
+  }
+
+  if (Lba > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Media->IoAlign > 1) && (((UINTN) Buffer & (Media->IoAlign - 1)) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  //
+  // If all the parameters are valid, then perform read sectors command
+  // to transfer data from device to host.
+  //
+  Status = ScsiDiskReadSectors (ScsiDiskDevice, Buffer, Lba, NumberOfBlocks);
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The function is to Write Block to SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL
+  @param  MediaId    The Id of Media detected
+  @param  Lba        The logic block address
+  @param  BufferSize The size of Buffer
+  @param  Buffer     The buffer to fill the read out data
+
+  @retval EFI_SUCCESS           Successfully to read out block.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      Fail to detect media.
+  @retval EFI_NO_MEDIA          Media is not present.
+  @retval EFI_MEDIA_CHNAGED     Media has changed.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER Invalid parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  UINTN               BlockSize;
+  UINTN               NumberOfBlocks;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+  //
+  // Get the intrinsic block size
+  //
+  Media           = ScsiDiskDevice->BlkIo.Media;
+  BlockSize       = Media->BlockSize;
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto Done;
+  }
+
+  if (Media->ReadOnly) {
+    Status = EFI_WRITE_PROTECTED;
+    goto Done;
+  }
+
+  if (BufferSize == 0) {
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto Done;
+  }
+
+  if (Lba > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Media->IoAlign > 1) && (((UINTN) Buffer & (Media->IoAlign - 1)) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+  //
+  // if all the parameters are valid, then perform read sectors command
+  // to transfer data from device to host.
+  //
+  Status = ScsiDiskWriteSectors (ScsiDiskDevice, Buffer, Lba, NumberOfBlocks);
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Flush Block to Disk.
+
+  EFI_SUCCESS is returned directly.
+
+  @param  This              The pointer of EFI_BLOCK_IO_PROTOCOL
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  )
+{
+  //
+  // return directly
+  //
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset SCSI Disk.
+
+  @param  This                 The pointer of EFI_BLOCK_IO2_PROTOCOL.
+  @param  ExtendedVerification The flag about if extend verificate.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+  @return EFI_STATUS is returned from EFI_SCSI_IO_PROTOCOL.ResetDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskResetEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_TPL       OldTpl;
+  SCSI_DISK_DEV *ScsiDiskDevice;
+  EFI_STATUS    Status;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_BLKIO2 (This);
+
+  Status          = ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_UNSUPPORTED) {
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+  }
+
+  if (!ExtendedVerification) {
+    goto Done;
+  }
+
+  Status = ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Done;
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The function is to Read Block from SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected.
+  @param  Lba        The logic block address.
+  @param  Token      A pointer to the token associated with the transaction.
+  @param  BufferSize The size of Buffer.
+  @param  Buffer     The buffer to fill the read out data.
+
+  @retval EFI_SUCCESS           The read request was queued if Token-> Event is
+                                not NULL. The data was read correctly from the
+                                device if theToken-> Event is NULL.
+  @retval EFI_DEVICE_ERROR      The device reported an error while attempting
+                                to perform the read operation.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of
+                                the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not
+                                valid, or the buffer is not on proper
+                                alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReadBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL   *This,
+  IN     UINT32                   MediaId,
+  IN     EFI_LBA                  Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN      *Token,
+  IN     UINTN                    BufferSize,
+  OUT    VOID                     *Buffer
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  UINTN               BlockSize;
+  UINTN               NumberOfBlocks;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO2 (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+  //
+  // Get the intrinsic block size
+  //
+  Media           = ScsiDiskDevice->BlkIo2.Media;
+  BlockSize       = Media->BlockSize;
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto Done;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (BufferSize == 0) {
+    if ((Token != NULL) && (Token->Event != NULL)) {
+      Token->TransactionStatus = EFI_SUCCESS;
+      gBS->SignalEvent (Token->Event);
+    }
+
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto Done;
+  }
+
+  if (Lba > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Media->IoAlign > 1) && (((UINTN) Buffer & (Media->IoAlign - 1)) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  //
+  // If all the parameters are valid, then perform read sectors command
+  // to transfer data from device to host.
+  //
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Token->TransactionStatus = EFI_SUCCESS;
+    Status = ScsiDiskAsyncReadSectors (
+               ScsiDiskDevice,
+               Buffer,
+               Lba,
+               NumberOfBlocks,
+               Token
+               );
+  } else {
+    Status = ScsiDiskReadSectors (
+               ScsiDiskDevice,
+               Buffer,
+               Lba,
+               NumberOfBlocks
+               );
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The function is to Write Block to SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected.
+  @param  Lba        The logic block address.
+  @param  Token      A pointer to the token associated with the transaction.
+  @param  BufferSize The size of Buffer.
+  @param  Buffer     The buffer to fill the read out data.
+
+  @retval EFI_SUCCESS           The data were written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device cannot be written to.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_DEVICE_ERROR      The device reported an error while attempting
+                                to perform the write operation.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of
+                                the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not
+                                valid, or the buffer is not on proper
+                                alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskWriteBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL *This,
+  IN     UINT32                 MediaId,
+  IN     EFI_LBA                Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN    *Token,
+  IN     UINTN                  BufferSize,
+  IN     VOID                   *Buffer
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  UINTN               BlockSize;
+  UINTN               NumberOfBlocks;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO2 (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+  //
+  // Get the intrinsic block size
+  //
+  Media           = ScsiDiskDevice->BlkIo2.Media;
+  BlockSize       = Media->BlockSize;
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto Done;
+  }
+
+  if (Media->ReadOnly) {
+    Status = EFI_WRITE_PROTECTED;
+    goto Done;
+  }
+
+  if (BufferSize == 0) {
+    if ((Token != NULL) && (Token->Event != NULL)) {
+      Token->TransactionStatus = EFI_SUCCESS;
+      gBS->SignalEvent (Token->Event);
+    }
+
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto Done;
+  }
+
+  if (Lba > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Media->IoAlign > 1) && (((UINTN) Buffer & (Media->IoAlign - 1)) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  //
+  // if all the parameters are valid, then perform write sectors command
+  // to transfer data from device to host.
+  //
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Token->TransactionStatus = EFI_SUCCESS;
+    Status = ScsiDiskAsyncWriteSectors (
+               ScsiDiskDevice,
+               Buffer,
+               Lba,
+               NumberOfBlocks,
+               Token
+               );
+  } else {
+    Status = ScsiDiskWriteSectors (
+               ScsiDiskDevice,
+               Buffer,
+               Lba,
+               NumberOfBlocks
+               );
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Flush the Block Device.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  Token      A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS         All outstanding data was written to the device.
+  @retval EFI_DEVICE_ERROR    The device reported an error while attempting to
+                              write data.
+  @retval EFI_WRITE_PROTECTED The device cannot be written to.
+  @retval EFI_NO_MEDIA        There is no media in the device.
+  @retval EFI_MEDIA_CHANGED   The MediaId is not for the current media.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskFlushBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN OUT EFI_BLOCK_IO2_TOKEN     *Token
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO2 (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+
+  Media = ScsiDiskDevice->BlkIo2.Media;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (Media->ReadOnly) {
+    Status = EFI_WRITE_PROTECTED;
+    goto Done;
+  }
+
+  //
+  // Wait for the BlockIo2 requests queue to become empty
+  //
+  while (!IsListEmpty (&ScsiDiskDevice->BlkIo2Queue));
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Signal caller event
+  //
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Token->TransactionStatus = EFI_SUCCESS;
+    gBS->SignalEvent (Token->Event);
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Detect Device and read out capacity ,if error occurs, parse the sense key.
+
+  @param  ScsiDiskDevice    The pointer of SCSI_DISK_DEV
+  @param  MustReadCapacity  The flag about reading device capacity
+  @param  MediaChange       The pointer of flag indicates if media has changed 
+
+  @retval EFI_DEVICE_ERROR  Indicates that error occurs
+  @retval EFI_SUCCESS       Successfully to detect media
+
+**/
+EFI_STATUS
+ScsiDiskDetectMedia (
+  IN   SCSI_DISK_DEV   *ScsiDiskDevice,
+  IN   BOOLEAN         MustReadCapacity,
+  OUT  BOOLEAN         *MediaChange
+  )
+{
+  EFI_STATUS          Status;
+  EFI_SCSI_SENSE_DATA *SenseData;
+  UINTN               NumberOfSenseKeys;
+  BOOLEAN             NeedRetry;
+  BOOLEAN             NeedReadCapacity;
+  UINT8               Retry;
+  UINT8               MaxRetry;
+  EFI_BLOCK_IO_MEDIA  OldMedia;
+  UINTN               Action;
+  EFI_EVENT           TimeoutEvt;
+
+  Status              = EFI_SUCCESS;
+  SenseData           = NULL;
+  NumberOfSenseKeys   = 0;
+  Retry               = 0;
+  MaxRetry            = 3;
+  Action              = ACTION_NO_ACTION;
+  NeedReadCapacity    = FALSE;
+  *MediaChange        = FALSE;
+  TimeoutEvt          = NULL;
+
+  CopyMem (&OldMedia, ScsiDiskDevice->BlkIo.Media, sizeof (OldMedia));
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &TimeoutEvt
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->SetTimer (TimeoutEvt, TimerRelative, EFI_TIMER_PERIOD_SECONDS(120));
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  //
+  // Sending Test_Unit cmd to poll device status.
+  // If the sense data shows the drive is not ready or reset before, we need poll the device status again.
+  // We limit the upper boundary to 120 seconds.
+  //
+  while (EFI_ERROR (gBS->CheckEvent (TimeoutEvt))) {
+    Status = ScsiDiskTestUnitReady (
+              ScsiDiskDevice,
+              &NeedRetry,
+              &SenseData,
+              &NumberOfSenseKeys
+              );
+    if (!EFI_ERROR (Status)) {
+      Status = DetectMediaParsingSenseKeys (
+                 ScsiDiskDevice,
+                 SenseData,
+                 NumberOfSenseKeys,
+                 &Action
+                 );
+      if (EFI_ERROR (Status)) {
+        goto EXIT;
+      } else if (Action == ACTION_RETRY_COMMAND_LATER) {
+        continue;
+      } else {
+        break;
+      }
+    } else {
+      Retry++;
+      if (!NeedRetry || (Retry >= MaxRetry)) {
+        goto EXIT;
+      }
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  //
+  // ACTION_NO_ACTION: need not read capacity
+  // other action code: need read capacity
+  //
+  if (Action == ACTION_READ_CAPACITY) {
+    NeedReadCapacity = TRUE;
+  }
+
+  //
+  // either NeedReadCapacity is TRUE, or MustReadCapacity is TRUE,
+  // retrieve capacity via Read Capacity command
+  //
+  if (NeedReadCapacity || MustReadCapacity) {
+    //
+    // retrieve media information
+    //
+    for (Retry = 0; Retry < MaxRetry; Retry++) {
+      Status = ScsiDiskReadCapacity (
+                 ScsiDiskDevice,
+                 &NeedRetry,
+                 &SenseData,
+                 &NumberOfSenseKeys
+                 );
+      if (!EFI_ERROR (Status)) {
+        //
+        // analyze sense key to action
+        //
+        Status = DetectMediaParsingSenseKeys (
+                   ScsiDiskDevice,
+                   SenseData,
+                   NumberOfSenseKeys,
+                   &Action
+                   );
+        if (EFI_ERROR (Status)) {
+          //
+          // if Status is error, it may indicate crisis error,
+          // so return without retry.
+          //
+          goto EXIT;
+        } else if (Action == ACTION_RETRY_COMMAND_LATER) {
+          Retry = 0;
+          continue;
+        } else {
+          break;
+        }
+      } else {   
+        Retry++;
+        if (!NeedRetry || (Retry >= MaxRetry)) {
+          goto EXIT;
+        }
+      }
+    }
+
+    if (EFI_ERROR (Status)) {
+      goto EXIT;
+    }
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->MediaId != OldMedia.MediaId) {
+    //
+    // Media change information got from the device
+    //
+    *MediaChange = TRUE;
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->ReadOnly != OldMedia.ReadOnly) {
+    *MediaChange = TRUE;
+    ScsiDiskDevice->BlkIo.Media->MediaId += 1;
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->BlockSize != OldMedia.BlockSize) {
+    *MediaChange = TRUE;
+    ScsiDiskDevice->BlkIo.Media->MediaId += 1;
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->LastBlock != OldMedia.LastBlock) {
+    *MediaChange = TRUE;
+    ScsiDiskDevice->BlkIo.Media->MediaId += 1;
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->MediaPresent != OldMedia.MediaPresent) {
+    if (ScsiDiskDevice->BlkIo.Media->MediaPresent) {
+      //
+      // when change from no media to media present, reset the MediaId to 1.
+      //
+      ScsiDiskDevice->BlkIo.Media->MediaId = 1;
+    } else {
+      //
+      // when no media, reset the MediaId to zero.
+      //
+      ScsiDiskDevice->BlkIo.Media->MediaId = 0;
+    }
+
+    *MediaChange = TRUE;
+  }
+
+EXIT:
+  if (TimeoutEvt != NULL) {
+    gBS->CloseEvent (TimeoutEvt);
+  }
+  return Status;
+}
+
+
+/**
+  Send out Inquiry command to Device.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  NeedRetry       Indicates if needs try again when error happens
+
+  @retval  EFI_DEVICE_ERROR  Indicates that error occurs
+  @retval  EFI_SUCCESS       Successfully to detect media
+
+**/
+EFI_STATUS
+ScsiDiskInquiryDevice (
+  IN OUT  SCSI_DISK_DEV   *ScsiDiskDevice,
+     OUT  BOOLEAN         *NeedRetry
+  )
+{
+  UINT32                                InquiryDataLength;
+  UINT8                                 SenseDataLength;
+  UINT8                                 HostAdapterStatus;
+  UINT8                                 TargetStatus;
+  EFI_SCSI_SENSE_DATA                   *SenseDataArray;
+  UINTN                                 NumberOfSenseKeys;
+  EFI_STATUS                            Status;
+  UINT8                                 MaxRetry;
+  UINT8                                 Index;
+  EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE *SupportedVpdPages;
+  EFI_SCSI_BLOCK_LIMITS_VPD_PAGE        *BlockLimits;
+  UINTN                                 PageLength;
+
+  InquiryDataLength = sizeof (EFI_SCSI_INQUIRY_DATA);
+  SenseDataLength   = 0;
+
+  Status = ScsiInquiryCommand (
+            ScsiDiskDevice->ScsiIo,
+            SCSI_DISK_TIMEOUT,
+            NULL,
+            &SenseDataLength,
+            &HostAdapterStatus,
+            &TargetStatus,
+            (VOID *) &(ScsiDiskDevice->InquiryData),
+            &InquiryDataLength,
+            FALSE
+            );
+    //
+    // no need to check HostAdapterStatus and TargetStatus
+    //
+  if ((Status == EFI_SUCCESS) || (Status == EFI_WARN_BUFFER_TOO_SMALL)) {
+    ParseInquiryData (ScsiDiskDevice);
+
+    if (ScsiDiskDevice->DeviceType == EFI_SCSI_TYPE_DISK) {
+      //
+      // Check whether the device supports Block Limits VPD page (0xB0)
+      //
+      SupportedVpdPages = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE));
+      if (SupportedVpdPages == NULL) {
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      ZeroMem (SupportedVpdPages, sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE));
+      InquiryDataLength = sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE);
+      SenseDataLength   = 0;
+      Status = ScsiInquiryCommandEx (
+                 ScsiDiskDevice->ScsiIo,
+                 SCSI_DISK_TIMEOUT,
+                 NULL,
+                 &SenseDataLength,
+                 &HostAdapterStatus,
+                 &TargetStatus,
+                 (VOID *) SupportedVpdPages,
+                 &InquiryDataLength,
+                 TRUE,
+                 EFI_SCSI_PAGE_CODE_SUPPORTED_VPD
+                 );
+      if (!EFI_ERROR (Status)) {
+        PageLength = (SupportedVpdPages->PageLength2 << 8)
+                   |  SupportedVpdPages->PageLength1;
+        for (Index = 0; Index < PageLength; Index++) {
+          if (SupportedVpdPages->SupportedVpdPageList[Index] == EFI_SCSI_PAGE_CODE_BLOCK_LIMITS_VPD) {
+            break;
+          }
+        }
+
+        //
+        // Query the Block Limits VPD page
+        //
+        if (Index < PageLength) {
+          BlockLimits = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_BLOCK_LIMITS_VPD_PAGE));
+          if (BlockLimits == NULL) {
+            FreeAlignedBuffer (SupportedVpdPages, sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE));
+            *NeedRetry = FALSE;
+            return EFI_DEVICE_ERROR;
+          }
+          ZeroMem (BlockLimits, sizeof (EFI_SCSI_BLOCK_LIMITS_VPD_PAGE));
+          InquiryDataLength = sizeof (EFI_SCSI_BLOCK_LIMITS_VPD_PAGE);
+          SenseDataLength   = 0;
+          Status = ScsiInquiryCommandEx (
+                     ScsiDiskDevice->ScsiIo,
+                     SCSI_DISK_TIMEOUT,
+                     NULL,
+                     &SenseDataLength,
+                     &HostAdapterStatus,
+                     &TargetStatus,
+                     (VOID *) BlockLimits,
+                     &InquiryDataLength,
+                     TRUE,
+                     EFI_SCSI_PAGE_CODE_BLOCK_LIMITS_VPD
+                     );
+          if (!EFI_ERROR (Status)) {
+            ScsiDiskDevice->BlkIo.Media->OptimalTransferLengthGranularity = 
+              (BlockLimits->OptimalTransferLengthGranularity2 << 8) |
+               BlockLimits->OptimalTransferLengthGranularity1;
+          }
+
+          FreeAlignedBuffer (BlockLimits, sizeof (EFI_SCSI_BLOCK_LIMITS_VPD_PAGE));
+        }
+      }
+
+      FreeAlignedBuffer (SupportedVpdPages, sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE));
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    return EFI_SUCCESS;
+
+  } else if (Status == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+ 
+  } else if ((Status == EFI_INVALID_PARAMETER) || (Status == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR)
+  //
+
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+      //
+      // reset the scsi channel
+      //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  
+  //
+  // if goes here, meant ScsiInquiryCommand() failed.
+  // if ScsiDiskRequestSenseKeys() succeeds at last,
+  // better retry ScsiInquiryCommand(). (by setting *NeedRetry = TRUE)
+  //
+  MaxRetry = 3;
+  for (Index = 0; Index < MaxRetry; Index++) {
+    Status = ScsiDiskRequestSenseKeys (
+              ScsiDiskDevice,
+              NeedRetry,
+              &SenseDataArray,
+              &NumberOfSenseKeys,
+              TRUE
+              );
+    if (!EFI_ERROR (Status)) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    }
+
+    if (!*NeedRetry) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  //
+  // ScsiDiskRequestSenseKeys() failed after several rounds of retry.
+  // set *NeedRetry = FALSE to avoid the outside caller try again.
+  //
+  *NeedRetry = FALSE;
+  return EFI_DEVICE_ERROR;
+}
+
+/**
+  To test device.
+
+  When Test Unit Ready command succeeds, retrieve Sense Keys via Request Sense;
+  When Test Unit Ready command encounters any error caused by host adapter or
+  target, return error without retrieving Sense Keys.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates try again
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The pointer of the number of sense data array
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to test unit
+
+**/
+EFI_STATUS
+ScsiDiskTestUnitReady (
+  IN  SCSI_DISK_DEV         *ScsiDiskDevice,
+  OUT BOOLEAN               *NeedRetry,
+  OUT EFI_SCSI_SENSE_DATA   **SenseDataArray,
+  OUT UINTN                 *NumberOfSenseKeys
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       SenseDataLength;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINT8       Index;
+  UINT8       MaxRetry;
+
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  *NumberOfSenseKeys  = 0;
+
+  //
+  // Parameter 3 and 4: do not require sense data, retrieve it when needed.
+  //
+  Status = ScsiTestUnitReadyCommand (
+            ScsiDiskDevice->ScsiIo,
+            SCSI_DISK_TIMEOUT,
+            ScsiDiskDevice->SenseData,
+            &SenseDataLength,
+            &HostAdapterStatus,
+            &TargetStatus
+            );
+  //
+  // no need to check HostAdapterStatus and TargetStatus
+  //
+  if (Status == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if ((Status == EFI_INVALID_PARAMETER) || (Status == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus(in case of EFI_DEVICE_ERROR)
+  //
+
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (SenseDataLength != 0) {
+    *NumberOfSenseKeys = SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA);
+    *SenseDataArray    = ScsiDiskDevice->SenseData;
+    return EFI_SUCCESS;
+  }
+
+  MaxRetry = 3;
+  for (Index = 0; Index < MaxRetry; Index++) {
+    Status = ScsiDiskRequestSenseKeys (
+              ScsiDiskDevice,
+              NeedRetry,
+              SenseDataArray,
+              NumberOfSenseKeys,
+              FALSE
+              );
+    if (!EFI_ERROR (Status)) {
+      return EFI_SUCCESS;
+    }
+
+    if (!*NeedRetry) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  //
+  // ScsiDiskRequestSenseKeys() failed after several rounds of retry.
+  // set *NeedRetry = FALSE to avoid the outside caller try again.
+  //
+  *NeedRetry = FALSE;
+  return EFI_DEVICE_ERROR;
+}
+
+/**
+  Parsing Sense Keys which got from request sense command.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  SenseData          The pointer of EFI_SCSI_SENSE_DATA
+  @param  NumberOfSenseKeys  The number of sense key  
+  @param  Action             The pointer of action which indicates what is need to do next
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to complete the parsing
+
+**/
+EFI_STATUS
+DetectMediaParsingSenseKeys (
+  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+  IN   EFI_SCSI_SENSE_DATA     *SenseData,
+  IN   UINTN                   NumberOfSenseKeys,
+  OUT  UINTN                   *Action
+  )
+{
+  BOOLEAN RetryLater;
+
+  //
+  // Default is to read capacity, unless..
+  //
+  *Action = ACTION_READ_CAPACITY;
+
+  if (NumberOfSenseKeys == 0) {
+    if (ScsiDiskDevice->BlkIo.Media->MediaPresent == TRUE) {
+      *Action = ACTION_NO_ACTION;
+    }
+    return EFI_SUCCESS;
+  }
+
+  if (!ScsiDiskHaveSenseKey (SenseData, NumberOfSenseKeys)) {
+    //
+    // No Sense Key returned from last submitted command
+    //
+    if (ScsiDiskDevice->BlkIo.Media->MediaPresent == TRUE) {
+      *Action = ACTION_NO_ACTION;
+    }
+    return EFI_SUCCESS;
+  }
+
+  if (ScsiDiskIsNoMedia (SenseData, NumberOfSenseKeys)) {
+    ScsiDiskDevice->BlkIo.Media->MediaPresent = FALSE;
+    ScsiDiskDevice->BlkIo.Media->LastBlock    = 0;
+    *Action = ACTION_NO_ACTION;
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsNoMedia\n"));
+    return EFI_SUCCESS;
+  }
+
+  if (ScsiDiskIsMediaChange (SenseData, NumberOfSenseKeys)) {
+    ScsiDiskDevice->BlkIo.Media->MediaId++;
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsMediaChange!\n"));
+    return EFI_SUCCESS;
+  }
+
+  if (ScsiDiskIsResetBefore (SenseData, NumberOfSenseKeys)) {
+    *Action = ACTION_RETRY_COMMAND_LATER;
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsResetBefore!\n"));
+    return EFI_SUCCESS;
+  }
+
+  if (ScsiDiskIsMediaError (SenseData, NumberOfSenseKeys)) {
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsMediaError\n"));
+    *Action = ACTION_RETRY_WITH_BACKOFF_ALGO;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (ScsiDiskIsHardwareError (SenseData, NumberOfSenseKeys)) {
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsHardwareError\n"));
+    *Action = ACTION_RETRY_WITH_BACKOFF_ALGO;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (!ScsiDiskIsDriveReady (SenseData, NumberOfSenseKeys, &RetryLater)) {
+    if (RetryLater) {
+      *Action = ACTION_RETRY_COMMAND_LATER;
+      DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskDriveNotReady!\n"));
+      return EFI_SUCCESS;
+    }
+    *Action = ACTION_NO_ACTION;
+    return EFI_DEVICE_ERROR;
+  }
+
+  *Action = ACTION_RETRY_WITH_BACKOFF_ALGO;
+  DEBUG ((EFI_D_VERBOSE, "ScsiDisk: Sense Key = 0x%x ASC = 0x%x!\n", SenseData->Sense_Key, SenseData->Addnl_Sense_Code));
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Send read capacity command to device and get the device parameter.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates if need a retry
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The number of sense key
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to read capacity or sense data is received.
+
+**/
+EFI_STATUS
+ScsiDiskReadCapacity (
+  IN  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+      OUT  BOOLEAN                 *NeedRetry,
+      OUT  EFI_SCSI_SENSE_DATA     **SenseDataArray,
+      OUT  UINTN                   *NumberOfSenseKeys
+  )
+{
+  UINT8                         HostAdapterStatus;
+  UINT8                         TargetStatus;
+  EFI_STATUS                    CommandStatus;
+  EFI_STATUS                    Status;
+  UINT8                         Index;
+  UINT8                         MaxRetry;
+  UINT8                         SenseDataLength;
+  UINT32                        DataLength10;
+  UINT32                        DataLength16;
+  EFI_SCSI_DISK_CAPACITY_DATA   *CapacityData10;
+  EFI_SCSI_DISK_CAPACITY_DATA16 *CapacityData16;
+
+  CapacityData10 = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+  if (CapacityData10 == NULL) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  CapacityData16 = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));
+  if (CapacityData16 == NULL) {
+    FreeAlignedBuffer (CapacityData10, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  SenseDataLength       = 0;
+  DataLength10          = sizeof (EFI_SCSI_DISK_CAPACITY_DATA);
+  DataLength16          = sizeof (EFI_SCSI_DISK_CAPACITY_DATA16);
+  ZeroMem (CapacityData10, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+  ZeroMem (CapacityData16, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));
+
+  *NumberOfSenseKeys  = 0;
+  *NeedRetry          = FALSE;
+
+  //
+  // submit Read Capacity(10) Command. If it returns capacity of FFFFFFFFh, 
+  // 16 byte command should be used to access large hard disk >2TB
+  //
+  CommandStatus = ScsiReadCapacityCommand (
+                    ScsiDiskDevice->ScsiIo,
+                    SCSI_DISK_TIMEOUT,
+                    NULL,
+                    &SenseDataLength,
+                    &HostAdapterStatus,
+                    &TargetStatus,
+                    (VOID *) CapacityData10,
+                    &DataLength10,
+                    FALSE
+                    );
+
+  ScsiDiskDevice->Cdb16Byte = FALSE;
+  if ((!EFI_ERROR (CommandStatus)) && (CapacityData10->LastLba3 == 0xff) && (CapacityData10->LastLba2 == 0xff) &&
+      (CapacityData10->LastLba1 == 0xff) && (CapacityData10->LastLba0 == 0xff)) {
+    //
+    // use Read Capacity (16), Read (16) and Write (16) next when hard disk size > 2TB
+    //
+    ScsiDiskDevice->Cdb16Byte = TRUE;
+    //
+    // submit Read Capacity(16) Command to get parameter LogicalBlocksPerPhysicalBlock
+    // and LowestAlignedLba
+    //
+    CommandStatus = ScsiReadCapacity16Command (
+                      ScsiDiskDevice->ScsiIo,
+                      SCSI_DISK_TIMEOUT,
+                      NULL,
+                      &SenseDataLength,
+                      &HostAdapterStatus,
+                      &TargetStatus,
+                      (VOID *) CapacityData16,
+                      &DataLength16,
+                      FALSE
+                      );
+  }
+
+    //
+    // no need to check HostAdapterStatus and TargetStatus
+    //
+   if (CommandStatus == EFI_SUCCESS) {
+     GetMediaInfo (ScsiDiskDevice, CapacityData10, CapacityData16);
+     FreeAlignedBuffer (CapacityData10, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+     FreeAlignedBuffer (CapacityData16, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));
+     return EFI_SUCCESS;
+   }
+
+   FreeAlignedBuffer (CapacityData10, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+   FreeAlignedBuffer (CapacityData16, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));
+
+   if (CommandStatus == EFI_NOT_READY) {
+     *NeedRetry = TRUE;
+     return EFI_DEVICE_ERROR;
+   } else if ((CommandStatus == EFI_INVALID_PARAMETER) || (CommandStatus == EFI_UNSUPPORTED)) {
+     *NeedRetry = FALSE;
+     return EFI_DEVICE_ERROR;
+   }
+
+   //
+   // go ahead to check HostAdapterStatus and TargetStatus
+   // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+   //
+ 
+   Status = CheckHostAdapterStatus (HostAdapterStatus);
+   if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+     *NeedRetry = TRUE;
+     return EFI_DEVICE_ERROR;
+ 
+   } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  
+  //
+  // if goes here, meant ScsiReadCapacityCommand() failed.
+  // if ScsiDiskRequestSenseKeys() succeeds at last,
+  // better retry ScsiReadCapacityCommand(). (by setting *NeedRetry = TRUE)
+  //
+  MaxRetry = 3;
+  for (Index = 0; Index < MaxRetry; Index++) {
+
+    Status = ScsiDiskRequestSenseKeys (
+              ScsiDiskDevice,
+              NeedRetry,
+              SenseDataArray,
+              NumberOfSenseKeys,
+              TRUE
+              );
+    if (!EFI_ERROR (Status)) {
+      return EFI_SUCCESS;
+    }
+
+    if (!*NeedRetry) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  //
+  // ScsiDiskRequestSenseKeys() failed after several rounds of retry.
+  // set *NeedRetry = FALSE to avoid the outside caller try again.
+  //
+  *NeedRetry = FALSE;
+  return EFI_DEVICE_ERROR;
+}
+
+/**
+  Check the HostAdapter status and re-interpret it in EFI_STATUS.
+
+  @param  HostAdapterStatus  Host Adapter status
+
+  @retval  EFI_SUCCESS       Host adapter is OK.
+  @retval  EFI_TIMEOUT       Timeout.
+  @retval  EFI_NOT_READY     Adapter NOT ready.
+  @retval  EFI_DEVICE_ERROR  Adapter device error.
+
+**/
+EFI_STATUS
+CheckHostAdapterStatus (
+  IN UINT8   HostAdapterStatus
+  )
+{
+  switch (HostAdapterStatus) {
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_OK:
+    return EFI_SUCCESS;
+
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_SELECTION_TIMEOUT:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_TIMEOUT:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_TIMEOUT_COMMAND:
+    return EFI_TIMEOUT;
+
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_MESSAGE_REJECT:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_PARITY_ERROR:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_REQUEST_SENSE_FAILED:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_DATA_OVERRUN_UNDERRUN:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_BUS_RESET:
+    return EFI_NOT_READY;
+
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_BUS_FREE:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_PHASE_ERROR:
+    return EFI_DEVICE_ERROR;
+
+  default:
+    return EFI_SUCCESS;
+  }
+}
+
+
+/**
+  Check the target status and re-interpret it in EFI_STATUS.
+
+  @param  TargetStatus  Target status
+
+  @retval EFI_NOT_READY       Device is NOT ready.
+  @retval EFI_DEVICE_ERROR 
+  @retval EFI_SUCCESS
+
+**/
+EFI_STATUS
+CheckTargetStatus (
+  IN  UINT8   TargetStatus
+  )
+{
+  switch (TargetStatus) {
+  case EFI_EXT_SCSI_STATUS_TARGET_GOOD:
+  case EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION:
+  case EFI_EXT_SCSI_STATUS_TARGET_CONDITION_MET:
+    return EFI_SUCCESS;
+
+  case EFI_EXT_SCSI_STATUS_TARGET_INTERMEDIATE:
+  case EFI_EXT_SCSI_STATUS_TARGET_INTERMEDIATE_CONDITION_MET:
+  case EFI_EXT_SCSI_STATUS_TARGET_BUSY:
+  case EFI_EXT_SCSI_STATUS_TARGET_TASK_SET_FULL:
+    return EFI_NOT_READY;
+
+  case EFI_EXT_SCSI_STATUS_TARGET_RESERVATION_CONFLICT:
+    return EFI_DEVICE_ERROR;
+
+  default:
+    return EFI_SUCCESS;
+  }
+}
+
+
+/**
+  Retrieve all sense keys from the device.
+
+  When encountering error during the process, if retrieve sense keys before
+  error encountered, it returns the sense keys with return status set to EFI_SUCCESS,
+  and NeedRetry set to FALSE; otherwize, return the proper return status.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates if need a retry
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The number of sense key
+  @param  AskResetIfError    The flag indicates if need reset when error occurs
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to request sense key
+
+**/
+EFI_STATUS
+ScsiDiskRequestSenseKeys (
+  IN  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+      OUT  BOOLEAN                 *NeedRetry,
+      OUT  EFI_SCSI_SENSE_DATA     **SenseDataArray,
+      OUT  UINTN                   *NumberOfSenseKeys,
+  IN       BOOLEAN                 AskResetIfError
+  )
+{
+  EFI_SCSI_SENSE_DATA *PtrSenseData;
+  UINT8               SenseDataLength;
+  BOOLEAN             SenseReq;
+  EFI_STATUS          Status;
+  EFI_STATUS          FallStatus;
+  UINT8               HostAdapterStatus;
+  UINT8               TargetStatus;
+
+  FallStatus      = EFI_SUCCESS;
+  SenseDataLength = (UINT8) sizeof (EFI_SCSI_SENSE_DATA);
+
+  ZeroMem (
+    ScsiDiskDevice->SenseData,
+    sizeof (EFI_SCSI_SENSE_DATA) * (ScsiDiskDevice->SenseDataNumber)
+    );
+
+  *NumberOfSenseKeys  = 0;
+  *SenseDataArray     = ScsiDiskDevice->SenseData;
+  Status              = EFI_SUCCESS;
+  PtrSenseData        = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_SENSE_DATA));
+  if (PtrSenseData == NULL) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  for (SenseReq = TRUE; SenseReq;) {
+    ZeroMem (PtrSenseData, sizeof (EFI_SCSI_SENSE_DATA));
+    Status = ScsiRequestSenseCommand (
+              ScsiDiskDevice->ScsiIo,
+              SCSI_DISK_TIMEOUT,
+              PtrSenseData,
+              &SenseDataLength,
+              &HostAdapterStatus,
+              &TargetStatus
+              );
+     if ((Status == EFI_SUCCESS) || (Status == EFI_WARN_BUFFER_TOO_SMALL)) {
+        FallStatus = EFI_SUCCESS;
+  
+     } else if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+       *NeedRetry  = TRUE;
+       FallStatus  = EFI_DEVICE_ERROR;
+ 
+     } else if ((Status == EFI_INVALID_PARAMETER) || (Status == EFI_UNSUPPORTED)) {
+       *NeedRetry  = FALSE;
+       FallStatus  = EFI_DEVICE_ERROR;
+ 
+     } else if (Status == EFI_DEVICE_ERROR) {
+        if (AskResetIfError) {
+          ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+        }
+  
+        FallStatus = EFI_DEVICE_ERROR;
+    }
+
+    if (EFI_ERROR (FallStatus)) {
+      if (*NumberOfSenseKeys != 0) {
+        *NeedRetry = FALSE;
+        Status = EFI_SUCCESS;
+        goto EXIT;
+      } else {
+        Status = EFI_DEVICE_ERROR;
+        goto EXIT;
+      }
+    }
+
+    CopyMem (ScsiDiskDevice->SenseData + *NumberOfSenseKeys, PtrSenseData, SenseDataLength);
+    (*NumberOfSenseKeys) += 1;
+
+    //
+    // no more sense key or number of sense keys exceeds predefined,
+    // skip the loop.
+    //
+    if ((PtrSenseData->Sense_Key == EFI_SCSI_SK_NO_SENSE) || 
+        (*NumberOfSenseKeys == ScsiDiskDevice->SenseDataNumber)) {
+      SenseReq = FALSE;
+    }
+  }
+
+EXIT:
+  FreeAlignedBuffer (PtrSenseData, sizeof (EFI_SCSI_SENSE_DATA));
+  return Status;
+}
+
+
+/**
+  Get information from media read capacity command.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Capacity10      The pointer of EFI_SCSI_DISK_CAPACITY_DATA
+  @param  Capacity16      The pointer of EFI_SCSI_DISK_CAPACITY_DATA16
+
+**/
+VOID
+GetMediaInfo (
+  IN OUT SCSI_DISK_DEV                  *ScsiDiskDevice,
+  IN     EFI_SCSI_DISK_CAPACITY_DATA    *Capacity10,
+  IN     EFI_SCSI_DISK_CAPACITY_DATA16  *Capacity16
+  )
+{
+  UINT8       *Ptr;
+
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    ScsiDiskDevice->BlkIo.Media->LastBlock =  (Capacity10->LastLba3 << 24) |
+                                              (Capacity10->LastLba2 << 16) |
+                                              (Capacity10->LastLba1 << 8)  |
+                                               Capacity10->LastLba0;
+  
+    ScsiDiskDevice->BlkIo.Media->BlockSize = (Capacity10->BlockSize3 << 24) |
+                                             (Capacity10->BlockSize2 << 16) | 
+                                             (Capacity10->BlockSize1 << 8)  |
+                                              Capacity10->BlockSize0;
+    ScsiDiskDevice->BlkIo.Media->LowestAlignedLba               = 0;
+    ScsiDiskDevice->BlkIo.Media->LogicalBlocksPerPhysicalBlock  = 0;
+  } else {
+    Ptr = (UINT8*)&ScsiDiskDevice->BlkIo.Media->LastBlock;
+    *Ptr++ = Capacity16->LastLba0;
+    *Ptr++ = Capacity16->LastLba1;
+    *Ptr++ = Capacity16->LastLba2;
+    *Ptr++ = Capacity16->LastLba3;
+    *Ptr++ = Capacity16->LastLba4;
+    *Ptr++ = Capacity16->LastLba5;
+    *Ptr++ = Capacity16->LastLba6;
+    *Ptr   = Capacity16->LastLba7;
+
+    ScsiDiskDevice->BlkIo.Media->BlockSize = (Capacity16->BlockSize3 << 24) |
+                                             (Capacity16->BlockSize2 << 16) | 
+                                             (Capacity16->BlockSize1 << 8)  |
+                                              Capacity16->BlockSize0;
+
+    ScsiDiskDevice->BlkIo.Media->LowestAlignedLba = (Capacity16->LowestAlignLogic2 << 8) |
+                                                     Capacity16->LowestAlignLogic1;
+    ScsiDiskDevice->BlkIo.Media->LogicalBlocksPerPhysicalBlock  = (1 << Capacity16->LogicPerPhysical);
+  }
+
+  ScsiDiskDevice->BlkIo.Media->MediaPresent = TRUE;
+}
+
+/**
+  Parse Inquiry data.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+
+**/
+VOID
+ParseInquiryData (
+  IN OUT SCSI_DISK_DEV   *ScsiDiskDevice
+  )
+{
+  ScsiDiskDevice->FixedDevice               = (BOOLEAN) ((ScsiDiskDevice->InquiryData.Rmb == 1) ? 0 : 1);
+  ScsiDiskDevice->BlkIoMedia.RemovableMedia = (BOOLEAN) (!ScsiDiskDevice->FixedDevice);
+}
+
+/**
+  Read sector from SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Buffer          The buffer to fill in the read out data
+  @param  Lba             Logic block address
+  @param  NumberOfBlocks  The number of blocks to read
+
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskReadSectors (
+  IN   SCSI_DISK_DEV     *ScsiDiskDevice,
+  OUT  VOID              *Buffer,
+  IN   EFI_LBA           Lba,
+  IN   UINTN             NumberOfBlocks
+  )
+{
+  UINTN               BlocksRemaining;
+  UINT8               *PtrBuffer;
+  UINT32              BlockSize;
+  UINT32              ByteCount;
+  UINT32              MaxBlock;
+  UINT32              SectorCount;
+  UINT64              Timeout;
+  EFI_STATUS          Status;
+  UINT8               Index;
+  UINT8               MaxRetry;
+  BOOLEAN             NeedRetry;
+
+  Status            = EFI_SUCCESS;
+
+  BlocksRemaining   = NumberOfBlocks;
+  BlockSize         = ScsiDiskDevice->BlkIo.Media->BlockSize;
+  
+  //
+  // limit the data bytes that can be transferred by one Read(10) or Read(16) Command
+  //
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    MaxBlock         = 0xFFFF;
+  } else {
+    MaxBlock         = 0xFFFFFFFF;
+  }
+
+  PtrBuffer = Buffer;
+
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        SectorCount = (UINT16) BlocksRemaining;
+      } else {
+        SectorCount = (UINT32) BlocksRemaining;
+      }
+    } else {
+      SectorCount = MaxBlock;
+    }
+
+    ByteCount = SectorCount * BlockSize;
+    //
+    // |------------------------|-----------------|------------------|-----------------|
+    // |   ATA Transfer Mode    |  Transfer Rate  |  SCSI Interface  |  Transfer Rate  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 0       |  3.3Mbytes/sec  |     SCSI-1       |    5Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 1       |  5.2Mbytes/sec  |    Fast SCSI     |   10Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 2       |  8.3Mbytes/sec  |  Fast-Wide SCSI  |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 3       | 11.1Mbytes/sec  |    Ultra SCSI    |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 4       | 16.6Mbytes/sec  |  Ultra Wide SCSI |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 0 |  2.1Mbytes/sec  |    Ultra2 SCSI   |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 1 |  4.2Mbytes/sec  | Ultra2 Wide SCSI |   80Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 2 |  8.4Mbytes/sec  |    Ultra3 SCSI   |  160Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |  Ultra-320 SCSI  |  320Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |  Ultra-640 SCSI  |  640Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    //
+    // As ScsiDisk and ScsiBus driver are used to manage SCSI or ATAPI devices, we have to use
+    // the lowest transfer rate to calculate the possible maximum timeout value for each operation.
+    // From the above table, we could know 2.1Mbytes per second is lowest one.
+    // The timout value is rounded up to nearest integar and here an additional 30s is added
+    // to follow ATA spec in which it mentioned that the device may take up to 30s to respond
+    // commands in the Standby/Idle mode.
+    //
+    Timeout   = EFI_TIMER_PERIOD_SECONDS (ByteCount / 2100000 + 31);
+
+    MaxRetry  = 2;
+    for (Index = 0; Index < MaxRetry; Index++) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        Status = ScsiDiskRead10 (
+                  ScsiDiskDevice,
+                  &NeedRetry,
+                  Timeout,
+                  PtrBuffer,
+                  &ByteCount,
+                  (UINT32) Lba,
+                  SectorCount
+                  );
+      } else {
+        Status = ScsiDiskRead16 (
+                  ScsiDiskDevice,
+                  &NeedRetry,
+                  Timeout,
+                  PtrBuffer,
+                  &ByteCount,
+                  Lba,
+                  SectorCount
+                  );
+      }
+      if (!EFI_ERROR (Status)) {
+        break;
+      }
+
+      if (!NeedRetry) {
+        return EFI_DEVICE_ERROR;
+      }
+
+    }
+
+    if ((Index == MaxRetry) && (Status != EFI_SUCCESS)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    //
+    // actual transferred sectors
+    //
+    SectorCount = ByteCount / BlockSize;
+
+    Lba += SectorCount;
+    PtrBuffer = PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Write sector to SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Buffer          The buffer of data to be written into SCSI Disk
+  @param  Lba             Logic block address
+  @param  NumberOfBlocks  The number of blocks to read
+
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskWriteSectors (
+  IN  SCSI_DISK_DEV     *ScsiDiskDevice,
+  IN  VOID              *Buffer,
+  IN  EFI_LBA           Lba,
+  IN  UINTN             NumberOfBlocks
+  )
+{
+  UINTN               BlocksRemaining;
+  UINT8               *PtrBuffer;
+  UINT32              BlockSize;
+  UINT32              ByteCount;
+  UINT32              MaxBlock;
+  UINT32              SectorCount;
+  UINT64              Timeout;
+  EFI_STATUS          Status;
+  UINT8               Index;
+  UINT8               MaxRetry;
+  BOOLEAN             NeedRetry;
+
+  Status            = EFI_SUCCESS;
+
+  BlocksRemaining   = NumberOfBlocks;
+  BlockSize         = ScsiDiskDevice->BlkIo.Media->BlockSize;
+
+  //
+  // limit the data bytes that can be transferred by one Read(10) or Read(16) Command
+  //
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    MaxBlock         = 0xFFFF;
+  } else {
+    MaxBlock         = 0xFFFFFFFF;
+  }
+
+  PtrBuffer = Buffer;
+
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        SectorCount = (UINT16) BlocksRemaining;
+      } else {
+        SectorCount = (UINT32) BlocksRemaining;
+      }
+    } else {
+      SectorCount = MaxBlock;
+    }
+
+    ByteCount = SectorCount * BlockSize;
+    //
+    // |------------------------|-----------------|------------------|-----------------|
+    // |   ATA Transfer Mode    |  Transfer Rate  |  SCSI Interface  |  Transfer Rate  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 0       |  3.3Mbytes/sec  |     SCSI-1       |    5Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 1       |  5.2Mbytes/sec  |    Fast SCSI     |   10Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 2       |  8.3Mbytes/sec  |  Fast-Wide SCSI  |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 3       | 11.1Mbytes/sec  |    Ultra SCSI    |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 4       | 16.6Mbytes/sec  |  Ultra Wide SCSI |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 0 |  2.1Mbytes/sec  |    Ultra2 SCSI   |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 1 |  4.2Mbytes/sec  | Ultra2 Wide SCSI |   80Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 2 |  8.4Mbytes/sec  |    Ultra3 SCSI   |  160Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |  Ultra-320 SCSI  |  320Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |  Ultra-640 SCSI  |  640Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    //
+    // As ScsiDisk and ScsiBus driver are used to manage SCSI or ATAPI devices, we have to use
+    // the lowest transfer rate to calculate the possible maximum timeout value for each operation.
+    // From the above table, we could know 2.1Mbytes per second is lowest one.
+    // The timout value is rounded up to nearest integar and here an additional 30s is added
+    // to follow ATA spec in which it mentioned that the device may take up to 30s to respond
+    // commands in the Standby/Idle mode.
+    //
+    Timeout   = EFI_TIMER_PERIOD_SECONDS (ByteCount / 2100000 + 31);
+    MaxRetry  = 2;
+    for (Index = 0; Index < MaxRetry; Index++) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        Status = ScsiDiskWrite10 (
+                  ScsiDiskDevice,
+                  &NeedRetry,
+                  Timeout,
+                  PtrBuffer,
+                  &ByteCount,
+                  (UINT32) Lba,
+                  SectorCount
+                  );
+      } else {
+        Status = ScsiDiskWrite16 (
+                  ScsiDiskDevice,
+                  &NeedRetry,
+                  Timeout,
+                  PtrBuffer,
+                  &ByteCount,
+                  Lba,
+                  SectorCount
+                  );         
+        }
+      if (!EFI_ERROR (Status)) {
+        break;
+      }
+
+      if (!NeedRetry) {
+        return EFI_DEVICE_ERROR;
+      }
+    }
+
+    if ((Index == MaxRetry) && (Status != EFI_SUCCESS)) {
+      return EFI_DEVICE_ERROR;
+    }
+    //
+    // actual transferred sectors
+    //
+    SectorCount = ByteCount / BlockSize;
+
+    Lba += SectorCount;
+    PtrBuffer = PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Asynchronously read sector from SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  Buffer          The buffer to fill in the read out data.
+  @param  Lba             Logic block address.
+  @param  NumberOfBlocks  The number of blocks to read.
+  @param  Token           A pointer to the token associated with the
+                          non-blocking read request.
+
+  @retval EFI_INVALID_PARAMETER  Token is NULL or Token->Event is NULL.
+  @retval EFI_DEVICE_ERROR       Indicates a device error.
+  @retval EFI_SUCCESS            Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskAsyncReadSectors (
+  IN   SCSI_DISK_DEV         *ScsiDiskDevice,
+  OUT  VOID                  *Buffer,
+  IN   EFI_LBA               Lba,
+  IN   UINTN                 NumberOfBlocks,
+  IN   EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  UINTN                 BlocksRemaining;
+  UINT8                 *PtrBuffer;
+  UINT32                BlockSize;
+  UINT32                ByteCount;
+  UINT32                MaxBlock;
+  UINT32                SectorCount;
+  UINT64                Timeout;
+  SCSI_BLKIO2_REQUEST   *BlkIo2Req;
+  EFI_STATUS            Status;
+  EFI_TPL               OldTpl;
+
+  if ((Token == NULL) || (Token->Event == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BlkIo2Req = AllocateZeroPool (sizeof (SCSI_BLKIO2_REQUEST));
+  if (BlkIo2Req == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  BlkIo2Req->Token  = Token;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&ScsiDiskDevice->BlkIo2Queue, &BlkIo2Req->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  InitializeListHead (&BlkIo2Req->ScsiRWQueue);
+
+  Status            = EFI_SUCCESS;
+
+  BlocksRemaining   = NumberOfBlocks;
+  BlockSize         = ScsiDiskDevice->BlkIo.Media->BlockSize;
+
+  //
+  // Limit the data bytes that can be transferred by one Read(10) or Read(16)
+  // Command
+  //
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    MaxBlock         = 0xFFFF;
+  } else {
+    MaxBlock         = 0xFFFFFFFF;
+  }
+
+  PtrBuffer = Buffer;
+
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        SectorCount = (UINT16) BlocksRemaining;
+      } else {
+        SectorCount = (UINT32) BlocksRemaining;
+      }
+    } else {
+      SectorCount = MaxBlock;
+    }
+
+    ByteCount = SectorCount * BlockSize;
+    //
+    // |------------------------|-----------------|------------------|-----------------|
+    // |   ATA Transfer Mode    |  Transfer Rate  |  SCSI Interface  |  Transfer Rate  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 0       |  3.3Mbytes/sec  |     SCSI-1       |    5Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 1       |  5.2Mbytes/sec  |    Fast SCSI     |   10Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 2       |  8.3Mbytes/sec  |  Fast-Wide SCSI  |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 3       | 11.1Mbytes/sec  |    Ultra SCSI    |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 4       | 16.6Mbytes/sec  |  Ultra Wide SCSI |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 0 |  2.1Mbytes/sec  |    Ultra2 SCSI   |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 1 |  4.2Mbytes/sec  | Ultra2 Wide SCSI |   80Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 2 |  8.4Mbytes/sec  |    Ultra3 SCSI   |  160Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |  Ultra-320 SCSI  |  320Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |  Ultra-640 SCSI  |  640Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    //
+    // As ScsiDisk and ScsiBus driver are used to manage SCSI or ATAPI devices,
+    // we have to use the lowest transfer rate to calculate the possible
+    // maximum timeout value for each operation.
+    // From the above table, we could know 2.1Mbytes per second is lowest one.
+    // The timout value is rounded up to nearest integar and here an additional
+    // 30s is added to follow ATA spec in which it mentioned that the device
+    // may take up to 30s to respond commands in the Standby/Idle mode.
+    //
+    Timeout   = EFI_TIMER_PERIOD_SECONDS (ByteCount / 2100000 + 31);
+
+    if (!ScsiDiskDevice->Cdb16Byte) {
+      Status = ScsiDiskAsyncRead10 (
+                 ScsiDiskDevice,
+                 Timeout,
+                 0,
+                 PtrBuffer,
+                 ByteCount,
+                 (UINT32) Lba,
+                 SectorCount,
+                 BlkIo2Req,
+                 Token
+                 );
+    } else {
+      Status = ScsiDiskAsyncRead16 (
+                 ScsiDiskDevice,
+                 Timeout,
+                 0,
+                 PtrBuffer,
+                 ByteCount,
+                 Lba,
+                 SectorCount,
+                 BlkIo2Req,
+                 Token
+                 );
+    }
+    if (EFI_ERROR (Status)) {
+      //
+      // Some devices will return EFI_DEVICE_ERROR or EFI_TIMEOUT when the data
+      // length of a SCSI I/O command is too large.
+      // In this case, we retry sending the SCSI command with a data length
+      // half of its previous value.
+      //
+      if ((Status == EFI_DEVICE_ERROR) || (Status == EFI_TIMEOUT)) {
+        if ((MaxBlock > 1) && (SectorCount > 1)) {
+          MaxBlock = MIN (MaxBlock, SectorCount) >> 1;
+          continue;
+        }
+      }
+
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+      if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {
+        //
+        // Free the SCSI_BLKIO2_REQUEST structure only when there is no other
+        // SCSI sub-task running. Otherwise, it will be freed in the callback
+        // function ScsiDiskNotify().
+        //
+        RemoveEntryList (&BlkIo2Req->Link);
+        FreePool (BlkIo2Req);
+        BlkIo2Req = NULL;
+        gBS->RestoreTPL (OldTpl);
+
+        //
+        // It is safe to return error status to the caller, since there is no
+        // previous SCSI sub-task executing.
+        //
+        Status = EFI_DEVICE_ERROR;
+        goto Done;
+      } else {
+        gBS->RestoreTPL (OldTpl);
+
+        //
+        // There are previous SCSI commands still running, EFI_SUCCESS should
+        // be returned to make sure that the caller does not free resources
+        // still using by these SCSI commands.
+        //
+        Status = EFI_SUCCESS;
+        goto Done;
+      }
+    }
+
+    //
+    // Sectors submitted for transfer
+    //
+    SectorCount = ByteCount / BlockSize;
+
+    Lba += SectorCount;
+    PtrBuffer = PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  Status = EFI_SUCCESS;
+
+Done:
+  if (BlkIo2Req != NULL) {
+    BlkIo2Req->LastScsiRW = TRUE;
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {
+      RemoveEntryList (&BlkIo2Req->Link);
+      FreePool (BlkIo2Req);
+      BlkIo2Req = NULL;
+
+      gBS->SignalEvent (Token->Event);
+    }
+    gBS->RestoreTPL (OldTpl);
+  }
+
+  return Status;
+}
+
+/**
+  Asynchronously write sector to SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  Buffer          The buffer of data to be written into SCSI Disk.
+  @param  Lba             Logic block address.
+  @param  NumberOfBlocks  The number of blocks to read.
+  @param  Token           A pointer to the token associated with the
+                          non-blocking read request.
+
+  @retval EFI_INVALID_PARAMETER  Token is NULL or Token->Event is NULL
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWriteSectors (
+  IN  SCSI_DISK_DEV          *ScsiDiskDevice,
+  IN  VOID                   *Buffer,
+  IN  EFI_LBA                Lba,
+  IN  UINTN                  NumberOfBlocks,
+  IN  EFI_BLOCK_IO2_TOKEN    *Token
+  )
+{
+  UINTN                 BlocksRemaining;
+  UINT8                 *PtrBuffer;
+  UINT32                BlockSize;
+  UINT32                ByteCount;
+  UINT32                MaxBlock;
+  UINT32                SectorCount;
+  UINT64                Timeout;
+  SCSI_BLKIO2_REQUEST   *BlkIo2Req;
+  EFI_STATUS            Status;
+  EFI_TPL               OldTpl;
+
+  if ((Token == NULL) || (Token->Event == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BlkIo2Req = AllocateZeroPool (sizeof (SCSI_BLKIO2_REQUEST));
+  if (BlkIo2Req == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  BlkIo2Req->Token  = Token;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&ScsiDiskDevice->BlkIo2Queue, &BlkIo2Req->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  InitializeListHead (&BlkIo2Req->ScsiRWQueue);
+
+  Status            = EFI_SUCCESS;
+
+  BlocksRemaining   = NumberOfBlocks;
+  BlockSize         = ScsiDiskDevice->BlkIo.Media->BlockSize;
+
+  //
+  // Limit the data bytes that can be transferred by one Read(10) or Read(16)
+  // Command
+  //
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    MaxBlock         = 0xFFFF;
+  } else {
+    MaxBlock         = 0xFFFFFFFF;
+  }
+
+  PtrBuffer = Buffer;
+
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        SectorCount = (UINT16) BlocksRemaining;
+      } else {
+        SectorCount = (UINT32) BlocksRemaining;
+      }
+    } else {
+      SectorCount = MaxBlock;
+    }
+
+    ByteCount = SectorCount * BlockSize;
+    //
+    // |------------------------|-----------------|------------------|-----------------|
+    // |   ATA Transfer Mode    |  Transfer Rate  |  SCSI Interface  |  Transfer Rate  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 0       |  3.3Mbytes/sec  |     SCSI-1       |    5Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 1       |  5.2Mbytes/sec  |    Fast SCSI     |   10Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 2       |  8.3Mbytes/sec  |  Fast-Wide SCSI  |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 3       | 11.1Mbytes/sec  |    Ultra SCSI    |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 4       | 16.6Mbytes/sec  |  Ultra Wide SCSI |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 0 |  2.1Mbytes/sec  |    Ultra2 SCSI   |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 1 |  4.2Mbytes/sec  | Ultra2 Wide SCSI |   80Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 2 |  8.4Mbytes/sec  |    Ultra3 SCSI   |  160Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |  Ultra-320 SCSI  |  320Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |  Ultra-640 SCSI  |  640Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    //
+    // As ScsiDisk and ScsiBus driver are used to manage SCSI or ATAPI devices,
+    // we have to use the lowest transfer rate to calculate the possible
+    // maximum timeout value for each operation.
+    // From the above table, we could know 2.1Mbytes per second is lowest one.
+    // The timout value is rounded up to nearest integar and here an additional
+    // 30s is added to follow ATA spec in which it mentioned that the device
+    // may take up to 30s to respond commands in the Standby/Idle mode.
+    //
+    Timeout   = EFI_TIMER_PERIOD_SECONDS (ByteCount / 2100000 + 31);
+
+    if (!ScsiDiskDevice->Cdb16Byte) {
+      Status = ScsiDiskAsyncWrite10 (
+                 ScsiDiskDevice,
+                 Timeout,
+                 0,
+                 PtrBuffer,
+                 ByteCount,
+                 (UINT32) Lba,
+                 SectorCount,
+                 BlkIo2Req,
+                 Token
+                 );
+    } else {
+      Status = ScsiDiskAsyncWrite16 (
+                 ScsiDiskDevice,
+                 Timeout,
+                 0,
+                 PtrBuffer,
+                 ByteCount,
+                 Lba,
+                 SectorCount,
+                 BlkIo2Req,
+                 Token
+                 );
+    }
+    if (EFI_ERROR (Status)) {
+      //
+      // Some devices will return EFI_DEVICE_ERROR or EFI_TIMEOUT when the data
+      // length of a SCSI I/O command is too large.
+      // In this case, we retry sending the SCSI command with a data length
+      // half of its previous value.
+      //
+      if ((Status == EFI_DEVICE_ERROR) || (Status == EFI_TIMEOUT)) {
+        if ((MaxBlock > 1) && (SectorCount > 1)) {
+          MaxBlock = MIN (MaxBlock, SectorCount) >> 1;
+          continue;
+        }
+      }
+
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+      if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {
+        //
+        // Free the SCSI_BLKIO2_REQUEST structure only when there is no other
+        // SCSI sub-task running. Otherwise, it will be freed in the callback
+        // function ScsiDiskNotify().
+        //
+        RemoveEntryList (&BlkIo2Req->Link);
+        FreePool (BlkIo2Req);
+        BlkIo2Req = NULL;
+        gBS->RestoreTPL (OldTpl);
+
+        //
+        // It is safe to return error status to the caller, since there is no
+        // previous SCSI sub-task executing.
+        //
+        Status = EFI_DEVICE_ERROR;
+        goto Done;
+      } else {
+        gBS->RestoreTPL (OldTpl);
+
+        //
+        // There are previous SCSI commands still running, EFI_SUCCESS should
+        // be returned to make sure that the caller does not free resources
+        // still using by these SCSI commands.
+        //
+        Status = EFI_SUCCESS;
+        goto Done;
+      }
+    }
+
+    //
+    // Sectors submitted for transfer
+    //
+    SectorCount = ByteCount / BlockSize;
+
+    Lba += SectorCount;
+    PtrBuffer = PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  Status = EFI_SUCCESS;
+
+Done:
+  if (BlkIo2Req != NULL) {
+    BlkIo2Req->LastScsiRW = TRUE;
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {
+      RemoveEntryList (&BlkIo2Req->Link);
+      FreePool (BlkIo2Req);
+      BlkIo2Req = NULL;
+
+      gBS->SignalEvent (Token->Event);
+    }
+    gBS->RestoreTPL (OldTpl);
+  }
+
+  return Status;
+}
+
+
+/**
+  Submit Read(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to read
+
+  @return  EFI_STATUS is returned by calling ScsiRead10Command().
+**/
+EFI_STATUS
+ScsiDiskRead10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+     OUT UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount
+  )
+{
+  UINT8       SenseDataLength;
+  EFI_STATUS  Status;
+  EFI_STATUS  ReturnStatus;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINTN       Action;
+
+  //
+  // Implement a backoff algorithem to resolve some compatibility issues that
+  // some SCSI targets or ATAPI devices couldn't correctly response reading/writing
+  // big data in a single operation.
+  // This algorithem will at first try to execute original request. If the request fails
+  // with media error sense data or else, it will reduce the transfer length to half and
+  // try again till the operation succeeds or fails with one sector transfer length.
+  //
+BackOff:
+  *NeedRetry          = FALSE;
+  Action              = ACTION_NO_ACTION;
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  ReturnStatus = ScsiRead10Command (
+                   ScsiDiskDevice->ScsiIo,
+                   Timeout,
+                   ScsiDiskDevice->SenseData,
+                   &SenseDataLength,
+                   &HostAdapterStatus,
+                   &TargetStatus,
+                   DataBuffer,
+                   DataLength,
+                   StartLba,
+                   SectorCount
+                   );
+
+  if (ReturnStatus == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if ((ReturnStatus == EFI_INVALID_PARAMETER) || (ReturnStatus == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return ReturnStatus;
+  }
+
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) || (EFI_ERROR (ReturnStatus))) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskRead10: Check Condition happened!\n"));
+    Status = DetectMediaParsingSenseKeys (ScsiDiskDevice, ScsiDiskDevice->SenseData, SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA), &Action);
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer length.
+        //
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      //
+      // Try again with half length if the sense data shows we need to retry.
+      //
+      SectorCount >>= 1;
+      *DataLength = SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      goto BackOff;
+    } else {
+      *NeedRetry = FALSE;
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Submit Write(10) Command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to write
+
+  @return  EFI_STATUS is returned by calling ScsiWrite10Command().
+
+**/
+EFI_STATUS
+ScsiDiskWrite10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+  IN     UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount
+  )
+{
+  EFI_STATUS  Status;
+  EFI_STATUS  ReturnStatus;
+  UINT8       SenseDataLength;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINTN       Action;
+
+  //
+  // Implement a backoff algorithem to resolve some compatibility issues that
+  // some SCSI targets or ATAPI devices couldn't correctly response reading/writing
+  // big data in a single operation.
+  // This algorithem will at first try to execute original request. If the request fails
+  // with media error sense data or else, it will reduce the transfer length to half and
+  // try again till the operation succeeds or fails with one sector transfer length.
+  //
+BackOff:
+  *NeedRetry          = FALSE;
+  Action              = ACTION_NO_ACTION;
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  ReturnStatus = ScsiWrite10Command (
+                   ScsiDiskDevice->ScsiIo,
+                   Timeout,
+                   ScsiDiskDevice->SenseData,
+                   &SenseDataLength,
+                   &HostAdapterStatus,
+                   &TargetStatus,
+                   DataBuffer,
+                   DataLength,
+                   StartLba,
+                   SectorCount
+                   );
+  if (ReturnStatus == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if ((ReturnStatus == EFI_INVALID_PARAMETER) || (ReturnStatus == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return ReturnStatus;
+  }
+
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) || (EFI_ERROR (ReturnStatus))) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskWrite10: Check Condition happened!\n"));
+    Status = DetectMediaParsingSenseKeys (ScsiDiskDevice, ScsiDiskDevice->SenseData, SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA), &Action);
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer length.
+        //
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      //
+      // Try again with half length if the sense data shows we need to retry.
+      //
+      SectorCount >>= 1;
+      *DataLength = SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      goto BackOff;
+    } else {
+      *NeedRetry = FALSE;
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Submit Read(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to read
+
+  @return  EFI_STATUS is returned by calling ScsiRead16Command().
+**/
+EFI_STATUS
+ScsiDiskRead16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+     OUT UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount
+  )
+{
+  UINT8       SenseDataLength;
+  EFI_STATUS  Status;
+  EFI_STATUS  ReturnStatus;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINTN       Action;
+
+  //
+  // Implement a backoff algorithem to resolve some compatibility issues that
+  // some SCSI targets or ATAPI devices couldn't correctly response reading/writing
+  // big data in a single operation.
+  // This algorithem will at first try to execute original request. If the request fails
+  // with media error sense data or else, it will reduce the transfer length to half and
+  // try again till the operation succeeds or fails with one sector transfer length.
+  //
+BackOff:
+  *NeedRetry          = FALSE;
+  Action              = ACTION_NO_ACTION;
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  ReturnStatus = ScsiRead16Command (
+                   ScsiDiskDevice->ScsiIo,
+                   Timeout,
+                   ScsiDiskDevice->SenseData,
+                   &SenseDataLength,
+                   &HostAdapterStatus,
+                   &TargetStatus,
+                   DataBuffer,
+                   DataLength,
+                   StartLba,
+                   SectorCount
+                   );
+  if (ReturnStatus == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if ((ReturnStatus == EFI_INVALID_PARAMETER) || (ReturnStatus == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return ReturnStatus;
+  }
+
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) || (EFI_ERROR (ReturnStatus))) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskRead16: Check Condition happened!\n"));
+    Status = DetectMediaParsingSenseKeys (ScsiDiskDevice, ScsiDiskDevice->SenseData, SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA), &Action);
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer length.
+        //
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      //
+      // Try again with half length if the sense data shows we need to retry.
+      //
+      SectorCount >>= 1;
+      *DataLength = SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      goto BackOff;
+    } else {
+      *NeedRetry = FALSE;
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Submit Write(16) Command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to write
+
+  @return  EFI_STATUS is returned by calling ScsiWrite16Command().
+
+**/
+EFI_STATUS
+ScsiDiskWrite16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+  IN     UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount
+  )
+{
+  EFI_STATUS  Status;
+  EFI_STATUS  ReturnStatus;
+  UINT8       SenseDataLength;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINTN       Action;
+
+  //
+  // Implement a backoff algorithem to resolve some compatibility issues that
+  // some SCSI targets or ATAPI devices couldn't correctly response reading/writing
+  // big data in a single operation.
+  // This algorithem will at first try to execute original request. If the request fails
+  // with media error sense data or else, it will reduce the transfer length to half and
+  // try again till the operation succeeds or fails with one sector transfer length.
+  //
+BackOff:
+  *NeedRetry          = FALSE;
+  Action              = ACTION_NO_ACTION;
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  ReturnStatus = ScsiWrite16Command (
+                   ScsiDiskDevice->ScsiIo,
+                   Timeout,
+                   ScsiDiskDevice->SenseData,
+                   &SenseDataLength,
+                   &HostAdapterStatus,
+                   &TargetStatus,
+                   DataBuffer,
+                   DataLength,
+                   StartLba,
+                   SectorCount
+                   );
+  if (ReturnStatus == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if ((ReturnStatus == EFI_INVALID_PARAMETER) || (ReturnStatus == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return ReturnStatus;
+  }
+
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) || (EFI_ERROR (ReturnStatus))) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskWrite16: Check Condition happened!\n"));
+    Status = DetectMediaParsingSenseKeys (ScsiDiskDevice, ScsiDiskDevice->SenseData, SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA), &Action);
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer length.
+        //
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      //
+      // Try again with half length if the sense data shows we need to retry.
+      //
+      SectorCount >>= 1;
+      *DataLength = SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      goto BackOff;
+    } else {
+      *NeedRetry = FALSE;
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Internal helper notify function in which determine whether retry of a SCSI
+  Read/Write command is needed and signal the event passed from Block I/O(2) if
+  the SCSI I/O operation completes.
+
+  @param  Event    The instance of EFI_EVENT.
+  @param  Context  The parameter passed in.
+
+**/
+VOID
+EFIAPI
+ScsiDiskNotify (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  )
+{
+  EFI_STATUS                       Status;
+  SCSI_ASYNC_RW_REQUEST            *Request;
+  SCSI_DISK_DEV                    *ScsiDiskDevice;
+  EFI_BLOCK_IO2_TOKEN              *Token;
+  UINTN                            Action;
+  UINT32                           OldDataLength;
+  UINT32                           OldSectorCount;
+  UINT8                            MaxRetry;
+
+  gBS->CloseEvent (Event);
+
+  Request         = (SCSI_ASYNC_RW_REQUEST *) Context;
+  ScsiDiskDevice  = Request->ScsiDiskDevice;
+  Token           = Request->BlkIo2Req->Token;
+  OldDataLength   = Request->DataLength;
+  OldSectorCount  = Request->SectorCount;
+  MaxRetry        = 2;
+
+  //
+  // If previous sub-tasks already fails, no need to process this sub-task.
+  //
+  if (Token->TransactionStatus != EFI_SUCCESS) {
+    goto Exit;
+  }
+
+  //
+  // Check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (Request->HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    if (++Request->TimesRetry > MaxRetry) {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    } else {
+      goto Retry;
+    }
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    Token->TransactionStatus = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  Status = CheckTargetStatus (Request->TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    if (++Request->TimesRetry > MaxRetry) {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    } else {
+      goto Retry;
+    }
+  } else if (Status == EFI_DEVICE_ERROR) {
+    Token->TransactionStatus = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Request->TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskNotify: Check Condition happened!\n"));
+
+    Status = DetectMediaParsingSenseKeys (
+               ScsiDiskDevice,
+               Request->SenseData,
+               Request->SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA),
+               &Action
+               );
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      if (++Request->TimesRetry > MaxRetry) {
+        Token->TransactionStatus = EFI_DEVICE_ERROR;
+        goto Exit;
+      } else {
+        goto Retry;
+      }
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (Request->SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer
+        // length.
+        //
+        Token->TransactionStatus = EFI_DEVICE_ERROR;
+        goto Exit;
+      }
+      //
+      // Try again with two half length request if the sense data shows we need
+      // to retry.
+      //
+      Request->SectorCount >>= 1;
+      Request->DataLength = Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      Request->TimesRetry  = 0;
+
+      goto Retry;
+    } else {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+  }
+
+  //
+  // This sub-task succeeds, no need to retry.
+  //
+  goto Exit;
+
+Retry:
+  if (Request->InBuffer != NULL) {
+    //
+    // SCSI read command
+    //
+    if (!ScsiDiskDevice->Cdb16Byte) {
+      Status = ScsiDiskAsyncRead10 (
+                 ScsiDiskDevice,
+                 Request->Timeout,
+                 Request->TimesRetry,
+                 Request->InBuffer,
+                 Request->DataLength,
+                 (UINT32) Request->StartLba,
+                 Request->SectorCount,
+                 Request->BlkIo2Req,
+                 Token
+                 );
+    } else {
+      Status = ScsiDiskAsyncRead16 (
+                 ScsiDiskDevice,
+                 Request->Timeout,
+                 Request->TimesRetry,
+                 Request->InBuffer,
+                 Request->DataLength,
+                 Request->StartLba,
+                 Request->SectorCount,
+                 Request->BlkIo2Req,
+                 Token
+                 );
+    }
+
+    if (EFI_ERROR (Status)) {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    } else if (OldSectorCount != Request->SectorCount) {
+      //
+      // Original sub-task will be split into two new sub-tasks with smaller
+      // DataLength
+      //
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        Status = ScsiDiskAsyncRead10 (
+                   ScsiDiskDevice,
+                   Request->Timeout,
+                   0,
+                   Request->InBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,
+                   OldDataLength - Request->DataLength,
+                   (UINT32) Request->StartLba + Request->SectorCount,
+                   OldSectorCount - Request->SectorCount,
+                   Request->BlkIo2Req,
+                   Token
+                   );
+      } else {
+        Status = ScsiDiskAsyncRead16 (
+                   ScsiDiskDevice,
+                   Request->Timeout,
+                   0,
+                   Request->InBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,
+                   OldDataLength - Request->DataLength,
+                   Request->StartLba + Request->SectorCount,
+                   OldSectorCount - Request->SectorCount,
+                   Request->BlkIo2Req,
+                   Token
+                   );
+      }
+      if (EFI_ERROR (Status)) {
+        Token->TransactionStatus = EFI_DEVICE_ERROR;
+        goto Exit;
+      }
+    }
+  } else {
+    //
+    // SCSI write command
+    //
+    if (!ScsiDiskDevice->Cdb16Byte) {
+      Status = ScsiDiskAsyncWrite10 (
+                 ScsiDiskDevice,
+                 Request->Timeout,
+                 Request->TimesRetry,
+                 Request->OutBuffer,
+                 Request->DataLength,
+                 (UINT32) Request->StartLba,
+                 Request->SectorCount,
+                 Request->BlkIo2Req,
+                 Token
+                 );
+    } else {
+      Status = ScsiDiskAsyncWrite16 (
+                 ScsiDiskDevice,
+                 Request->Timeout,
+                 Request->TimesRetry,
+                 Request->OutBuffer,
+                 Request->DataLength,
+                 Request->StartLba,
+                 Request->SectorCount,
+                 Request->BlkIo2Req,
+                 Token
+                 );
+    }
+
+    if (EFI_ERROR (Status)) {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    } else if (OldSectorCount != Request->SectorCount) {
+      //
+      // Original sub-task will be split into two new sub-tasks with smaller
+      // DataLength
+      //
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        Status = ScsiDiskAsyncWrite10 (
+                   ScsiDiskDevice,
+                   Request->Timeout,
+                   0,
+                   Request->OutBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,
+                   OldDataLength - Request->DataLength,
+                   (UINT32) Request->StartLba + Request->SectorCount,
+                   OldSectorCount - Request->SectorCount,
+                   Request->BlkIo2Req,
+                   Token
+                   );
+      } else {
+        Status = ScsiDiskAsyncWrite16 (
+                   ScsiDiskDevice,
+                   Request->Timeout,
+                   0,
+                   Request->OutBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,
+                   OldDataLength - Request->DataLength,
+                   Request->StartLba + Request->SectorCount,
+                   OldSectorCount - Request->SectorCount,
+                   Request->BlkIo2Req,
+                   Token
+                   );
+      }
+      if (EFI_ERROR (Status)) {
+        Token->TransactionStatus = EFI_DEVICE_ERROR;
+        goto Exit;
+      }
+    }
+  }
+
+Exit:
+  RemoveEntryList (&Request->Link);
+  if ((IsListEmpty (&Request->BlkIo2Req->ScsiRWQueue)) &&
+      (Request->BlkIo2Req->LastScsiRW)) {
+    //
+    // The last SCSI R/W command of a BlockIo2 request completes
+    //
+    RemoveEntryList (&Request->BlkIo2Req->Link);
+    FreePool (Request->BlkIo2Req);  // Should be freed only once
+    gBS->SignalEvent (Token->Event);
+  }
+
+  FreePool (Request->SenseData);
+  FreePool (Request);
+}
+
+
+/**
+  Submit Async Read(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer to fill with the read out data.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to read.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiRead10CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncRead10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+     OUT UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  EFI_STATUS                   Status;
+  SCSI_ASYNC_RW_REQUEST        *Request;
+  EFI_EVENT                    AsyncIoEvent;
+  EFI_TPL                      OldTpl;
+
+  AsyncIoEvent = NULL;
+
+  Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));
+  if (Request == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));
+  Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);
+  if (Request->SenseData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  Request->ScsiDiskDevice  = ScsiDiskDevice;
+  Request->Timeout         = Timeout;
+  Request->TimesRetry      = TimesRetry;
+  Request->InBuffer        = DataBuffer;
+  Request->DataLength      = DataLength;
+  Request->StartLba        = StartLba;
+  Request->SectorCount     = SectorCount;
+  Request->BlkIo2Req       = BlkIo2Req;
+
+  //
+  // Create Event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ScsiDiskNotify,
+                  Request,
+                  &AsyncIoEvent
+                  );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  Status = ScsiRead10CommandEx (
+             ScsiDiskDevice->ScsiIo,
+             Request->Timeout,
+             Request->SenseData,
+             &Request->SenseDataLength,
+             &Request->HostAdapterStatus,
+             &Request->TargetStatus,
+             Request->InBuffer,
+             &Request->DataLength,
+             (UINT32) Request->StartLba,
+             Request->SectorCount,
+             AsyncIoEvent
+             );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (AsyncIoEvent != NULL) {
+    gBS->CloseEvent (AsyncIoEvent);
+  }
+
+  if (Request != NULL) {
+    if (Request->SenseData != NULL) {
+      FreePool (Request->SenseData);
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    RemoveEntryList (&Request->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    FreePool (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  Submit Async Write(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer contains the data to write.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to write.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiWrite10CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWrite10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+  IN     UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  EFI_STATUS                   Status;
+  SCSI_ASYNC_RW_REQUEST        *Request;
+  EFI_EVENT                    AsyncIoEvent;
+  EFI_TPL                      OldTpl;
+
+  AsyncIoEvent = NULL;
+
+  Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));
+  if (Request == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));
+  Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);
+  if (Request->SenseData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  Request->ScsiDiskDevice  = ScsiDiskDevice;
+  Request->Timeout         = Timeout;
+  Request->TimesRetry      = TimesRetry;
+  Request->OutBuffer       = DataBuffer;
+  Request->DataLength      = DataLength;
+  Request->StartLba        = StartLba;
+  Request->SectorCount     = SectorCount;
+  Request->BlkIo2Req       = BlkIo2Req;
+
+  //
+  // Create Event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ScsiDiskNotify,
+                  Request,
+                  &AsyncIoEvent
+                  );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  Status = ScsiWrite10CommandEx (
+             ScsiDiskDevice->ScsiIo,
+             Request->Timeout,
+             Request->SenseData,
+             &Request->SenseDataLength,
+             &Request->HostAdapterStatus,
+             &Request->TargetStatus,
+             Request->OutBuffer,
+             &Request->DataLength,
+             (UINT32) Request->StartLba,
+             Request->SectorCount,
+             AsyncIoEvent
+             );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (AsyncIoEvent != NULL) {
+    gBS->CloseEvent (AsyncIoEvent);
+  }
+
+  if (Request != NULL) {
+    if (Request->SenseData != NULL) {
+      FreePool (Request->SenseData);
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    RemoveEntryList (&Request->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    FreePool (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  Submit Async Read(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer to fill with the read out data.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to read.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiRead16CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncRead16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+     OUT UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  EFI_STATUS                   Status;
+  SCSI_ASYNC_RW_REQUEST        *Request;
+  EFI_EVENT                    AsyncIoEvent;
+  EFI_TPL                      OldTpl;
+
+  AsyncIoEvent = NULL;
+
+  Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));
+  if (Request == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));
+  Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);
+  if (Request->SenseData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  Request->ScsiDiskDevice  = ScsiDiskDevice;
+  Request->Timeout         = Timeout;
+  Request->TimesRetry      = TimesRetry;
+  Request->InBuffer        = DataBuffer;
+  Request->DataLength      = DataLength;
+  Request->StartLba        = StartLba;
+  Request->SectorCount     = SectorCount;
+  Request->BlkIo2Req       = BlkIo2Req;
+
+  //
+  // Create Event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ScsiDiskNotify,
+                  Request,
+                  &AsyncIoEvent
+                  );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  Status = ScsiRead16CommandEx (
+             ScsiDiskDevice->ScsiIo,
+             Request->Timeout,
+             Request->SenseData,
+             &Request->SenseDataLength,
+             &Request->HostAdapterStatus,
+             &Request->TargetStatus,
+             Request->InBuffer,
+             &Request->DataLength,
+             Request->StartLba,
+             Request->SectorCount,
+             AsyncIoEvent
+             );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (AsyncIoEvent != NULL) {
+    gBS->CloseEvent (AsyncIoEvent);
+  }
+
+  if (Request != NULL) {
+    if (Request->SenseData != NULL) {
+      FreePool (Request->SenseData);
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    RemoveEntryList (&Request->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    FreePool (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  Submit Async Write(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer contains the data to write.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to write.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiWrite16CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWrite16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+  IN     UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  EFI_STATUS                   Status;
+  SCSI_ASYNC_RW_REQUEST        *Request;
+  EFI_EVENT                    AsyncIoEvent;
+  EFI_TPL                      OldTpl;
+
+  AsyncIoEvent = NULL;
+
+  Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));
+  if (Request == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));
+  Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);
+  if (Request->SenseData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  Request->ScsiDiskDevice  = ScsiDiskDevice;
+  Request->Timeout         = Timeout;
+  Request->TimesRetry      = TimesRetry;
+  Request->OutBuffer       = DataBuffer;
+  Request->DataLength      = DataLength;
+  Request->StartLba        = StartLba;
+  Request->SectorCount     = SectorCount;
+  Request->BlkIo2Req       = BlkIo2Req;
+
+  //
+  // Create Event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ScsiDiskNotify,
+                  Request,
+                  &AsyncIoEvent
+                  );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  Status = ScsiWrite16CommandEx (
+             ScsiDiskDevice->ScsiIo,
+             Request->Timeout,
+             Request->SenseData,
+             &Request->SenseDataLength,
+             &Request->HostAdapterStatus,
+             &Request->TargetStatus,
+             Request->OutBuffer,
+             &Request->DataLength,
+             Request->StartLba,
+             Request->SectorCount,
+             AsyncIoEvent
+             );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (AsyncIoEvent != NULL) {
+    gBS->CloseEvent (AsyncIoEvent);
+  }
+
+  if (Request != NULL) {
+    if (Request->SenseData != NULL) {
+      FreePool (Request->SenseData);
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    RemoveEntryList (&Request->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    FreePool (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  Check sense key to find if media presents.
+
+  @param  SenseData   The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts The number of sense key
+
+  @retval TRUE    NOT any media
+  @retval FALSE   Media presents
+**/
+BOOLEAN
+ScsiDiskIsNoMedia (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsNoMedia;
+
+  IsNoMedia = FALSE;
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    //
+    // Sense Key is EFI_SCSI_SK_NOT_READY (0x2),
+    // Additional Sense Code is ASC_NO_MEDIA (0x3A)
+    //
+    if ((SensePtr->Sense_Key == EFI_SCSI_SK_NOT_READY) &&
+        (SensePtr->Addnl_Sense_Code == EFI_SCSI_ASC_NO_MEDIA)) {
+      IsNoMedia = TRUE;
+    }
+    SensePtr++;
+  }
+
+  return IsNoMedia;
+}
+
+
+/**
+  Parse sense key.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE   Error
+  @retval FALSE  NOT error
+
+**/
+BOOLEAN
+ScsiDiskIsMediaError (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsError;
+
+  IsError   = FALSE;
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->Sense_Key) {
+
+    case EFI_SCSI_SK_MEDIUM_ERROR:
+      //
+      // Sense Key is EFI_SCSI_SK_MEDIUM_ERROR (0x3)
+      //
+      switch (SensePtr->Addnl_Sense_Code) {
+
+      //
+      // fall through
+      //
+      case EFI_SCSI_ASC_MEDIA_ERR1:
+
+      //
+      // fall through
+      //
+      case EFI_SCSI_ASC_MEDIA_ERR2:
+
+      //
+      // fall through
+      //
+      case EFI_SCSI_ASC_MEDIA_ERR3:
+      case EFI_SCSI_ASC_MEDIA_ERR4:
+        IsError = TRUE;
+        break;
+
+      default:
+        break;
+      }
+
+      break;
+
+    case EFI_SCSI_SK_NOT_READY:
+      //
+      // Sense Key is EFI_SCSI_SK_NOT_READY (0x2)
+      //
+      switch (SensePtr->Addnl_Sense_Code) {
+      //
+      // Additional Sense Code is ASC_MEDIA_UPSIDE_DOWN (0x6)
+      //
+      case EFI_SCSI_ASC_MEDIA_UPSIDE_DOWN:
+        IsError = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return IsError;
+}
+
+
+/**
+  Check sense key to find if hardware error happens.
+
+  @param  SenseData     The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts   The number of sense key
+
+  @retval TRUE  Hardware error exits.
+  @retval FALSE NO error.
+
+**/
+BOOLEAN
+ScsiDiskIsHardwareError (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsError;
+
+  IsError   = FALSE;
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    
+    //
+    // Sense Key is EFI_SCSI_SK_HARDWARE_ERROR (0x4)
+    //
+    if (SensePtr->Sense_Key == EFI_SCSI_SK_HARDWARE_ERROR) {
+      IsError = TRUE;
+    }
+
+    SensePtr++;
+  }
+
+  return IsError;
+}
+
+
+/**
+  Check sense key to find if media has changed.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE   Media is changed.
+  @retval FALSE  Media is NOT changed.
+**/
+BOOLEAN
+ScsiDiskIsMediaChange (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsMediaChanged;
+
+  IsMediaChanged  = FALSE;
+  SensePtr        = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    //
+    // Sense Key is EFI_SCSI_SK_UNIT_ATTENTION (0x6),
+    // Additional sense code is EFI_SCSI_ASC_MEDIA_CHANGE (0x28)
+    //
+    if ((SensePtr->Sense_Key == EFI_SCSI_SK_UNIT_ATTENTION) &&
+        (SensePtr->Addnl_Sense_Code == EFI_SCSI_ASC_MEDIA_CHANGE)) {
+      IsMediaChanged = TRUE;
+    }
+
+    SensePtr++;
+  }
+
+  return IsMediaChanged;
+}
+
+/**
+  Check sense key to find if reset happens.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE  It is reset before.
+  @retval FALSE It is NOT reset before.
+
+**/
+BOOLEAN
+ScsiDiskIsResetBefore (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsResetBefore;
+
+  IsResetBefore = FALSE;
+  SensePtr      = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    
+    //
+    // Sense Key is EFI_SCSI_SK_UNIT_ATTENTION (0x6)
+    // Additional Sense Code is EFI_SCSI_ASC_RESET (0x29)
+    //
+    if ((SensePtr->Sense_Key == EFI_SCSI_SK_UNIT_ATTENTION) &&
+        (SensePtr->Addnl_Sense_Code == EFI_SCSI_ASC_RESET)) {
+      IsResetBefore = TRUE;
+    }
+
+    SensePtr++;
+  }
+
+  return IsResetBefore;
+}
+
+/**
+  Check sense key to find if the drive is ready.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+  @param  RetryLater   The flag means if need a retry 
+
+  @retval TRUE  Drive is ready.
+  @retval FALSE Drive is NOT ready.
+
+**/
+BOOLEAN
+ScsiDiskIsDriveReady (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts,
+  OUT BOOLEAN               *RetryLater
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsReady;
+
+  IsReady     = TRUE;
+  *RetryLater = FALSE;
+  SensePtr    = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->Sense_Key) {
+
+    case EFI_SCSI_SK_NOT_READY:
+      //
+      // Sense Key is EFI_SCSI_SK_NOT_READY (0x2)
+      //
+      switch (SensePtr->Addnl_Sense_Code) {
+      case EFI_SCSI_ASC_NOT_READY:
+        //
+        // Additional Sense Code is EFI_SCSI_ASC_NOT_READY (0x4)
+        //
+        switch (SensePtr->Addnl_Sense_Code_Qualifier) {
+        case EFI_SCSI_ASCQ_IN_PROGRESS:
+          //
+          // Additional Sense Code Qualifier is
+          // EFI_SCSI_ASCQ_IN_PROGRESS (0x1)
+          //
+          IsReady     = FALSE;
+          *RetryLater = TRUE;
+          break;
+
+        default:
+          IsReady     = FALSE;
+          *RetryLater = FALSE;
+          break;
+        }
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return IsReady;
+}
+
+/**
+  Check sense key to find if it has sense key.
+
+  @param  SenseData   - The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts - The number of sense key
+
+  @retval TRUE  It has sense key.
+  @retval FALSE It has NOT any sense key.
+
+**/
+BOOLEAN
+ScsiDiskHaveSenseKey (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             HaveSenseKey;
+
+  if (SenseCounts == 0) {
+    HaveSenseKey = FALSE;
+  } else {
+    HaveSenseKey = TRUE;
+  }
+
+  SensePtr = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    
+    //
+    // Sense Key is SK_NO_SENSE (0x0)
+    //
+    if ((SensePtr->Sense_Key == EFI_SCSI_SK_NO_SENSE) &&
+        (Index == 0)) {
+      HaveSenseKey = FALSE;
+    }
+
+    SensePtr++;
+  }
+
+  return HaveSenseKey;
+}
+
+/**
+  Release resource about disk device.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+
+**/
+VOID
+ReleaseScsiDiskDeviceResources (
+  IN  SCSI_DISK_DEV   *ScsiDiskDevice
+  )
+{
+  if (ScsiDiskDevice == NULL) {
+    return ;
+  }
+
+  if (ScsiDiskDevice->SenseData != NULL) {
+    FreePool (ScsiDiskDevice->SenseData);
+    ScsiDiskDevice->SenseData = NULL;
+  }
+
+  if (ScsiDiskDevice->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (ScsiDiskDevice->ControllerNameTable);
+    ScsiDiskDevice->ControllerNameTable = NULL;
+  }
+
+  FreePool (ScsiDiskDevice);
+
+  ScsiDiskDevice = NULL;
+}
+
+/**
+  Determine if Block Io & Block Io2 should be produced.
+  
+
+  @param  ChildHandle  Child Handle to retrieve Parent information.
+  
+  @retval  TRUE    Should produce Block Io & Block Io2.
+  @retval  FALSE   Should not produce Block Io & Block Io2.
+
+**/  
+BOOLEAN
+DetermineInstallBlockIo (
+  IN  EFI_HANDLE      ChildHandle
+  )  
+{
+  EFI_SCSI_PASS_THRU_PROTOCOL           *ScsiPassThru;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL       *ExtScsiPassThru;
+
+  //
+  // Firstly, check if ExtScsiPassThru Protocol parent handle exists. If existence,
+  // check its attribute, logic or physical.
+  //
+  ExtScsiPassThru = (EFI_EXT_SCSI_PASS_THRU_PROTOCOL *)GetParentProtocol (&gEfiExtScsiPassThruProtocolGuid, ChildHandle);
+  if (ExtScsiPassThru != NULL) {
+    if ((ExtScsiPassThru->Mode->Attributes & EFI_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL) != 0) {
+      return TRUE;
+    }
+  }
+
+  //
+  // Secondly, check if ScsiPassThru Protocol parent handle exists. If existence,
+  // check its attribute, logic or physical.
+  //
+  ScsiPassThru = (EFI_SCSI_PASS_THRU_PROTOCOL *)GetParentProtocol (&gEfiScsiPassThruProtocolGuid, ChildHandle);
+  if (ScsiPassThru != NULL) {
+    if ((ScsiPassThru->Mode->Attributes & EFI_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL) != 0) {
+      return TRUE;
+    }
+  }
+  
+  return FALSE;
+}
+
+/**
+  Search protocol database and check to see if the protocol
+  specified by ProtocolGuid is present on a ControllerHandle and opened by
+  ChildHandle with an attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  If the ControllerHandle is found, then the protocol specified by ProtocolGuid
+  will be opened on it.  
+  
+
+  @param  ProtocolGuid   ProtocolGuid pointer.
+  @param  ChildHandle    Child Handle to retrieve Parent information.
+  
+**/ 
+VOID *
+EFIAPI
+GetParentProtocol (
+  IN  EFI_GUID                          *ProtocolGuid,
+  IN  EFI_HANDLE                        ChildHandle
+  ) 
+{
+  UINTN                                 Index;
+  UINTN                                 HandleCount;
+  VOID                                  *Interface;  
+  EFI_STATUS                            Status;
+  EFI_HANDLE                            *HandleBuffer;
+
+  //
+  // Retrieve the list of all handles from the handle database
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  ProtocolGuid,
+                  NULL,
+                  &HandleCount,
+                  &HandleBuffer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  //
+  // Iterate to find who is parent handle that is opened with ProtocolGuid by ChildHandle 
+  //
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = EfiTestChildHandle (HandleBuffer[Index], ChildHandle, ProtocolGuid);
+    if (!EFI_ERROR (Status)) {
+      Status = gBS->HandleProtocol (HandleBuffer[Index], ProtocolGuid, (VOID **)&Interface);
+      if (!EFI_ERROR (Status)) {
+        gBS->FreePool (HandleBuffer);
+        return Interface;
+      }
+    }
+  }
+
+  gBS->FreePool (HandleBuffer);
+  return NULL;
+} 
+
+/**
+  Provides inquiry information for the controller type.
+  
+  This function is used by the IDE bus driver to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device 
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  )
+{
+  EFI_STATUS      Status;
+  SCSI_DISK_DEV   *ScsiDiskDevice;
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_DISKINFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*InquiryDataSize >= sizeof (ScsiDiskDevice->InquiryData)) {
+    Status = EFI_SUCCESS;
+    CopyMem (InquiryData, &ScsiDiskDevice->InquiryData, sizeof (ScsiDiskDevice->InquiryData));
+  }
+  *InquiryDataSize = sizeof (ScsiDiskDevice->InquiryData);
+  return Status;
+}
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used by the IDE bus driver to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL 
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device 
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  )
+{
+  EFI_STATUS      Status;
+  SCSI_DISK_DEV   *ScsiDiskDevice;
+
+  if (CompareGuid (&This->Interface, &gEfiDiskInfoScsiInterfaceGuid) || CompareGuid (&This->Interface, &gEfiDiskInfoUfsInterfaceGuid)) {
+    //
+    // Physical SCSI bus does not support this data class. 
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_DISKINFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*IdentifyDataSize >= sizeof (ScsiDiskDevice->IdentifyData)) {
+    Status = EFI_SUCCESS;
+    CopyMem (IdentifyData, &ScsiDiskDevice->IdentifyData, sizeof (ScsiDiskDevice->IdentifyData));
+  }
+  *IdentifyDataSize = sizeof (ScsiDiskDevice->IdentifyData);
+  return Status;
+}
+
+/**
+  Provides sense data information for the controller type.
+  
+  This function is used by the IDE bus driver to get sense data. 
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function is used by the IDE bus driver to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance. 
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  )
+{
+  SCSI_DISK_DEV   *ScsiDiskDevice;
+
+  if (CompareGuid (&This->Interface, &gEfiDiskInfoScsiInterfaceGuid) || CompareGuid (&This->Interface, &gEfiDiskInfoUfsInterfaceGuid)) {
+    //
+    // This is not an IDE physical device.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_DISKINFO (This);
+  *IdeChannel     = ScsiDiskDevice->Channel;
+  *IdeDevice      = ScsiDiskDevice->Device;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Issues ATA IDENTIFY DEVICE command to identify ATAPI device.
+
+  This function tries to fill 512-byte ATAPI_IDENTIFY_DATA for ATAPI device to
+  implement Identify() interface for DiskInfo protocol. The ATA command is sent
+  via SCSI Request Packet.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  
+  @retval EFI_SUCCESS     The ATAPI device identify data were retrieved successfully.
+  @retval others          Some error occurred during the identification that ATAPI device.
+
+**/  
+EFI_STATUS
+AtapiIdentifyDevice (
+  IN OUT SCSI_DISK_DEV   *ScsiDiskDevice
+  )
+{
+  EFI_SCSI_IO_SCSI_REQUEST_PACKET CommandPacket;
+  UINT8                           Cdb[6];
+
+  //
+  // Initialize SCSI REQUEST_PACKET and 6-byte Cdb
+  //
+  ZeroMem (&CommandPacket, sizeof (CommandPacket));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0] = ATA_CMD_IDENTIFY_DEVICE;
+  CommandPacket.Timeout = SCSI_DISK_TIMEOUT;
+  CommandPacket.Cdb = Cdb;
+  CommandPacket.CdbLength = (UINT8) sizeof (Cdb);
+  CommandPacket.InDataBuffer = &ScsiDiskDevice->IdentifyData;
+  CommandPacket.InTransferLength = sizeof (ScsiDiskDevice->IdentifyData);
+
+  return ScsiDiskDevice->ScsiIo->ExecuteScsiCommand (ScsiDiskDevice->ScsiIo, &CommandPacket, NULL);
+}
+
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with SCSI interface GUID. If it further
+  detects that the physical device is an ATAPI/AHCI device, it then updates interface GUID
+  to be IDE/AHCI interface GUID.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  ChildHandle     Child handle to install DiskInfo protocol.
+  
+**/  
+VOID
+InitializeInstallDiskInfo (
+  IN  SCSI_DISK_DEV   *ScsiDiskDevice,
+  IN  EFI_HANDLE      ChildHandle
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL  *ChildDevicePathNode;
+  ATAPI_DEVICE_PATH         *AtapiDevicePath;
+  SATA_DEVICE_PATH          *SataDevicePath;
+  UINTN                     IdentifyRetry;
+
+  Status = gBS->HandleProtocol (ChildHandle, &gEfiDevicePathProtocolGuid, (VOID **) &DevicePathNode);
+  //
+  // Device Path protocol must be installed on the device handle. 
+  //
+  ASSERT_EFI_ERROR (Status);
+  //
+  // Copy the DiskInfo protocol template.
+  //
+  CopyMem (&ScsiDiskDevice->DiskInfo, &gScsiDiskInfoProtocolTemplate, sizeof (gScsiDiskInfoProtocolTemplate));
+
+  while (!IsDevicePathEnd (DevicePathNode)) {
+    ChildDevicePathNode = NextDevicePathNode (DevicePathNode);
+    if ((DevicePathType (DevicePathNode) == HARDWARE_DEVICE_PATH) &&
+        (DevicePathSubType (DevicePathNode) == HW_PCI_DP) &&
+        (DevicePathType (ChildDevicePathNode) == MESSAGING_DEVICE_PATH) &&
+       ((DevicePathSubType (ChildDevicePathNode) == MSG_ATAPI_DP) ||
+        (DevicePathSubType (ChildDevicePathNode) == MSG_SATA_DP))) {
+
+      IdentifyRetry = 3;
+      do {
+        //
+        // Issue ATA Identify Device Command via SCSI command, which is required to publish DiskInfo protocol
+        // with IDE/AHCI interface GUID.
+        //
+        Status = AtapiIdentifyDevice (ScsiDiskDevice);
+        if (!EFI_ERROR (Status)) {
+          if (DevicePathSubType(ChildDevicePathNode) == MSG_ATAPI_DP) {
+            //
+            // We find the valid ATAPI device path
+            //
+            AtapiDevicePath = (ATAPI_DEVICE_PATH *) ChildDevicePathNode;
+            ScsiDiskDevice->Channel = AtapiDevicePath->PrimarySecondary;
+            ScsiDiskDevice->Device = AtapiDevicePath->SlaveMaster;
+            //
+            // Update the DiskInfo.Interface to IDE interface GUID for the physical ATAPI device. 
+            //
+            CopyGuid (&ScsiDiskDevice->DiskInfo.Interface, &gEfiDiskInfoIdeInterfaceGuid);
+          } else {
+            //
+            // We find the valid SATA device path
+            //
+            SataDevicePath = (SATA_DEVICE_PATH *) ChildDevicePathNode;
+            ScsiDiskDevice->Channel = SataDevicePath->HBAPortNumber;
+            ScsiDiskDevice->Device = SataDevicePath->PortMultiplierPortNumber;
+            //
+            // Update the DiskInfo.Interface to AHCI interface GUID for the physical AHCI device. 
+            //
+            CopyGuid (&ScsiDiskDevice->DiskInfo.Interface, &gEfiDiskInfoAhciInterfaceGuid);
+          }
+          return;
+        }
+      } while (--IdentifyRetry > 0);
+    } else if ((DevicePathType (ChildDevicePathNode) == MESSAGING_DEVICE_PATH) &&
+       (DevicePathSubType (ChildDevicePathNode) == MSG_UFS_DP)) {
+      CopyGuid (&ScsiDiskDevice->DiskInfo.Interface, &gEfiDiskInfoUfsInterfaceGuid);
+      break;
+    }
+    DevicePathNode = ChildDevicePathNode;
+  }
+
+  return;
+}
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.h b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.h
new file mode 100644
index 0000000000..2406df51b8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.h
@@ -0,0 +1,1354 @@
+/** @file
+  Header file for SCSI Disk Driver.
+
+Copyright (c) 2004 - 2015, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _SCSI_DISK_H_
+#define _SCSI_DISK_H_
+
+
+#include <Uefi.h>
+
+
+#include <Protocol/ScsiIo.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/BlockIo2.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/ScsiPassThruExt.h>
+#include <Protocol/ScsiPassThru.h>
+#include <Protocol/DiskInfo.h>
+
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiScsiLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+
+#include <IndustryStandard/Scsi.h>
+#include <IndustryStandard/Atapi.h>
+
+#define IS_DEVICE_FIXED(a)        (a)->FixedDevice ? 1 : 0
+
+#define SCSI_DISK_DEV_SIGNATURE SIGNATURE_32 ('s', 'c', 'd', 'k')
+
+typedef struct {
+  UINT32                    Signature;
+
+  EFI_HANDLE                Handle;
+
+  EFI_BLOCK_IO_PROTOCOL     BlkIo;
+  EFI_BLOCK_IO2_PROTOCOL    BlkIo2;
+  EFI_BLOCK_IO_MEDIA        BlkIoMedia;
+  EFI_SCSI_IO_PROTOCOL      *ScsiIo;
+  UINT8                     DeviceType;
+  BOOLEAN                   FixedDevice;
+  UINT16                    Reserved;
+
+  EFI_SCSI_SENSE_DATA       *SenseData;
+  UINTN                     SenseDataNumber;
+  EFI_SCSI_INQUIRY_DATA     InquiryData;
+
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+
+  EFI_DISK_INFO_PROTOCOL    DiskInfo;
+
+  //
+  // The following fields are only valid for ATAPI/SATA device
+  //
+  UINT32                    Channel;
+  UINT32                    Device;
+  ATAPI_IDENTIFY_DATA       IdentifyData;
+  
+  //
+  // The flag indicates if 16-byte command can be used
+  //
+  BOOLEAN                   Cdb16Byte;
+
+  //
+  // The queue for BlockIo2 requests
+  //
+  LIST_ENTRY                BlkIo2Queue;
+} SCSI_DISK_DEV;
+
+#define SCSI_DISK_DEV_FROM_BLKIO(a)  CR (a, SCSI_DISK_DEV, BlkIo, SCSI_DISK_DEV_SIGNATURE)
+#define SCSI_DISK_DEV_FROM_BLKIO2(a)  CR (a, SCSI_DISK_DEV, BlkIo2, SCSI_DISK_DEV_SIGNATURE)
+
+#define SCSI_DISK_DEV_FROM_DISKINFO(a) CR (a, SCSI_DISK_DEV, DiskInfo, SCSI_DISK_DEV_SIGNATURE)
+
+//
+// Asynchronous I/O request
+//
+//
+// Private data structure for a BlockIo2 request
+//
+typedef struct {
+  EFI_BLOCK_IO2_TOKEN                  *Token;
+  //
+  // The flag indicates if the last Scsi Read/Write sub-task for a BlockIo2
+  // request is sent to device
+  //
+  BOOLEAN                              LastScsiRW;
+
+  //
+  // The queue for Scsi Read/Write sub-tasks of a BlockIo2 request
+  //
+  LIST_ENTRY                           ScsiRWQueue;
+
+  LIST_ENTRY                           Link;
+} SCSI_BLKIO2_REQUEST;
+
+//
+// Private data structure for a SCSI Read/Write request
+//
+typedef struct {
+  SCSI_DISK_DEV                        *ScsiDiskDevice;
+  UINT64                               Timeout;
+  EFI_SCSI_SENSE_DATA                  *SenseData;
+  UINT8                                SenseDataLength;
+  UINT8                                HostAdapterStatus;
+  UINT8                                TargetStatus;
+  UINT8                                *InBuffer;
+  UINT8                                *OutBuffer;
+  UINT32                               DataLength;
+  UINT64                               StartLba;
+  UINT32                               SectorCount;
+  UINT8                                TimesRetry;
+
+  //
+  // The BlockIo2 request this SCSI command belongs to
+  //
+  SCSI_BLKIO2_REQUEST                  *BlkIo2Req;
+
+  LIST_ENTRY                           Link;
+} SCSI_ASYNC_RW_REQUEST;
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gScsiDiskDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gScsiDiskComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gScsiDiskComponentName2;
+//
+// action code used in detect media process
+//
+#define ACTION_NO_ACTION               0x00
+#define ACTION_READ_CAPACITY           0x01
+#define ACTION_RETRY_COMMAND_LATER     0x02
+#define ACTION_RETRY_WITH_BACKOFF_ALGO 0x03
+
+#define SCSI_COMMAND_VERSION_1      0x01
+#define SCSI_COMMAND_VERSION_2      0x02
+#define SCSI_COMMAND_VERSION_3      0x03
+
+//
+// SCSI Disk Timeout Experience Value
+//
+#define SCSI_DISK_TIMEOUT           EFI_TIMER_PERIOD_SECONDS (3)
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions.
+  If any other agent wishes to call Supported() it must also follow these
+  calling restrictions.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath   OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Start() it must also follow these calling
+  restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath   OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle.
+
+  This service is called by the EFI boot service DisconnectController().
+  In order to make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController() must follow these
+  calling restrictions. If any other agent wishes to call Stop() it must
+  also follow these calling restrictions.
+  
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer   OPTIONAL
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Reset SCSI Disk.
+
+
+  @param  This                 The pointer of EFI_BLOCK_IO_PROTOCOL
+  @param  ExtendedVerification The flag about if extend verificate
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+  @return EFI_STATUS is retured from EFI_SCSI_IO_PROTOCOL.ResetDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+
+/**
+  The function is to Read Block from SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected
+  @param  Lba        The logic block address
+  @param  BufferSize The size of Buffer
+  @param  Buffer     The buffer to fill the read out data
+
+  @retval EFI_SUCCESS           Successfully to read out block.
+  @retval EFI_DEVICE_ERROR      Fail to detect media.
+  @retval EFI_NO_MEDIA          Media is not present.
+  @retval EFI_MEDIA_CHANGED     Media has changed.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER Invalid parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+
+/**
+  The function is to Write Block to SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL
+  @param  MediaId    The Id of Media detected
+  @param  Lba        The logic block address
+  @param  BufferSize The size of Buffer
+  @param  Buffer     The buffer to fill the read out data
+
+  @retval EFI_SUCCESS           Successfully to read out block.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      Fail to detect media.
+  @retval EFI_NO_MEDIA          Media is not present.
+  @retval EFI_MEDIA_CHNAGED     Media has changed.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER Invalid parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  );
+
+
+/**
+  Flush Block to Disk.
+
+  EFI_SUCCESS is returned directly.
+
+  @param  This              The pointer of EFI_BLOCK_IO_PROTOCOL
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  );
+
+
+/**
+  Reset SCSI Disk.
+
+  @param  This                 The pointer of EFI_BLOCK_IO2_PROTOCOL.
+  @param  ExtendedVerification The flag about if extend verificate.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+  @return EFI_STATUS is returned from EFI_SCSI_IO_PROTOCOL.ResetDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskResetEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  The function is to Read Block from SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected.
+  @param  Lba        The logic block address.
+  @param  Token      A pointer to the token associated with the transaction.
+  @param  BufferSize The size of Buffer.
+  @param  Buffer     The buffer to fill the read out data.
+
+  @retval EFI_SUCCESS           The read request was queued if Token-> Event is
+                                not NULL. The data was read correctly from the
+                                device if theToken-> Event is NULL.
+  @retval EFI_DEVICE_ERROR      The device reported an error while attempting
+                                to perform the read operation.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of
+                                the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not
+                                valid, or the buffer is not on proper
+                                alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReadBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL   *This,
+  IN     UINT32                   MediaId,
+  IN     EFI_LBA                  Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN      *Token,
+  IN     UINTN                    BufferSize,
+  OUT    VOID                     *Buffer
+  );
+
+/**
+  The function is to Write Block to SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected.
+  @param  Lba        The logic block address.
+  @param  Token      A pointer to the token associated with the transaction.
+  @param  BufferSize The size of Buffer.
+  @param  Buffer     The buffer to fill the read out data.
+
+  @retval EFI_SUCCESS           The data were written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device cannot be written to.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_DEVICE_ERROR      The device reported an error while attempting
+                                to perform the write operation.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of
+                                the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not
+                                valid, or the buffer is not on proper
+                                alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskWriteBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL *This,
+  IN     UINT32                 MediaId,
+  IN     EFI_LBA                Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN    *Token,
+  IN     UINTN                  BufferSize,
+  IN     VOID                   *Buffer
+  );
+
+/**
+  Flush the Block Device.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  Token      A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS         All outstanding data was written to the device.
+  @retval EFI_DEVICE_ERROR    The device reported an error while attempting to
+                              write data.
+  @retval EFI_WRITE_PROTECTED The device cannot be written to.
+  @retval EFI_NO_MEDIA        There is no media in the device.
+  @retval EFI_MEDIA_CHANGED   The MediaId is not for the current media.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskFlushBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN OUT EFI_BLOCK_IO2_TOKEN     *Token
+  );
+
+
+/**
+  Provides inquiry information for the controller type.
+  
+  This function is used by the IDE bus driver to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device 
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  );
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used by the IDE bus driver to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]     This               Pointer to the EFI_DISK_INFO_PROTOCOL 
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device 
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  );
+
+
+/**
+  Provides sense data information for the controller type.
+  
+  This function is used by the IDE bus driver to get sense data. 
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  );
+
+/**
+  This function is used by the IDE bus driver to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance. 
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  );
+
+
+/**
+  Detect Device and read out capacity ,if error occurs, parse the sense key.
+
+  @param  ScsiDiskDevice    The pointer of SCSI_DISK_DEV
+  @param  MustReadCapacity  The flag about reading device capacity
+  @param  MediaChange       The pointer of flag indicates if media has changed 
+
+  @retval EFI_DEVICE_ERROR  Indicates that error occurs
+  @retval EFI_SUCCESS       Successfully to detect media
+
+**/
+EFI_STATUS
+ScsiDiskDetectMedia (
+  IN   SCSI_DISK_DEV   *ScsiDiskDevice,
+  IN   BOOLEAN         MustReadCapacity,
+  OUT  BOOLEAN         *MediaChange
+  );
+
+/**
+  To test device.
+
+  When Test Unit Ready command succeeds, retrieve Sense Keys via Request Sense;
+  When Test Unit Ready command encounters any error caused by host adapter or
+  target, return error without retrieving Sense Keys.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates try again
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The pointer of the number of sense data array
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to test unit
+
+**/
+EFI_STATUS
+ScsiDiskTestUnitReady (
+  IN  SCSI_DISK_DEV         *ScsiDiskDevice,
+  OUT BOOLEAN               *NeedRetry,
+  OUT EFI_SCSI_SENSE_DATA   **SenseDataArray,
+  OUT UINTN                 *NumberOfSenseKeys
+  );
+
+
+/**
+  Parsing Sense Keys which got from request sense command.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  SenseData          The pointer of EFI_SCSI_SENSE_DATA
+  @param  NumberOfSenseKeys  The number of sense key  
+  @param  Action             The pointer of action which indicates what is need to do next
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to complete the parsing
+
+**/
+EFI_STATUS
+DetectMediaParsingSenseKeys (
+  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+  IN   EFI_SCSI_SENSE_DATA     *SenseData,
+  IN   UINTN                   NumberOfSenseKeys,
+  OUT  UINTN                   *Action
+  );
+
+
+/**
+  Send read capacity command to device and get the device parameter.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates if need a retry
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The number of sense key
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to read capacity
+
+**/
+EFI_STATUS
+ScsiDiskReadCapacity (
+  IN  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+      OUT  BOOLEAN                 *NeedRetry,
+      OUT  EFI_SCSI_SENSE_DATA     **SenseDataArray,
+      OUT  UINTN                   *NumberOfSenseKeys
+  );
+
+/**
+  Check the HostAdapter status and re-interpret it in EFI_STATUS.
+
+  @param  HostAdapterStatus  Host Adapter status
+
+  @retval  EFI_SUCCESS       Host adapter is OK.
+  @retval  EFI_TIMEOUT       Timeout.
+  @retval  EFI_NOT_READY     Adapter NOT ready.
+  @retval  EFI_DEVICE_ERROR  Adapter device error.
+
+**/
+EFI_STATUS
+CheckHostAdapterStatus (
+  IN UINT8   HostAdapterStatus
+  );
+
+
+/**
+  Check the target status and re-interpret it in EFI_STATUS.
+
+  @param  TargetStatus  Target status
+
+  @retval EFI_NOT_READY       Device is NOT ready.
+  @retval EFI_DEVICE_ERROR 
+  @retval EFI_SUCCESS
+
+**/
+EFI_STATUS
+CheckTargetStatus (
+  IN  UINT8   TargetStatus
+  );
+
+/**
+  Retrieve all sense keys from the device.
+
+  When encountering error during the process, if retrieve sense keys before
+  error encountered, it returns the sense keys with return status set to EFI_SUCCESS,
+  and NeedRetry set to FALSE; otherwize, return the proper return status.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates if need a retry
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The number of sense key
+  @param  AskResetIfError    The flag indicates if need reset when error occurs
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to request sense key
+
+**/
+EFI_STATUS
+ScsiDiskRequestSenseKeys (
+  IN  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+      OUT  BOOLEAN                 *NeedRetry,
+      OUT  EFI_SCSI_SENSE_DATA     **SenseDataArray,
+      OUT  UINTN                   *NumberOfSenseKeys,
+  IN       BOOLEAN                 AskResetIfError
+  );
+
+/**
+  Send out Inquiry command to Device.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  NeedRetry       Indicates if needs try again when error happens
+
+  @retval  EFI_DEVICE_ERROR  Indicates that error occurs
+  @retval  EFI_SUCCESS       Successfully to detect media
+
+**/
+EFI_STATUS
+ScsiDiskInquiryDevice (
+  IN OUT  SCSI_DISK_DEV   *ScsiDiskDevice,
+     OUT  BOOLEAN         *NeedRetry
+  );
+
+/**
+  Parse Inquiry data.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+
+**/
+VOID
+ParseInquiryData (
+  IN OUT SCSI_DISK_DEV   *ScsiDiskDevice
+  );
+
+/**
+  Read sector from SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Buffer          The buffer to fill in the read out data
+  @param  Lba             Logic block address
+  @param  NumberOfBlocks  The number of blocks to read
+
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskReadSectors (
+  IN   SCSI_DISK_DEV     *ScsiDiskDevice,
+  OUT  VOID              *Buffer,
+  IN   EFI_LBA           Lba,
+  IN   UINTN             NumberOfBlocks
+  );
+
+/**
+  Write sector to SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Buffer          The buffer of data to be written into SCSI Disk
+  @param  Lba             Logic block address
+  @param  NumberOfBlocks  The number of blocks to read
+
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskWriteSectors (
+  IN  SCSI_DISK_DEV     *ScsiDiskDevice,
+  IN  VOID              *Buffer,
+  IN  EFI_LBA           Lba,
+  IN  UINTN             NumberOfBlocks
+  );
+
+/**
+  Asynchronously read sector from SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  Buffer          The buffer to fill in the read out data.
+  @param  Lba             Logic block address.
+  @param  NumberOfBlocks  The number of blocks to read.
+  @param  Token           A pointer to the token associated with the
+                          non-blocking read request.
+
+  @retval EFI_INVALID_PARAMETER  Token is NULL or Token->Event is NULL.
+  @retval EFI_DEVICE_ERROR       Indicates a device error.
+  @retval EFI_SUCCESS            Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskAsyncReadSectors (
+  IN   SCSI_DISK_DEV         *ScsiDiskDevice,
+  OUT  VOID                  *Buffer,
+  IN   EFI_LBA               Lba,
+  IN   UINTN                 NumberOfBlocks,
+  IN   EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Asynchronously write sector to SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  Buffer          The buffer of data to be written into SCSI Disk.
+  @param  Lba             Logic block address.
+  @param  NumberOfBlocks  The number of blocks to read.
+  @param  Token           A pointer to the token associated with the
+                          non-blocking read request.
+
+  @retval EFI_INVALID_PARAMETER  Token is NULL or Token->Event is NULL
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWriteSectors (
+  IN  SCSI_DISK_DEV          *ScsiDiskDevice,
+  IN  VOID                   *Buffer,
+  IN  EFI_LBA                Lba,
+  IN  UINTN                  NumberOfBlocks,
+  IN  EFI_BLOCK_IO2_TOKEN    *Token
+  );
+
+/**
+  Submit Read(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to read
+
+  @return  EFI_STATUS is returned by calling ScsiRead10Command().
+**/
+EFI_STATUS
+ScsiDiskRead10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+     OUT UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount
+  );
+
+/**
+  Submit Write(10) Command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to write
+
+  @return  EFI_STATUS is returned by calling ScsiWrite10Command().
+
+**/
+EFI_STATUS
+ScsiDiskWrite10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+  IN     UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount
+  );
+
+/**
+  Submit Read(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to read
+
+  @return  EFI_STATUS is returned by calling ScsiRead16Command().
+**/
+EFI_STATUS
+ScsiDiskRead16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+     OUT UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount
+  );
+  
+/**
+  Submit Write(16) Command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to write
+
+  @return  EFI_STATUS is returned by calling ScsiWrite16Command().
+
+**/
+EFI_STATUS
+ScsiDiskWrite16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+  IN     UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount
+  );  
+
+/**
+  Submit Async Read(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer to fill with the read out data.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to read.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiRead10CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncRead10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+     OUT UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Submit Async Write(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer contains the data to write.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to write.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiWrite10CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWrite10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+  IN     UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Submit Async Read(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer to fill with the read out data.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to read.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiRead16CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncRead16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+     OUT UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Submit Async Write(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer contains the data to write.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to write.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiWrite16CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWrite16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+  IN     UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Get information from media read capacity command.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Capacity10      The pointer of EFI_SCSI_DISK_CAPACITY_DATA
+  @param  Capacity16      The pointer of EFI_SCSI_DISK_CAPACITY_DATA16
+**/
+VOID
+GetMediaInfo (
+  IN OUT SCSI_DISK_DEV                  *ScsiDiskDevice,
+  IN     EFI_SCSI_DISK_CAPACITY_DATA    *Capacity10,
+  IN     EFI_SCSI_DISK_CAPACITY_DATA16  *Capacity16
+  );
+
+/**
+  Check sense key to find if media presents.
+
+  @param  SenseData   The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts The number of sense key
+
+  @retval TRUE    NOT any media
+  @retval FALSE   Media presents
+**/
+BOOLEAN
+ScsiDiskIsNoMedia (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Parse sense key.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE   Error
+  @retval FALSE  NOT error
+
+**/
+BOOLEAN
+ScsiDiskIsMediaError (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Check sense key to find if hardware error happens.
+
+  @param  SenseData     The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts   The number of sense key
+
+  @retval TRUE  Hardware error exits.
+  @retval FALSE NO error.
+
+**/
+BOOLEAN
+ScsiDiskIsHardwareError (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Check sense key to find if media has changed.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE   Media is changed.
+  @retval FALSE  Medit is NOT changed.
+**/
+BOOLEAN
+ScsiDiskIsMediaChange (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Check sense key to find if reset happens.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE  It is reset before.
+  @retval FALSE It is NOT reset before.
+
+**/
+BOOLEAN
+ScsiDiskIsResetBefore (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Check sense key to find if the drive is ready.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+  @param  RetryLater   The flag means if need a retry 
+
+  @retval TRUE  Drive is ready.
+  @retval FALSE Drive is NOT ready.
+
+**/
+BOOLEAN
+ScsiDiskIsDriveReady (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts,
+  OUT BOOLEAN               *RetryLater
+  );
+
+/**
+  Check sense key to find if it has sense key.
+
+  @param  SenseData   - The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts - The number of sense key
+
+  @retval TRUE  It has sense key.
+  @retval FALSE It has NOT any sense key.
+
+**/
+BOOLEAN
+ScsiDiskHaveSenseKey (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Release resource about disk device.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+
+**/
+VOID
+ReleaseScsiDiskDeviceResources (
+  IN  SCSI_DISK_DEV   *ScsiDiskDevice
+  );
+
+/**
+  Determine if Block Io should be produced.
+  
+
+  @param  ChildHandle  Child Handle to retrieve Parent information.
+  
+  @retval  TRUE    Should produce Block Io.
+  @retval  FALSE   Should not produce Block Io.
+
+**/  
+BOOLEAN
+DetermineInstallBlockIo (
+  IN  EFI_HANDLE      ChildHandle
+  );
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with SCSI interface GUID. If it further
+  detects that the physical device is an ATAPI/AHCI device, it then updates interface GUID
+  to be IDE/AHCI interface GUID.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  ChildHandle     Child handle to install DiskInfo protocol.
+  
+**/  
+VOID
+InitializeInstallDiskInfo (
+  IN  SCSI_DISK_DEV   *ScsiDiskDevice,
+  IN  EFI_HANDLE      ChildHandle
+  ); 
+
+/**
+  Search protocol database and check to see if the protocol
+  specified by ProtocolGuid is present on a ControllerHandle and opened by
+  ChildHandle with an attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  If the ControllerHandle is found, then the protocol specified by ProtocolGuid
+  will be opened on it.  
+  
+
+  @param  ProtocolGuid   ProtocolGuid pointer.
+  @param  ChildHandle    Child Handle to retrieve Parent information.
+  
+**/ 
+VOID *
+EFIAPI
+GetParentProtocol (
+  IN  EFI_GUID                          *ProtocolGuid,
+  IN  EFI_HANDLE                        ChildHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.uni b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.uni
new file mode 100644
index 0000000000..1a3d294a05
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskDxe.inf b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskDxe.inf
new file mode 100644
index 0000000000..47539e55c8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskDxe.inf
@@ -0,0 +1,75 @@
+## @file
+#  The Scsi Disk driver is used to retrieve the media info in the attached SCSI disk.
+#  It detects the SCSI disk media and installs Block I/O and Block I/O2 Protocol on
+#  the device handle.
+#  
+#  Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution.  The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ScsiDisk
+  MODULE_UNI_FILE                = ScsiDisk.uni
+  FILE_GUID                      = 0A66E322-3740-4cce-AD62-BD172CECCA35
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeScsiDisk
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gScsiDiskDriverBinding
+#  COMPONENT_NAME                =  gScsiDiskComponentName
+#  COMPONENT_NAME2               =  gScsiDiskComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  ScsiDisk.c
+  ScsiDisk.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  UefiScsiLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  DevicePathLib
+
+[Protocols]
+  gEfiDiskInfoProtocolGuid                      ## BY_START
+  gEfiBlockIoProtocolGuid                       ## BY_START
+  gEfiBlockIo2ProtocolGuid                      ## BY_START
+  gEfiScsiIoProtocolGuid                        ## TO_START
+  gEfiScsiPassThruProtocolGuid                  ## TO_START
+  gEfiExtScsiPassThruProtocolGuid               ## TO_START
+
+[Guids]
+  gEfiDiskInfoScsiInterfaceGuid                 ## SOMETIMES_PRODUCES ## UNDEFINED
+  gEfiDiskInfoIdeInterfaceGuid                  ## SOMETIMES_PRODUCES ## UNDEFINED
+  gEfiDiskInfoAhciInterfaceGuid                 ## SOMETIMES_PRODUCES ## UNDEFINED
+  gEfiDiskInfoUfsInterfaceGuid                  ## SOMETIMES_PRODUCES ## UNDEFINED
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ScsiDiskExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskExtra.uni b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskExtra.uni
new file mode 100644
index 0000000000..0cb5610ce5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.c b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.c
new file mode 100644
index 0000000000..0a76e26aa2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.c
@@ -0,0 +1,1193 @@
+/** @file
+
+  Copyright (c) 2014, 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 "UfsBlockIoPei.h"
+
+//
+// Template for UFS HC Peim Private Data.
+//
+UFS_PEIM_HC_PRIVATE_DATA   gUfsHcPeimTemplate = {
+  UFS_PEIM_HC_SIG,                // Signature
+  NULL,                           // Controller
+  NULL,                           // Pool
+  {                               // BlkIoPpi
+    UfsBlockIoPeimGetDeviceNo,
+    UfsBlockIoPeimGetMediaInfo,
+    UfsBlockIoPeimReadBlocks
+  },
+  {                               // BlkIo2Ppi
+    EFI_PEI_RECOVERY_BLOCK_IO2_PPI_REVISION,
+    UfsBlockIoPeimGetDeviceNo2,
+    UfsBlockIoPeimGetMediaInfo2,
+    UfsBlockIoPeimReadBlocks2
+  },
+  {                               // BlkIoPpiList
+    EFI_PEI_PPI_DESCRIPTOR_PPI,
+    &gEfiPeiVirtualBlockIoPpiGuid,
+    NULL
+  },
+  {                               // BlkIo2PpiList
+    EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
+    &gEfiPeiVirtualBlockIo2PpiGuid,
+    NULL
+  },
+  {                               // Media
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    }
+  },
+  0,                              // UfsHcBase
+  0,                              // Capabilities
+  0,                              // TaskTag
+  0,                              // UtpTrlBase
+  0,                              // Nutrs
+  0,                              // UtpTmrlBase
+  0,                              // Nutmrs
+  {                               // Luns
+    {
+      UFS_LUN_0,                      // Ufs Common Lun 0
+      UFS_LUN_1,                      // Ufs Common Lun 1
+      UFS_LUN_2,                      // Ufs Common Lun 2
+      UFS_LUN_3,                      // Ufs Common Lun 3
+      UFS_LUN_4,                      // Ufs Common Lun 4
+      UFS_LUN_5,                      // Ufs Common Lun 5
+      UFS_LUN_6,                      // Ufs Common Lun 6
+      UFS_LUN_7,                      // Ufs Common Lun 7
+    },
+    0x0000,                           // By default exposing all Luns.
+    0x0
+  }
+};
+
+/**
+  Execute Request Sense SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] DataBuffer           A pointer to output sense data.
+  @param[out] DataBufferLength     The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimRequestSense (
+  IN     UFS_PEIM_HC_PRIVATE_DATA        *Private,
+  IN     UINTN                           Lun,  
+     OUT VOID                            *DataBuffer,
+     OUT UINT32                          *DataBufferLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET                Packet;
+  UINT8                                  Cdb[UFS_SCSI_OP_LENGTH_SIX];
+  EFI_STATUS                             Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0]  = EFI_SCSI_OP_REQUEST_SENSE;
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.DataDirection    = UfsDataIn;
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataBufferLength;
+  Packet.SenseData        = NULL;
+  Packet.SenseDataLength  = 0;
+
+  Status = UfsExecScsiCmds (Private,(UINT8)Lun, &Packet);
+
+  if (!EFI_ERROR (Status)) {
+    *DataBufferLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute TEST UNITY READY SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimTestUnitReady (
+  IN     UFS_PEIM_HC_PRIVATE_DATA        *Private,
+  IN     UINTN                           Lun,  
+     OUT VOID                            *SenseData,  OPTIONAL
+     OUT UINT8                           *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET                Packet;
+  UINT8                                  Cdb[UFS_SCSI_OP_LENGTH_SIX];
+  EFI_STATUS                             Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0]  = EFI_SCSI_OP_TEST_UNIT_READY;
+
+  Packet.Timeout         = UFS_TIMEOUT;
+  Packet.Cdb             = Cdb;
+  Packet.CdbLength       = sizeof (Cdb);
+  Packet.DataDirection   = UfsNoData;
+  Packet.SenseData       = SenseData;
+  Packet.SenseDataLength = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private,(UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute INQUIRY SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] Inquiry              A pointer to Inquiry data buffer.
+  @param[out] InquiryLengths       The length of output Inquiry data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimInquiry (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+     OUT VOID                         *Inquiry,
+     OUT UINT32                       *InquiryLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_SIX];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0]  = EFI_SCSI_OP_INQUIRY;
+  Cdb[4]  = sizeof (EFI_SCSI_INQUIRY_DATA);
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = Inquiry;
+  Packet.InTransferLength = *InquiryLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *InquiryLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute READ CAPACITY(10) SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] DataBuffer           A pointer to READ_CAPACITY data buffer.
+  @param[out] DataLength           The length of output READ_CAPACITY data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimReadCapacity (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+     OUT VOID                         *DataBuffer,
+     OUT UINT32                       *DataLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_TEN];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0] = EFI_SCSI_OP_READ_CAPACITY;
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *DataLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute READ CAPACITY(16) SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] DataBuffer           A pointer to READ_CAPACITY data buffer.
+  @param[out] DataLength           The length of output READ_CAPACITY data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimReadCapacity16 (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+     OUT VOID                         *DataBuffer,
+     OUT UINT32                       *DataLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_SIXTEEN];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0]  = EFI_SCSI_OP_READ_CAPACITY16;
+  Cdb[1]  = 0x10;          // Service Action should be 0x10 for UFS device.
+  Cdb[13] = 0x20;          // The maximum number of bytes for returned data.
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *DataLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute READ (10) SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[in]  StartLba             The start LBA.
+  @param[in]  SectorNum            The sector number to be read.
+  @param[out] DataBuffer           A pointer to data buffer.
+  @param[out] DataLength           The length of output data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimRead10 (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+  IN     UINTN                        StartLba,
+  IN     UINT32                       SectorNum,
+     OUT VOID                         *DataBuffer,
+     OUT UINT32                       *DataLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_TEN];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0] = EFI_SCSI_OP_READ10;
+  WriteUnaligned32 ((UINT32 *)&Cdb[2], SwapBytes32 ((UINT32) StartLba));
+  WriteUnaligned16 ((UINT16 *)&Cdb[7], SwapBytes16 ((UINT16) SectorNum));
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *DataLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute READ (16) SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[in]  StartLba             The start LBA.
+  @param[in]  SectorNum            The sector number to be read.
+  @param[out] DataBuffer           A pointer to data buffer.
+  @param[out] DataLength           The length of output data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimRead16 (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+  IN     UINTN                        StartLba,
+  IN     UINT32                       SectorNum,
+     OUT VOID                         *DataBuffer,
+     OUT UINT32                       *DataLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_SIXTEEN];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0] = EFI_SCSI_OP_READ16;
+  WriteUnaligned64 ((UINT64 *)&Cdb[2], SwapBytes64 (StartLba));
+  WriteUnaligned32 ((UINT32 *)&Cdb[10], SwapBytes32 (SectorNum));
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *DataLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Parsing Sense Keys from sense data.
+
+  @param  Media              The pointer of EFI_PEI_BLOCK_IO_MEDIA
+  @param  SenseData          The pointer of EFI_SCSI_SENSE_DATA
+  @param  NeedRetry          The pointer of action which indicates what is need to retry
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to complete the parsing
+
+**/
+EFI_STATUS
+UfsPeimParsingSenseKeys (
+  IN     EFI_PEI_BLOCK_IO2_MEDIA   *Media,
+  IN     EFI_SCSI_SENSE_DATA       *SenseData,
+     OUT BOOLEAN                   *NeedRetry
+  )
+{
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_NOT_READY) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_NO_MEDIA)) {
+    Media->MediaPresent = FALSE;
+    *NeedRetry = FALSE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Is No Media\n"));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_UNIT_ATTENTION) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_MEDIA_CHANGE)) {
+    *NeedRetry = TRUE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Is Media Change\n"));
+    return EFI_SUCCESS;
+  }
+
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_UNIT_ATTENTION) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_RESET)) {
+    *NeedRetry = TRUE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Was Reset Before\n"));
+    return EFI_SUCCESS;
+  }
+
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_MEDIUM_ERROR) ||
+      ((SenseData->Sense_Key == EFI_SCSI_SK_NOT_READY) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_MEDIA_UPSIDE_DOWN))) {
+    *NeedRetry = FALSE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Media Error\n"));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (SenseData->Sense_Key == EFI_SCSI_SK_HARDWARE_ERROR) {
+    *NeedRetry = FALSE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Hardware Error\n"));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_NOT_READY) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_NOT_READY) &&
+      (SenseData->Addnl_Sense_Code_Qualifier == EFI_SCSI_ASCQ_IN_PROGRESS)) {
+    *NeedRetry = TRUE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Was Reset Before\n"));
+    return EFI_SUCCESS;
+  }
+
+  *NeedRetry = FALSE;
+  DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Sense Key = 0x%x ASC = 0x%x!\n", SenseData->Sense_Key, SenseData->Addnl_Sense_Code));
+  return EFI_DEVICE_ERROR;
+}
+
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          The operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetDeviceNo (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  OUT UINTN                          *NumberBlockDevices
+  )
+{
+  //
+  // For Ufs device, it has up to 8 normal Luns plus some well-known Luns.
+  // At PEI phase, we will only expose normal Luns to user.
+  // For those disabled Lun, when user try to access it, the operation would fail.
+  //
+  *NumberBlockDevices = UFS_PEIM_MAX_LUNS;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @par Note: 
+      The MediaInfo structure describes an enumeration of possible block device 
+      types.  This enumeration exists because no device paths are actually passed 
+      across interfaces that describe the type or class of hardware that is publishing 
+      the block I/O interface. This enumeration will allow for policy decisions
+      in the Recovery PEIM, such as "Try to recover from legacy floppy first, 
+      LS-120 second, CD-ROM third." If there are multiple partitions abstracted 
+      by a given device type, they should be reported in ascending order; this 
+      order also applies to nested partitions, such as legacy MBR, where the 
+      outermost partitions would have precedence in the reporting order. The 
+      same logic applies to systems such as IDE that have precedence relationships 
+      like "Master/Slave" or "Primary/Secondary". The master device should be 
+      reported first, the slave second.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetMediaInfo (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  IN  UINTN                          DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO_MEDIA         *MediaInfo
+  )
+{
+  EFI_STATUS                         Status;
+  UFS_PEIM_HC_PRIVATE_DATA           *Private;
+  EFI_SCSI_SENSE_DATA                SenseData;
+  UINT8                              SenseDataLength;
+  EFI_SCSI_DISK_CAPACITY_DATA        Capacity;
+  EFI_SCSI_DISK_CAPACITY_DATA16      Capacity16;
+  UINTN                              DataLength;
+  BOOLEAN                            NeedRetry;  
+
+  Private   = GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS (This);
+  NeedRetry = TRUE;
+
+  if (DeviceIndex >= UFS_PEIM_MAX_LUNS) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Private->Luns.BitMask & (BIT0 << DeviceIndex)) == 0) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  ZeroMem (&SenseData, sizeof (SenseData));
+  ZeroMem (&Capacity, sizeof (Capacity));
+  ZeroMem (&Capacity16, sizeof (Capacity16));
+  SenseDataLength = sizeof (SenseData);
+  //
+  // First test unit ready
+  //
+  do {
+    Status = UfsPeimTestUnitReady (
+               Private,
+               DeviceIndex,
+               &SenseData,
+               &SenseDataLength
+               );
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+  
+    if (SenseDataLength == 0) {
+      continue;
+    }
+
+    Status = UfsPeimParsingSenseKeys (&(Private->Media[DeviceIndex]), &SenseData, &NeedRetry);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+  } while (NeedRetry);
+
+  DataLength      = sizeof (EFI_SCSI_DISK_CAPACITY_DATA);
+  SenseDataLength = 0;
+  Status = UfsPeimReadCapacity (Private, DeviceIndex, &Capacity, (UINT32 *)&DataLength, NULL, &SenseDataLength);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((Capacity.LastLba3 == 0xff) && (Capacity.LastLba2 == 0xff) &&
+      (Capacity.LastLba1 == 0xff) && (Capacity.LastLba0 == 0xff)) {
+    DataLength      = sizeof (EFI_SCSI_DISK_CAPACITY_DATA16);
+    SenseDataLength = 0;
+    Status = UfsPeimReadCapacity16 (Private, DeviceIndex, &Capacity16, (UINT32 *)&DataLength, NULL, &SenseDataLength);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+    Private->Media[DeviceIndex].LastBlock  = (Capacity16.LastLba3 << 24) | (Capacity16.LastLba2 << 16) | (Capacity16.LastLba1 << 8) | Capacity16.LastLba0;
+    Private->Media[DeviceIndex].LastBlock |= ((UINT64)Capacity16.LastLba7 << 56) | ((UINT64)Capacity16.LastLba6 << 48) | ((UINT64)Capacity16.LastLba5 << 40) | ((UINT64)Capacity16.LastLba4 << 32);
+    Private->Media[DeviceIndex].BlockSize  = (Capacity16.BlockSize3 << 24) | (Capacity16.BlockSize2 << 16) | (Capacity16.BlockSize1 << 8) | Capacity16.BlockSize0;
+  } else {
+    Private->Media[DeviceIndex].LastBlock  = (Capacity.LastLba3 << 24) | (Capacity.LastLba2 << 16) | (Capacity.LastLba1 << 8) | Capacity.LastLba0;
+    Private->Media[DeviceIndex].BlockSize  = (Capacity.BlockSize3 << 24) | (Capacity.BlockSize2 << 16) | (Capacity.BlockSize1 << 8) | Capacity.BlockSize0;
+  }
+
+  MediaInfo->DeviceType   = UfsDevice;
+  MediaInfo->MediaPresent = Private->Media[DeviceIndex].MediaPresent;
+  MediaInfo->LastBlock    = (UINTN)Private->Media[DeviceIndex].LastBlock;
+  MediaInfo->BlockSize    = Private->Media[DeviceIndex].BlockSize;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimReadBlocks (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  IN  UINTN                          DeviceIndex,
+  IN  EFI_PEI_LBA                    StartLBA,
+  IN  UINTN                          BufferSize,
+  OUT VOID                           *Buffer
+  )
+{
+  EFI_STATUS                         Status;
+  UINTN                              BlockSize;
+  UINTN                              NumberOfBlocks;
+  UFS_PEIM_HC_PRIVATE_DATA           *Private;
+  EFI_SCSI_SENSE_DATA                SenseData;
+  UINT8                              SenseDataLength;
+  BOOLEAN                            NeedRetry;  
+
+  Status    = EFI_SUCCESS;
+  NeedRetry = TRUE;
+  Private   = GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS (This);
+
+  ZeroMem (&SenseData, sizeof (SenseData));
+  SenseDataLength = sizeof (SenseData);
+
+  //
+  // Check parameters
+  //
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (DeviceIndex >= UFS_PEIM_MAX_LUNS) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Private->Luns.BitMask & (BIT0 << DeviceIndex)) == 0) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  BlockSize = Private->Media[DeviceIndex].BlockSize;
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+  }
+
+  if (StartLBA > Private->Media[DeviceIndex].LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  NumberOfBlocks = BufferSize / BlockSize;
+
+  do {
+    Status = UfsPeimTestUnitReady (
+               Private,
+               DeviceIndex,
+               &SenseData,
+               &SenseDataLength
+               );
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+  
+    if (SenseDataLength == 0) {
+      continue;
+    }
+
+    Status = UfsPeimParsingSenseKeys (&(Private->Media[DeviceIndex]), &SenseData, &NeedRetry);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+  } while (NeedRetry);
+
+  SenseDataLength = 0;
+  if (Private->Media[DeviceIndex].LastBlock < 0xfffffffful) {
+    Status = UfsPeimRead10 (
+               Private,
+               DeviceIndex,
+               (UINT32)StartLBA,
+               (UINT32)NumberOfBlocks,
+               Buffer,
+               (UINT32 *)&BufferSize,
+               NULL,
+               &SenseDataLength
+               );
+  } else {
+    Status = UfsPeimRead16 (
+               Private,
+               DeviceIndex,
+               (UINT32)StartLBA,
+               (UINT32)NumberOfBlocks,
+               Buffer,
+               (UINT32 *)&BufferSize,
+               NULL,
+               &SenseDataLength
+               );
+  }
+  return Status;
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          The operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetDeviceNo2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  OUT UINTN                          *NumberBlockDevices
+  )
+{
+  //
+  // For Ufs device, it has up to 8 normal Luns plus some well-known Luns.
+  // At PEI phase, we will only expose normal Luns to user.
+  // For those disabled Lun, when user try to access it, the operation would fail.
+  //
+  *NumberBlockDevices = UFS_PEIM_MAX_LUNS;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @par Note: 
+      The MediaInfo structure describes an enumeration of possible block device 
+      types.  This enumeration exists because no device paths are actually passed 
+      across interfaces that describe the type or class of hardware that is publishing 
+      the block I/O interface. This enumeration will allow for policy decisions
+      in the Recovery PEIM, such as "Try to recover from legacy floppy first, 
+      LS-120 second, CD-ROM third." If there are multiple partitions abstracted 
+      by a given device type, they should be reported in ascending order; this 
+      order also applies to nested partitions, such as legacy MBR, where the 
+      outermost partitions would have precedence in the reporting order. The 
+      same logic applies to systems such as IDE that have precedence relationships 
+      like "Master/Slave" or "Primary/Secondary". The master device should be 
+      reported first, the slave second.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetMediaInfo2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  IN  UINTN                          DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA        *MediaInfo
+  )
+{
+  EFI_STATUS                         Status;
+  UFS_PEIM_HC_PRIVATE_DATA           *Private;
+  EFI_PEI_BLOCK_IO_MEDIA             Media;
+
+  Private   = GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS2 (This);
+  
+  Status    = UfsBlockIoPeimGetMediaInfo (
+                PeiServices,
+                &Private->BlkIoPpi,
+                DeviceIndex,
+                &Media
+                );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CopyMem (MediaInfo, &(Private->Media[DeviceIndex]), sizeof (EFI_PEI_BLOCK_IO2_MEDIA));
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimReadBlocks2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  IN  UINTN                          DeviceIndex,
+  IN  EFI_PEI_LBA                    StartLBA,
+  IN  UINTN                          BufferSize,
+  OUT VOID                           *Buffer
+  )
+{
+  EFI_STATUS                         Status;
+  UFS_PEIM_HC_PRIVATE_DATA           *Private; 
+
+  Status    = EFI_SUCCESS;
+  Private   = GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS2 (This);
+
+  Status  = UfsBlockIoPeimReadBlocks (
+              PeiServices,
+              &Private->BlkIoPpi,
+              DeviceIndex,
+              StartLBA,
+              BufferSize,
+              Buffer
+              );
+  return Status;
+}
+
+/**
+  The user code starts with this function.
+  
+  @param  FileHandle             Handle of the file being invoked.
+  @param  PeiServices            Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            The driver is successfully initialized.
+  @retval Others                 Can't initialize the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeUfsBlockIoPeim (
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS                    Status;
+  UFS_PEIM_HC_PRIVATE_DATA      *Private;
+  EDKII_UFS_HOST_CONTROLLER_PPI *UfsHcPpi;
+  UINT32                        Index;
+  UFS_CONFIG_DESC               Config;
+  UINTN                         MmioBase;
+  UINT8                         Controller;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // locate ufs host controller PPI
+  //
+  Status = PeiServicesLocatePpi (
+             &gEdkiiPeiUfsHostControllerPpiGuid,
+             0,
+             NULL,
+             (VOID **) &UfsHcPpi
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Controller = 0;
+  MmioBase   = 0;
+  while (TRUE) {
+    Status = UfsHcPpi->GetUfsHcMmioBar (UfsHcPpi, Controller, &MmioBase);
+    //
+    // When status is error, meant no controller is found
+    //
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    Private = AllocateCopyPool (sizeof (UFS_PEIM_HC_PRIVATE_DATA), &gUfsHcPeimTemplate);
+    if (Private == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      break;
+    }
+
+    Private->BlkIoPpiList.Ppi  = &Private->BlkIoPpi;
+    Private->BlkIo2PpiList.Ppi = &Private->BlkIo2Ppi;
+    Private->UfsHcBase         = MmioBase;
+
+    //
+    // Initialize the memory pool which will be used in all transactions.
+    //
+    Status = UfsPeimInitMemPool (Private);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_OUT_OF_RESOURCES;
+      break;
+    }
+
+    //
+    // Initialize UFS Host Controller H/W.
+    //
+    Status = UfsControllerInit (Private);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "UfsDevicePei: Host Controller Initialization Error, Status = %r\n", Status));
+      Controller++;
+      continue;
+    }
+
+    //
+    // UFS 2.0 spec Section 13.1.3.3:
+    // At the end of the UFS Interconnect Layer initialization on both host and device side, 
+    // the host shall send a NOP OUT UPIU to verify that the device UTP Layer is ready. 
+    //
+    Status = UfsExecNopCmds (Private);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ufs Sending NOP IN command Error, Status = %r\n", Status));
+      Controller++;
+      continue;
+    }
+
+    //
+    // The host enables the device initialization completion by setting fDeviceInit flag.
+    //
+    Status = UfsSetFlag (Private, UfsFlagDevInit);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ufs Set fDeviceInit Flag Error, Status = %r\n", Status));
+      Controller++;
+      continue;
+    }
+
+    //
+    // Get Ufs Device's Lun Info by reading Configuration Descriptor.
+    //
+    Status = UfsRwDeviceDesc (Private, TRUE, UfsConfigDesc, 0, 0, &Config, sizeof (UFS_CONFIG_DESC));
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ufs Get Configuration Descriptor Error, Status = %r\n", Status));
+      Controller++;
+      continue;
+    }
+
+    for (Index = 0; Index < UFS_PEIM_MAX_LUNS; Index++) {
+      if (Config.UnitDescConfParams[Index].LunEn != 0) {
+        Private->Luns.BitMask |= (BIT0 << Index);
+        DEBUG ((EFI_D_INFO, "Ufs %d Lun %d is enabled\n", Controller, Index));
+      }
+    }
+    
+    Status = PeiServicesInstallPpi (&Private->BlkIoPpiList);
+    Controller++;
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.h b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.h
new file mode 100644
index 0000000000..46e9bfe03f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.h
@@ -0,0 +1,560 @@
+/** @file
+
+  Copyright (c) 2014, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _UFS_BLOCK_IO_PEI_H_
+#define _UFS_BLOCK_IO_PEI_H_
+
+#include <PiPei.h>
+
+#include <Ppi/UfsHostController.h>
+#include <Ppi/BlockIo.h>
+#include <Ppi/BlockIo2.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/IoLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PeiServicesLib.h>
+
+#include <IndustryStandard/Scsi.h>
+
+#include "UfsHci.h"
+#include "UfsHcMem.h"
+
+#define UFS_PEIM_HC_SIG             SIGNATURE_32 ('U', 'F', 'S', 'H')
+
+#define UFS_PEIM_MAX_LUNS           8
+
+typedef struct {
+  UINT8    Lun[UFS_PEIM_MAX_LUNS];
+  UINT16   BitMask:12;              // Bit 0~7 is for common luns. Bit 8~11 is reserved for those well known luns
+  UINT16   Rsvd:4;
+} UFS_PEIM_EXPOSED_LUNS;
+
+typedef struct {
+  ///
+  /// The timeout, in 100 ns units, to use for the execution of this SCSI
+  /// Request Packet. A Timeout value of 0 means that this function
+  /// will wait indefinitely for the SCSI Request Packet to execute. If
+  /// Timeout is greater than zero, then this function will return
+  /// EFI_TIMEOUT if the time required to execute the SCSI
+  /// Request Packet is greater than Timeout.
+  ///
+  UINT64 Timeout;
+  ///
+  /// A pointer to the data buffer to transfer between the SCSI
+  /// controller and the SCSI device for read and bidirectional commands.
+  ///
+  VOID   *InDataBuffer;
+  ///
+  /// A pointer to the data buffer to transfer between the SCSI
+  /// controller and the SCSI device for write or bidirectional commands.
+  ///
+  VOID   *OutDataBuffer;
+  ///
+  /// A pointer to the sense data that was generated by the execution of
+  /// the SCSI Request Packet.
+  ///
+  VOID   *SenseData;
+  ///
+  /// A pointer to buffer that contains the Command Data Block to
+  /// send to the SCSI device specified by Target and Lun.
+  ///
+  VOID   *Cdb;
+  ///
+  /// On Input, the size, in bytes, of InDataBuffer. On output, the
+  /// number of bytes transferred between the SCSI controller and the SCSI device.
+  ///
+  UINT32 InTransferLength;
+  ///
+  /// On Input, the size, in bytes of OutDataBuffer. On Output, the
+  /// Number of bytes transferred between SCSI Controller and the SCSI device.
+  ///
+  UINT32 OutTransferLength;
+  ///
+  /// The length, in bytes, of the buffer Cdb. The standard values are 6,
+  /// 10, 12, and 16, but other values are possible if a variable length CDB is used.
+  ///
+  UINT8  CdbLength;
+  ///
+  /// The direction of the data transfer. 0 for reads, 1 for writes. A
+  /// value of 2 is Reserved for Bi-Directional SCSI commands.
+  ///
+  UINT8  DataDirection;
+  ///
+  /// On input, the length in bytes of the SenseData buffer. On
+  /// output, the number of bytes written to the SenseData buffer.
+  ///
+  UINT8  SenseDataLength;
+} UFS_SCSI_REQUEST_PACKET;
+
+typedef struct _UFS_PEIM_HC_PRIVATE_DATA {  
+  UINT32                            Signature;
+  EFI_HANDLE                        Controller;
+
+  UFS_PEIM_MEM_POOL                 *Pool;
+
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI     BlkIoPpi;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI    BlkIo2Ppi;
+  EFI_PEI_PPI_DESCRIPTOR            BlkIoPpiList;
+  EFI_PEI_PPI_DESCRIPTOR            BlkIo2PpiList;
+  EFI_PEI_BLOCK_IO2_MEDIA           Media[UFS_PEIM_MAX_LUNS];
+
+  UINTN                             UfsHcBase;
+  UINT32                            Capabilities;
+
+  UINT8                             TaskTag;
+
+  VOID                              *UtpTrlBase;
+  UINT8                             Nutrs;
+  VOID                              *UtpTmrlBase;
+  UINT8                             Nutmrs;
+
+  UFS_PEIM_EXPOSED_LUNS             Luns;
+} UFS_PEIM_HC_PRIVATE_DATA;
+
+#define UFS_TIMEOUT                 MultU64x32((UINT64)(3), 10000000)
+
+#define ROUNDUP8(x) (((x) % 8 == 0) ? (x) : ((x) / 8 + 1) * 8)
+
+#define IS_ALIGNED(addr, size)      (((UINTN) (addr) & (size - 1)) == 0)
+
+#define GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS(a) CR (a, UFS_PEIM_HC_PRIVATE_DATA, BlkIoPpi, UFS_PEIM_HC_SIG)
+#define GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS2(a) CR (a, UFS_PEIM_HC_PRIVATE_DATA, BlkIo2Ppi, UFS_PEIM_HC_SIG)
+
+#define UFS_SCSI_OP_LENGTH_SIX      0x6
+#define UFS_SCSI_OP_LENGTH_TEN      0xa
+#define UFS_SCSI_OP_LENGTH_SIXTEEN  0x10
+
+typedef struct _UFS_DEVICE_MANAGEMENT_REQUEST_PACKET {
+  UINT64                            Timeout;
+  VOID                              *InDataBuffer;
+  VOID                              *OutDataBuffer;
+  UINT8                             Opcode;
+  UINT8                             DescId;
+  UINT8                             Index;
+  UINT8                             Selector;
+  UINT32                            InTransferLength;
+  UINT32                            OutTransferLength;
+  UINT8                             DataDirection;
+  UINT8                             Ocs;
+} UFS_DEVICE_MANAGEMENT_REQUEST_PACKET;
+
+/**
+  Sends a UFS-supported SCSI Request Packet to a UFS device that is attached to the UFS host controller.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Lun           The LUN of the UFS device to send the SCSI Request Packet.
+  @param[in, out] Packet        A pointer to the SCSI Request Packet to send to a specified Lun of the
+                                UFS device.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsExecScsiCmds (
+  IN     UFS_PEIM_HC_PRIVATE_DATA      *Private,
+  IN     UINT8                         Lun,
+  IN OUT UFS_SCSI_REQUEST_PACKET       *Packet
+  );
+
+/**
+  Initialize the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+UfsControllerInit (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  );
+
+/**
+  Stop the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is stopped successfully.
+  @retval Others                     A device error occurred while stopping the controller.
+
+**/
+EFI_STATUS
+UfsControllerStop (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  );
+
+/**
+  Set specified flag to 1 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be set.
+
+  @retval EFI_SUCCESS           The flag was set successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to set the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of setting the flag.
+
+**/
+EFI_STATUS
+UfsSetFlag (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        FlagId
+  );
+
+/**
+  Read or write specified device descriptor of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      DescId        The ID of device descriptor.
+  @param[in]      Index         The Index of device descriptor.
+  @param[in]      Selector      The Selector of device descriptor.
+  @param[in, out] Descriptor    The buffer of device descriptor to be read or written.
+  @param[in]      DescSize      The size of device descriptor buffer.
+
+  @retval EFI_SUCCESS           The device descriptor was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the device descriptor.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the device descriptor.
+
+**/
+EFI_STATUS
+UfsRwDeviceDesc (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        DescId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT VOID                         *Descriptor,
+  IN     UINT32                       DescSize
+  );
+
+/**
+  Sends NOP IN cmd to a UFS device for initialization process request.
+  For more details, please refer to UFS 2.0 spec Figure 13.3.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS           The NOP IN command was sent by the host. The NOP OUT response was
+                                received successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to execute NOP IN command.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the NOP IN command to execute.
+
+**/
+EFI_STATUS
+UfsExecNopCmds (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  );
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          The operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetDeviceNo (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  OUT UINTN                          *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @par Note: 
+      The MediaInfo structure describes an enumeration of possible block device 
+      types.  This enumeration exists because no device paths are actually passed 
+      across interfaces that describe the type or class of hardware that is publishing 
+      the block I/O interface. This enumeration will allow for policy decisions
+      in the Recovery PEIM, such as "Try to recover from legacy floppy first, 
+      LS-120 second, CD-ROM third." If there are multiple partitions abstracted 
+      by a given device type, they should be reported in ascending order; this 
+      order also applies to nested partitions, such as legacy MBR, where the 
+      outermost partitions would have precedence in the reporting order. The 
+      same logic applies to systems such as IDE that have precedence relationships 
+      like "Master/Slave" or "Primary/Secondary". The master device should be 
+      reported first, the slave second.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetMediaInfo (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  IN  UINTN                          DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO_MEDIA         *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimReadBlocks (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  IN  UINTN                          DeviceIndex,
+  IN  EFI_PEI_LBA                    StartLBA,
+  IN  UINTN                          BufferSize,
+  OUT VOID                           *Buffer
+  );
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          The operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetDeviceNo2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  OUT UINTN                          *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @par Note: 
+      The MediaInfo structure describes an enumeration of possible block device 
+      types.  This enumeration exists because no device paths are actually passed 
+      across interfaces that describe the type or class of hardware that is publishing 
+      the block I/O interface. This enumeration will allow for policy decisions
+      in the Recovery PEIM, such as "Try to recover from legacy floppy first, 
+      LS-120 second, CD-ROM third." If there are multiple partitions abstracted 
+      by a given device type, they should be reported in ascending order; this 
+      order also applies to nested partitions, such as legacy MBR, where the 
+      outermost partitions would have precedence in the reporting order. The 
+      same logic applies to systems such as IDE that have precedence relationships 
+      like "Master/Slave" or "Primary/Secondary". The master device should be 
+      reported first, the slave second.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetMediaInfo2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  IN  UINTN                          DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA        *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimReadBlocks2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  IN  UINTN                          DeviceIndex,
+  IN  EFI_PEI_LBA                    StartLBA,
+  IN  UINTN                          BufferSize,
+  OUT VOID                           *Buffer
+  );
+
+/**
+  Initialize the memory management pool for the host controller.
+  
+  @param  Private               The Ufs Peim driver private data.
+
+  @retval EFI_SUCCESS           The memory pool is initialized.
+  @retval Others                Fail to init the memory pool.
+
+**/
+EFI_STATUS
+UfsPeimInitMemPool (
+  IN  UFS_PEIM_HC_PRIVATE_DATA      *Private
+  );
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+  
+  @param  Pool      The host controller's memory pool.
+  @param  Size      Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UfsPeimAllocateMem (
+  IN  UFS_PEIM_MEM_POOL        *Pool,
+  IN  UINTN                    Size
+  );
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UfsPeimFreeMem (
+  IN UFS_PEIM_MEM_POOL    *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.inf b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.inf
new file mode 100644
index 0000000000..80fe0392b9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.inf
@@ -0,0 +1,62 @@
+## @file
+# Description file for the Universal Flash Storage (UFS) Peim driver.
+#
+# Copyright (c) 2014 - 2015, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UfsBlockIoPei
+  MODULE_UNI_FILE                = UfsBlockIoPei.uni
+  FILE_GUID                      = BE189D38-C963-41CF-B695-D90E9E545A13
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 0.9
+
+  ENTRY_POINT                    = InitializeUfsBlockIoPeim
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  UfsBlockIoPei.c
+  UfsBlockIoPei.h
+  UfsHci.c
+  UfsHci.h
+  UfsHcMem.c
+  UfsHcMem.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  IoLib
+  TimerLib
+  BaseMemoryLib
+  PeimEntryPoint
+  PeiServicesLib
+  DebugLib
+
+[Ppis]
+  gEfiPeiVirtualBlockIoPpiGuid                  ## PRODUCES
+  gEfiPeiVirtualBlockIo2PpiGuid                 ## PRODUCES
+  gEdkiiPeiUfsHostControllerPpiGuid             ## CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gEdkiiPeiUfsHostControllerPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UfsBlockIoPeiExtra.uni
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.uni b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.uni
new file mode 100644
index 0000000000..23b6e633b9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPeiExtra.uni b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPeiExtra.uni
new file mode 100644
index 0000000000..706fcdc8f4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.c b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.c
new file mode 100644
index 0000000000..cc6c3d4e3e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.c
@@ -0,0 +1,455 @@
+/** @file
+
+Copyright (c) 2014, 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 "UfsBlockIoPei.h"
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+UFS_PEIM_MEM_BLOCK *
+UfsPeimAllocMemBlock (
+  IN  UINTN                    Pages
+  )
+{
+  UFS_PEIM_MEM_BLOCK           *Block;
+  EFI_STATUS                   Status;
+  VOID                         *TempPtr;
+  EFI_PHYSICAL_ADDRESS         Address;
+
+  TempPtr = NULL;
+  Block   = NULL;
+
+  Status = PeiServicesAllocatePool (sizeof(UFS_PEIM_MEM_BLOCK), &TempPtr);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  ZeroMem ((VOID*)(UINTN)TempPtr, sizeof(UFS_PEIM_MEM_BLOCK));
+  
+  //
+  // each bit in the bit array represents UFS_PEIM_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (UFS_PEIM_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+  
+  Block = (UFS_PEIM_MEM_BLOCK*)(UINTN)TempPtr;
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (UFS_PEIM_MEM_UNIT * 8);
+  
+  Status = PeiServicesAllocatePool (Block->BitsLen, &TempPtr);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  ZeroMem ((VOID*)(UINTN)TempPtr, Block->BitsLen);
+
+  Block->Bits = (UINT8*)(UINTN)TempPtr;
+
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             Pages,
+             &Address
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  ZeroMem ((VOID*)(UINTN)Address, EFI_PAGES_TO_SIZE (Pages));
+
+  Block->Buf  = (UINT8*)((UINTN)Address);
+  Block->Next = NULL;
+
+  return Block;
+}
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UfsPeimFreeMemBlock (
+  IN UFS_PEIM_MEM_POOL       *Pool,
+  IN UFS_PEIM_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Pool != NULL) && (Block != NULL));
+}
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block          The memory block to allocate memory from.
+  @param  Units          Number of memory units to allocate.
+
+  @return The pointer to the allocated memory. If couldn't allocate the needed memory,
+          the return value is NULL.
+
+**/
+VOID *
+UfsPeimAllocMemFromBlock (
+  IN  UFS_PEIM_MEM_BLOCK  *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!UFS_PEIM_MEM_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      UFS_PEIM_NEXT_BIT (Byte, Bit);
+
+    } else {
+      UFS_PEIM_NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+  
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!UFS_PEIM_MEM_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | (UINT8) UFS_PEIM_MEM_BIT (Bit));
+    UFS_PEIM_NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->Buf + (StartByte * 8 + StartBit) * UFS_PEIM_MEM_UNIT;
+}
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head           The head of the memory pool's block list.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UfsPeimInsertMemBlockToPool (
+  IN UFS_PEIM_MEM_BLOCK      *Head,
+  IN UFS_PEIM_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+/**
+  Is the memory block empty?
+
+  @param  Block   The memory block to check.
+
+  @retval TRUE    The memory block is empty.
+  @retval FALSE   The memory block isn't empty.
+
+**/
+BOOLEAN
+UfsPeimIsMemBlockEmpty (
+  IN UFS_PEIM_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UfsPeimUnlinkMemBlock (
+  IN UFS_PEIM_MEM_BLOCK      *Head,
+  IN UFS_PEIM_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  UFS_PEIM_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+/**
+  Initialize the memory management pool for the host controller.
+  
+  @param  Private               The Ufs Peim driver private data.
+
+  @retval EFI_SUCCESS           The memory pool is initialized.
+  @retval Others                Fail to init the memory pool.
+
+**/
+EFI_STATUS
+UfsPeimInitMemPool (
+  IN  UFS_PEIM_HC_PRIVATE_DATA  *Private
+  )
+{
+  UFS_PEIM_MEM_POOL          *Pool;
+  EFI_STATUS                 Status;
+  VOID                       *TempPtr;
+
+  TempPtr = NULL;
+  Pool    = NULL;
+  
+  Status = PeiServicesAllocatePool (sizeof (UFS_PEIM_MEM_POOL), &TempPtr);
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ZeroMem ((VOID*)(UINTN)TempPtr, sizeof (UFS_PEIM_MEM_POOL));
+
+  Pool = (UFS_PEIM_MEM_POOL *)((UINTN)TempPtr);
+
+  Pool->Head = UfsPeimAllocMemBlock (UFS_PEIM_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Private->Pool = Pool;
+  return EFI_SUCCESS;
+}
+
+/**
+  Release the memory management pool.
+  
+  @param  Pool                  The memory pool to free.
+
+  @retval EFI_DEVICE_ERROR      Fail to free the memory pool.
+  @retval EFI_SUCCESS           The memory pool is freed.
+
+**/
+EFI_STATUS
+UfsPeimFreeMemPool (
+  IN UFS_PEIM_MEM_POOL       *Pool
+  )
+{
+  UFS_PEIM_MEM_BLOCK         *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UfsPeimUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    UfsPeimFreeMemBlock (Pool, Block);
+  }
+
+  UfsPeimFreeMemBlock (Pool, Pool->Head);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+  
+  @param  Pool      The host controller's memory pool.
+  @param  Size      Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UfsPeimAllocateMem (
+  IN  UFS_PEIM_MEM_POOL        *Pool,
+  IN  UINTN                    Size
+  )
+{
+  UFS_PEIM_MEM_BLOCK         *Head;
+  UFS_PEIM_MEM_BLOCK         *Block;
+  UFS_PEIM_MEM_BLOCK         *NewBlock;
+  VOID                       *Mem;
+  UINTN                      AllocSize;
+  UINTN                      Pages;
+
+  Mem       = NULL;
+  AllocSize = UFS_PEIM_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UfsPeimAllocMemFromBlock (Block, AllocSize / UFS_PEIM_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (UFS_PEIM_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = UFS_PEIM_MEM_DEFAULT_PAGES;
+  }
+
+  NewBlock = UfsPeimAllocMemBlock (Pages);
+  if (NewBlock == NULL) {
+    return NULL;
+  }
+  
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UfsPeimInsertMemBlockToPool (Head, NewBlock);
+  Mem = UfsPeimAllocMemFromBlock (NewBlock, AllocSize / UFS_PEIM_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UfsPeimFreeMem (
+  IN UFS_PEIM_MEM_POOL    *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  UFS_PEIM_MEM_BLOCK      *Head;
+  UFS_PEIM_MEM_BLOCK      *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = UFS_PEIM_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->Buf <= ToFree) && ((ToFree + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->Buf) / UFS_PEIM_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->Buf) / UFS_PEIM_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / UFS_PEIM_MEM_UNIT); Count++) {
+        ASSERT (UFS_PEIM_MEM_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ UFS_PEIM_MEM_BIT (Bit));
+        UFS_PEIM_NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UfsPeimIsMemBlockEmpty (Block)) {
+    UfsPeimFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.h b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.h
new file mode 100644
index 0000000000..3c4b2407c8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.h
@@ -0,0 +1,61 @@
+/** @file
+
+Copyright (c) 2014, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _UFS_PEIM_MEM_H_
+#define _UFS_PEIM_MEM_H_
+
+#define UFS_PEIM_MEM_BIT(a)          ((UINTN)(1 << (a)))
+
+#define UFS_PEIM_MEM_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & UFS_PEIM_MEM_BIT(Bit)) == UFS_PEIM_MEM_BIT(Bit)))
+
+typedef struct _UFS_PEIM_MEM_BLOCK UFS_PEIM_MEM_BLOCK;
+
+struct _UFS_PEIM_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen; 
+  UINT8                   *Buf;
+  UINTN                   BufLen;   // Memory size in bytes
+  UFS_PEIM_MEM_BLOCK      *Next;
+};
+
+typedef struct _UFS_PEIM_MEM_POOL {
+  UFS_PEIM_MEM_BLOCK         *Head;
+} UFS_PEIM_MEM_POOL;
+
+//
+// Memory allocation unit, note that the value must meet UFS spec alignment requirement.
+//
+#define UFS_PEIM_MEM_UNIT           128
+
+#define UFS_PEIM_MEM_UNIT_MASK      (UFS_PEIM_MEM_UNIT - 1)
+#define UFS_PEIM_MEM_DEFAULT_PAGES  16
+
+#define UFS_PEIM_MEM_ROUND(Len)  (((Len) + UFS_PEIM_MEM_UNIT_MASK) & (~UFS_PEIM_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define UFS_PEIM_NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.c b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.c
new file mode 100644
index 0000000000..68800570cb
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.c
@@ -0,0 +1,1787 @@
+/** @file
+
+  Copyright (c) 2014, 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 "UfsBlockIoPei.h"
+
+/**
+  Wait for the value of the specified system memory set to the test value.
+
+  @param  Address           The system memory address to test.
+  @param  MaskValue         The mask value of memory.
+  @param  TestValue         The test value of memory.
+  @param  Timeout           The time out value for wait memory set, uses 100ns as a unit.
+
+  @retval EFI_TIMEOUT       The system memory setting is time out.
+  @retval EFI_SUCCESS       The system memory is correct set.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsWaitMemSet (
+  IN  UINTN                     Address,
+  IN  UINT32                    MaskValue,
+  IN  UINT32                    TestValue,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32     Value;
+  UINT64     Delay;
+  BOOLEAN    InfiniteWait;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32 (Timeout, 10) + 1;
+
+  do {
+    //
+    // Access PCI MMIO space to see if the value is the tested one.
+    //
+    Value = MmioRead32 (Address) & MaskValue;
+
+    if (Value == TestValue) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 1 microseconds.
+    //
+    MicroSecondDelay (1);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Dump UIC command execution result for debugging.
+
+  @param[in]   UicOpcode  The executed UIC opcode.
+  @param[in]   Result     The result to be parsed.
+
+**/
+VOID
+DumpUicCmdExecResult (
+  IN  UINT8     UicOpcode,
+  IN  UINT8     Result
+  )
+{
+  if (UicOpcode <= UfsUicDmePeerSet) {
+    switch (Result) {
+      case 0x00:
+        break;
+      case 0x01:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - INVALID_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x02:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - INVALID_MIB_ATTRIBUTE_VALUE\n"));
+        break;
+      case 0x03:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - READ_ONLY_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x04:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - WRITE_ONLY_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x05:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BAD_INDEX\n"));
+        break;
+      case 0x06:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - LOCKED_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x07:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BAD_TEST_FEATURE_INDEX\n"));
+        break;
+      case 0x08:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - PEER_COMMUNICATION_FAILURE\n"));
+        break; 
+      case 0x09:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BUSY\n"));
+        break;
+      case 0x0A:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - DME_FAILURE\n"));
+        break;        
+      default :
+        ASSERT (FALSE);
+        break;
+    }
+  } else {
+    switch (Result) {
+      case 0x00:
+        break;
+      case 0x01:
+        DEBUG ((EFI_D_VERBOSE, "UIC control command fails - FAILURE\n"));
+        break;     
+      default :
+        ASSERT (FALSE);
+        break;
+    }
+  }
+}
+
+/**
+  Dump QUERY RESPONSE UPIU result for debugging.
+
+  @param[in]   Result  The result to be parsed.
+
+**/
+VOID
+DumpQueryResponseResult (
+  IN  UINT8     Result
+  )
+{
+  switch (Result) {
+    case 0xF6:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Not Readable\n"));
+      break;
+    case 0xF7:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Not Writeable\n"));
+      break;
+    case 0xF8:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Already Written\n"));
+      break;
+    case 0xF9:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Length\n"));
+      break;
+    case 0xFA:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Value\n"));
+      break;
+    case 0xFB:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Selector\n"));
+      break;
+    case 0xFC:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Index\n"));
+      break;
+    case 0xFD:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Idn\n"));
+      break;
+    case 0xFE:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Opcode\n"));
+      break; 
+    case 0xFF:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with General Failure\n"));
+      break;
+    default :
+      ASSERT (FALSE);
+      break;
+  }
+}
+
+/**
+  Swap little endian to big endian.
+
+  @param[in, out] Buffer      The data buffer. In input, it contains little endian data.
+                              In output, it will become big endian.
+  @param[in]      BufferSize  The length of converted data.
+
+**/
+VOID
+SwapLittleEndianToBigEndian (
+  IN OUT UINT8         *Buffer,
+  IN     UINT32        BufferSize
+  )
+{
+  UINT32 Index;
+  UINT8  Temp;
+  UINT32 SwapCount;
+
+  SwapCount = BufferSize / 2;
+  for (Index = 0; Index < SwapCount; Index++) {
+    Temp = Buffer[Index];
+    Buffer[Index] = Buffer[BufferSize - 1 - Index];
+    Buffer[BufferSize - 1 - Index] = Temp;
+  }
+}
+
+/**
+  Fill TSF field of QUERY REQUEST UPIU.
+
+  @param[in, out] TsfBase      The base address of TSF field of QUERY REQUEST UPIU.
+  @param[in]      Opcode       The opcode of request.
+  @param[in]      DescId       The descriptor ID of request.
+  @param[in]      Index        The index of request.
+  @param[in]      Selector     The selector of request.
+  @param[in]      Length       The length of transferred data. The maximum is 4.
+  @param[in]      Value        The value of transferred data.
+
+**/
+VOID
+UfsFillTsfOfQueryReqUpiu (
+  IN OUT UTP_UPIU_TSF        *TsfBase,
+  IN     UINT8               Opcode,
+  IN     UINT8               DescId    OPTIONAL,
+  IN     UINT8               Index     OPTIONAL,
+  IN     UINT8               Selector  OPTIONAL,
+  IN     UINT16              Length    OPTIONAL,
+  IN     UINT32              Value     OPTIONAL
+  )
+{
+  ASSERT (TsfBase != NULL);
+  ASSERT (Opcode <= UtpQueryFuncOpcodeTogFlag);
+
+  TsfBase->Opcode   = Opcode;
+  if (Opcode != UtpQueryFuncOpcodeNop) {
+    TsfBase->DescId   = DescId;
+    TsfBase->Index    = Index;
+    TsfBase->Selector = Selector;
+
+    if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeWrDesc)) {
+      SwapLittleEndianToBigEndian ((UINT8*)&Length, sizeof (Length));
+      TsfBase->Length = Length;
+    }
+  
+    if (Opcode == UtpQueryFuncOpcodeWrAttr) {
+      SwapLittleEndianToBigEndian ((UINT8*)&Value, sizeof (Value));
+      TsfBase->Value  = Value;
+    }
+  }
+}
+
+/**
+  Initialize COMMAND UPIU.
+
+  @param[in, out] Command         The base address of COMMAND UPIU.
+  @param[in]      Lun             The Lun on which the SCSI command is executed.
+  @param[in]      TaskTag         The task tag of request.
+  @param[in]      Cdb             The cdb buffer containing SCSI command.
+  @param[in]      CdbLength       The cdb length.
+  @param[in]      DataDirection   The direction of data transfer.
+  @param[in]      ExpDataTranLen  The expected transfer data length.
+
+  @retval EFI_SUCCESS     The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitCommandUpiu (
+  IN OUT UTP_COMMAND_UPIU              *Command,
+  IN     UINT8                         Lun,
+  IN     UINT8                         TaskTag,
+  IN     UINT8                         *Cdb,
+  IN     UINT8                         CdbLength,
+  IN     UFS_DATA_DIRECTION            DataDirection,
+  IN     UINT32                        ExpDataTranLen
+  )
+{
+  UINT8                   Flags;
+
+  ASSERT ((Command != NULL) && (Cdb != NULL));
+
+  //
+  // Task attribute is hard-coded to Ordered.
+  //
+  if (DataDirection == UfsDataIn) {
+    Flags = BIT0 | BIT6;
+  } else if (DataDirection == UfsDataOut) {
+    Flags = BIT0 | BIT5;
+  } else {
+    Flags = BIT0;
+  }
+
+  //
+  // Fill UTP COMMAND UPIU associated fields.
+  //
+  Command->TransCode = 0x01;
+  Command->Flags     = Flags;
+  Command->Lun       = Lun;
+  Command->TaskTag   = TaskTag;
+  Command->CmdSet    = 0x00;
+  SwapLittleEndianToBigEndian ((UINT8*)&ExpDataTranLen, sizeof (ExpDataTranLen));
+  Command->ExpDataTranLen = ExpDataTranLen;
+
+  CopyMem (Command->Cdb, Cdb, CdbLength);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UTP PRDT for data transfer.
+
+  @param[in] Prdt         The base address of PRDT.
+  @param[in] Buffer       The buffer to be read or written.
+  @param[in] BufferSize   The data size to be read or written.
+
+  @retval EFI_SUCCESS     The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitUtpPrdt (
+  IN  UTP_TR_PRD                       *Prdt,
+  IN  VOID                             *Buffer,
+  IN  UINT32                           BufferSize
+  )
+{
+  UINT32     PrdtIndex;
+  UINT32     RemainingLen;
+  UINT8      *Remaining;
+  UINTN      PrdtNumber;
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  ASSERT (((UINTN)Buffer & (BIT0 | BIT1)) == 0);
+
+  RemainingLen = BufferSize;
+  Remaining    = Buffer;
+  PrdtNumber   = (UINTN)DivU64x32 ((UINT64)BufferSize + UFS_MAX_DATA_LEN_PER_PRD - 1, UFS_MAX_DATA_LEN_PER_PRD);
+
+  for (PrdtIndex = 0; PrdtIndex < PrdtNumber; PrdtIndex++) {
+    if (RemainingLen < UFS_MAX_DATA_LEN_PER_PRD) {
+      Prdt[PrdtIndex].DbCount = (UINT32)RemainingLen - 1;
+    } else {
+      Prdt[PrdtIndex].DbCount = UFS_MAX_DATA_LEN_PER_PRD - 1;
+    }
+
+    Prdt[PrdtIndex].DbAddr  = (UINT32)RShiftU64 ((UINT64)(UINTN)Remaining, 2);
+    Prdt[PrdtIndex].DbAddrU = (UINT32)RShiftU64 ((UINT64)(UINTN)Remaining, 32);
+    RemainingLen -= UFS_MAX_DATA_LEN_PER_PRD;
+    Remaining    += UFS_MAX_DATA_LEN_PER_PRD;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize QUERY REQUEST UPIU.
+
+  @param[in, out] QueryReq      The base address of QUERY REQUEST UPIU.
+  @param[in]      TaskTag       The task tag of request.
+  @param[in]      Opcode        The opcode of request.
+  @param[in]      DescId        The descriptor ID of request.
+  @param[in]      Index         The index of request.
+  @param[in]      Selector      The selector of request.
+  @param[in]      DataSize      The data size to be read or written.
+  @param[in]      Data          The buffer to be read or written.
+
+  @retval EFI_SUCCESS           The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitQueryRequestUpiu (
+  IN OUT UTP_QUERY_REQ_UPIU            *QueryReq,
+  IN     UINT8                         TaskTag,
+  IN     UINT8                         Opcode,
+  IN     UINT8                         DescId,
+  IN     UINT8                         Index,
+  IN     UINT8                         Selector,
+  IN     UINTN                         DataSize   OPTIONAL,
+  IN     UINT8                         *Data      OPTIONAL
+  )
+{
+  ASSERT (QueryReq != NULL);
+
+  QueryReq->TransCode = 0x16;
+  QueryReq->TaskTag   = TaskTag;
+  if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeRdFlag) || (Opcode == UtpQueryFuncOpcodeRdAttr)) {
+    QueryReq->QueryFunc = QUERY_FUNC_STD_READ_REQ;
+  } else {
+    QueryReq->QueryFunc = QUERY_FUNC_STD_WRITE_REQ;
+  }
+
+  if (Opcode == UtpQueryFuncOpcodeWrAttr) {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, 0, *(UINT32*)Data);
+  } else if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeWrDesc)) {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, (UINT16)DataSize, 0);
+  } else {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, 0, 0);
+  }
+
+  if (Opcode == UtpQueryFuncOpcodeWrDesc) {
+    CopyMem (QueryReq + 1, Data, DataSize);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate COMMAND/RESPONSE UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun               The Lun on which the SCSI command is executed.
+  @param[in]  Packet            The pointer to the UFS_SCSI_REQUEST_PACKET data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsCreateScsiCommandDesc (
+  IN  UFS_PEIM_HC_PRIVATE_DATA            *Private,
+  IN  UINT8                               Lun,
+  IN  UFS_SCSI_REQUEST_PACKET             *Packet,
+  IN  UTP_TRD                             *Trd
+  )
+{
+  UINT8                    *CommandDesc;
+  UINTN                    TotalLen;
+  UINTN                    PrdtNumber;
+  VOID                     *Buffer;
+  UINT32                   Length;
+  UTP_COMMAND_UPIU         *CommandUpiu;
+  UTP_TR_PRD               *PrdtBase;
+  UFS_DATA_DIRECTION       DataDirection;
+
+  ASSERT ((Private != NULL) && (Packet != NULL) && (Trd != NULL));
+
+  if (Packet->DataDirection == UfsDataIn) {
+    Buffer = Packet->InDataBuffer;
+    Length = Packet->InTransferLength;
+    DataDirection = UfsDataIn;
+  } else {
+    Buffer = Packet->OutDataBuffer;
+    Length = Packet->OutTransferLength;
+    DataDirection = UfsDataOut;
+  }
+
+  if (Length == 0) {
+    DataDirection = UfsNoData;
+  }
+
+  PrdtNumber = (UINTN)DivU64x32 ((UINT64)Length + UFS_MAX_DATA_LEN_PER_PRD - 1, UFS_MAX_DATA_LEN_PER_PRD);
+
+  TotalLen    = ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)) + PrdtNumber * sizeof (UTP_TR_PRD);
+  CommandDesc = UfsPeimAllocateMem (Private->Pool, TotalLen);
+  if (CommandDesc == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CommandUpiu  = (UTP_COMMAND_UPIU*)CommandDesc;
+  PrdtBase     = (UTP_TR_PRD*)(CommandDesc + ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)));
+
+  UfsInitCommandUpiu (CommandUpiu, Lun, Private->TaskTag++, Packet->Cdb, Packet->CdbLength, DataDirection, Length);
+  UfsInitUtpPrdt (PrdtBase, Buffer, Length);
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Response UPIU and the Physical Region Description Table
+  // *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = DataDirection;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)(UINTN)CommandUpiu, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)(UINTN)CommandUpiu, 32);
+  Trd->RuL    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)), sizeof (UINT32));
+  Trd->RuO    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)), sizeof (UINT32));
+  Trd->PrdtL  = (UINT16)PrdtNumber;
+  Trd->PrdtO  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU))), sizeof (UINT32));
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate QUERY REQUEST/QUERY RESPONSE UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Packet            The pointer to the UFS_DEVICE_MANAGEMENT_REQUEST_PACKET data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+  @retval EFI_INVALID_PARAMETER The parameter passed in is invalid.
+
+**/
+EFI_STATUS
+UfsCreateDMCommandDesc (
+  IN  UFS_PEIM_HC_PRIVATE_DATA              *Private,
+  IN  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET  *Packet,
+  IN  UTP_TRD                               *Trd
+  )
+{
+  UINT8                         *CommandDesc;
+  UINTN                         TotalLen;
+  UTP_QUERY_REQ_UPIU            *QueryReqUpiu;
+  UINT8                         Opcode;
+  UINT32                        DataSize;
+  UINT8                         *Data;
+  UINT8                         DataDirection;
+
+  ASSERT ((Private != NULL) && (Packet != NULL) && (Trd != NULL));
+
+  Opcode = Packet->Opcode;
+  if ((Opcode > UtpQueryFuncOpcodeTogFlag) || (Opcode == UtpQueryFuncOpcodeNop)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DataDirection = Packet->DataDirection;
+  if (DataDirection == UfsDataIn) {
+    DataSize = Packet->InTransferLength;
+    Data     = Packet->InDataBuffer;
+  } else if (DataDirection == UfsDataOut) {
+    DataSize = Packet->OutTransferLength;
+    Data     = Packet->OutDataBuffer;
+  } else {
+    DataSize = 0;
+    Data     = NULL;
+  }
+
+  if (((Opcode != UtpQueryFuncOpcodeSetFlag) && (Opcode != UtpQueryFuncOpcodeClrFlag) && (Opcode != UtpQueryFuncOpcodeTogFlag))
+    && ((DataSize == 0) || (Data == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((Opcode == UtpQueryFuncOpcodeSetFlag) || (Opcode == UtpQueryFuncOpcodeClrFlag) || (Opcode == UtpQueryFuncOpcodeTogFlag))
+    && ((DataSize != 0) || (Data != NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Opcode == UtpQueryFuncOpcodeWrAttr) && (DataSize != sizeof (UINT32))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Opcode == UtpQueryFuncOpcodeWrDesc) || (Opcode == UtpQueryFuncOpcodeRdDesc)) {
+    TotalLen = ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)) + ROUNDUP8 (DataSize);
+  } else {
+    TotalLen = ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU));
+  }
+
+  CommandDesc = UfsPeimAllocateMem (Private->Pool, TotalLen);
+  if (CommandDesc == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Initialize UTP QUERY REQUEST UPIU
+  //
+  QueryReqUpiu = (UTP_QUERY_REQ_UPIU*)CommandDesc;
+  UfsInitQueryRequestUpiu (
+    QueryReqUpiu,
+    Private->TaskTag++,
+    Opcode,
+    Packet->DescId,
+    Packet->Index,
+    Packet->Selector,
+    DataSize,
+    Data
+    );
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Query Response UPIU *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = DataDirection;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->Ocs    = 0x0F;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)(UINTN)QueryReqUpiu, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)(UINTN)QueryReqUpiu, 32);
+  if (Opcode == UtpQueryFuncOpcodeWrDesc) {
+    Trd->RuL  = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)), sizeof (UINT32));
+    Trd->RuO  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (DataSize)), sizeof (UINT32));
+  } else {
+    Trd->RuL  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)) + ROUNDUP8 (DataSize)), sizeof (UINT32));
+    Trd->RuO  = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)), sizeof (UINT32));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate NOP IN and NOP OUT UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsCreateNopCommandDesc (
+  IN  UFS_PEIM_HC_PRIVATE_DATA                    *Private,
+  IN  UTP_TRD                                     *Trd
+  )
+{
+  UINT8                    *CommandDesc;
+  UINTN                    TotalLen;
+  UTP_NOP_OUT_UPIU         *NopOutUpiu;
+
+  ASSERT ((Private != NULL) && (Trd != NULL));
+
+  TotalLen    = ROUNDUP8 (sizeof (UTP_NOP_OUT_UPIU)) + ROUNDUP8 (sizeof (UTP_NOP_IN_UPIU));
+  CommandDesc = UfsPeimAllocateMem (Private->Pool, TotalLen);
+  if (CommandDesc == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NopOutUpiu = (UTP_NOP_OUT_UPIU*)CommandDesc;
+
+  NopOutUpiu->TaskTag = Private->TaskTag++;
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Nop Out UPIU *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = 0x00;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)(UINTN)NopOutUpiu, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)(UINTN)NopOutUpiu, 32);
+  Trd->RuL    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_NOP_IN_UPIU)), sizeof (UINT32));
+  Trd->RuO    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_NOP_OUT_UPIU)), sizeof (UINT32));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Find out available slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[out] Slot          The available slot.
+
+  @retval EFI_SUCCESS       The available slot was found successfully.
+
+**/
+EFI_STATUS
+UfsFindAvailableSlotInTrl (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+     OUT UINT8                        *Slot
+  )
+{
+  ASSERT ((Private != NULL) && (Slot != NULL));
+
+  //
+  // The simplest algo to always use slot 0.
+  // TODO: enhance it to support async transfer with multiple slot.
+  //
+  *Slot = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Find out available slot in task management transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[out] Slot          The available slot.
+
+  @retval EFI_SUCCESS       The available slot was found successfully.
+
+**/
+EFI_STATUS
+UfsFindAvailableSlotInTmrl (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+     OUT UINT8                        *Slot
+  )
+{
+  ASSERT ((Private != NULL) && (Slot != NULL));
+
+  //
+  // The simplest algo to always use slot 0.
+  // TODO: enhance it to support async transfer with multiple slot.
+  //
+  *Slot = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start specified slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Slot          The slot to be started.
+
+**/
+VOID
+UfsStartExecCmd (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        Slot
+  ) 
+{
+  UINTN         UfsHcBase;
+  UINTN         Address;
+  UINT32        Data;
+
+  UfsHcBase = Private->UfsHcBase;
+
+  Address = UfsHcBase + UFS_HC_UTRLRSR_OFFSET;  
+  Data    = MmioRead32 (Address);
+  if ((Data & UFS_HC_UTRLRSR) != UFS_HC_UTRLRSR) {
+    MmioWrite32 (Address, UFS_HC_UTRLRSR);
+  }
+
+  Address = UfsHcBase + UFS_HC_UTRLDBR_OFFSET;
+  MmioWrite32 (Address, BIT0 << Slot);
+}
+
+/**
+  Stop specified slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Slot          The slot to be stop.
+
+**/
+VOID
+UfsStopExecCmd (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        Slot
+  ) 
+{
+  UINTN         UfsHcBase;
+  UINTN         Address;
+  UINT32        Data;
+
+  UfsHcBase = Private->UfsHcBase;
+
+  Address = UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Data    = MmioRead32 (Address);
+  if ((Data & (BIT0 << Slot)) != 0) {
+    Address = UfsHcBase + UFS_HC_UTRLCLR_OFFSET;  
+    Data    = MmioRead32 (Address);
+    MmioWrite32 (Address, (Data & ~(BIT0 << Slot)));
+  }
+}
+
+/**
+  Read or write specified device descriptor of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      DescId        The ID of device descriptor.
+  @param[in]      Index         The Index of device descriptor.
+  @param[in]      Selector      The Selector of device descriptor.
+  @param[in, out] Descriptor    The buffer of device descriptor to be read or written.
+  @param[in]      DescSize      The size of device descriptor buffer.
+
+  @retval EFI_SUCCESS           The device descriptor was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the device descriptor.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the device descriptor.
+
+**/
+EFI_STATUS
+UfsRwDeviceDesc (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        DescId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT VOID                         *Descriptor,
+  IN     UINT32                       DescSize
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UINTN                                Address;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+  UINT16                               ReturnDataSize;
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    Packet.DataDirection     = UfsDataIn;
+    Packet.InDataBuffer      = Descriptor;
+    Packet.InTransferLength  = DescSize;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdDesc;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    Packet.OutDataBuffer     = Descriptor;
+    Packet.OutTransferLength = DescSize;
+    Packet.Opcode            = UtpQueryFuncOpcodeWrDesc;
+  }
+  Packet.DescId              = DescId;
+  Packet.Index               = Index;
+  Packet.Selector            = Selector;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  CmdDescBase = (UINT8 *)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    ReturnDataSize = QueryResp->Tsf.Length;
+    SwapLittleEndianToBigEndian ((UINT8*)&ReturnDataSize, sizeof (UINT16));
+
+    if (Read) {
+      CopyMem (Packet.InDataBuffer, (QueryResp + 1), ReturnDataSize);
+      Packet.InTransferLength = ReturnDataSize;
+    } else {
+      Packet.OutTransferLength = ReturnDataSize;
+    }
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+/**
+  Read or write specified attribute of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      AttrId        The ID of Attribute.
+  @param[in]      Index         The Index of Attribute.
+  @param[in]      Selector      The Selector of Attribute.
+  @param[in, out] Attributes    The value of Attribute to be read or written.
+
+  @retval EFI_SUCCESS           The Attribute was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the Attribute.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the Attribute.
+
+**/
+EFI_STATUS
+UfsRwAttributes (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        AttrId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT UINT32                       *Attributes
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UINTN                                Address;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+  UINT32                               ReturnData;
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    Packet.DataDirection     = UfsDataIn;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdAttr;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    Packet.Opcode            = UtpQueryFuncOpcodeWrAttr;
+  }
+  Packet.DescId              = AttrId;
+  Packet.Index               = Index;
+  Packet.Selector            = Selector;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  CmdDescBase = (UINT8 *)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    ReturnData = QueryResp->Tsf.Value;
+    SwapLittleEndianToBigEndian ((UINT8*)&ReturnData, sizeof (UINT32));
+    *Attributes = ReturnData;
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+/**
+  Read or write specified flag of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      FlagId        The ID of flag to be read or written.
+  @param[in, out] Value         The value to set or clear flag.
+
+  @retval EFI_SUCCESS           The flag was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the flag.
+
+**/
+EFI_STATUS
+UfsRwFlags (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        FlagId,
+  IN OUT UINT8                        *Value
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UINTN                                Address;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+
+  if (Value == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    ASSERT (Value != NULL);
+    Packet.DataDirection     = UfsDataIn;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdFlag;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    if (*Value == 1) {
+      Packet.Opcode          = UtpQueryFuncOpcodeSetFlag;
+    } else if (*Value == 0) {
+      Packet.Opcode          = UtpQueryFuncOpcodeClrFlag;
+    } else {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  Packet.DescId              = FlagId;
+  Packet.Index               = 0;
+  Packet.Selector            = 0;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Trd    = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  CmdDescBase = (UINT8 *)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    *Value = (UINT8)QueryResp->Tsf.Value;
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+/**
+  Set specified flag to 1 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be set.
+
+  @retval EFI_SUCCESS           The flag was set successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to set the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of setting the flag.
+
+**/
+EFI_STATUS
+UfsSetFlag (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        FlagId
+  )
+{
+  EFI_STATUS             Status;
+  UINT8                  Value;
+
+  Value  = 1;
+  Status = UfsRwFlags (Private, FALSE, FlagId, &Value);
+
+  return Status;
+}
+
+/**
+  Clear specified flag to 0 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be cleared.
+
+  @retval EFI_SUCCESS           The flag was cleared successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to clear the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of clearing the flag.
+
+**/
+EFI_STATUS
+UfsClearFlag (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        FlagId
+  )
+{
+  EFI_STATUS             Status;
+  UINT8                  Value;
+
+  Value  = 0;
+  Status = UfsRwFlags (Private, FALSE, FlagId, &Value);
+
+  return Status;
+}
+
+/**
+  Read specified flag from a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be read.
+  @param[out] Value             The flag's value.
+
+  @retval EFI_SUCCESS           The flag was read successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to read the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of reading the flag.
+
+**/
+EFI_STATUS
+UfsReadFlag (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINT8                        FlagId,
+     OUT UINT8                        *Value
+  )
+{
+  EFI_STATUS                           Status;
+
+  Status = UfsRwFlags (Private, TRUE, FlagId, Value);
+
+  return Status;
+}
+
+/**
+  Sends NOP IN cmd to a UFS device for initialization process request.
+  For more details, please refer to UFS 2.0 spec Figure 13.3.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS           The NOP IN command was sent by the host. The NOP OUT response was
+                                received successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to execute NOP IN command.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the NOP IN command to execute.
+
+**/
+EFI_STATUS
+UfsExecNopCmds (
+  IN  UFS_PEIM_HC_PRIVATE_DATA         *Private
+  )
+{
+  EFI_STATUS                           Status;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_NOP_IN_UPIU                      *NopInUpiu;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+  UINTN                                Address;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Trd    = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  Status = UfsCreateNopCommandDesc (Private, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  CmdDescBase = (UINT8 *)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  NopInUpiu   = (UTP_NOP_IN_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (NopInUpiu->Resp != 0) {
+    Status = EFI_DEVICE_ERROR;
+  } else {
+    Status = EFI_SUCCESS;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+/**
+  Sends a UFS-supported SCSI Request Packet to a UFS device that is attached to the UFS host controller.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Lun           The LUN of the UFS device to send the SCSI Request Packet.
+  @param[in, out] Packet        A pointer to the SCSI Request Packet to send to a specified Lun of the
+                                UFS device.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsExecScsiCmds (
+  IN     UFS_PEIM_HC_PRIVATE_DATA      *Private,
+  IN     UINT8                         Lun,
+  IN OUT UFS_SCSI_REQUEST_PACKET       *Packet
+  )
+{
+  EFI_STATUS                           Status;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UINTN                                Address;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+  UTP_RESPONSE_UPIU                    *Response;
+  UINT16                               SenseDataLen;
+  UINT32                               ResTranCount;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateScsiCommandDesc (Private, Lun, Packet, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CmdDescBase = (UINT8*)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  CmdDescSize = Trd->PrdtO * sizeof (UINT32) + Trd->PrdtL * sizeof (UTP_TR_PRD);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, Packet->Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Get sense data if exists
+  //
+  Response     = (UTP_RESPONSE_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  SenseDataLen = Response->SenseDataLen;
+  SwapLittleEndianToBigEndian ((UINT8*)&SenseDataLen, sizeof (UINT16));
+  
+  if ((Packet->SenseDataLength != 0) && (Packet->SenseData != NULL)) {
+    CopyMem (Packet->SenseData, Response->SenseData, SenseDataLen);
+    Packet->SenseDataLength = (UINT8)SenseDataLen;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  if (Response->Response != 0) {
+    DEBUG ((EFI_D_ERROR, "UfsExecScsiCmds() fails with Target Failure\n"));
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    if (Packet->DataDirection == UfsDataIn) {
+      if ((Response->Flags & BIT5) == BIT5) {
+        ResTranCount = Response->ResTranCount;
+        SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+        Packet->InTransferLength -= ResTranCount;
+      }
+    } else if (Packet->DataDirection == UfsDataOut) {
+      if ((Response->Flags & BIT5) == BIT5) {
+        ResTranCount = Response->ResTranCount;
+        SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+        Packet->OutTransferLength -= ResTranCount;
+      }
+    }
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+
+/**
+  Sent UIC DME_LINKSTARTUP command to start the link startup procedure.
+
+  @param[in] Private          The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in] UicOpcode        The opcode of the UIC command.
+  @param[in] Arg1             The value for 1st argument of the UIC command.
+  @param[in] Arg2             The value for 2nd argument of the UIC command.
+  @param[in] Arg3             The value for 3rd argument of the UIC command.
+
+  @return EFI_SUCCESS      Successfully execute this UIC command and detect attached UFS device.
+  @return EFI_DEVICE_ERROR Fail to execute this UIC command and detect attached UFS device.
+  @return EFI_NOT_FOUND    The presence of the UFS device isn't detected.
+
+**/
+EFI_STATUS
+UfsExecUicCommands (
+  IN  UFS_PEIM_HC_PRIVATE_DATA      *Private,
+  IN  UINT8                         UicOpcode,
+  IN  UINT32                        Arg1,
+  IN  UINT32                        Arg2,
+  IN  UINT32                        Arg3
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Address;
+  UINT32      Data;
+  UINTN       UfsHcBase;
+
+  UfsHcBase = Private->UfsHcBase;
+  Address   = UfsHcBase + UFS_HC_IS_OFFSET;
+  Data      = MmioRead32 (Address);
+  if ((Data & UFS_HC_IS_UCCS) == UFS_HC_IS_UCCS) {
+    //
+    // Clear IS.BIT10 UIC Command Completion Status (UCCS) at first.
+    //
+    MmioWrite32 (Address, Data);
+  }
+
+  //
+  // When programming UIC command registers, host software shall set the register UICCMD
+  // only after all the UIC command argument registers (UICCMDARG1, UICCMDARG2 and UICCMDARG3)
+  // are set.
+  //
+  Address = UfsHcBase + UFS_HC_UCMD_ARG1_OFFSET;
+  MmioWrite32 (Address, Arg1);
+
+  Address = UfsHcBase + UFS_HC_UCMD_ARG2_OFFSET;
+  MmioWrite32 (Address, Arg2);
+
+  Address = UfsHcBase + UFS_HC_UCMD_ARG3_OFFSET;
+  MmioWrite32 (Address, Arg3);
+
+  //
+  // Host software shall only set the UICCMD if HCS.UCRDY is set to 1.
+  //
+  Address = Private->UfsHcBase + UFS_HC_STATUS_OFFSET;
+  Status = UfsWaitMemSet (Address, UFS_HC_HCS_UCRDY, UFS_HC_HCS_UCRDY, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Address = UfsHcBase + UFS_HC_UIC_CMD_OFFSET;
+  MmioWrite32 (Address, (UINT32)UicOpcode);
+
+  //
+  // UFS 2.0 spec section 5.3.1 Offset:0x20 IS.Bit10 UIC Command Completion Status (UCCS)
+  // This bit is set to '1' by the host controller upon completion of a UIC command. 
+  //
+  Address = UfsHcBase + UFS_HC_IS_OFFSET;
+  Data    = MmioRead32 (Address);
+  Status  = UfsWaitMemSet (Address, UFS_HC_IS_UCCS, UFS_HC_IS_UCCS, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (UicOpcode != UfsUicDmeReset) {
+    Address = UfsHcBase + UFS_HC_UCMD_ARG2_OFFSET;
+    Data    = MmioRead32 (Address);
+    if ((Data & 0xFF) != 0) {
+      DEBUG_CODE_BEGIN();
+        DumpUicCmdExecResult (UicOpcode, (UINT8)(Data & 0xFF));
+      DEBUG_CODE_END();
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Check value of HCS.DP and make sure that there is a device attached to the Link.
+  //
+  Address = UfsHcBase + UFS_HC_STATUS_OFFSET;  
+  Data    = MmioRead32 (Address);
+  if ((Data & UFS_HC_HCS_DP) == 0) {
+    Address = UfsHcBase + UFS_HC_IS_OFFSET;
+    Status  = UfsWaitMemSet (Address, UFS_HC_IS_ULSS, UFS_HC_IS_ULSS, UFS_TIMEOUT);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+    return EFI_NOT_FOUND;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsblockioPei: found a attached UFS device\n"));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Enable the UFS host controller for accessing.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS host controller enabling was executed successfully.
+  @retval EFI_DEVICE_ERROR           A device error occurred while enabling the UFS host controller.
+
+**/
+EFI_STATUS
+UfsEnableHostController (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS             Status;
+  UINTN                  Address;
+  UINT32                 Data;
+
+  //
+  // UFS 2.0 spec section 7.1.1 - Host Controller Initialization
+  //
+  // Reinitialize the UFS host controller if HCE bit of HC register is set.
+  //
+  Address = Private->UfsHcBase + UFS_HC_ENABLE_OFFSET;
+  Data    = MmioRead32 (Address);
+  if ((Data & UFS_HC_HCE_EN) == UFS_HC_HCE_EN) {
+    //
+    // Write a 0 to the HCE register at first to disable the host controller.
+    //
+    MmioWrite32 (Address, 0);
+    //
+    // Wait until HCE is read as '0' before continuing.
+    //
+    Status = UfsWaitMemSet (Address, UFS_HC_HCE_EN, 0, UFS_TIMEOUT);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Write a 1 to the HCE register to enable the UFS host controller.
+  //
+  MmioWrite32 (Address, UFS_HC_HCE_EN);
+  //
+  // Wait until HCE is read as '1' before continuing.
+  //
+  Status = UfsWaitMemSet (Address, UFS_HC_HCE_EN, UFS_HC_HCE_EN, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Detect if a UFS device attached.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS device detection was executed successfully.
+  @retval EFI_NOT_FOUND              Not found a UFS device attached.
+  @retval EFI_DEVICE_ERROR           A device error occurred while detecting the UFS device.
+
+**/
+EFI_STATUS
+UfsDeviceDetection (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  UINTN                  Retry;
+  EFI_STATUS             Status;
+
+  //
+  // Start UFS device detection.
+  // Try up to 3 times for establishing data link with device.
+  //
+  for (Retry = 0; Retry < 3; Retry++) {
+    Status = UfsExecUicCommands (Private, UfsUicDmeLinkStartup, 0, 0, 0);
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+
+    if (Status == EFI_NOT_FOUND) {
+      continue;
+    }
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (Retry == 3) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UFS task management request list related h/w context.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS task management list was initialzed successfully.
+  @retval EFI_DEVICE_ERROR           The initialization fails.
+
+**/
+EFI_STATUS
+UfsInitTaskManagementRequestList (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  UINTN                  Address;
+  UINT32                 Data;
+  UINT8                  Nutmrs;
+  EFI_PHYSICAL_ADDRESS   Buffer;
+  EFI_STATUS             Status;
+  
+  //
+  // Initial h/w and s/w context for future operations.
+  //
+  Address = Private->UfsHcBase + UFS_HC_CAP_OFFSET;  
+  Data    = MmioRead32 (Address);
+  Private->Capabilities = Data;
+
+  //
+  // Allocate and initialize UTP Task Management Request List.
+  //
+  Nutmrs = (UINT8) (RShiftU64 ((Private->Capabilities & UFS_HC_CAP_NUTMRS), 16) + 1);
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             EFI_SIZE_TO_PAGES (Nutmrs * sizeof (UTP_TMRD)),
+             &Buffer
+             );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  ZeroMem ((VOID*)(UINTN)Buffer, EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Nutmrs * sizeof (UTP_TMRD))));
+
+  //
+  // Program the UTP Task Management Request List Base Address and UTP Task Management
+  // Request List Base Address with a 64-bit address allocated at step 6.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTMRLBA_OFFSET;  
+  MmioWrite32 (Address, (UINT32)(UINTN)Buffer);
+  Address = Private->UfsHcBase + UFS_HC_UTMRLBAU_OFFSET;  
+  MmioWrite32 (Address, (UINT32)RShiftU64 ((UINT64)Buffer, 32));
+  Private->UtpTmrlBase = (VOID*)(UINTN)Buffer;
+  Private->Nutmrs      = Nutmrs;
+
+  //
+  // Enable the UTP Task Management Request List by setting the UTP Task Management
+  // Request List RunStop Register (UTMRLRSR) to '1'.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTMRLRSR_OFFSET;  
+  MmioWrite32 (Address, UFS_HC_UTMRLRSR);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UFS transfer request list related h/w context.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS transfer list was initialzed successfully.
+  @retval EFI_DEVICE_ERROR           The initialization fails.
+
+**/
+EFI_STATUS
+UfsInitTransferRequestList (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  UINTN                  Address;
+  UINT32                 Data;
+  UINT8                  Nutrs;
+  EFI_PHYSICAL_ADDRESS   Buffer;
+  EFI_STATUS             Status;
+  
+  //
+  // Initial h/w and s/w context for future operations.
+  //
+  Address = Private->UfsHcBase + UFS_HC_CAP_OFFSET;  
+  Data    = MmioRead32 (Address);
+  Private->Capabilities = Data;
+
+  //
+  // Allocate and initialize UTP Transfer Request List.
+  //
+  Nutrs  = (UINT8)((Private->Capabilities & UFS_HC_CAP_NUTRS) + 1);
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             EFI_SIZE_TO_PAGES (Nutrs * sizeof (UTP_TRD)),
+             &Buffer
+             );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  ZeroMem ((VOID*)(UINTN)Buffer, EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Nutrs * sizeof (UTP_TRD))));
+
+  //
+  // Program the UTP Transfer Request List Base Address and UTP Transfer Request List
+  // Base Address with a 64-bit address allocated at step 8.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTRLBA_OFFSET;  
+  MmioWrite32 (Address, (UINT32)(UINTN)Buffer);
+  Address = Private->UfsHcBase + UFS_HC_UTRLBAU_OFFSET;  
+  MmioWrite32 (Address, (UINT32)RShiftU64 ((UINT64)Buffer, 32));
+  Private->UtpTrlBase = (VOID*)(UINTN)Buffer;
+  Private->Nutrs      = Nutrs;
+  
+  //
+  // Enable the UTP Transfer Request List by setting the UTP Transfer Request List
+  // RunStop Register (UTRLRSR) to '1'.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTRLRSR_OFFSET;  
+  MmioWrite32 (Address, UFS_HC_UTRLRSR);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+UfsControllerInit (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS             Status;
+
+  Status = UfsEnableHostController (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsDevicePei: Enable Host Controller Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsDeviceDetection (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsDevicePei: Device Detection Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsInitTaskManagementRequestList (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsDevicePei: Task management list initialization Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsInitTransferRequestList (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsDevicePei: Transfer list initialization Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsDevicePei Finished\n"));
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is stopped successfully.
+  @retval Others                     A device error occurred while stopping the controller.
+
+**/
+EFI_STATUS
+UfsControllerStop (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS             Status;
+  UINTN                  Address;
+  UINT32                 Data;
+
+  //
+  // Enable the UTP Task Management Request List by setting the UTP Task Management
+  // Request List RunStop Register (UTMRLRSR) to '1'.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTMRLRSR_OFFSET;  
+  MmioWrite32 (Address, 0);
+
+  //
+  // Enable the UTP Transfer Request List by setting the UTP Transfer Request List
+  // RunStop Register (UTRLRSR) to '1'.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTRLRSR_OFFSET;  
+  MmioWrite32 (Address, 0);
+
+  //
+  // Write a 0 to the HCE register in order to disable the host controller.
+  //
+  Address = Private->UfsHcBase + UFS_HC_ENABLE_OFFSET;
+  Data    = MmioRead32 (Address);
+  ASSERT ((Data & UFS_HC_HCE_EN) == UFS_HC_HCE_EN);
+  MmioWrite32 (Address, 0);
+
+  //
+  // Wait until HCE is read as '0' before continuing.
+  //
+  Status = UfsWaitMemSet (Address, UFS_HC_HCE_EN, 0, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsDevicePei: Stop the UFS Host Controller\n"));
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.h b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.h
new file mode 100644
index 0000000000..a423a921de
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.h
@@ -0,0 +1,1339 @@
+/** @file
+  UfsPassThruDxe driver is used to produce EFI_EXT_SCSI_PASS_THRU protocol interface
+  for upper layer application to execute UFS-supported SCSI cmds.
+
+  Copyright (c) 2014, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _UFS_PASS_THRU_HCI_H_
+#define _UFS_PASS_THRU_HCI_H_
+
+//
+// Host Capabilities Register Offsets
+//
+#define UFS_HC_CAP_OFFSET          0x0000  // Controller Capabilities
+#define UFS_HC_VER_OFFSET          0x0008  // Version
+#define UFS_HC_DDID_OFFSET         0x0010  // Device ID and Device Class
+#define UFS_HC_PMID_OFFSET         0x0014  // Product ID and Manufacturer ID
+#define UFS_HC_AHIT_OFFSET         0x0018  // Auto-Hibernate Idle Timer
+//
+// Operation and Runtime Register Offsets
+//
+#define UFS_HC_IS_OFFSET           0x0020  // Interrupt Status
+#define UFS_HC_IE_OFFSET           0x0024  // Interrupt Enable
+#define UFS_HC_STATUS_OFFSET       0x0030  // Host Controller Status
+#define UFS_HC_ENABLE_OFFSET       0x0034  // Host Controller Enable
+#define UFS_HC_UECPA_OFFSET        0x0038  // Host UIC Error Code PHY Adapter Layer
+#define UFS_HC_UECDL_OFFSET        0x003c  // Host UIC Error Code Data Link Layer
+#define UFS_HC_UECN_OFFSET         0x0040  // Host UIC Error Code Network Layer
+#define UFS_HC_UECT_OFFSET         0x0044  // Host UIC Error Code Transport Layer
+#define UFS_HC_UECDME_OFFSET       0x0048  // Host UIC Error Code DME
+#define UFS_HC_UTRIACR_OFFSET      0x004c  // UTP Transfer Request Interrupt Aggregation Control Register
+//
+// UTP Transfer Register Offsets
+//
+#define UFS_HC_UTRLBA_OFFSET       0x0050  // UTP Transfer Request List Base Address
+#define UFS_HC_UTRLBAU_OFFSET      0x0054  // UTP Transfer Request List Base Address Upper 32-Bits
+#define UFS_HC_UTRLDBR_OFFSET      0x0058  // UTP Transfer Request List Door Bell Register
+#define UFS_HC_UTRLCLR_OFFSET      0x005c  // UTP Transfer Request List CLear Register
+#define UFS_HC_UTRLRSR_OFFSET      0x0060  // UTP Transfer Request Run-Stop Register
+//
+// UTP Task Management Register Offsets
+//
+#define UFS_HC_UTMRLBA_OFFSET      0x0070  // UTP Task Management Request List Base Address
+#define UFS_HC_UTMRLBAU_OFFSET     0x0074  // UTP Task Management Request List Base Address Upper 32-Bits
+#define UFS_HC_UTMRLDBR_OFFSET     0x0078  // UTP Task Management Request List Door Bell Register
+#define UFS_HC_UTMRLCLR_OFFSET     0x007c  // UTP Task Management Request List CLear Register
+#define UFS_HC_UTMRLRSR_OFFSET     0x0080  // UTP Task Management Run-Stop Register
+//
+// UIC Command Register Offsets
+//
+#define UFS_HC_UIC_CMD_OFFSET      0x0090  // UIC Command Register
+#define UFS_HC_UCMD_ARG1_OFFSET    0x0094  // UIC Command Argument 1
+#define UFS_HC_UCMD_ARG2_OFFSET    0x0098  // UIC Command Argument 2
+#define UFS_HC_UCMD_ARG3_OFFSET    0x009c  // UIC Command Argument 3
+//
+// UMA Register Offsets
+//
+#define UFS_HC_UMA_OFFSET          0x00b0  // Reserved for Unified Memory Extension
+
+#define UFS_HC_HCE_EN              BIT0
+#define UFS_HC_HCS_DP              BIT0
+#define UFS_HC_HCS_UCRDY           BIT3
+#define UFS_HC_IS_ULSS             BIT8
+#define UFS_HC_IS_UCCS             BIT10
+#define UFS_HC_CAP_64ADDR          BIT24
+#define UFS_HC_CAP_NUTMRS          (BIT16 | BIT17 | BIT18)
+#define UFS_HC_CAP_NUTRS           (BIT0 | BIT1 | BIT2 | BIT3 | BIT4)
+#define UFS_HC_UTMRLRSR            BIT0
+#define UFS_HC_UTRLRSR             BIT0
+
+//
+// A maximum of length of 256KB is supported by PRDT entry
+//
+#define UFS_MAX_DATA_LEN_PER_PRD   0x40000
+
+#define UFS_STORAGE_COMMAND_TYPE   0x01
+
+#define UFS_REGULAR_COMMAND        0x00
+#define UFS_INTERRUPT_COMMAND      0x01
+
+#define UFS_LUN_0                  0x00
+#define UFS_LUN_1                  0x01
+#define UFS_LUN_2                  0x02
+#define UFS_LUN_3                  0x03
+#define UFS_LUN_4                  0x04
+#define UFS_LUN_5                  0x05
+#define UFS_LUN_6                  0x06
+#define UFS_LUN_7                  0x07
+#define UFS_WLUN_REPORT_LUNS       0x81
+#define UFS_WLUN_UFS_DEV           0xD0
+#define UFS_WLUN_BOOT              0xB0
+#define UFS_WLUN_RPMB              0xC4
+
+#pragma pack(1)
+
+//
+// UFSHCI 2.0 Spec Section 5.2.1 Offset 00h: CAP - Controller Capabilities
+//
+typedef struct {
+  UINT8  Nutrs:4;      // Number of UTP Transfer Request Slots
+  UINT8  Rsvd1:4;
+
+  UINT8  NoRtt;        // Number of outstanding READY TO TRANSFER (RTT) requests supported
+
+  UINT8  Nutmrs:3;     // Number of UTP Task Management Request Slots
+  UINT8  Rsvd2:4;
+  UINT8  AutoHs:1;     // Auto-Hibernation Support
+
+  UINT8  As64:1;       // 64-bit addressing supported
+  UINT8  Oodds:1;      // Out of order data delivery supported
+  UINT8  UicDmetms:1;  // UIC DME_TEST_MODE command supported
+  UINT8  Ume:1;        // Reserved for Unified Memory Extension
+  UINT8  Rsvd4:4;
+} UFS_HC_CAP;
+
+//
+// UFSHCI 2.0 Spec Section 5.2.2 Offset 08h: VER - UFS Version
+//
+typedef struct {
+  UINT8  Vs:4;         // Version Suffix
+  UINT8  Mnr:4;        // Minor version number
+
+  UINT8  Mjr;          // Major version number
+
+  UINT16 Rsvd1;
+} UFS_HC_VER;
+
+//
+// UFSHCI 2.0 Spec Section 5.2.3 Offset 10h: HCPID - Host Controller Product ID
+//
+#define UFS_HC_PID     UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.2.4 Offset 14h: HCMID - Host Controller Manufacturer ID
+//
+#define UFS_HC_MID     UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.2.5 Offset 18h: AHIT - Auto-Hibernate Idle Timer
+//
+typedef struct {
+  UINT32 Ahitv:10;     // Auto-Hibernate Idle Timer Value 
+  UINT32 Ts:3;         // Timer scale
+  UINT32 Rsvd1:19;
+} UFS_HC_AHIT;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.1 Offset 20h: IS - Interrupt Status
+//
+typedef struct {
+  UINT16 Utrcs:1;      // UTP Transfer Request Completion Status
+  UINT16 Udepri:1;     // UIC DME_ENDPOINT_RESET Indication
+  UINT16 Ue:1;         // UIC Error
+  UINT16 Utms:1;       // UIC Test Mode Status 
+
+  UINT16 Upms:1;       // UIC Power Mode Status 
+  UINT16 Uhxs:1;       // UIC Hibernate Exit Status 
+  UINT16 Uhes:1;       // UIC Hibernate Enter Status 
+  UINT16 Ulls:1;       // UIC Link Lost Status 
+
+  UINT16 Ulss:1;       // UIC Link Startup Status 
+  UINT16 Utmrcs:1;     // UTP Task  Management Request Completion Status 
+  UINT16 Uccs:1;       // UIC Command Completion Status 
+  UINT16 Dfes:1;       // Device Fatal Error Status  
+
+  UINT16 Utpes:1;      // UTP Error Status  
+  UINT16 Rsvd1:3;
+
+  UINT16 Hcfes:1;      // Host Controller Fatal Error Status
+  UINT16 Sbfes:1;      // System Bus Fatal Error Status
+  UINT16 Rsvd2:14;
+} UFS_HC_IS;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.2 Offset 24h: IE - Interrupt Enable
+//
+typedef struct {
+  UINT16 Utrce:1;      // UTP Transfer Request Completion Enable
+  UINT16 Udeprie:1;    // UIC DME_ENDPOINT_RESET Enable
+  UINT16 Uee:1;        // UIC Error Enable
+  UINT16 Utmse:1;      // UIC Test Mode Status Enable
+
+  UINT16 Upmse:1;      // UIC Power Mode Status Enable 
+  UINT16 Uhxse:1;      // UIC Hibernate Exit Status Enable
+  UINT16 Uhese:1;      // UIC Hibernate Enter Status Enable 
+  UINT16 Ullse:1;      // UIC Link Lost Status Enable
+
+  UINT16 Ulsse:1;      // UIC Link Startup Status Enable
+  UINT16 Utmrce:1;     // UTP Task  Management Request Completion Enable
+  UINT16 Ucce:1;       // UIC Command Completion Enable
+  UINT16 Dfee:1;       // Device Fatal Error Enable
+
+  UINT16 Utpee:1;      // UTP Error Enable
+  UINT16 Rsvd1:3;
+
+  UINT16 Hcfee:1;      // Host Controller Fatal Error Enable
+  UINT16 Sbfee:1;      // System Bus Fatal Error Enable
+  UINT16 Rsvd2:14;
+} UFS_HC_IE;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.3 Offset 30h: HCS - Host Controller Status
+//
+typedef struct {
+  UINT8  Dp:1;         // Device Present
+  UINT8  UtrlRdy:1;    // UTP Transfer Request List Ready
+  UINT8  UtmrlRdy:1;   // UTP Task Management Request List Ready
+  UINT8  UcRdy:1;      // UIC COMMAND Ready
+  UINT8  Rsvd1:4;
+
+  UINT8  Upmcrs:3;     // UIC Power Mode Change Request Status
+  UINT8  Rsvd2:1;      // UIC Hibernate Exit Status Enable
+  UINT8  Utpec:4;      // UTP Error Code
+
+  UINT8  TtagUtpE;     // Task Tag of UTP error
+  UINT8  TlunUtpE;     // Target LUN of UTP error
+} UFS_HC_STATUS;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.4 Offset 34h: HCE - Host Controller Enable
+//
+typedef struct {
+  UINT32 Hce:1;        // Host Controller Enable
+  UINT32 Rsvd1:31;
+} UFS_HC_ENABLE;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.5 Offset 38h: UECPA - Host UIC Error Code PHY Adapter Layer
+//
+typedef struct {
+  UINT32 Ec:5;         // UIC PHY Adapter Layer Error Code
+  UINT32 Rsvd1:26;
+  UINT32 Err:1;        // UIC PHY Adapter Layer Error
+} UFS_HC_UECPA;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.6 Offset 3ch: UECDL - Host UIC Error Code Data Link Layer
+//
+typedef struct {
+  UINT32 Ec:15;        // UIC Data Link Layer Error Code
+  UINT32 Rsvd1:16;
+  UINT32 Err:1;        // UIC Data Link Layer Error
+} UFS_HC_UECDL;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.7 Offset 40h: UECN - Host UIC Error Code Network Layer
+//
+typedef struct {
+  UINT32 Ec:3;         // UIC Network Layer Error Code
+  UINT32 Rsvd1:28;
+  UINT32 Err:1;        // UIC Network Layer Error
+} UFS_HC_UECN;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.8 Offset 44h: UECT - Host UIC Error Code Transport Layer
+//
+typedef struct {
+  UINT32 Ec:7;         // UIC Transport Layer Error Code
+  UINT32 Rsvd1:24;
+  UINT32 Err:1;        // UIC Transport Layer Error
+} UFS_HC_UECT;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.9 Offset 48h: UECDME - Host UIC Error Code
+//
+typedef struct {
+  UINT32 Ec:1;         // UIC DME Error Code
+  UINT32 Rsvd1:30;
+  UINT32 Err:1;        // UIC DME Error
+} UFS_HC_UECDME;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.10 Offset 4Ch: UTRIACR - UTP Transfer Request Interrupt Aggregation Control Register
+//
+typedef struct {
+  UINT8  IaToVal;      // Interrupt aggregation timeout value
+
+  UINT8  IacTh:5;      // Interrupt aggregation counter threshold
+  UINT8  Rsvd1:3;
+
+  UINT8  Ctr:1;        // Counter and Timer Reset
+  UINT8  Rsvd2:3;
+  UINT8  Iasb:1;       // Interrupt aggregation status bit
+  UINT8  Rsvd3:3;
+
+  UINT8  IapwEn:1;     // Interrupt aggregation parameter write enable
+  UINT8  Rsvd4:6;
+  UINT8  IaEn:1;       // Interrupt Aggregation Enable/Disable
+} UFS_HC_UTRIACR;
+
+//
+// UFSHCI 2.0 Spec Section 5.4.1 Offset 50h: UTRLBA - UTP Transfer Request List Base Address
+//
+typedef struct {
+  UINT32 Rsvd1:10;
+  UINT32 UtrlBa:22;    // UTP Transfer Request List Base Address
+} UFS_HC_UTRLBA;
+
+//
+// UFSHCI 2.0 Spec Section 5.4.2 Offset 54h: UTRLBAU - UTP Transfer Request List Base Address Upper 32-bits
+//
+#define UFS_HC_UTRLBAU UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.4.3 Offset 58h: UTRLDBR - UTP Transfer Request List Door Bell Register
+//
+#define UFS_HC_UTRLDBR UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.4.4 Offset 5Ch: UTRLCLR - UTP Transfer Request List CLear Register
+//
+#define UFS_HC_UTRLCLR UINT32
+
+#if 0
+//
+// UFSHCI 2.0 Spec Section 5.4.5 Offset 60h: UTRLRSR - UTP Transfer Request List Run Stop Register
+//
+typedef struct {
+  UINT32 UtrlRsr:1;    // UTP Transfer Request List Run-Stop Register
+  UINT32 Rsvd1:31;
+} UFS_HC_UTRLRSR;
+#endif
+
+//
+// UFSHCI 2.0 Spec Section 5.5.1 Offset 70h: UTMRLBA - UTP Task Management Request List Base Address
+//
+typedef struct {
+  UINT32 Rsvd1:10;
+  UINT32 UtmrlBa:22;   // UTP Task Management Request List Base Address
+} UFS_HC_UTMRLBA;
+
+//
+// UFSHCI 2.0 Spec Section 5.5.2 Offset 74h: UTMRLBAU - UTP Task Management Request List Base Address Upper 32-bits
+//
+#define UFS_HC_UTMRLBAU UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.5.3 Offset 78h: UTMRLDBR - UTP Task Management Request List Door Bell Register
+//
+typedef struct {
+  UINT32 UtmrlDbr:8;   // UTP Task Management Request List Door bell Register
+  UINT32 Rsvd1:24;
+} UFS_HC_UTMRLDBR;
+
+//
+// UFSHCI 2.0 Spec Section 5.5.4 Offset 7Ch: UTMRLCLR - UTP Task Management Request List CLear Register
+//
+typedef struct {
+  UINT32 UtmrlClr:8;   // UTP Task Management List Clear Register
+  UINT32 Rsvd1:24;
+} UFS_HC_UTMRLCLR;
+
+#if 0
+//
+// UFSHCI 2.0 Spec Section 5.5.5 Offset 80h: UTMRLRSR - UTP Task Management Request List Run Stop Register
+//
+typedef struct {
+  UINT32 UtmrlRsr:1;   // UTP Task Management Request List Run-Stop Register
+  UINT32 Rsvd1:31;
+} UFS_HC_UTMRLRSR;
+#endif
+
+//
+// UFSHCI 2.0 Spec Section 5.6.1 Offset 90h: UICCMD - UIC Command
+//
+typedef struct {
+  UINT32 CmdOp:8;      // Command Opcode
+  UINT32 Rsvd1:24;
+} UFS_HC_UICCMD;
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 94h: UICCMDARG1 - UIC Command Argument 1
+//
+#define UFS_HC_UICCMD_ARG1 UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 98h: UICCMDARG2 - UIC Command Argument 2
+//
+#define UFS_HC_UICCMD_ARG2 UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 9ch: UICCMDARG3 - UIC Command Argument 3
+//
+#define UFS_HC_UICCMD_ARG3 UINT32
+
+//
+// UIC command opcodes
+//
+typedef enum {
+  UfsUicDmeGet            = 0x01,
+  UfsUicDmeSet            = 0x02,
+  UfsUicDmePeerGet        = 0x03,
+  UfsUicDmePeerSet        = 0x04,
+  UfsUicDmePwrOn          = 0x10,
+  UfsUicDmePwrOff         = 0x11,
+  UfsUicDmeEnable         = 0x12,
+  UfsUicDmeReset          = 0x14,
+  UfsUicDmeEndpointReset  = 0x15,
+  UfsUicDmeLinkStartup    = 0x16,
+  UfsUicDmeHibernateEnter = 0x17,
+  UfsUicDmeHibernateExit  = 0x18,
+  UfsUicDmeTestMode       = 0x1A
+} UFS_UIC_OPCODE;
+
+//
+// UTP Transfer Request Descriptor
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:24;
+  UINT32 Int:1;               /* Interrupt */
+  UINT32 Dd:2;                /* Data Direction */
+  UINT32 Rsvd2:1;
+  UINT32 Ct:4;                /* Command Type */
+
+  //
+  // DW1
+  //
+  UINT32 Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT32 Ocs:8;               /* Overall Command Status */
+  UINT32 Rsvd4:24;
+
+  //
+  // DW3
+  //
+  UINT32 Rsvd5;
+
+  //
+  // DW4
+  //
+  UINT32 Rsvd6:7;
+  UINT32 UcdBa:25;            /* UTP Command Descriptor Base Address */
+  
+  //
+  // DW5
+  //
+  UINT32 UcdBaU;              /* UTP Command Descriptor Base Address Upper 32-bits */
+ 
+  //
+  // DW6
+  //
+  UINT16 RuL;                 /* Response UPIU Length */  
+  UINT16 RuO;                 /* Response UPIU Offset */
+
+  //
+  // DW7
+  //
+  UINT16 PrdtL;               /* PRDT Length */  
+  UINT16 PrdtO;               /* PRDT Offset */
+} UTP_TRD;
+
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:2;
+  UINT32 DbAddr:30;           /* Data Base Address */
+  
+  //
+  // DW1
+  //
+  UINT32 DbAddrU;             /* Data Base Address Upper 32-bits */
+ 
+  //
+  // DW2
+  //
+  UINT32 Rsvd2;
+
+  //
+  // DW3
+  //
+  UINT32 DbCount:18;          /* Data Byte Count */
+  UINT32 Rsvd3:14;
+} UTP_TR_PRD;
+
+//
+// UFS 2.0 Spec Section 10.5.3 - UTP Command UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x01*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  CmdSet:4;            /* Command Set Type */
+  UINT8  Rsvd1:4;
+  UINT8  Rsvd2;
+  UINT8  Rsvd3;
+  UINT8  Rsvd4;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd5;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 ExpDataTranLen;      /* Expected Data Transfer Length - Big Endian */
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Cdb[16];
+} UTP_COMMAND_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.4 - UTP Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x21*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  CmdSet:4;            /* Command Set Type */
+  UINT8  Rsvd1:4;
+  UINT8  Rsvd2;
+  UINT8  Response;            /* Response */
+  UINT8  Status;              /* Status */
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 ResTranCount;        /* Residual Transfer Count - Big Endian */
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Rsvd3[16];
+
+  //
+  // Data Segment - Sense Data
+  //
+  UINT16 SenseDataLen;        /* Sense Data Length - Big Endian */
+  UINT8  SenseData[18];       /* Sense Data */
+} UTP_RESPONSE_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.5 - UTP Data-Out UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x02*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be sent out
+  //
+  //UINT8  Data[];            /* Data to be sent out, maximum is 65535 bytes */
+} UTP_DATA_OUT_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.6 - UTP Data-In UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x22*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be read
+  //
+  //UINT8  Data[];            /* Data to be read, maximum is 65535 bytes */
+} UTP_DATA_IN_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.7 - UTP Ready-To-Transfer UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x31*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be read
+  //
+  //UINT8  Data[];            /* Data to be read, maximum is 65535 bytes */
+} UTP_RDY_TO_TRAN_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.8 - UTP Task Management Request UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x04*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1;
+  UINT8  TskManFunc;          /* Task Management Function */
+  UINT8  Rsvd2[2];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 InputParam1;         /* Input Parameter 1 - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 InputParam2;         /* Input Parameter 2 - Big Endian */
+
+  //
+  // DW5
+  //
+  UINT32 InputParam3;         /* Input Parameter 3 - Big Endian */
+
+  //
+  // DW6 - DW7
+  //
+  UINT8  Rsvd4[8];
+} UTP_TM_REQ_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.9 - UTP Task Management Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x24*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[2];
+  UINT8  Resp;                /* Response */
+  UINT8  Rsvd2;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 OutputParam1;        /* Output Parameter 1 - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 OutputParam2;        /* Output Parameter 2 - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd4[12];
+} UTP_TM_RESP_UPIU;
+
+//
+// UTP Task Management Request Descriptor
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:24;
+  UINT32 Int:1;               /* Interrupt */
+  UINT32 Rsvd2:7;
+
+  //
+  // DW1
+  //
+  UINT32 Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT32 Ocs:8;               /* Overall Command Status */
+  UINT32 Rsvd4:24;
+
+  //
+  // DW3
+  //
+  UINT32 Rsvd5;
+
+  //
+  // DW4 - DW11
+  //
+  UTP_TM_REQ_UPIU TmReq;      /* Task Management Request UPIU */
+  
+  //
+  // DW12 - DW19
+  //
+  UTP_TM_RESP_UPIU TmResp;    /* Task Management Response UPIU */
+} UTP_TMRD;
+
+
+typedef struct {
+  UINT8  Opcode;
+  UINT8  DescId;
+  UINT8  Index;
+  UINT8  Selector;
+  UINT16 Rsvd1;
+  UINT16 Length;
+  UINT32 Value;
+  UINT32 Rsvd2;
+} UTP_UPIU_TSF;
+
+//
+// UFS 2.0 Spec Section 10.5.10 - UTP Query Request UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8        TransCode:6;   /* Transaction Type - 0x16*/
+  UINT8        Dd:1;
+  UINT8        Hd:1;
+  UINT8        Flags;
+  UINT8        Rsvd1;
+  UINT8        TaskTag;       /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8        Rsvd2;
+  UINT8        QueryFunc;     /* Query Function */
+  UINT8        Rsvd3[2];
+
+  //
+  // DW2
+  //
+  UINT8        EhsLen;        /* Total EHS Length - 0x00 */
+  UINT8        Rsvd4;
+  UINT16       DataSegLen;    /* Data Segment Length - Big Endian */
+
+  //
+  // DW3 - 6
+  //
+  UTP_UPIU_TSF Tsf;           /* Transaction Specific Fields */
+
+  //
+  // DW7
+  //
+  UINT8        Rsvd5[4];
+
+  //
+  // Data Segment - Data to be transferred
+  //
+  //UINT8  Data[];            /* Data to be transferred, maximum is 65535 bytes */
+} UTP_QUERY_REQ_UPIU;
+
+#define QUERY_FUNC_STD_READ_REQ     0x01
+#define QUERY_FUNC_STD_WRITE_REQ    0x81
+
+typedef enum {
+  UtpQueryFuncOpcodeNop      = 0x00,
+  UtpQueryFuncOpcodeRdDesc   = 0x01,
+  UtpQueryFuncOpcodeWrDesc   = 0x02,
+  UtpQueryFuncOpcodeRdAttr   = 0x03,
+  UtpQueryFuncOpcodeWrAttr   = 0x04,
+  UtpQueryFuncOpcodeRdFlag   = 0x05,
+  UtpQueryFuncOpcodeSetFlag  = 0x06,
+  UtpQueryFuncOpcodeClrFlag  = 0x07,
+  UtpQueryFuncOpcodeTogFlag  = 0x08
+} UTP_QUERY_FUNC_OPCODE;
+
+//
+// UFS 2.0 Spec Section 10.5.11 - UTP Query Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8        TransCode:6;   /* Transaction Type - 0x36*/
+  UINT8        Dd:1;
+  UINT8        Hd:1;
+  UINT8        Flags;
+  UINT8        Rsvd1;
+  UINT8        TaskTag;       /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8        Rsvd2;
+  UINT8        QueryFunc;     /* Query Function */
+  UINT8        QueryResp;     /* Query Response */
+  UINT8        Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT8        EhsLen;        /* Total EHS Length - 0x00 */
+  UINT8        DevInfo;       /* Device Information */
+  UINT16       DataSegLen;    /* Data Segment Length - Big Endian */
+
+  //
+  // DW3 - 6
+  //
+  UTP_UPIU_TSF Tsf;           /* Transaction Specific Fields */
+
+  //
+  // DW7
+  //
+  UINT8        Rsvd4[4];
+
+  //
+  // Data Segment - Data to be transferred
+  //
+  //UINT8      Data[];        /* Data to be transferred, maximum is 65535 bytes */
+} UTP_QUERY_RESP_UPIU;
+
+typedef enum {
+  UfsUtpQueryResponseSuccess             = 0x00,
+  UfsUtpQueryResponseParamNotReadable    = 0xF6,
+  UfsUtpQueryResponseParamNotWriteable   = 0xF7,  
+  UfsUtpQueryResponseParamAlreadyWritten = 0xF8,
+  UfsUtpQueryResponseInvalidLen          = 0xF9,
+  UfsUtpQueryResponseInvalidVal          = 0xFA,
+  UfsUtpQueryResponseInvalidSelector     = 0xFB,
+  UfsUtpQueryResponseInvalidIndex        = 0xFC,
+  UfsUtpQueryResponseInvalidIdn          = 0xFD,
+  UfsUtpQueryResponseInvalidOpc          = 0xFE,
+  UfsUtpQueryResponseGeneralFailure      = 0xFF
+} UTP_QUERY_RESP_CODE;
+
+//
+// UFS 2.0 Spec Section 10.5.12 - UTP Reject UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x3F*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[2];
+  UINT8  Response;            /* Response - 0x01 */
+  UINT8  Rsvd2;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information - 0x00 */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT8  HdrSts;              /* Basic Header Status */
+  UINT8  Rsvd3;
+  UINT8  E2ESts;              /* End-to-End Status */
+  UINT8  Rsvd4;
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Rsvd5[16];
+} UTP_REJ_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.13 - UTP NOP OUT UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x00*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Rsvd1;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd2[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3 - DW7
+  //
+  UINT8  Rsvd4[20];
+} UTP_NOP_OUT_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.14 - UTP NOP IN UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x20*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Rsvd1;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd2[2];
+  UINT8  Resp;                /* Response - 0x00 */
+  UINT8  Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information - 0x00 */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3 - DW7
+  //
+  UINT8  Rsvd4[20];
+} UTP_NOP_IN_UPIU;
+
+//
+// UFS Descriptors
+//
+typedef enum {
+  UfsDeviceDesc     = 0x00,
+  UfsConfigDesc     = 0x01,
+  UfsUnitDesc       = 0x02,
+  UfsInterConnDesc  = 0x04,
+  UfsStringDesc     = 0x05,
+  UfsGeometryDesc   = 0x07,
+  UfsPowerDesc      = 0x08
+} UFS_DESC_IDN;
+
+//
+// UFS 2.0 Spec Section 14.1.6.2 - Device Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  Device;
+  UINT8  DevClass;
+  UINT8  DevSubClass;
+  UINT8  Protocol;
+  UINT8  NumLun;
+  UINT8  NumWLun;
+  UINT8  BootEn;
+  UINT8  DescAccessEn;
+  UINT8  InitPowerMode;
+  UINT8  HighPriorityLun;
+  UINT8  SecureRemovalType;
+  UINT8  SecurityLun;
+  UINT8  BgOpsTermLat;
+  UINT8  InitActiveIccLevel;
+  UINT16 SpecVersion;
+  UINT16 ManufactureDate;
+  UINT8  ManufacturerName;
+  UINT8  ProductName;
+  UINT8  SerialName;
+  UINT8  OemId;
+  UINT16 ManufacturerId;
+  UINT8  Ud0BaseOffset;
+  UINT8  Ud0ConfParamLen;
+  UINT8  DevRttCap;
+  UINT16 PeriodicRtcUpdate;
+  UINT8  Rsvd1[17];
+  UINT8  Rsvd2[16];
+} UFS_DEV_DESC;
+
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  Rsvd1;
+  UINT8  BootEn;
+  UINT8  DescAccessEn;
+  UINT8  InitPowerMode;
+  UINT8  HighPriorityLun;
+  UINT8  SecureRemovalType;
+  UINT8  InitActiveIccLevel;
+  UINT16 PeriodicRtcUpdate;
+  UINT8  Rsvd2[5];
+} UFS_CONFIG_DESC_GEN_HEADER;
+
+typedef struct {
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  MemType;
+  UINT32 NumAllocUnits;
+  UINT8  DataReliability;
+  UINT8  LogicBlkSize;
+  UINT8  ProvisionType;
+  UINT16 CtxCap;
+  UINT8  Rsvd1[3];
+} UFS_UNIT_DESC_CONFIG_PARAMS;
+
+//
+// UFS 2.0 Spec Section 14.1.6.3 - Configuration Descriptor
+//
+typedef struct {
+  UFS_CONFIG_DESC_GEN_HEADER  Header;
+  UFS_UNIT_DESC_CONFIG_PARAMS UnitDescConfParams[8];
+} UFS_CONFIG_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.4 - Geometry Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  MediaTech;
+  UINT8  Rsvd1;
+  UINT64 TotalRawDevCapacity;
+  UINT8  Rsvd2;
+  UINT32 SegSize;
+  UINT8  AllocUnitSize;
+  UINT8  MinAddrBlkSize;
+  UINT8  OptReadBlkSize;
+  UINT8  OptWriteBlkSize;
+  UINT8  MaxInBufSize;
+  UINT8  MaxOutBufSize;
+  UINT8  RpmbRwSize;
+  UINT8  Rsvd3;
+  UINT8  DataOrder;
+  UINT8  MaxCtxIdNum;
+  UINT8  SysDataTagUnitSize;
+  UINT8  SysDataResUnitSize;
+  UINT8  SupSecRemovalTypes;
+  UINT16 SupMemTypes;
+  UINT32 SysCodeMaxNumAllocUnits;
+  UINT16 SupCodeCapAdjFac;
+  UINT32 NonPersMaxNumAllocUnits;
+  UINT16 NonPersCapAdjFac;
+  UINT32 Enhance1MaxNumAllocUnits;
+  UINT16 Enhance1CapAdjFac;
+  UINT32 Enhance2MaxNumAllocUnits;
+  UINT16 Enhance2CapAdjFac;
+  UINT32 Enhance3MaxNumAllocUnits;
+  UINT16 Enhance3CapAdjFac;
+  UINT32 Enhance4MaxNumAllocUnits;
+  UINT16 Enhance4CapAdjFac;
+} UFS_GEOMETRY_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.5 - Unit Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  UnitIdx;
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  LunQueueDep;
+  UINT8  Rsvd1;
+  UINT8  MemType;
+  UINT8  DataReliability;
+  UINT8  LogicBlkSize;
+  UINT64 LogicBlkCount;
+  UINT32 EraseBlkSize;
+  UINT8  ProvisionType;
+  UINT64 PhyMemResCount;
+  UINT16 CtxCap;
+  UINT8  LargeUnitGranularity;
+} UFS_UNIT_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.6 - RPMB Unit Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  UnitIdx;
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  LunQueueDep;
+  UINT8  Rsvd1;
+  UINT8  MemType;
+  UINT8  Rsvd2;
+  UINT8  LogicBlkSize;
+  UINT64 LogicBlkCount;
+  UINT32 EraseBlkSize;
+  UINT8  ProvisionType;
+  UINT64 PhyMemResCount;
+  UINT8  Rsvd3[3];
+} UFS_RPMB_UNIT_DESC;
+
+typedef struct {
+  UINT16 Value:10;
+  UINT16 Rsvd1:4;
+  UINT16 Unit:2;
+} UFS_POWER_PARAM_ELEMENT;
+
+//
+// UFS 2.0 Spec Section 14.1.6.7 - Power Parameter Descriptor
+//
+typedef struct {
+  UINT8                    Length;
+  UINT8                    DescType;
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVcc[16];
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVccQ[16];
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVccQ2[16];
+} UFS_POWER_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.8 - InterConnect Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT16 UniProVer;
+  UINT16 MphyVer;
+} UFS_INTER_CONNECT_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.9 - 14.1.6.12 - String Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  CHAR16 Unicode[126];
+} UFS_STRING_DESC;
+
+//
+// UFS 2.0 Spec Section 14.2 - Flags
+//
+typedef enum {
+  UfsFlagDevInit         = 0x01,
+  UfsFlagPermWpEn        = 0x02,
+  UfsFlagPowerOnWpEn     = 0x03,
+  UfsFlagBgOpsEn         = 0x04,
+  UfsFlagPurgeEn         = 0x06,
+  UfsFlagPhyResRemoval   = 0x08,
+  UfsFlagBusyRtc         = 0x09,
+  UfsFlagPermDisFwUpdate = 0x0B    
+} UFS_FLAGS_IDN;
+
+//
+// UFS 2.0 Spec Section 14.2 - Attributes
+//
+typedef enum {
+  UfsAttrBootLunEn         = 0x00,
+  UfsAttrCurPowerMode      = 0x02,
+  UfsAttrActiveIccLevel    = 0x03,
+  UfsAttrOutOfOrderDataEn  = 0x04,
+  UfsAttrBgOpStatus        = 0x05,
+  UfsAttrPurgeStatus       = 0x06,
+  UfsAttrMaxDataInSize     = 0x07,
+  UfsAttrMaxDataOutSize    = 0x08,
+  UfsAttrDynCapNeeded      = 0x09,
+  UfsAttrRefClkFreq        = 0x0a,
+  UfsAttrConfigDescLock    = 0x0b,
+  UfsAttrMaxNumOfRtt       = 0x0c,
+  UfsAttrExceptionEvtCtrl  = 0x0d,
+  UfsAttrExceptionEvtSts   = 0x0e,
+  UfsAttrSecondsPassed     = 0x0f,
+  UfsAttrContextConf       = 0x10,
+  UfsAttrCorrPrgBlkNum     = 0x11
+} UFS_ATTR_IDN;
+
+typedef enum {
+  UfsNoData                = 0,
+  UfsDataOut               = 1,
+  UfsDataIn                = 2,
+  UfsDdReserved
+} UFS_DATA_DIRECTION;
+
+
+#pragma pack()
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/ComponentName.c
new file mode 100644
index 0000000000..7da21110e3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/ComponentName.c
@@ -0,0 +1,222 @@
+/** @file
+
+  Copyright (c) 2014, 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 "UfsPassThru.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUfsPassThruComponentName = {
+  UfsPassThruComponentNameGetDriverName,
+  UfsPassThruComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUfsPassThruComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UfsPassThruComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UfsPassThruComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUfsPassThruDriverNameTable[] = {
+  { 
+    "eng;en",
+    L"Universal Flash Storage (UFS) Pass Thru Driver"
+  },
+  { 
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUfsPassThruControllerNameTable[] = {
+  { 
+    "eng;en",
+    L"Universal Flash Storage (UFS) Host Controller"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUfsPassThruDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUfsPassThruComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+
+  if (Language == NULL || ControllerName == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing Controller Handle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gUfsPassThruDriverBinding.DriverBindingHandle,
+             &gEdkiiUfsHostControllerProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUfsPassThruControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUfsPassThruComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.c b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.c
new file mode 100644
index 0000000000..7c831e92d0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.c
@@ -0,0 +1,1108 @@
+/** @file
+
+  Copyright (c) 2014 - 2015, 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 "UfsPassThru.h"
+
+//
+// Template for Ufs Pass Thru private data.
+//
+UFS_PASS_THRU_PRIVATE_DATA gUfsPassThruTemplate = {
+  UFS_PASS_THRU_SIG,              // Signature
+  NULL,                           // Handle  
+  {                               // ExtScsiPassThruMode
+    0xFFFFFFFF,
+    EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_PHYSICAL | EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL | EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_NONBLOCKIO,
+    sizeof (UINTN)
+  },
+  {                               // ExtScsiPassThru
+    NULL,
+    UfsPassThruPassThru,
+    UfsPassThruGetNextTargetLun,
+    UfsPassThruBuildDevicePath,
+    UfsPassThruGetTargetLun,
+    UfsPassThruResetChannel,
+    UfsPassThruResetTargetLun,
+    UfsPassThruGetNextTarget
+  },
+  0,                              // UfsHostController
+  0,                              // UfsHcBase
+  0,                              // Capabilities
+  0,                              // TaskTag
+  0,                              // UtpTrlBase
+  0,                              // Nutrs
+  0,                              // TrlMapping
+  0,                              // UtpTmrlBase
+  0,                              // Nutmrs
+  0,                              // TmrlMapping
+  {                               // Luns
+    {
+      UFS_LUN_0,                      // Ufs Common Lun 0
+      UFS_LUN_1,                      // Ufs Common Lun 1
+      UFS_LUN_2,                      // Ufs Common Lun 2
+      UFS_LUN_3,                      // Ufs Common Lun 3
+      UFS_LUN_4,                      // Ufs Common Lun 4
+      UFS_LUN_5,                      // Ufs Common Lun 5
+      UFS_LUN_6,                      // Ufs Common Lun 6
+      UFS_LUN_7,                      // Ufs Common Lun 7
+      UFS_WLUN_REPORT_LUNS,           // Ufs Reports Luns Well Known Lun
+      UFS_WLUN_UFS_DEV,               // Ufs Device Well Known Lun
+      UFS_WLUN_BOOT,                  // Ufs Boot Well Known Lun
+      UFS_WLUN_RPMB                   // RPMB Well Known Lun
+    },
+    0x0000,                           // By default don't expose any Luns.
+    0x0
+  },
+  NULL,                           // TimerEvent
+  {                               // Queue
+    NULL,
+    NULL
+  }
+};
+
+EFI_DRIVER_BINDING_PROTOCOL gUfsPassThruDriverBinding = {
+  UfsPassThruDriverBindingSupported,
+  UfsPassThruDriverBindingStart,
+  UfsPassThruDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+UFS_DEVICE_PATH    mUfsDevicePathTemplate = {
+  {
+    MESSAGING_DEVICE_PATH,
+    MSG_UFS_DP,
+    {
+      (UINT8) (sizeof (UFS_DEVICE_PATH)),
+      (UINT8) ((sizeof (UFS_DEVICE_PATH)) >> 8)
+    }
+  },
+  0,
+  0
+};
+
+UINT8 mUfsTargetId[TARGET_MAX_BYTES];
+
+/**
+  Sends a SCSI Request Packet to a SCSI device that is attached to the SCSI channel. This function
+  supports both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the
+  nonblocking I/O functionality is optional.
+
+  @param  This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target  The Target is an array of size TARGET_MAX_BYTES and it represents
+                  the id of the SCSI device to send the SCSI Request Packet. Each
+                  transport driver may choose to utilize a subset of this size to suit the needs
+                  of transport target representation. For example, a Fibre Channel driver
+                  may use only 8 bytes (WWN) to represent an FC target.
+  @param  Lun     The LUN of the SCSI device to send the SCSI Request Packet.
+  @param  Packet  A pointer to the SCSI Request Packet to send to the SCSI device
+                  specified by Target and Lun.
+  @param  Event   If nonblocking I/O is not supported then Event is ignored, and blocking
+                  I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                  Event is not NULL and non blocking I/O is supported, then
+                  nonblocking I/O is performed, and Event will be signaled when the
+                  SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE   The SCSI Request Packet was not executed. The number of bytes that
+                                could be transferred is returned in InTransferLength. For write
+                                and bi-directional commands, OutTransferLength bytes were
+                                transferred by OutDataBuffer.
+  @retval EFI_NOT_READY         The SCSI Request Packet could not be sent because there are too many
+                                SCSI Request Packets already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_INVALID_PARAMETER Target, Lun, or the contents of ScsiRequestPacket are invalid.
+  @retval EFI_UNSUPPORTED       The command described by the SCSI Request Packet is not supported
+                                by the host adapter. This includes the case of Bi-directional SCSI
+                                commands not supported by the implementation. The SCSI Request
+                                Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruPassThru (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                    *This,
+  IN UINT8                                              *Target,
+  IN UINT64                                             Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET     *Packet,
+  IN EFI_EVENT                                          Event OPTIONAL
+  )
+{
+  EFI_STATUS                      Status;
+  UFS_PASS_THRU_PRIVATE_DATA      *Private;
+  UINT8                           UfsLun;
+  UINT16                          Index;
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if ((Packet == NULL) || (Packet->Cdb == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Don't support variable length CDB
+  //
+  if ((Packet->CdbLength != 6) && (Packet->CdbLength != 10) &&
+      (Packet->CdbLength != 12) && (Packet->CdbLength != 16)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Packet->SenseDataLength != 0) && (Packet->SenseData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->InDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->OutDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->SenseData, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // For UFS 2.0 compatible device, 0 is always used to represent the location of the UFS device.
+  //
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0x00);
+  if ((Target == NULL) || (CompareMem(Target, mUfsTargetId, TARGET_MAX_BYTES) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // UFS 2.0 spec Section 10.6.7 - Translation of 8-bit UFS LUN to 64-bit SCSI LUN Address
+  // 0xC1 in the first 8 bits of the 64-bit address indicates a well known LUN address in the SAM SCSI format.
+  // The second 8 bits of the 64-bit address saves the corresponding 8-bit UFS LUN.
+  //
+  if ((UINT8)Lun == UFS_WLUN_PREFIX) {
+    UfsLun = BIT7 | (((UINT8*)&Lun)[1] & 0xFF);
+  } else if ((UINT8)Lun == 0) {
+    UfsLun = ((UINT8*)&Lun)[1] & 0xFF;
+  } else {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+    if ((Private->Luns.BitMask & (BIT0 << Index)) == 0) {
+      continue;
+    }
+  
+    if (Private->Luns.Lun[Index] == UfsLun) {
+      break;
+    }
+  }
+
+  if (Index == UFS_MAX_LUNS) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = UfsExecScsiCmds (Private, UfsLun, Packet, Event);
+
+  return Status;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs and LUNs for SCSI devices on a SCSI channel. These
+  can either be the list SCSI devices that are actually present on the SCSI channel, or the list of legal
+  Target Ids and LUNs for the SCSI channel. Regardless, the caller of this function must probe the
+  Target ID and LUN returned to see if a SCSI device is actually present at that location on the SCSI
+  channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target On input, a pointer to the Target ID (an array of size
+                 TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+  @param  Lun    On input, a pointer to the LUN of a SCSI device present on the SCSI
+                 channel. On output, a pointer to the LUN of the next SCSI device present
+                 on a SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID and LUN of the next SCSI device on the SCSI
+                                channel was returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER Target array is not all 0xF, and Target and Lun were
+                                not returned on a previous call to GetNextTargetLun().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetNextTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target,
+  IN OUT UINT64                          *Lun
+  )
+{
+  UFS_PASS_THRU_PRIVATE_DATA      *Private;
+  UINT8                           UfsLun;
+  UINT16                          Index;
+  UINT16                          Next;
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UfsLun = 0;
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0xFF);
+  if (CompareMem (*Target, mUfsTargetId, TARGET_MAX_BYTES) == 0) {
+    //
+    // If the array is all 0xFF's, return the first exposed Lun to caller.
+    //
+    SetMem (*Target, TARGET_MAX_BYTES, 0x00);
+    for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+      if ((Private->Luns.BitMask & (BIT0 << Index)) != 0) {
+        UfsLun = Private->Luns.Lun[Index];
+        break;
+      }
+    }
+    if (Index != UFS_MAX_LUNS) {
+      *Lun = 0;
+      if ((UfsLun & BIT7) == BIT7) {
+        ((UINT8*)Lun)[0] = UFS_WLUN_PREFIX;
+        ((UINT8*)Lun)[1] = UfsLun & ~BIT7;
+      } else {
+        ((UINT8*)Lun)[1] = UfsLun;
+      }
+      return EFI_SUCCESS;
+    } else {
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0x00);
+  if (CompareMem (*Target, mUfsTargetId, TARGET_MAX_BYTES) == 0) {
+    if (((UINT8*)Lun)[0] == UFS_WLUN_PREFIX) {
+      UfsLun = BIT7 | (((UINT8*)Lun)[1] & 0xFF);
+    } else if (((UINT8*)Lun)[0] == 0) {
+      UfsLun = ((UINT8*)Lun)[1] & 0xFF;
+    } else {
+      return EFI_NOT_FOUND;
+    }
+
+    for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+      if ((Private->Luns.BitMask & (BIT0 << Index)) == 0) {
+        continue;
+      }
+
+      if (Private->Luns.Lun[Index] != UfsLun) {
+        continue;
+      }
+
+      for (Next = Index + 1; Next < UFS_MAX_LUNS; Next++) {
+        if ((Private->Luns.BitMask & (BIT0 << Next)) != 0) {
+          UfsLun = Private->Luns.Lun[Next];
+          break;
+        }
+      }
+
+      if (Next == UFS_MAX_LUNS) {
+        return EFI_NOT_FOUND;
+      } else {
+        break;
+      }
+    }
+
+    if (Index != UFS_MAX_LUNS) {
+      *Lun = 0;
+      if ((UfsLun & BIT7) == BIT7) {
+        ((UINT8*)Lun)[0] = UFS_WLUN_PREFIX;
+        ((UINT8*)Lun)[1] = UfsLun & ~BIT7;
+      } else {
+        ((UINT8*)Lun)[1] = UfsLun;
+      }
+      return EFI_SUCCESS;
+    } else {
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Used to allocate and build a device path node for a SCSI device on a SCSI channel.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target     The Target is an array of size TARGET_MAX_BYTES and it specifies the
+                     Target ID of the SCSI device for which a device path node is to be
+                     allocated and built. Transport drivers may chose to utilize a subset of
+                     this size to suit the representation of targets. For example, a Fibre
+                     Channel driver may use only 8 bytes (WWN) in the array to represent a
+                     FC target.
+  @param  Lun        The LUN of the SCSI device for which a device path node is to be
+                     allocated and built.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     specified by Target and Lun. This function is responsible for
+                     allocating the buffer DevicePath with the boot service
+                     AllocatePool(). It is the caller's responsibility to free
+                     DevicePath when the caller is finished with DevicePath.
+
+  @retval EFI_SUCCESS           The device path node that describes the SCSI device specified by
+                                Target and Lun was allocated and returned in
+                                DevicePath.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_NOT_FOUND         The SCSI devices specified by Target and Lun does not exist
+                                on the SCSI channel.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruBuildDevicePath (
+  IN     EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN     UINT8                              *Target,
+  IN     UINT64                             Lun,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL           **DevicePath
+  )
+{
+  UFS_PASS_THRU_PRIVATE_DATA      *Private;
+  EFI_DEV_PATH                    *DevicePathNode;
+  UINT8                           UfsLun;
+  UINT16                          Index;
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0x00);
+  if (CompareMem (Target, mUfsTargetId, TARGET_MAX_BYTES) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((UINT8)Lun == UFS_WLUN_PREFIX) {
+    UfsLun = BIT7 | (((UINT8*)&Lun)[1] & 0xFF);
+  } else if ((UINT8)Lun == 0) {
+    UfsLun = ((UINT8*)&Lun)[1] & 0xFF;
+  } else {
+    return EFI_NOT_FOUND;
+  }
+
+  for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+    if ((Private->Luns.BitMask & (BIT0 << Index)) == 0) {
+      continue;
+    }
+  
+    if (Private->Luns.Lun[Index] == UfsLun) {
+      break;
+    }
+  }
+
+  if (Index == UFS_MAX_LUNS) {
+    return EFI_NOT_FOUND;
+  }
+
+  DevicePathNode = AllocateCopyPool (sizeof (UFS_DEVICE_PATH), &mUfsDevicePathTemplate);
+  if (DevicePathNode == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DevicePathNode->Ufs.Pun = 0;
+  DevicePathNode->Ufs.Lun = UfsLun;
+
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) DevicePathNode;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to translate a device path node to a Target ID and LUN.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     on the SCSI channel.
+  @param  Target     A pointer to the Target Array which represents the ID of a SCSI device
+                     on the SCSI channel.
+  @param  Lun        A pointer to the LUN of a SCSI device on the SCSI channel.
+
+  @retval EFI_SUCCESS           DevicePath was successfully translated to a Target ID and
+                                LUN, and they were returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER DevicePath or Target or Lun is NULL.
+  @retval EFI_NOT_FOUND         A valid translation from DevicePath to a Target ID and LUN
+                                does not exist.
+  @retval EFI_UNSUPPORTED       This driver does not support the device path node type in
+                                 DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  OUT UINT8                              **Target,
+  OUT UINT64                             *Lun
+  )
+{
+  UFS_PASS_THRU_PRIVATE_DATA      *Private;
+  EFI_DEV_PATH                    *DevicePathNode;
+  UINT8                           Pun;
+  UINT8                           UfsLun;
+  UINT16                          Index;
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL || Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether the DevicePath belongs to SCSI_DEVICE_PATH
+  //
+  if ((DevicePath->Type != MESSAGING_DEVICE_PATH) || (DevicePath->SubType != MSG_UFS_DP) ||
+      (DevicePathNodeLength(DevicePath) != sizeof(SCSI_DEVICE_PATH))) {
+    return EFI_UNSUPPORTED;
+  }
+
+  DevicePathNode = (EFI_DEV_PATH *) DevicePath;
+
+  Pun    = (UINT8) DevicePathNode->Ufs.Pun;
+  UfsLun = (UINT8) DevicePathNode->Ufs.Lun;
+
+  if (Pun != 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+    if ((Private->Luns.BitMask & (BIT0 << Index)) == 0) {
+      continue;
+    }
+  
+    if (Private->Luns.Lun[Index] == UfsLun) {
+      break;
+    }
+  }
+
+  if (Index == UFS_MAX_LUNS) {
+    return EFI_NOT_FOUND;
+  }
+
+  SetMem (*Target, TARGET_MAX_BYTES, 0x00);
+  *Lun = 0;
+  if ((UfsLun & BIT7) == BIT7) {
+    ((UINT8*)Lun)[0] = UFS_WLUN_PREFIX;
+    ((UINT8*)Lun)[1] = UfsLun & ~BIT7;
+  } else {
+    ((UINT8*)Lun)[1] = UfsLun;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a SCSI channel. This operation resets all the SCSI devices connected to the SCSI channel.
+
+  @param  This A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      The SCSI channel was reset.
+  @retval EFI_DEVICE_ERROR A device error occurred while attempting to reset the SCSI channel.
+  @retval EFI_TIMEOUT      A timeout occurred while attempting to reset the SCSI channel.
+  @retval EFI_UNSUPPORTED  The SCSI channel does not support a channel reset operation.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruResetChannel (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   *This
+  )
+{
+  //
+  // Return success directly then upper layer driver could think reset channel operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a SCSI logical unit that is connected to a SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target The Target is an array of size TARGET_MAX_BYTE and it represents the
+                 target port ID of the SCSI device containing the SCSI logical unit to
+                 reset. Transport drivers may chose to utilize a subset of this array to suit
+                 the representation of their targets.
+  @param  Lun    The LUN of the SCSI device to reset.
+
+  @retval EFI_SUCCESS           The SCSI device specified by Target and Lun was reset.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           A timeout occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+  @retval EFI_UNSUPPORTED       The SCSI channel does not support a target reset operation.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to reset the SCSI device
+                                 specified by Target and Lun.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruResetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN UINT8                              *Target,
+  IN UINT64                             Lun
+  )
+{
+  //
+  // Return success directly then upper layer driver could think reset target LUN operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs for SCSI devices on a SCSI channel. These can either
+  be the list SCSI devices that are actually present on the SCSI channel, or the list of legal Target IDs
+  for the SCSI channel. Regardless, the caller of this function must probe the Target ID returned to
+  see if a SCSI device is actually present at that location on the SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target (TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID of the next SCSI device on the SCSI
+                                channel was returned in Target.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           Target array is not all 0xF, and Target was not
+                                returned on a previous call to GetNextTarget().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetNextTarget (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target
+  )
+{
+  if (Target == NULL || *Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0xFF);
+  if (CompareMem(*Target, mUfsTargetId, TARGET_MAX_BYTES) == 0) {
+    SetMem (*Target, TARGET_MAX_BYTES, 0x00);
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN EFI_HANDLE                        Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL          *RemainingDevicePath
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_DEVICE_PATH_PROTOCOL             *ParentDevicePath;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHostController;
+
+  //
+  // Ufs Pass Thru driver is a device driver, and should ingore the
+  // "RemainingDevicePath" according to UEFI spec
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID *) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+  //
+  // Close the protocol because we don't use it here
+  //
+  gBS->CloseProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  This->DriverBindingHandle,
+                  Controller
+                  );
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEdkiiUfsHostControllerProtocolGuid,
+                  (VOID **) &UfsHostController,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEdkiiUfsHostControllerProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+        
+  return EFI_SUCCESS;
+}
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  )
+{
+  EFI_STATUS                            Status;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL    *UfsHc;
+  UFS_PASS_THRU_PRIVATE_DATA            *Private;
+  UINTN                                 UfsHcBase;
+  UINT32                                Index;
+  UFS_CONFIG_DESC                       Config;
+
+  Status    = EFI_SUCCESS;
+  UfsHc     = NULL;
+  Private   = NULL;
+  UfsHcBase = 0;
+
+  DEBUG ((EFI_D_INFO, "==UfsPassThru Start== Controller = %x\n", Controller));
+
+  Status  = gBS->OpenProtocol (
+                   Controller,
+                   &gEdkiiUfsHostControllerProtocolGuid,
+                   (VOID **) &UfsHc,
+                   This->DriverBindingHandle,
+                   Controller,
+                   EFI_OPEN_PROTOCOL_BY_DRIVER
+                   );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Open Ufs Host Controller Protocol Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // Get the UFS Host Controller MMIO Bar Base Address.
+  //
+  Status = UfsHc->GetUfsHcMmioBar (UfsHc, &UfsHcBase);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Get Ufs Host Controller Mmio Bar Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // Initialize Ufs Pass Thru private data for managed UFS Host Controller.
+  //
+  Private = AllocateCopyPool (sizeof (UFS_PASS_THRU_PRIVATE_DATA), &gUfsPassThruTemplate);
+  if (Private == NULL) {
+    DEBUG ((EFI_D_ERROR, "Unable to allocate Ufs Pass Thru private data\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+
+  Private->ExtScsiPassThru.Mode = &Private->ExtScsiPassThruMode;
+  Private->UfsHostController    = UfsHc;
+  Private->UfsHcBase            = UfsHcBase;
+  InitializeListHead (&Private->Queue);
+
+  //
+  // Initialize UFS Host Controller H/W.
+  //
+  Status = UfsControllerInit (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Host Controller Initialization Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // UFS 2.0 spec Section 13.1.3.3:
+  // At the end of the UFS Interconnect Layer initialization on both host and device side, 
+  // the host shall send a NOP OUT UPIU to verify that the device UTP Layer is ready. 
+  //
+  Status = UfsExecNopCmds (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Sending NOP IN command Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // The host enables the device initialization completion by setting fDeviceInit flag.
+  //
+  Status = UfsSetFlag (Private, UfsFlagDevInit);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Set fDeviceInit Flag Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // Get Ufs Device's Lun Info by reading Configuration Descriptor.
+  //
+  Status = UfsRwDeviceDesc (Private, TRUE, UfsConfigDesc, 0, 0, &Config, sizeof (UFS_CONFIG_DESC));
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Get Configuration Descriptor Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // Check if 8 common luns are active and set corresponding bit mask.
+  // TODO: Parse device descriptor to decide if exposing RPMB LUN to upper layer for authentication access.
+  //
+  for (Index = 0; Index < 8; Index++) {
+    if (Config.UnitDescConfParams[Index].LunEn != 0) {
+      Private->Luns.BitMask |= (BIT0 << Index);
+      DEBUG ((EFI_D_INFO, "Ufs Lun %d is enabled\n", Index));
+    }
+  }
+
+  //
+  // Start the asynchronous interrupt monitor
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ProcessAsyncTaskList,
+                  Private,
+                  &Private->TimerEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Create Async Tasks Event Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  Status = gBS->SetTimer (
+                  Private->TimerEvent,
+                  TimerPeriodic,
+                  UFS_HC_ASYNC_TIMER
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Set Periodic Timer Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &(Private->ExtScsiPassThru)
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+
+Error:
+  if (Private != NULL) {
+    if (Private->TmrlMapping != NULL) {
+      UfsHc->Unmap (UfsHc, Private->TmrlMapping);  
+    }
+    if (Private->UtpTmrlBase != NULL) {
+      UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (Private->Nutmrs * sizeof (UTP_TMRD)), Private->UtpTmrlBase);
+    }
+
+    if (Private->TrlMapping != NULL) {
+      UfsHc->Unmap (UfsHc, Private->TrlMapping);
+    }
+    if (Private->UtpTrlBase != NULL) {
+      UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (Private->Nutrs * sizeof (UTP_TMRD)), Private->UtpTrlBase);
+    }
+
+    if (Private->TimerEvent != NULL) {
+      gBS->CloseEvent (Private->TimerEvent);
+    }
+
+    FreePool (Private);
+  }
+
+  if (UfsHc != NULL) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEdkiiUfsHostControllerProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+
+  return Status;
+}
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                            Status;
+  UFS_PASS_THRU_PRIVATE_DATA            *Private;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL       *ExtScsiPassThru;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL    *UfsHc;
+  UFS_PASS_THRU_TRANS_REQ               *TransReq;
+  LIST_ENTRY                            *Entry;
+  LIST_ENTRY                            *NextEntry;
+
+  DEBUG ((EFI_D_INFO, "==UfsPassThru Stop== Controller Controller = %x\n", Controller));
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  (VOID **) &ExtScsiPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (ExtScsiPassThru);
+  UfsHc   = Private->UfsHostController;
+
+  //
+  // Cleanup the resources of I/O requests in the async I/O queue
+  //
+  if (!IsListEmpty(&Private->Queue)) {
+    EFI_LIST_FOR_EACH_SAFE (Entry, NextEntry, &Private->Queue) {
+      TransReq  = UFS_PASS_THRU_TRANS_REQ_FROM_THIS (Entry);
+
+      //
+      // TODO: Should find/add a proper host adapter return status for this
+      // case.
+      //
+      TransReq->Packet->HostAdapterStatus =
+        EFI_EXT_SCSI_STATUS_HOST_ADAPTER_PHASE_ERROR;
+
+      SignalCallerEvent (Private, TransReq);
+    }
+  }
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  &(Private->ExtScsiPassThru)
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Stop Ufs Host Controller
+  //
+  Status = UfsControllerStop (Private);
+  ASSERT_EFI_ERROR (Status);
+
+  if (Private->TmrlMapping != NULL) {
+    UfsHc->Unmap (UfsHc, Private->TmrlMapping);
+  }
+  if (Private->UtpTmrlBase != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (Private->Nutmrs * sizeof (UTP_TMRD)), Private->UtpTmrlBase);
+  }
+
+  if (Private->TrlMapping != NULL) {
+    UfsHc->Unmap (UfsHc, Private->TrlMapping);
+  }
+  if (Private->UtpTrlBase != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (Private->Nutrs * sizeof (UTP_TMRD)), Private->UtpTrlBase);
+  }
+
+  if (Private->TimerEvent != NULL) {
+    gBS->CloseEvent (Private->TimerEvent);
+  }
+
+  FreePool (Private);
+
+  //
+  // Close protocols opened by UfsPassThru controller driver
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEdkiiUfsHostControllerProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+
+/**
+  The user Entry Point for module UfsPassThru. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeUfsPassThru (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUfsPassThruDriverBinding,
+             ImageHandle,
+             &gUfsPassThruComponentName,
+             &gUfsPassThruComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.h b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.h
new file mode 100644
index 0000000000..4f7087f44f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.h
@@ -0,0 +1,799 @@
+/** @file
+
+  Copyright (c) 2014 - 2015, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _UFS_PASS_THRU_H_
+#define _UFS_PASS_THRU_H_
+
+#include <Uefi.h>
+
+#include <Protocol/ScsiPassThruExt.h>
+#include <Protocol/UfsHostController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/TimerLib.h>
+
+#include "UfsPassThruHci.h"
+
+#define UFS_PASS_THRU_SIG           SIGNATURE_32 ('U', 'F', 'S', 'P')
+
+//
+// Lun 0~7 is for 8 common luns. 
+// Lun 8~11 is for those 4 well known luns (Refer to UFS 2.0 spec Table 10.58 for details):
+//  Lun 8:  REPORT LUNS
+//  Lun 9:  UFS DEVICE
+//  Lun 10: BOOT
+//  Lun 11: RPMB
+//
+#define UFS_MAX_LUNS                12
+#define UFS_WLUN_PREFIX             0xC1
+
+typedef struct {
+  UINT8    Lun[UFS_MAX_LUNS];
+  UINT16   BitMask:12;              // Bit 0~7 is 1/1 mapping to common luns. Bit 8~11 is 1/1 mapping to well-known luns.
+  UINT16   Rsvd:4;
+} UFS_EXPOSED_LUNS;
+
+//
+// Iterate through the doule linked list. This is delete-safe.
+// Do not touch NextEntry
+//
+#define EFI_LIST_FOR_EACH_SAFE(Entry, NextEntry, ListHead)            \
+  for(Entry = (ListHead)->ForwardLink, NextEntry = Entry->ForwardLink;\
+      Entry != (ListHead); Entry = NextEntry, NextEntry = Entry->ForwardLink)
+
+typedef struct _UFS_PASS_THRU_PRIVATE_DATA {  
+  UINT32                              Signature;
+  EFI_HANDLE                          Handle;
+  EFI_EXT_SCSI_PASS_THRU_MODE         ExtScsiPassThruMode;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL     ExtScsiPassThru;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL  *UfsHostController;
+  UINTN                               UfsHcBase;
+  UINT32                              Capabilities;
+
+  UINT8                               TaskTag;
+
+  VOID                                *UtpTrlBase;
+  UINT8                               Nutrs;
+  VOID                                *TrlMapping;
+  VOID                                *UtpTmrlBase;
+  UINT8                               Nutmrs;
+  VOID                                *TmrlMapping;
+
+  UFS_EXPOSED_LUNS                    Luns;
+
+  //
+  // For Non-blocking operation.
+  //
+  EFI_EVENT                           TimerEvent;
+  LIST_ENTRY                          Queue;
+} UFS_PASS_THRU_PRIVATE_DATA;
+
+#define UFS_PASS_THRU_TRANS_REQ_SIG   SIGNATURE_32 ('U', 'F', 'S', 'T')
+
+typedef struct {
+  UINT32                                        Signature;
+  LIST_ENTRY                                    TransferList;
+
+  UINT8                                         Slot;
+  UTP_TRD                                       *Trd;
+  UINT32                                        CmdDescSize;
+  VOID                                          *CmdDescHost;
+  VOID                                          *CmdDescMapping;
+  VOID                                          *DataBufMapping;
+
+  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet;
+  UINT64                                        TimeoutRemain;
+  EFI_EVENT                                     CallerEvent;
+} UFS_PASS_THRU_TRANS_REQ;
+
+#define UFS_PASS_THRU_TRANS_REQ_FROM_THIS(a) \
+    CR(a, UFS_PASS_THRU_TRANS_REQ, TransferList, UFS_PASS_THRU_TRANS_REQ_SIG)
+
+#define UFS_TIMEOUT                   EFI_TIMER_PERIOD_SECONDS(3)
+#define UFS_HC_ASYNC_TIMER            EFI_TIMER_PERIOD_MILLISECONDS(1)
+
+#define ROUNDUP8(x) (((x) % 8 == 0) ? (x) : ((x) / 8 + 1) * 8)
+
+#define IS_ALIGNED(addr, size)        (((UINTN) (addr) & (size - 1)) == 0)
+
+#define UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      UFS_PASS_THRU_PRIVATE_DATA, \
+      ExtScsiPassThru, \
+      UFS_PASS_THRU_SIG \
+      )
+
+typedef struct _UFS_DEVICE_MANAGEMENT_REQUEST_PACKET {
+  UINT64           Timeout;
+  VOID             *InDataBuffer;
+  VOID             *OutDataBuffer;
+  UINT8            Opcode;
+  UINT8            DescId;
+  UINT8            Index;
+  UINT8            Selector;
+  UINT32           InTransferLength;
+  UINT32           OutTransferLength;
+  UINT8            DataDirection;
+  UINT8            Ocs;
+} UFS_DEVICE_MANAGEMENT_REQUEST_PACKET;
+
+//
+// function prototype
+//
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Sends a SCSI Request Packet to a SCSI device that is attached to the SCSI channel. This function
+  supports both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the
+  nonblocking I/O functionality is optional.
+
+  @param  This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target  The Target is an array of size TARGET_MAX_BYTES and it represents
+                  the id of the SCSI device to send the SCSI Request Packet. Each
+                  transport driver may choose to utilize a subset of this size to suit the needs
+                  of transport target representation. For example, a Fibre Channel driver
+                  may use only 8 bytes (WWN) to represent an FC target.
+  @param  Lun     The LUN of the SCSI device to send the SCSI Request Packet.
+  @param  Packet  A pointer to the SCSI Request Packet to send to the SCSI device
+                  specified by Target and Lun.
+  @param  Event   If nonblocking I/O is not supported then Event is ignored, and blocking
+                  I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                  Event is not NULL and non blocking I/O is supported, then
+                  nonblocking I/O is performed, and Event will be signaled when the
+                  SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE   The SCSI Request Packet was not executed. The number of bytes that
+                                could be transferred is returned in InTransferLength. For write
+                                and bi-directional commands, OutTransferLength bytes were
+                                transferred by OutDataBuffer.
+  @retval EFI_NOT_READY         The SCSI Request Packet could not be sent because there are too many
+                                SCSI Request Packets already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_INVALID_PARAMETER Target, Lun, or the contents of ScsiRequestPacket are invalid.
+  @retval EFI_UNSUPPORTED       The command described by the SCSI Request Packet is not supported
+                                by the host adapter. This includes the case of Bi-directional SCSI
+                                commands not supported by the implementation. The SCSI Request
+                                Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruPassThru (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                    *This,
+  IN UINT8                                              *Target,
+  IN UINT64                                             Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET     *Packet,
+  IN EFI_EVENT                                          Event OPTIONAL
+  );
+
+/**
+  Used to retrieve the list of legal Target IDs and LUNs for SCSI devices on a SCSI channel. These
+  can either be the list SCSI devices that are actually present on the SCSI channel, or the list of legal
+  Target Ids and LUNs for the SCSI channel. Regardless, the caller of this function must probe the
+  Target ID and LUN returned to see if a SCSI device is actually present at that location on the SCSI
+  channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target On input, a pointer to the Target ID (an array of size
+                 TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+  @param  Lun    On input, a pointer to the LUN of a SCSI device present on the SCSI
+                 channel. On output, a pointer to the LUN of the next SCSI device present
+                 on a SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID and LUN of the next SCSI device on the SCSI
+                                channel was returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER Target array is not all 0xF, and Target and Lun were
+                                not returned on a previous call to GetNextTargetLun().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetNextTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target,
+  IN OUT UINT64                          *Lun
+  );
+
+/**
+  Used to allocate and build a device path node for a SCSI device on a SCSI channel.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target     The Target is an array of size TARGET_MAX_BYTES and it specifies the
+                     Target ID of the SCSI device for which a device path node is to be
+                     allocated and built. Transport drivers may chose to utilize a subset of
+                     this size to suit the representation of targets. For example, a Fibre
+                     Channel driver may use only 8 bytes (WWN) in the array to represent a
+                     FC target.
+  @param  Lun        The LUN of the SCSI device for which a device path node is to be
+                     allocated and built.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     specified by Target and Lun. This function is responsible for
+                     allocating the buffer DevicePath with the boot service
+                     AllocatePool(). It is the caller's responsibility to free
+                     DevicePath when the caller is finished with DevicePath.
+
+  @retval EFI_SUCCESS           The device path node that describes the SCSI device specified by
+                                Target and Lun was allocated and returned in
+                                DevicePath.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_NOT_FOUND         The SCSI devices specified by Target and Lun does not exist
+                                on the SCSI channel.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruBuildDevicePath (
+  IN     EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN     UINT8                              *Target,
+  IN     UINT64                             Lun,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL           **DevicePath
+  );
+
+/**
+  Used to translate a device path node to a Target ID and LUN.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     on the SCSI channel.
+  @param  Target     A pointer to the Target Array which represents the ID of a SCSI device
+                     on the SCSI channel.
+  @param  Lun        A pointer to the LUN of a SCSI device on the SCSI channel.
+
+  @retval EFI_SUCCESS           DevicePath was successfully translated to a Target ID and
+                                LUN, and they were returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER DevicePath or Target or Lun is NULL.
+  @retval EFI_NOT_FOUND         A valid translation from DevicePath to a Target ID and LUN
+                                does not exist.
+  @retval EFI_UNSUPPORTED       This driver does not support the device path node type in
+                                 DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  OUT UINT8                              **Target,
+  OUT UINT64                             *Lun
+  );
+
+/**
+  Resets a SCSI channel. This operation resets all the SCSI devices connected to the SCSI channel.
+
+  @param  This A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      The SCSI channel was reset.
+  @retval EFI_DEVICE_ERROR A device error occurred while attempting to reset the SCSI channel.
+  @retval EFI_TIMEOUT      A timeout occurred while attempting to reset the SCSI channel.
+  @retval EFI_UNSUPPORTED  The SCSI channel does not support a channel reset operation.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruResetChannel (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   *This
+  );
+
+/**
+  Resets a SCSI logical unit that is connected to a SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target The Target is an array of size TARGET_MAX_BYTE and it represents the
+                 target port ID of the SCSI device containing the SCSI logical unit to
+                 reset. Transport drivers may chose to utilize a subset of this array to suit
+                 the representation of their targets.
+  @param  Lun    The LUN of the SCSI device to reset.
+
+  @retval EFI_SUCCESS           The SCSI device specified by Target and Lun was reset.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           A timeout occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+  @retval EFI_UNSUPPORTED       The SCSI channel does not support a target reset operation.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to reset the SCSI device
+                                 specified by Target and Lun.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruResetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN UINT8                              *Target,
+  IN UINT64                             Lun
+  );
+
+/**
+  Used to retrieve the list of legal Target IDs for SCSI devices on a SCSI channel. These can either
+  be the list SCSI devices that are actually present on the SCSI channel, or the list of legal Target IDs
+  for the SCSI channel. Regardless, the caller of this function must probe the Target ID returned to
+  see if a SCSI device is actually present at that location on the SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target (TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID of the next SCSI device on the SCSI
+                                channel was returned in Target.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           Target array is not all 0xF, and Target was not
+                                returned on a previous call to GetNextTarget().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetNextTarget (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target
+  );
+
+/**
+  Sends a UFS-supported SCSI Request Packet to a UFS device that is attached to the UFS host controller.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Lun           The LUN of the UFS device to send the SCSI Request Packet.
+  @param[in, out] Packet        A pointer to the SCSI Request Packet to send to a specified Lun of the
+                                UFS device.
+  @param[in]      Event         If nonblocking I/O is not supported then Event is ignored, and blocking
+                                I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                                Event is not NULL and non blocking I/O is supported, then
+                                nonblocking I/O is performed, and Event will be signaled when the
+                                SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsExecScsiCmds (
+  IN     UFS_PASS_THRU_PRIVATE_DATA                  *Private,
+  IN     UINT8                                       Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet,
+  IN     EFI_EVENT                                   Event    OPTIONAL
+  );
+
+/**
+  Initialize the UFS host controller.
+
+  @param[in] Private                 The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The NVM Express Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+UfsControllerInit (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  );
+
+/**
+  Stop the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is stopped successfully.
+  @retval Others                     A device error occurred while stopping the controller.
+
+**/
+EFI_STATUS
+UfsControllerStop (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  );
+
+/**
+  Allocate common buffer for host and UFS bus master access simultaneously.
+
+  @param[in]  Private                The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Size                   The length of buffer to be allocated.
+  @param[out] CmdDescHost            A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescPhyAddr         The resulting map address for the UFS bus master to use to access the hosts CmdDescHost.
+  @param[out] CmdDescMapping         A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS                The common buffer was allocated successfully.
+  @retval EFI_DEVICE_ERROR           The allocation fails.
+  @retval EFI_OUT_OF_RESOURCES       The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsAllocateAlignCommonBuffer (
+  IN     UFS_PASS_THRU_PRIVATE_DATA    *Private,
+  IN     UINTN                         Size,
+     OUT VOID                          **CmdDescHost,
+     OUT EFI_PHYSICAL_ADDRESS          *CmdDescPhyAddr,
+     OUT VOID                          **CmdDescMapping
+  );
+
+/**
+  Set specified flag to 1 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be set.
+
+  @retval EFI_SUCCESS           The flag was set successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to set the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of setting the flag.
+
+**/
+EFI_STATUS
+UfsSetFlag (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        FlagId
+  );
+
+/**
+  Read or write specified device descriptor of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      DescId        The ID of device descriptor.
+  @param[in]      Index         The Index of device descriptor.
+  @param[in]      Selector      The Selector of device descriptor.
+  @param[in, out] Descriptor    The buffer of device descriptor to be read or written.
+  @param[in]      DescSize      The size of device descriptor buffer.
+
+  @retval EFI_SUCCESS           The device descriptor was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the device descriptor.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the device descriptor.
+
+**/
+EFI_STATUS
+UfsRwDeviceDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        DescId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT VOID                         *Descriptor,
+  IN     UINT32                       DescSize
+  );
+
+/**
+  Sends NOP IN cmd to a UFS device for initialization process request.
+  For more details, please refer to UFS 2.0 spec Figure 13.3.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS           The NOP IN command was sent by the host. The NOP OUT response was
+                                received successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to execute NOP IN command.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the NOP IN command to execute.
+
+**/
+EFI_STATUS
+UfsExecNopCmds (
+  IN  UFS_PASS_THRU_PRIVATE_DATA       *Private
+  );
+
+/**
+  Call back function when the timer event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the Event.
+
+**/
+VOID
+EFIAPI
+ProcessAsyncTaskList (
+  IN EFI_EVENT          Event,
+  IN VOID               *Context
+  );
+
+/**
+  Internal helper function which will signal the caller event and clean up
+  resources.
+
+  @param[in] Private   The pointer to the UFS_PASS_THRU_PRIVATE_DATA data
+                       structure.
+  @param[in] TransReq  The pointer to the UFS_PASS_THRU_TRANS_REQ data
+                       structure.
+
+**/
+VOID
+EFIAPI
+SignalCallerEvent (
+  IN UFS_PASS_THRU_PRIVATE_DATA      *Private,
+  IN UFS_PASS_THRU_TRANS_REQ         *TransReq
+  );
+
+extern EFI_COMPONENT_NAME_PROTOCOL  gUfsPassThruComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gUfsPassThruComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL  gUfsPassThruDriverBinding;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.uni b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.uni
new file mode 100644
index 0000000000..1e627bd9cd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruDxe.inf b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruDxe.inf
new file mode 100644
index 0000000000..c90c72f915
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruDxe.inf
@@ -0,0 +1,62 @@
+## @file
+# Description file for the Universal Flash Storage (UFS) Pass Thru driver.
+#
+# Copyright (c) 2014 - 2015, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UfsPassThruDxe
+  MODULE_UNI_FILE                = UfsPassThru.uni
+  FILE_GUID                      = E7F1DFF9-DAB6-498A-9ADF-57F344EDDF57
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeUfsPassThru
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUfsPassThruDriverBinding              
+#  COMPONENT_NAME                =  gUfsPassThruComponentName              
+#
+
+[Sources]
+  ComponentName.c
+  UfsPassThru.c
+  UfsPassThru.h
+  UfsPassThruHci.c
+  UfsPassThruHci.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiLib
+  BaseLib
+  UefiDriverEntryPoint
+  DebugLib
+  DevicePathLib
+  TimerLib
+
+[Protocols]
+  gEfiExtScsiPassThruProtocolGuid               ## BY_START
+  gEdkiiUfsHostControllerProtocolGuid           ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UfsPassThruExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruExtra.uni b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruExtra.uni
new file mode 100644
index 0000000000..771b7ba3b5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.c b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.c
new file mode 100644
index 0000000000..26986cbf26
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.c
@@ -0,0 +1,2343 @@
+/** @file
+  UfsPassThruDxe driver is used to produce EFI_EXT_SCSI_PASS_THRU protocol interface
+  for upper layer application to execute UFS-supported SCSI cmds.
+
+  Copyright (c) 2014 - 2015, 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 "UfsPassThru.h"
+
+/**
+  Read 32bits data from specified UFS MMIO register.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Offset        The offset within the UFS Host Controller MMIO space to start
+                            the memory operation.
+  @param[out] Value         The data buffer to store.
+
+  @retval EFI_TIMEOUT       The operation is time out.
+  @retval EFI_SUCCESS       The operation succeeds.
+  @retval Others            The operation fails.
+
+**/
+EFI_STATUS
+UfsMmioRead32 (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     UINTN                        Offset,
+     OUT UINT32                       *Value
+  )
+{
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL  *UfsHc;
+  EFI_STATUS                          Status;
+
+  UfsHc = Private->UfsHostController;
+
+  Status = UfsHc->Read (UfsHc, EfiUfsHcWidthUint32, Offset, 1, Value);
+
+  return Status;
+}
+
+/**
+  Write 32bits data to specified UFS MMIO register.
+
+  @param[in] Private        The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in] Offset         The offset within the UFS Host Controller MMIO space to start
+                            the memory operation.
+  @param[in] Value          The data to write.
+
+  @retval EFI_TIMEOUT       The operation is time out.
+  @retval EFI_SUCCESS       The operation succeeds.
+  @retval Others            The operation fails.
+
+**/
+EFI_STATUS
+UfsMmioWrite32 (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINTN                        Offset,
+  IN  UINT32                       Value
+  )
+{
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL  *UfsHc;
+  EFI_STATUS                          Status;
+
+  UfsHc = Private->UfsHostController;
+
+  Status = UfsHc->Write (UfsHc, EfiUfsHcWidthUint32, Offset, 1, &Value);
+
+  return Status;
+}
+
+/**
+  Wait for the value of the specified system memory set to the test value.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Offset        The offset within the UFS Host Controller MMIO space to start
+                            the memory operation.
+  @param[in]  MaskValue     The mask value of memory.
+  @param[in]  TestValue     The test value of memory.
+  @param[in]  Timeout       The time out value for wait memory set, uses 100ns as a unit.
+
+  @retval EFI_TIMEOUT       The system memory setting is time out.
+  @retval EFI_SUCCESS       The system memory is correct set.
+  @retval Others            The operation fails.
+
+**/
+EFI_STATUS
+UfsWaitMemSet (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINTN                        Offset,
+  IN  UINT32                       MaskValue,
+  IN  UINT32                       TestValue,
+  IN  UINT64                       Timeout
+  )
+{
+  UINT32     Value;
+  UINT64     Delay;
+  BOOLEAN    InfiniteWait;
+  EFI_STATUS Status;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32 (Timeout, 10) + 1;
+
+  do {
+    //
+    // Access PCI MMIO space to see if the value is the tested one.
+    //
+    Status = UfsMmioRead32 (Private, Offset, &Value);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Value &= MaskValue;
+
+    if (Value == TestValue) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 1 microseconds.
+    //
+    MicroSecondDelay (1);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Dump UIC command execution result for debugging.
+
+  @param[in]   UicOpcode  The executed UIC opcode.
+  @param[in]   Result     The result to be parsed.
+
+**/
+VOID
+DumpUicCmdExecResult (
+  IN  UINT8     UicOpcode,
+  IN  UINT8     Result
+  )
+{
+  if (UicOpcode <= UfsUicDmePeerSet) {
+    switch (Result) {
+      case 0x00:
+        break;
+      case 0x01:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - INVALID_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x02:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - INVALID_MIB_ATTRIBUTE_VALUE\n"));
+        break;
+      case 0x03:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - READ_ONLY_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x04:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - WRITE_ONLY_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x05:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BAD_INDEX\n"));
+        break;
+      case 0x06:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - LOCKED_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x07:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BAD_TEST_FEATURE_INDEX\n"));
+        break;
+      case 0x08:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - PEER_COMMUNICATION_FAILURE\n"));
+        break; 
+      case 0x09:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BUSY\n"));
+        break;
+      case 0x0A:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - DME_FAILURE\n"));
+        break;        
+      default :
+        ASSERT (FALSE);
+        break;
+    }
+  } else {
+    switch (Result) {
+      case 0x00:
+        break;
+      case 0x01:
+        DEBUG ((EFI_D_VERBOSE, "UIC control command fails - FAILURE\n"));
+        break;     
+      default :
+        ASSERT (FALSE);
+        break;
+    }
+  }
+}
+
+/**
+  Dump QUERY RESPONSE UPIU result for debugging.
+
+  @param[in]   Result  The result to be parsed.
+
+**/
+VOID
+DumpQueryResponseResult (
+  IN  UINT8     Result
+  )
+{
+  switch (Result) {
+    case 0xF6:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Not Readable\n"));
+      break;
+    case 0xF7:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Not Writeable\n"));
+      break;
+    case 0xF8:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Already Written\n"));
+      break;
+    case 0xF9:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Length\n"));
+      break;
+    case 0xFA:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Value\n"));
+      break;
+    case 0xFB:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Selector\n"));
+      break;
+    case 0xFC:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Index\n"));
+      break;
+    case 0xFD:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Idn\n"));
+      break;
+    case 0xFE:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Opcode\n"));
+      break; 
+    case 0xFF:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with General Failure\n"));
+      break;
+    default :
+      ASSERT (FALSE);
+      break;
+  }
+}
+
+/**
+  Swap little endian to big endian.
+
+  @param[in, out] Buffer      The data buffer. In input, it contains little endian data.
+                              In output, it will become big endian.
+  @param[in]      BufferSize  The length of converted data.
+
+**/
+VOID
+SwapLittleEndianToBigEndian (
+  IN OUT UINT8         *Buffer,
+  IN     UINT32        BufferSize
+  )
+{
+  UINT32 Index;
+  UINT8  Temp;
+  UINT32 SwapCount;
+
+  SwapCount = BufferSize / 2;
+  for (Index = 0; Index < SwapCount; Index++) {
+    Temp = Buffer[Index];
+    Buffer[Index] = Buffer[BufferSize - 1 - Index];
+    Buffer[BufferSize - 1 - Index] = Temp;
+  }
+}
+
+/**
+  Fill TSF field of QUERY REQUEST UPIU.
+
+  @param[in, out] TsfBase      The base address of TSF field of QUERY REQUEST UPIU.
+  @param[in]      Opcode       The opcode of request.
+  @param[in]      DescId       The descriptor ID of request.
+  @param[in]      Index        The index of request.
+  @param[in]      Selector     The selector of request.
+  @param[in]      Length       The length of transferred data. The maximum is 4.
+  @param[in]      Value        The value of transferred data.
+
+**/
+VOID
+UfsFillTsfOfQueryReqUpiu (
+  IN OUT UTP_UPIU_TSF        *TsfBase,
+  IN     UINT8               Opcode,
+  IN     UINT8               DescId    OPTIONAL,
+  IN     UINT8               Index     OPTIONAL,
+  IN     UINT8               Selector  OPTIONAL,
+  IN     UINT16              Length    OPTIONAL,
+  IN     UINT32              Value     OPTIONAL
+  )
+{
+  ASSERT (TsfBase != NULL);
+  ASSERT (Opcode <= UtpQueryFuncOpcodeTogFlag);
+
+  TsfBase->Opcode   = Opcode;
+  if (Opcode != UtpQueryFuncOpcodeNop) {
+    TsfBase->DescId   = DescId;
+    TsfBase->Index    = Index;
+    TsfBase->Selector = Selector;
+
+    if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeWrDesc)) {
+      SwapLittleEndianToBigEndian ((UINT8*)&Length, sizeof (Length));
+      TsfBase->Length = Length;
+    }
+  
+    if (Opcode == UtpQueryFuncOpcodeWrAttr) {
+      SwapLittleEndianToBigEndian ((UINT8*)&Value, sizeof (Value));
+      TsfBase->Value  = Value;
+    }
+  }
+}
+
+/**
+  Initialize COMMAND UPIU.
+
+  @param[in, out] Command         The base address of COMMAND UPIU.
+  @param[in]      Lun             The Lun on which the SCSI command is executed.
+  @param[in]      TaskTag         The task tag of request.
+  @param[in]      Cdb             The cdb buffer containing SCSI command.
+  @param[in]      CdbLength       The cdb length.
+  @param[in]      DataDirection   The direction of data transfer.
+  @param[in]      ExpDataTranLen  The expected transfer data length.
+
+  @retval EFI_SUCCESS     The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitCommandUpiu (
+  IN OUT UTP_COMMAND_UPIU              *Command,
+  IN     UINT8                         Lun,
+  IN     UINT8                         TaskTag,
+  IN     UINT8                         *Cdb,
+  IN     UINT8                         CdbLength,
+  IN     UFS_DATA_DIRECTION            DataDirection,
+  IN     UINT32                        ExpDataTranLen
+  )
+{
+  UINT8                   Flags;
+
+  ASSERT ((Command != NULL) && (Cdb != NULL));
+
+  //
+  // Task attribute is hard-coded to Ordered.
+  //
+  if (DataDirection == UfsDataIn) {
+    Flags = BIT0 | BIT6;
+  } else if (DataDirection == UfsDataOut) {
+    Flags = BIT0 | BIT5;
+  } else {
+    Flags = BIT0;
+  }
+
+  //
+  // Fill UTP COMMAND UPIU associated fields.
+  //
+  Command->TransCode = 0x01;
+  Command->Flags     = Flags;
+  Command->Lun       = Lun;
+  Command->TaskTag   = TaskTag;
+  Command->CmdSet    = 0x00;
+  SwapLittleEndianToBigEndian ((UINT8*)&ExpDataTranLen, sizeof (ExpDataTranLen));
+  Command->ExpDataTranLen = ExpDataTranLen;
+
+  CopyMem (Command->Cdb, Cdb, CdbLength);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UTP PRDT for data transfer.
+
+  @param[in] Prdt         The base address of PRDT.
+  @param[in] Buffer       The buffer to be read or written.
+  @param[in] BufferSize   The data size to be read or written.
+
+  @retval EFI_SUCCESS     The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitUtpPrdt (
+  IN  UTP_TR_PRD                       *Prdt,
+  IN  VOID                             *Buffer,
+  IN  UINT32                           BufferSize
+  )
+{
+  UINT32     PrdtIndex;
+  UINT32     RemainingLen;
+  UINT8      *Remaining;
+  UINTN      PrdtNumber;
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  ASSERT (((UINTN)Buffer & (BIT0 | BIT1)) == 0);
+
+  RemainingLen = BufferSize;
+  Remaining    = Buffer;
+  PrdtNumber   = (UINTN)DivU64x32 ((UINT64)BufferSize + UFS_MAX_DATA_LEN_PER_PRD - 1, UFS_MAX_DATA_LEN_PER_PRD);
+
+  for (PrdtIndex = 0; PrdtIndex < PrdtNumber; PrdtIndex++) {
+    if (RemainingLen < UFS_MAX_DATA_LEN_PER_PRD) {
+      Prdt[PrdtIndex].DbCount = (UINT32)RemainingLen - 1;
+    } else {
+      Prdt[PrdtIndex].DbCount = UFS_MAX_DATA_LEN_PER_PRD - 1;
+    }
+
+    Prdt[PrdtIndex].DbAddr  = (UINT32)RShiftU64 ((UINT64)(UINTN)Remaining, 2);
+    Prdt[PrdtIndex].DbAddrU = (UINT32)RShiftU64 ((UINT64)(UINTN)Remaining, 32);
+    RemainingLen -= UFS_MAX_DATA_LEN_PER_PRD;
+    Remaining    += UFS_MAX_DATA_LEN_PER_PRD;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize QUERY REQUEST UPIU.
+
+  @param[in, out] QueryReq      The base address of QUERY REQUEST UPIU.
+  @param[in]      TaskTag       The task tag of request.
+  @param[in]      Opcode        The opcode of request.
+  @param[in]      DescId        The descriptor ID of request.
+  @param[in]      Index         The index of request.
+  @param[in]      Selector      The selector of request.
+  @param[in]      DataSize      The data size to be read or written.
+  @param[in]      Data          The buffer to be read or written.
+
+  @retval EFI_SUCCESS           The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitQueryRequestUpiu (
+  IN OUT UTP_QUERY_REQ_UPIU            *QueryReq,
+  IN     UINT8                         TaskTag,
+  IN     UINT8                         Opcode,
+  IN     UINT8                         DescId,
+  IN     UINT8                         Index,
+  IN     UINT8                         Selector,
+  IN     UINTN                         DataSize   OPTIONAL,
+  IN     UINT8                         *Data      OPTIONAL
+  )
+{
+  ASSERT (QueryReq != NULL);
+
+  QueryReq->TransCode = 0x16;
+  QueryReq->TaskTag   = TaskTag;
+  if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeRdFlag) || (Opcode == UtpQueryFuncOpcodeRdAttr)) {
+    QueryReq->QueryFunc = QUERY_FUNC_STD_READ_REQ;
+  } else {
+    QueryReq->QueryFunc = QUERY_FUNC_STD_WRITE_REQ;
+  }
+
+  if (Opcode == UtpQueryFuncOpcodeWrAttr) {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, 0, *(UINT32*)Data);
+  } else if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeWrDesc)) {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, (UINT16)DataSize, 0);
+  } else {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, 0, 0);
+  }
+
+  if (Opcode == UtpQueryFuncOpcodeWrDesc) {
+    CopyMem (QueryReq + 1, Data, DataSize);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate COMMAND/RESPONSE UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Lun               The Lun on which the SCSI command is executed.
+  @param[in]  Packet            The pointer to the EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+  @param[out] CmdDescHost       A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescMapping    A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsCreateScsiCommandDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA                  *Private,
+  IN     UINT8                                       Lun,
+  IN     EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet,
+  IN     UTP_TRD                                     *Trd,
+     OUT VOID                                        **CmdDescHost,
+     OUT VOID                                        **CmdDescMapping
+  )
+{
+  UINTN                             TotalLen;
+  UINTN                             PrdtNumber;
+  UTP_COMMAND_UPIU                  *CommandUpiu;
+  EFI_PHYSICAL_ADDRESS              CmdDescPhyAddr;
+  EFI_STATUS                        Status;
+  UINT32                            DataLen;
+  UFS_DATA_DIRECTION                DataDirection;
+
+  ASSERT ((Private != NULL) && (Packet != NULL) && (Trd != NULL));
+
+  if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+    DataLen       = Packet->InTransferLength;
+    DataDirection = UfsDataIn;
+  } else {
+    DataLen       = Packet->OutTransferLength;
+    DataDirection = UfsDataOut;
+  }
+
+  if (DataLen == 0) {
+    DataDirection = UfsNoData;
+  }
+
+  PrdtNumber = (UINTN)DivU64x32 ((UINT64)DataLen + UFS_MAX_DATA_LEN_PER_PRD - 1, UFS_MAX_DATA_LEN_PER_PRD);
+
+  TotalLen   = ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)) + PrdtNumber * sizeof (UTP_TR_PRD);
+
+  Status = UfsAllocateAlignCommonBuffer (Private, TotalLen, CmdDescHost, &CmdDescPhyAddr, CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CommandUpiu = (UTP_COMMAND_UPIU*)*CmdDescHost;
+
+  UfsInitCommandUpiu (CommandUpiu, Lun, Private->TaskTag++, Packet->Cdb, Packet->CdbLength, DataDirection, DataLen);
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Response UPIU and the Physical Region Description Table
+  // *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = DataDirection;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32);
+  Trd->RuL    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)), sizeof (UINT32));
+  Trd->RuO    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)), sizeof (UINT32));
+  Trd->PrdtL  = (UINT16)PrdtNumber;
+  Trd->PrdtO  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU))), sizeof (UINT32));
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate QUERY REQUEST/QUERY RESPONSE UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Packet            The pointer to the UFS_DEVICE_MANAGEMENT_REQUEST_PACKET data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+  @param[out] CmdDescHost       A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescMapping    A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+  @retval EFI_INVALID_PARAMETER The parameter passed in is invalid.
+
+**/
+EFI_STATUS
+UfsCreateDMCommandDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA            *Private,
+  IN     UFS_DEVICE_MANAGEMENT_REQUEST_PACKET  *Packet,
+  IN     UTP_TRD                               *Trd,
+     OUT VOID                                  **CmdDescHost,
+     OUT VOID                                  **CmdDescMapping
+  )
+{
+  UINTN                         TotalLen;
+  UTP_QUERY_REQ_UPIU            *QueryReqUpiu;
+  UINT8                         Opcode;
+  UINT32                        DataSize;
+  UINT8                         *Data;
+  UINT8                         DataDirection;
+  EFI_PHYSICAL_ADDRESS          CmdDescPhyAddr;
+  EFI_STATUS                    Status;
+
+  ASSERT ((Private != NULL) && (Packet != NULL) && (Trd != NULL));
+
+  Opcode = Packet->Opcode;
+  if ((Opcode > UtpQueryFuncOpcodeTogFlag) || (Opcode == UtpQueryFuncOpcodeNop)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DataDirection = Packet->DataDirection;
+  if (DataDirection == UfsDataIn) {
+    DataSize = Packet->InTransferLength;
+    Data     = Packet->InDataBuffer;
+  } else if (DataDirection == UfsDataOut) {
+    DataSize = Packet->OutTransferLength;
+    Data     = Packet->OutDataBuffer;
+  } else {
+    DataSize = 0;
+    Data     = NULL;
+  }
+
+  if (((Opcode != UtpQueryFuncOpcodeSetFlag) && (Opcode != UtpQueryFuncOpcodeClrFlag) && (Opcode != UtpQueryFuncOpcodeTogFlag))
+    && ((DataSize == 0) || (Data == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((Opcode == UtpQueryFuncOpcodeSetFlag) || (Opcode == UtpQueryFuncOpcodeClrFlag) || (Opcode == UtpQueryFuncOpcodeTogFlag))
+    && ((DataSize != 0) || (Data != NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Opcode == UtpQueryFuncOpcodeWrAttr) && (DataSize != sizeof (UINT32))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Opcode == UtpQueryFuncOpcodeWrDesc) || (Opcode == UtpQueryFuncOpcodeRdDesc)) {
+    TotalLen = ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)) + ROUNDUP8 (DataSize);
+  } else {
+    TotalLen = ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU));
+  }
+
+  Status = UfsAllocateAlignCommonBuffer (Private, TotalLen, CmdDescHost, &CmdDescPhyAddr, CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Initialize UTP QUERY REQUEST UPIU
+  //
+  QueryReqUpiu = (UTP_QUERY_REQ_UPIU*)*CmdDescHost;
+  ASSERT (QueryReqUpiu != NULL);
+  UfsInitQueryRequestUpiu (
+    QueryReqUpiu,
+    Private->TaskTag++,
+    Opcode,
+    Packet->DescId,
+    Packet->Index,
+    Packet->Selector,
+    DataSize,
+    Data
+    );
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Query Response UPIU *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = DataDirection;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->Ocs    = 0x0F;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32);
+  if (Opcode == UtpQueryFuncOpcodeWrDesc) {
+    Trd->RuL  = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)), sizeof (UINT32));
+    Trd->RuO  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (DataSize)), sizeof (UINT32));
+  } else {
+    Trd->RuL  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)) + ROUNDUP8 (DataSize)), sizeof (UINT32));
+    Trd->RuO  = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)), sizeof (UINT32));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate NOP IN and NOP OUT UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+  @param[out] CmdDescHost       A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescMapping    A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsCreateNopCommandDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA        *Private,
+  IN     UTP_TRD                           *Trd,
+     OUT VOID                              **CmdDescHost,
+     OUT VOID                              **CmdDescMapping
+  )
+{
+  UINTN                    TotalLen;
+  UTP_NOP_OUT_UPIU         *NopOutUpiu;
+  EFI_STATUS               Status;
+  EFI_PHYSICAL_ADDRESS     CmdDescPhyAddr;
+
+  ASSERT ((Private != NULL) && (Trd != NULL));
+
+  TotalLen = ROUNDUP8 (sizeof (UTP_NOP_OUT_UPIU)) + ROUNDUP8 (sizeof (UTP_NOP_IN_UPIU));
+  Status   = UfsAllocateAlignCommonBuffer (Private, TotalLen, CmdDescHost, &CmdDescPhyAddr, CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  NopOutUpiu = (UTP_NOP_OUT_UPIU*)*CmdDescHost;
+  ASSERT (NopOutUpiu != NULL);
+  NopOutUpiu->TaskTag = Private->TaskTag++;
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Nop Out UPIU *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = 0x00;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32);
+  Trd->RuL    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_NOP_IN_UPIU)), sizeof (UINT32));
+  Trd->RuO    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_NOP_OUT_UPIU)), sizeof (UINT32));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Find out available slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[out] Slot          The available slot.
+
+  @retval EFI_SUCCESS       The available slot was found successfully.
+  @retval EFI_NOT_READY     No slot is available at this moment.
+
+**/
+EFI_STATUS
+UfsFindAvailableSlotInTrl (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+     OUT UINT8                        *Slot
+  )
+{
+  UINT8            Nutrs;
+  UINT8            Index;
+  UINT32           Data;
+  EFI_STATUS       Status;
+
+  ASSERT ((Private != NULL) && (Slot != NULL));
+
+  Status  = UfsMmioRead32 (Private, UFS_HC_UTRLDBR_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Nutrs   = (UINT8)((Private->Capabilities & UFS_HC_CAP_NUTRS) + 1);
+
+  for (Index = 0; Index < Nutrs; Index++) {
+    if ((Data & (BIT0 << Index)) == 0) {
+      *Slot = Index;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_READY;
+}
+
+/**
+  Find out available slot in task management transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[out] Slot          The available slot.
+
+  @retval EFI_SUCCESS       The available slot was found successfully.
+
+**/
+EFI_STATUS
+UfsFindAvailableSlotInTmrl (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+     OUT UINT8                        *Slot
+  )
+{
+  ASSERT ((Private != NULL) && (Slot != NULL));
+
+  //
+  // The simplest algo to always use slot 0.
+  // TODO: enhance it to support async transfer with multiple slot.
+  //
+  *Slot = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start specified slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Slot          The slot to be started.
+
+**/
+EFI_STATUS
+UfsStartExecCmd (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        Slot
+  ) 
+{
+  UINT32        Data;
+  EFI_STATUS    Status;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_UTRLRSR_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & UFS_HC_UTRLRSR) != UFS_HC_UTRLRSR) {
+    Status = UfsMmioWrite32 (Private, UFS_HC_UTRLRSR_OFFSET, UFS_HC_UTRLRSR);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLDBR_OFFSET, BIT0 << Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop specified slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Slot          The slot to be stop.
+
+**/
+EFI_STATUS
+UfsStopExecCmd (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        Slot
+  ) 
+{
+  UINT32        Data;
+  EFI_STATUS    Status;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_UTRLDBR_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & (BIT0 << Slot)) != 0) {
+    Status = UfsMmioRead32 (Private, UFS_HC_UTRLCLR_OFFSET, &Data);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Status = UfsMmioWrite32 (Private, UFS_HC_UTRLCLR_OFFSET, Data & ~(BIT0 << Slot));
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read or write specified device descriptor of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      DescId        The ID of device descriptor.
+  @param[in]      Index         The Index of device descriptor.
+  @param[in]      Selector      The Selector of device descriptor.
+  @param[in, out] Descriptor    The buffer of device descriptor to be read or written.
+  @param[in]      DescSize      The size of device descriptor buffer.
+
+  @retval EFI_SUCCESS           The device descriptor was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the device descriptor.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the device descriptor.
+
+**/
+EFI_STATUS
+UfsRwDeviceDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        DescId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT VOID                         *Descriptor,
+  IN     UINT32                       DescSize
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT32                               CmdDescSize;
+  UINT16                               ReturnDataSize;
+  VOID                                 *CmdDescHost;
+  VOID                                 *CmdDescMapping;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    Packet.DataDirection     = UfsDataIn;
+    Packet.InDataBuffer      = Descriptor;
+    Packet.InTransferLength  = DescSize;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdDesc;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    Packet.OutDataBuffer     = Descriptor;
+    Packet.OutTransferLength = DescSize;
+    Packet.Opcode            = UtpQueryFuncOpcodeWrDesc;
+  }
+  Packet.DescId              = DescId;
+  Packet.Index               = Index;
+  Packet.Selector            = Selector;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd, &CmdDescHost, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  UfsHc       = Private->UfsHostController;
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)((UINT8*)CmdDescHost + Trd->RuO * sizeof (UINT32));
+  ASSERT (QueryResp != NULL);
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    ReturnDataSize = QueryResp->Tsf.Length;
+    SwapLittleEndianToBigEndian ((UINT8*)&ReturnDataSize, sizeof (UINT16));
+
+    if (Read) {
+      CopyMem (Packet.InDataBuffer, (QueryResp + 1), ReturnDataSize);
+      Packet.InTransferLength = ReturnDataSize;
+    } else {
+      Packet.OutTransferLength = ReturnDataSize;
+    }
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, Slot);
+
+  if (CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, CmdDescMapping);
+  }
+  if (CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (CmdDescSize), CmdDescHost);
+  }
+
+  return Status;
+}
+
+/**
+  Read or write specified attribute of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      AttrId        The ID of Attribute.
+  @param[in]      Index         The Index of Attribute.
+  @param[in]      Selector      The Selector of Attribute.
+  @param[in, out] Attributes    The value of Attribute to be read or written.
+
+  @retval EFI_SUCCESS           The Attribute was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the Attribute.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the Attribute.
+
+**/
+EFI_STATUS
+UfsRwAttributes (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        AttrId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT UINT32                       *Attributes
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT32                               CmdDescSize;
+  UINT32                               ReturnData;
+  VOID                                 *CmdDescHost;
+  VOID                                 *CmdDescMapping;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    Packet.DataDirection     = UfsDataIn;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdAttr;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    Packet.Opcode            = UtpQueryFuncOpcodeWrAttr;
+  }
+  Packet.DescId              = AttrId;
+  Packet.Index               = Index;
+  Packet.Selector            = Selector;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd, &CmdDescHost, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  UfsHc       = Private->UfsHostController;
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)((UINT8*)CmdDescHost + Trd->RuO * sizeof (UINT32));
+  ASSERT (QueryResp != NULL);
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    ReturnData = QueryResp->Tsf.Value;
+    SwapLittleEndianToBigEndian ((UINT8*)&ReturnData, sizeof (UINT32));
+    *Attributes = ReturnData;
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, Slot);
+
+  if (CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, CmdDescMapping);
+  }
+
+  if (CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (CmdDescSize), CmdDescHost);
+  }
+
+  return Status;
+}
+
+/**
+  Read or write specified flag of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      FlagId        The ID of flag to be read or written.
+  @param[in, out] Value         The value to set or clear flag.
+
+  @retval EFI_SUCCESS           The flag was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the flag.
+
+**/
+EFI_STATUS
+UfsRwFlags (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        FlagId,
+  IN OUT UINT8                        *Value
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT32                               CmdDescSize;
+  VOID                                 *CmdDescHost;
+  VOID                                 *CmdDescMapping;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  if (Value == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    ASSERT (Value != NULL);
+    Packet.DataDirection     = UfsDataIn;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdFlag;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    if (*Value == 1) {
+      Packet.Opcode          = UtpQueryFuncOpcodeSetFlag;
+    } else if (*Value == 0) {
+      Packet.Opcode          = UtpQueryFuncOpcodeClrFlag;
+    } else {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  Packet.DescId              = FlagId;
+  Packet.Index               = 0;
+  Packet.Selector            = 0;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Trd    = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd, &CmdDescHost, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  UfsHc       = Private->UfsHostController;
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)((UINT8*)CmdDescHost + Trd->RuO * sizeof (UINT32));
+  ASSERT (QueryResp != NULL);
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    *Value = (UINT8)QueryResp->Tsf.Value;
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, Slot);
+
+  if (CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, CmdDescMapping);
+  }
+  if (CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (CmdDescSize), CmdDescHost);
+  }
+
+  return Status;
+}
+
+/**
+  Set specified flag to 1 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be set.
+
+  @retval EFI_SUCCESS           The flag was set successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to set the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of setting the flag.
+
+**/
+EFI_STATUS
+UfsSetFlag (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        FlagId
+  )
+{
+  EFI_STATUS             Status;
+  UINT8                  Value;
+
+  Value  = 1;
+  Status = UfsRwFlags (Private, FALSE, FlagId, &Value);
+
+  return Status;
+}
+
+/**
+  Clear specified flag to 0 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be cleared.
+
+  @retval EFI_SUCCESS           The flag was cleared successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to clear the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of clearing the flag.
+
+**/
+EFI_STATUS
+UfsClearFlag (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        FlagId
+  )
+{
+  EFI_STATUS             Status;
+  UINT8                  Value;
+
+  Value  = 0;
+  Status = UfsRwFlags (Private, FALSE, FlagId, &Value);
+
+  return Status;
+}
+
+/**
+  Read specified flag from a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be read.
+  @param[out] Value             The flag's value.
+
+  @retval EFI_SUCCESS           The flag was read successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to read the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of reading the flag.
+
+**/
+EFI_STATUS
+UfsReadFlag (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     UINT8                        FlagId,
+     OUT UINT8                        *Value
+  )
+{
+  EFI_STATUS                           Status;
+
+  Status = UfsRwFlags (Private, TRUE, FlagId, Value);
+
+  return Status;
+}
+
+/**
+  Sends NOP IN cmd to a UFS device for initialization process request.
+  For more details, please refer to UFS 2.0 spec Figure 13.3.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS           The NOP IN command was sent by the host. The NOP OUT response was
+                                received successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to execute NOP IN command.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the NOP IN command to execute.
+
+**/
+EFI_STATUS
+UfsExecNopCmds (
+  IN  UFS_PASS_THRU_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS                           Status;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_NOP_IN_UPIU                      *NopInUpiu;
+  UINT32                               CmdDescSize;
+  VOID                                 *CmdDescHost;
+  VOID                                 *CmdDescMapping;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Trd    = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  Status = UfsCreateNopCommandDesc (Private, Trd, &CmdDescHost, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  UfsHc       = Private->UfsHostController;
+  NopInUpiu   = (UTP_NOP_IN_UPIU*)((UINT8*)CmdDescHost + Trd->RuO * sizeof (UINT32));
+  ASSERT (NopInUpiu != NULL);
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (NopInUpiu->Resp != 0) {
+    Status = EFI_DEVICE_ERROR;
+  } else {
+    Status = EFI_SUCCESS;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, Slot);
+
+  if (CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, CmdDescMapping);
+  }
+  if (CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (CmdDescSize), CmdDescHost);
+  }
+
+  return Status;
+}
+
+/**
+  Sends a UFS-supported SCSI Request Packet to a UFS device that is attached to the UFS host controller.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Lun           The LUN of the UFS device to send the SCSI Request Packet.
+  @param[in, out] Packet        A pointer to the SCSI Request Packet to send to a specified Lun of the
+                                UFS device.
+  @param[in]      Event         If nonblocking I/O is not supported then Event is ignored, and blocking
+                                I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                                Event is not NULL and non blocking I/O is supported, then
+                                nonblocking I/O is performed, and Event will be signaled when the
+                                SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsExecScsiCmds (
+  IN     UFS_PASS_THRU_PRIVATE_DATA                  *Private,
+  IN     UINT8                                       Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet,
+  IN     EFI_EVENT                                   Event    OPTIONAL
+  )
+{
+  EFI_STATUS                           Status;
+  UTP_RESPONSE_UPIU                    *Response;
+  UINT16                               SenseDataLen;
+  UINT32                               ResTranCount;
+  VOID                                 *DataBuf;
+  EFI_PHYSICAL_ADDRESS                 DataBufPhyAddr;
+  UINT32                               DataLen;
+  UINTN                                MapLength;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+  EDKII_UFS_HOST_CONTROLLER_OPERATION  Flag;
+  UTP_TR_PRD                           *PrdtBase;
+  EFI_TPL                              OldTpl;
+  UFS_PASS_THRU_TRANS_REQ              *TransReq;
+
+  TransReq       = AllocateZeroPool (sizeof (UFS_PASS_THRU_TRANS_REQ));
+  if (TransReq == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  TransReq->Signature     = UFS_PASS_THRU_TRANS_REQ_SIG;
+  TransReq->TimeoutRemain = Packet->Timeout;
+  DataBufPhyAddr = 0;
+  UfsHc          = Private->UfsHostController;
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &TransReq->Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  TransReq->Trd = ((UTP_TRD*)Private->UtpTrlBase) + TransReq->Slot;
+
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateScsiCommandDesc (
+             Private,
+             Lun,
+             Packet,
+             TransReq->Trd,
+             &TransReq->CmdDescHost,
+             &TransReq->CmdDescMapping
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  TransReq->CmdDescSize = TransReq->Trd->PrdtO * sizeof (UINT32) + TransReq->Trd->PrdtL * sizeof (UTP_TR_PRD);
+
+  if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+    DataBuf       = Packet->InDataBuffer;
+    DataLen       = Packet->InTransferLength;
+    Flag          = EdkiiUfsHcOperationBusMasterWrite;
+  } else {
+    DataBuf       = Packet->OutDataBuffer;
+    DataLen       = Packet->OutTransferLength;
+    Flag          = EdkiiUfsHcOperationBusMasterRead;
+  }
+
+  if (DataLen != 0) {
+    MapLength = DataLen;
+    Status    = UfsHc->Map (
+                         UfsHc,
+                         Flag,
+                         DataBuf,
+                         &MapLength,
+                         &DataBufPhyAddr,
+                         &TransReq->DataBufMapping
+                         );
+
+    if (EFI_ERROR (Status) || (DataLen != MapLength)) {
+      goto Exit1;
+    }
+  }
+  //
+  // Fill PRDT table of Command UPIU for executed SCSI cmd.
+  //
+  PrdtBase = (UTP_TR_PRD*)((UINT8*)TransReq->CmdDescHost + ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)));
+  ASSERT (PrdtBase != NULL);
+  UfsInitUtpPrdt (PrdtBase, (VOID*)(UINTN)DataBufPhyAddr, DataLen);
+
+  //
+  // Insert the async SCSI cmd to the Async I/O list
+  //
+  if (Event != NULL) {
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    TransReq->Packet      = Packet;
+    TransReq->CallerEvent = Event;
+    InsertTailList (&Private->Queue, &TransReq->TransferList);
+    gBS->RestoreTPL (OldTpl);
+  }
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, TransReq->Slot);
+
+  //
+  // Immediately return for async I/O.
+  //
+  if (Event != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Wait for the completion of the transfer request.
+  // 
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, Packet->Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Get sense data if exists
+  //
+  Response     = (UTP_RESPONSE_UPIU*)((UINT8*)TransReq->CmdDescHost + TransReq->Trd->RuO * sizeof (UINT32));
+  ASSERT (Response != NULL);
+  SenseDataLen = Response->SenseDataLen;
+  SwapLittleEndianToBigEndian ((UINT8*)&SenseDataLen, sizeof (UINT16));
+  
+  if ((Packet->SenseDataLength != 0) && (Packet->SenseData != NULL)) {
+    CopyMem (Packet->SenseData, Response->SenseData, SenseDataLen);
+    Packet->SenseDataLength = (UINT8)SenseDataLen;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  Packet->TargetStatus = Response->Status;
+  if (Response->Response != 0) {
+    DEBUG ((EFI_D_ERROR, "UfsExecScsiCmds() fails with Target Failure\n"));
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (TransReq->Trd->Ocs == 0) {
+    if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+      if ((Response->Flags & BIT5) == BIT5) {
+        ResTranCount = Response->ResTranCount;
+        SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+        Packet->InTransferLength -= ResTranCount;
+      }
+    } else {
+      if ((Response->Flags & BIT5) == BIT5) {
+        ResTranCount = Response->ResTranCount;
+        SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+        Packet->OutTransferLength -= ResTranCount;
+      }
+    }
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, TransReq->Slot);
+
+  if (TransReq->DataBufMapping != NULL) {
+    UfsHc->Unmap (UfsHc, TransReq->DataBufMapping);
+  }
+
+Exit1:
+  if (TransReq->CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, TransReq->CmdDescMapping);
+  }
+  if (TransReq->CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (TransReq->CmdDescSize), TransReq->CmdDescHost);
+  }
+  if (TransReq != NULL) {
+    FreePool (TransReq);
+  }
+  return Status;
+}
+
+
+/**
+  Sent UIC DME_LINKSTARTUP command to start the link startup procedure.
+
+  @param[in] Private          The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in] UicOpcode        The opcode of the UIC command.
+  @param[in] Arg1             The value for 1st argument of the UIC command.
+  @param[in] Arg2             The value for 2nd argument of the UIC command.
+  @param[in] Arg3             The value for 3rd argument of the UIC command.
+
+  @return EFI_SUCCESS      Successfully execute this UIC command and detect attached UFS device.
+  @return EFI_DEVICE_ERROR Fail to execute this UIC command and detect attached UFS device.
+  @return EFI_NOT_FOUND    The presence of the UFS device isn't detected.
+
+**/
+EFI_STATUS
+UfsExecUicCommands (
+  IN  UFS_PASS_THRU_PRIVATE_DATA    *Private,
+  IN  UINT8                         UicOpcode,
+  IN  UINT32                        Arg1,
+  IN  UINT32                        Arg2,
+  IN  UINT32                        Arg3
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      Data;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_IS_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & UFS_HC_IS_UCCS) == UFS_HC_IS_UCCS) {
+    //
+    // Clear IS.BIT10 UIC Command Completion Status (UCCS) at first.
+    //
+    Status = UfsMmioWrite32 (Private, UFS_HC_IS_OFFSET, Data);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // When programming UIC command registers, host software shall set the register UICCMD
+  // only after all the UIC command argument registers (UICCMDARG1, UICCMDARG2 and UICCMDARG3)
+  // are set.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UCMD_ARG1_OFFSET, Arg1);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UCMD_ARG2_OFFSET, Arg2);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UCMD_ARG3_OFFSET, Arg3);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Host software shall only set the UICCMD if HCS.UCRDY is set to 1.
+  //
+  Status = UfsWaitMemSet (Private, UFS_HC_STATUS_OFFSET, UFS_HC_HCS_UCRDY, UFS_HC_HCS_UCRDY, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UIC_CMD_OFFSET, (UINT32)UicOpcode);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // UFS 2.0 spec section 5.3.1 Offset:0x20 IS.Bit10 UIC Command Completion Status (UCCS)
+  // This bit is set to '1' by the host controller upon completion of a UIC command. 
+  //
+  Status  = UfsWaitMemSet (Private, UFS_HC_IS_OFFSET, UFS_HC_IS_UCCS, UFS_HC_IS_UCCS, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (UicOpcode != UfsUicDmeReset) {
+    Status = UfsMmioRead32 (Private, UFS_HC_UCMD_ARG2_OFFSET, &Data);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    if ((Data & 0xFF) != 0) {
+      DEBUG_CODE_BEGIN();
+        DumpUicCmdExecResult (UicOpcode, (UINT8)(Data & 0xFF));
+      DEBUG_CODE_END();
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Check value of HCS.DP and make sure that there is a device attached to the Link.
+  //
+  Status = UfsMmioRead32 (Private, UFS_HC_STATUS_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & UFS_HC_HCS_DP) == 0) {
+    Status  = UfsWaitMemSet (Private, UFS_HC_IS_OFFSET, UFS_HC_IS_ULSS, UFS_HC_IS_ULSS, UFS_TIMEOUT);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+    return EFI_NOT_FOUND;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsPassThruDxe: found a attached UFS device\n"));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate common buffer for host and UFS bus master access simultaneously.
+
+  @param[in]  Private                The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Size                   The length of buffer to be allocated.
+  @param[out] CmdDescHost            A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescPhyAddr         The resulting map address for the UFS bus master to use to access the hosts CmdDescHost.
+  @param[out] CmdDescMapping         A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS                The common buffer was allocated successfully.
+  @retval EFI_DEVICE_ERROR           The allocation fails.
+  @retval EFI_OUT_OF_RESOURCES       The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsAllocateAlignCommonBuffer (
+  IN     UFS_PASS_THRU_PRIVATE_DATA    *Private,
+  IN     UINTN                         Size,
+     OUT VOID                          **CmdDescHost,
+     OUT EFI_PHYSICAL_ADDRESS          *CmdDescPhyAddr,
+     OUT VOID                          **CmdDescMapping
+  )
+{
+  EFI_STATUS                           Status;
+  UINTN                                Bytes;
+  BOOLEAN                              Is32BitAddr;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  if ((Private->Capabilities & UFS_HC_CAP_64ADDR) == UFS_HC_CAP_64ADDR) {
+    Is32BitAddr = TRUE;
+  } else {
+    Is32BitAddr = FALSE;
+  }
+
+  UfsHc  = Private->UfsHostController;
+  Status = UfsHc->AllocateBuffer (
+                    UfsHc,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    EFI_SIZE_TO_PAGES (Size),
+                    CmdDescHost,
+                    0
+                    );
+  if (EFI_ERROR (Status)) {
+    *CmdDescMapping = NULL;
+    *CmdDescHost    = NULL;
+    *CmdDescPhyAddr = 0;
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Bytes  = EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size));
+  Status = UfsHc->Map (
+                    UfsHc,
+                    EdkiiUfsHcOperationBusMasterCommonBuffer,
+                    *CmdDescHost,
+                    &Bytes,
+                    CmdDescPhyAddr,
+                    CmdDescMapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size)))) {
+    UfsHc->FreeBuffer (
+             UfsHc,
+             EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size)),
+             *CmdDescHost
+             );
+    *CmdDescHost = NULL;
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (Is32BitAddr && ((*CmdDescPhyAddr) > 0x100000000ULL)) {
+    //
+    // The UFS host controller doesn't support 64bit addressing, so should not get a >4G UFS bus master address.
+    //
+    UfsHc->Unmap (
+             UfsHc,
+             *CmdDescMapping
+             );
+    UfsHc->FreeBuffer (
+             UfsHc,
+             EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size)),
+             *CmdDescHost
+             );
+    *CmdDescMapping = NULL;
+    *CmdDescHost    = NULL;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ZeroMem (*CmdDescHost, EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size)));
+  return EFI_SUCCESS;
+}
+
+/**
+  Enable the UFS host controller for accessing.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS host controller enabling was executed successfully.
+  @retval EFI_DEVICE_ERROR           A device error occurred while enabling the UFS host controller.
+
+**/
+EFI_STATUS
+UfsEnableHostController (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  EFI_STATUS             Status;
+  UINT32                 Data;
+
+  //
+  // UFS 2.0 spec section 7.1.1 - Host Controller Initialization
+  //
+  // Reinitialize the UFS host controller if HCE bit of HC register is set.
+  //
+  Status = UfsMmioRead32 (Private, UFS_HC_ENABLE_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & UFS_HC_HCE_EN) == UFS_HC_HCE_EN) {
+    //
+    // Write a 0 to the HCE register at first to disable the host controller.
+    //
+    Status = UfsMmioWrite32 (Private, UFS_HC_ENABLE_OFFSET, 0);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Wait until HCE is read as '0' before continuing.
+    //
+    Status = UfsWaitMemSet (Private, UFS_HC_ENABLE_OFFSET, UFS_HC_HCE_EN, 0, UFS_TIMEOUT);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Write a 1 to the HCE register to enable the UFS host controller.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_ENABLE_OFFSET, UFS_HC_HCE_EN);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Wait until HCE is read as '1' before continuing.
+  //
+  Status = UfsWaitMemSet (Private, UFS_HC_ENABLE_OFFSET, UFS_HC_HCE_EN, UFS_HC_HCE_EN, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Detect if a UFS device attached.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS device detection was executed successfully.
+  @retval EFI_NOT_FOUND              Not found a UFS device attached.
+  @retval EFI_DEVICE_ERROR           A device error occurred while detecting the UFS device.
+
+**/
+EFI_STATUS
+UfsDeviceDetection (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  UINTN                  Retry;
+  EFI_STATUS             Status;
+
+  //
+  // Start UFS device detection.
+  // Try up to 3 times for establishing data link with device.
+  //
+  for (Retry = 0; Retry < 3; Retry++) {
+    Status = UfsExecUicCommands (Private, UfsUicDmeLinkStartup, 0, 0, 0);
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+
+    if (Status == EFI_NOT_FOUND) {
+      continue;
+    }
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (Retry == 3) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UFS task management request list related h/w context.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS task management list was initialzed successfully.
+  @retval EFI_DEVICE_ERROR           The initialization fails.
+
+**/
+EFI_STATUS
+UfsInitTaskManagementRequestList (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  UINT32                 Data;
+  UINT8                  Nutmrs;
+  VOID                   *CmdDescHost;
+  EFI_PHYSICAL_ADDRESS   CmdDescPhyAddr;
+  VOID                   *CmdDescMapping;
+  EFI_STATUS             Status;
+  
+  //
+  // Initial h/w and s/w context for future operations.
+  //
+  CmdDescHost    = NULL;
+  CmdDescMapping = NULL;
+  CmdDescPhyAddr = 0;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_CAP_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Private->Capabilities = Data;
+
+  //
+  // Allocate and initialize UTP Task Management Request List.
+  //
+  Nutmrs = (UINT8) (RShiftU64 ((Private->Capabilities & UFS_HC_CAP_NUTMRS), 16) + 1);
+  Status = UfsAllocateAlignCommonBuffer (Private, Nutmrs * sizeof (UTP_TMRD), &CmdDescHost, &CmdDescPhyAddr, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Program the UTP Task Management Request List Base Address and UTP Task Management
+  // Request List Base Address with a 64-bit address allocated at step 6.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTMRLBA_OFFSET, (UINT32)(UINTN)CmdDescPhyAddr);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTMRLBAU_OFFSET, (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32));
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Private->UtpTmrlBase = CmdDescHost;
+  Private->Nutmrs      = Nutmrs;
+  Private->TmrlMapping = CmdDescMapping;
+
+  //
+  // Enable the UTP Task Management Request List by setting the UTP Task Management
+  // Request List RunStop Register (UTMRLRSR) to '1'.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTMRLRSR_OFFSET, UFS_HC_UTMRLRSR);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UFS transfer request list related h/w context.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS transfer list was initialzed successfully.
+  @retval EFI_DEVICE_ERROR           The initialization fails.
+
+**/
+EFI_STATUS
+UfsInitTransferRequestList (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  UINT32                 Data;
+  UINT8                  Nutrs;
+  VOID                   *CmdDescHost;
+  EFI_PHYSICAL_ADDRESS   CmdDescPhyAddr;
+  VOID                   *CmdDescMapping;  
+  EFI_STATUS             Status;
+
+  //
+  // Initial h/w and s/w context for future operations.
+  //
+  CmdDescHost    = NULL;
+  CmdDescMapping = NULL;
+  CmdDescPhyAddr = 0;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_CAP_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Private->Capabilities = Data;
+
+  //
+  // Allocate and initialize UTP Transfer Request List.
+  //
+  Nutrs  = (UINT8)((Private->Capabilities & UFS_HC_CAP_NUTRS) + 1);
+  Status = UfsAllocateAlignCommonBuffer (Private, Nutrs * sizeof (UTP_TRD), &CmdDescHost, &CmdDescPhyAddr, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Program the UTP Transfer Request List Base Address and UTP Transfer Request List
+  // Base Address with a 64-bit address allocated at step 8.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLBA_OFFSET, (UINT32)(UINTN)CmdDescPhyAddr);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLBAU_OFFSET, (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32));
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Private->UtpTrlBase = CmdDescHost;
+  Private->Nutrs      = Nutrs;  
+  Private->TrlMapping = CmdDescMapping;
+
+  //
+  // Enable the UTP Transfer Request List by setting the UTP Transfer Request List
+  // RunStop Register (UTRLRSR) to '1'.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLRSR_OFFSET, UFS_HC_UTRLRSR);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+UfsControllerInit (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  EFI_STATUS             Status;
+
+  Status = UfsEnableHostController (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsControllerInit: Enable Host Controller Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsDeviceDetection (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsControllerInit: Device Detection Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsInitTaskManagementRequestList (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsControllerInit: Task management list initialization Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsInitTransferRequestList (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsControllerInit: Transfer list initialization Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsControllerInit Finished\n"));
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is stopped successfully.
+  @retval Others                     A device error occurred while stopping the controller.
+
+**/
+EFI_STATUS
+UfsControllerStop (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  EFI_STATUS             Status;
+  UINT32                 Data;
+
+  //
+  // Enable the UTP Task Management Request List by setting the UTP Task Management
+  // Request List RunStop Register (UTMRLRSR) to '1'.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTMRLRSR_OFFSET, 0);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Enable the UTP Transfer Request List by setting the UTP Transfer Request List
+  // RunStop Register (UTRLRSR) to '1'.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLRSR_OFFSET, 0);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Write a 0 to the HCE register in order to disable the host controller.
+  //
+  Status = UfsMmioRead32 (Private, UFS_HC_ENABLE_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ASSERT ((Data & UFS_HC_HCE_EN) == UFS_HC_HCE_EN);
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_ENABLE_OFFSET, 0);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Wait until HCE is read as '0' before continuing.
+  //
+  Status = UfsWaitMemSet (Private, UFS_HC_ENABLE_OFFSET, UFS_HC_HCE_EN, 0, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsController is stopped\n"));
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Internal helper function which will signal the caller event and clean up
+  resources.
+
+  @param[in] Private   The pointer to the UFS_PASS_THRU_PRIVATE_DATA data
+                       structure.
+  @param[in] TransReq  The pointer to the UFS_PASS_THRU_TRANS_REQ data
+                       structure.
+
+**/
+VOID
+EFIAPI
+SignalCallerEvent (
+  IN UFS_PASS_THRU_PRIVATE_DATA      *Private,
+  IN UFS_PASS_THRU_TRANS_REQ         *TransReq
+  )
+{
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL *UfsHc;
+  EFI_EVENT                          CallerEvent;
+
+  ASSERT ((Private != NULL) && (TransReq != NULL));
+
+  UfsHc        = Private->UfsHostController;
+  CallerEvent  = TransReq->CallerEvent;
+
+  RemoveEntryList (&TransReq->TransferList);
+
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, TransReq->Slot);
+
+  if (TransReq->DataBufMapping != NULL) {
+    UfsHc->Unmap (UfsHc, TransReq->DataBufMapping);
+  }
+
+  if (TransReq->CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, TransReq->CmdDescMapping);
+  }
+  if (TransReq->CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (
+             UfsHc,
+             EFI_SIZE_TO_PAGES (TransReq->CmdDescSize),
+             TransReq->CmdDescHost
+             );
+  }
+
+  FreePool (TransReq);
+
+  gBS->SignalEvent (CallerEvent);
+  return;
+}
+
+/**
+  Call back function when the timer event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the Event.
+
+**/
+VOID
+EFIAPI
+ProcessAsyncTaskList (
+  IN EFI_EVENT          Event,
+  IN VOID               *Context
+  )
+{
+  UFS_PASS_THRU_PRIVATE_DATA                    *Private;
+  LIST_ENTRY                                    *Entry;
+  LIST_ENTRY                                    *NextEntry;
+  UFS_PASS_THRU_TRANS_REQ                       *TransReq;
+  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet;
+  UTP_RESPONSE_UPIU                             *Response;
+  UINT16                                        SenseDataLen;
+  UINT32                                        ResTranCount;
+  UINT32                                        SlotsMap;
+  UINT32                                        Value;
+  EFI_STATUS                                    Status;
+
+  Private   = (UFS_PASS_THRU_PRIVATE_DATA*) Context;
+  SlotsMap  = 0;
+
+  //
+  // Check the entries in the async I/O queue are done or not.
+  //
+  if (!IsListEmpty(&Private->Queue)) {
+    EFI_LIST_FOR_EACH_SAFE (Entry, NextEntry, &Private->Queue) {
+      TransReq  = UFS_PASS_THRU_TRANS_REQ_FROM_THIS (Entry);
+      Packet    = TransReq->Packet;
+
+      if ((SlotsMap & (BIT0 << TransReq->Slot)) != 0) {
+        return;
+      }
+      SlotsMap |= BIT0 << TransReq->Slot;
+
+      Status = UfsMmioRead32 (Private, UFS_HC_UTRLDBR_OFFSET, &Value);
+      if (EFI_ERROR (Status)) {
+        //
+        // TODO: Should find/add a proper host adapter return status for this
+        // case.
+        //
+        Packet->HostAdapterStatus = EFI_EXT_SCSI_STATUS_HOST_ADAPTER_PHASE_ERROR;
+        DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p UfsMmioRead32() Error.\n", TransReq->CallerEvent));
+        SignalCallerEvent (Private, TransReq);
+        continue;
+      }
+
+      if ((Value & (BIT0 << TransReq->Slot)) != 0) {
+        //
+        // Scsi cmd not finished yet.
+        //
+        if (TransReq->TimeoutRemain > UFS_HC_ASYNC_TIMER) {
+          TransReq->TimeoutRemain -= UFS_HC_ASYNC_TIMER;
+          continue;
+        } else {
+          //
+          // Timeout occurs.
+          //
+          Packet->HostAdapterStatus = EFI_EXT_SCSI_STATUS_HOST_ADAPTER_TIMEOUT_COMMAND;
+          DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p EFI_TIMEOUT.\n", TransReq->CallerEvent));
+          SignalCallerEvent (Private, TransReq);
+          continue;
+        }
+      } else {
+        //
+        // Scsi cmd finished.
+        //
+        // Get sense data if exists
+        //
+        Response = (UTP_RESPONSE_UPIU*)((UINT8*)TransReq->CmdDescHost + TransReq->Trd->RuO * sizeof (UINT32));
+        ASSERT (Response != NULL);
+        SenseDataLen = Response->SenseDataLen;
+        SwapLittleEndianToBigEndian ((UINT8*)&SenseDataLen, sizeof (UINT16));
+
+        if ((Packet->SenseDataLength != 0) && (Packet->SenseData != NULL)) {
+          CopyMem (Packet->SenseData, Response->SenseData, SenseDataLen);
+          Packet->SenseDataLength = (UINT8)SenseDataLen;
+        }
+
+        //
+        // Check the transfer request result.
+        //
+        Packet->TargetStatus = Response->Status;
+        if (Response->Response != 0) {
+          DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p Target Failure.\n", TransReq->CallerEvent));
+          SignalCallerEvent (Private, TransReq);
+          continue;
+        }
+
+        if (TransReq->Trd->Ocs == 0) {
+          if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+            if ((Response->Flags & BIT5) == BIT5) {
+              ResTranCount = Response->ResTranCount;
+              SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+              Packet->InTransferLength -= ResTranCount;
+            }
+          } else {
+            if ((Response->Flags & BIT5) == BIT5) {
+              ResTranCount = Response->ResTranCount;
+              SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+              Packet->OutTransferLength -= ResTranCount;
+            }
+          }
+        } else {
+          DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p Target Device Error.\n", TransReq->CallerEvent));
+          SignalCallerEvent (Private, TransReq);
+          continue;
+        }
+
+        DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p Success.\n", TransReq->CallerEvent));
+        SignalCallerEvent (Private, TransReq);
+      }
+    }
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.h b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.h
new file mode 100644
index 0000000000..a423a921de
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.h
@@ -0,0 +1,1339 @@
+/** @file
+  UfsPassThruDxe driver is used to produce EFI_EXT_SCSI_PASS_THRU protocol interface
+  for upper layer application to execute UFS-supported SCSI cmds.
+
+  Copyright (c) 2014, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _UFS_PASS_THRU_HCI_H_
+#define _UFS_PASS_THRU_HCI_H_
+
+//
+// Host Capabilities Register Offsets
+//
+#define UFS_HC_CAP_OFFSET          0x0000  // Controller Capabilities
+#define UFS_HC_VER_OFFSET          0x0008  // Version
+#define UFS_HC_DDID_OFFSET         0x0010  // Device ID and Device Class
+#define UFS_HC_PMID_OFFSET         0x0014  // Product ID and Manufacturer ID
+#define UFS_HC_AHIT_OFFSET         0x0018  // Auto-Hibernate Idle Timer
+//
+// Operation and Runtime Register Offsets
+//
+#define UFS_HC_IS_OFFSET           0x0020  // Interrupt Status
+#define UFS_HC_IE_OFFSET           0x0024  // Interrupt Enable
+#define UFS_HC_STATUS_OFFSET       0x0030  // Host Controller Status
+#define UFS_HC_ENABLE_OFFSET       0x0034  // Host Controller Enable
+#define UFS_HC_UECPA_OFFSET        0x0038  // Host UIC Error Code PHY Adapter Layer
+#define UFS_HC_UECDL_OFFSET        0x003c  // Host UIC Error Code Data Link Layer
+#define UFS_HC_UECN_OFFSET         0x0040  // Host UIC Error Code Network Layer
+#define UFS_HC_UECT_OFFSET         0x0044  // Host UIC Error Code Transport Layer
+#define UFS_HC_UECDME_OFFSET       0x0048  // Host UIC Error Code DME
+#define UFS_HC_UTRIACR_OFFSET      0x004c  // UTP Transfer Request Interrupt Aggregation Control Register
+//
+// UTP Transfer Register Offsets
+//
+#define UFS_HC_UTRLBA_OFFSET       0x0050  // UTP Transfer Request List Base Address
+#define UFS_HC_UTRLBAU_OFFSET      0x0054  // UTP Transfer Request List Base Address Upper 32-Bits
+#define UFS_HC_UTRLDBR_OFFSET      0x0058  // UTP Transfer Request List Door Bell Register
+#define UFS_HC_UTRLCLR_OFFSET      0x005c  // UTP Transfer Request List CLear Register
+#define UFS_HC_UTRLRSR_OFFSET      0x0060  // UTP Transfer Request Run-Stop Register
+//
+// UTP Task Management Register Offsets
+//
+#define UFS_HC_UTMRLBA_OFFSET      0x0070  // UTP Task Management Request List Base Address
+#define UFS_HC_UTMRLBAU_OFFSET     0x0074  // UTP Task Management Request List Base Address Upper 32-Bits
+#define UFS_HC_UTMRLDBR_OFFSET     0x0078  // UTP Task Management Request List Door Bell Register
+#define UFS_HC_UTMRLCLR_OFFSET     0x007c  // UTP Task Management Request List CLear Register
+#define UFS_HC_UTMRLRSR_OFFSET     0x0080  // UTP Task Management Run-Stop Register
+//
+// UIC Command Register Offsets
+//
+#define UFS_HC_UIC_CMD_OFFSET      0x0090  // UIC Command Register
+#define UFS_HC_UCMD_ARG1_OFFSET    0x0094  // UIC Command Argument 1
+#define UFS_HC_UCMD_ARG2_OFFSET    0x0098  // UIC Command Argument 2
+#define UFS_HC_UCMD_ARG3_OFFSET    0x009c  // UIC Command Argument 3
+//
+// UMA Register Offsets
+//
+#define UFS_HC_UMA_OFFSET          0x00b0  // Reserved for Unified Memory Extension
+
+#define UFS_HC_HCE_EN              BIT0
+#define UFS_HC_HCS_DP              BIT0
+#define UFS_HC_HCS_UCRDY           BIT3
+#define UFS_HC_IS_ULSS             BIT8
+#define UFS_HC_IS_UCCS             BIT10
+#define UFS_HC_CAP_64ADDR          BIT24
+#define UFS_HC_CAP_NUTMRS          (BIT16 | BIT17 | BIT18)
+#define UFS_HC_CAP_NUTRS           (BIT0 | BIT1 | BIT2 | BIT3 | BIT4)
+#define UFS_HC_UTMRLRSR            BIT0
+#define UFS_HC_UTRLRSR             BIT0
+
+//
+// A maximum of length of 256KB is supported by PRDT entry
+//
+#define UFS_MAX_DATA_LEN_PER_PRD   0x40000
+
+#define UFS_STORAGE_COMMAND_TYPE   0x01
+
+#define UFS_REGULAR_COMMAND        0x00
+#define UFS_INTERRUPT_COMMAND      0x01
+
+#define UFS_LUN_0                  0x00
+#define UFS_LUN_1                  0x01
+#define UFS_LUN_2                  0x02
+#define UFS_LUN_3                  0x03
+#define UFS_LUN_4                  0x04
+#define UFS_LUN_5                  0x05
+#define UFS_LUN_6                  0x06
+#define UFS_LUN_7                  0x07
+#define UFS_WLUN_REPORT_LUNS       0x81
+#define UFS_WLUN_UFS_DEV           0xD0
+#define UFS_WLUN_BOOT              0xB0
+#define UFS_WLUN_RPMB              0xC4
+
+#pragma pack(1)
+
+//
+// UFSHCI 2.0 Spec Section 5.2.1 Offset 00h: CAP - Controller Capabilities
+//
+typedef struct {
+  UINT8  Nutrs:4;      // Number of UTP Transfer Request Slots
+  UINT8  Rsvd1:4;
+
+  UINT8  NoRtt;        // Number of outstanding READY TO TRANSFER (RTT) requests supported
+
+  UINT8  Nutmrs:3;     // Number of UTP Task Management Request Slots
+  UINT8  Rsvd2:4;
+  UINT8  AutoHs:1;     // Auto-Hibernation Support
+
+  UINT8  As64:1;       // 64-bit addressing supported
+  UINT8  Oodds:1;      // Out of order data delivery supported
+  UINT8  UicDmetms:1;  // UIC DME_TEST_MODE command supported
+  UINT8  Ume:1;        // Reserved for Unified Memory Extension
+  UINT8  Rsvd4:4;
+} UFS_HC_CAP;
+
+//
+// UFSHCI 2.0 Spec Section 5.2.2 Offset 08h: VER - UFS Version
+//
+typedef struct {
+  UINT8  Vs:4;         // Version Suffix
+  UINT8  Mnr:4;        // Minor version number
+
+  UINT8  Mjr;          // Major version number
+
+  UINT16 Rsvd1;
+} UFS_HC_VER;
+
+//
+// UFSHCI 2.0 Spec Section 5.2.3 Offset 10h: HCPID - Host Controller Product ID
+//
+#define UFS_HC_PID     UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.2.4 Offset 14h: HCMID - Host Controller Manufacturer ID
+//
+#define UFS_HC_MID     UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.2.5 Offset 18h: AHIT - Auto-Hibernate Idle Timer
+//
+typedef struct {
+  UINT32 Ahitv:10;     // Auto-Hibernate Idle Timer Value 
+  UINT32 Ts:3;         // Timer scale
+  UINT32 Rsvd1:19;
+} UFS_HC_AHIT;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.1 Offset 20h: IS - Interrupt Status
+//
+typedef struct {
+  UINT16 Utrcs:1;      // UTP Transfer Request Completion Status
+  UINT16 Udepri:1;     // UIC DME_ENDPOINT_RESET Indication
+  UINT16 Ue:1;         // UIC Error
+  UINT16 Utms:1;       // UIC Test Mode Status 
+
+  UINT16 Upms:1;       // UIC Power Mode Status 
+  UINT16 Uhxs:1;       // UIC Hibernate Exit Status 
+  UINT16 Uhes:1;       // UIC Hibernate Enter Status 
+  UINT16 Ulls:1;       // UIC Link Lost Status 
+
+  UINT16 Ulss:1;       // UIC Link Startup Status 
+  UINT16 Utmrcs:1;     // UTP Task  Management Request Completion Status 
+  UINT16 Uccs:1;       // UIC Command Completion Status 
+  UINT16 Dfes:1;       // Device Fatal Error Status  
+
+  UINT16 Utpes:1;      // UTP Error Status  
+  UINT16 Rsvd1:3;
+
+  UINT16 Hcfes:1;      // Host Controller Fatal Error Status
+  UINT16 Sbfes:1;      // System Bus Fatal Error Status
+  UINT16 Rsvd2:14;
+} UFS_HC_IS;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.2 Offset 24h: IE - Interrupt Enable
+//
+typedef struct {
+  UINT16 Utrce:1;      // UTP Transfer Request Completion Enable
+  UINT16 Udeprie:1;    // UIC DME_ENDPOINT_RESET Enable
+  UINT16 Uee:1;        // UIC Error Enable
+  UINT16 Utmse:1;      // UIC Test Mode Status Enable
+
+  UINT16 Upmse:1;      // UIC Power Mode Status Enable 
+  UINT16 Uhxse:1;      // UIC Hibernate Exit Status Enable
+  UINT16 Uhese:1;      // UIC Hibernate Enter Status Enable 
+  UINT16 Ullse:1;      // UIC Link Lost Status Enable
+
+  UINT16 Ulsse:1;      // UIC Link Startup Status Enable
+  UINT16 Utmrce:1;     // UTP Task  Management Request Completion Enable
+  UINT16 Ucce:1;       // UIC Command Completion Enable
+  UINT16 Dfee:1;       // Device Fatal Error Enable
+
+  UINT16 Utpee:1;      // UTP Error Enable
+  UINT16 Rsvd1:3;
+
+  UINT16 Hcfee:1;      // Host Controller Fatal Error Enable
+  UINT16 Sbfee:1;      // System Bus Fatal Error Enable
+  UINT16 Rsvd2:14;
+} UFS_HC_IE;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.3 Offset 30h: HCS - Host Controller Status
+//
+typedef struct {
+  UINT8  Dp:1;         // Device Present
+  UINT8  UtrlRdy:1;    // UTP Transfer Request List Ready
+  UINT8  UtmrlRdy:1;   // UTP Task Management Request List Ready
+  UINT8  UcRdy:1;      // UIC COMMAND Ready
+  UINT8  Rsvd1:4;
+
+  UINT8  Upmcrs:3;     // UIC Power Mode Change Request Status
+  UINT8  Rsvd2:1;      // UIC Hibernate Exit Status Enable
+  UINT8  Utpec:4;      // UTP Error Code
+
+  UINT8  TtagUtpE;     // Task Tag of UTP error
+  UINT8  TlunUtpE;     // Target LUN of UTP error
+} UFS_HC_STATUS;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.4 Offset 34h: HCE - Host Controller Enable
+//
+typedef struct {
+  UINT32 Hce:1;        // Host Controller Enable
+  UINT32 Rsvd1:31;
+} UFS_HC_ENABLE;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.5 Offset 38h: UECPA - Host UIC Error Code PHY Adapter Layer
+//
+typedef struct {
+  UINT32 Ec:5;         // UIC PHY Adapter Layer Error Code
+  UINT32 Rsvd1:26;
+  UINT32 Err:1;        // UIC PHY Adapter Layer Error
+} UFS_HC_UECPA;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.6 Offset 3ch: UECDL - Host UIC Error Code Data Link Layer
+//
+typedef struct {
+  UINT32 Ec:15;        // UIC Data Link Layer Error Code
+  UINT32 Rsvd1:16;
+  UINT32 Err:1;        // UIC Data Link Layer Error
+} UFS_HC_UECDL;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.7 Offset 40h: UECN - Host UIC Error Code Network Layer
+//
+typedef struct {
+  UINT32 Ec:3;         // UIC Network Layer Error Code
+  UINT32 Rsvd1:28;
+  UINT32 Err:1;        // UIC Network Layer Error
+} UFS_HC_UECN;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.8 Offset 44h: UECT - Host UIC Error Code Transport Layer
+//
+typedef struct {
+  UINT32 Ec:7;         // UIC Transport Layer Error Code
+  UINT32 Rsvd1:24;
+  UINT32 Err:1;        // UIC Transport Layer Error
+} UFS_HC_UECT;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.9 Offset 48h: UECDME - Host UIC Error Code
+//
+typedef struct {
+  UINT32 Ec:1;         // UIC DME Error Code
+  UINT32 Rsvd1:30;
+  UINT32 Err:1;        // UIC DME Error
+} UFS_HC_UECDME;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.10 Offset 4Ch: UTRIACR - UTP Transfer Request Interrupt Aggregation Control Register
+//
+typedef struct {
+  UINT8  IaToVal;      // Interrupt aggregation timeout value
+
+  UINT8  IacTh:5;      // Interrupt aggregation counter threshold
+  UINT8  Rsvd1:3;
+
+  UINT8  Ctr:1;        // Counter and Timer Reset
+  UINT8  Rsvd2:3;
+  UINT8  Iasb:1;       // Interrupt aggregation status bit
+  UINT8  Rsvd3:3;
+
+  UINT8  IapwEn:1;     // Interrupt aggregation parameter write enable
+  UINT8  Rsvd4:6;
+  UINT8  IaEn:1;       // Interrupt Aggregation Enable/Disable
+} UFS_HC_UTRIACR;
+
+//
+// UFSHCI 2.0 Spec Section 5.4.1 Offset 50h: UTRLBA - UTP Transfer Request List Base Address
+//
+typedef struct {
+  UINT32 Rsvd1:10;
+  UINT32 UtrlBa:22;    // UTP Transfer Request List Base Address
+} UFS_HC_UTRLBA;
+
+//
+// UFSHCI 2.0 Spec Section 5.4.2 Offset 54h: UTRLBAU - UTP Transfer Request List Base Address Upper 32-bits
+//
+#define UFS_HC_UTRLBAU UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.4.3 Offset 58h: UTRLDBR - UTP Transfer Request List Door Bell Register
+//
+#define UFS_HC_UTRLDBR UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.4.4 Offset 5Ch: UTRLCLR - UTP Transfer Request List CLear Register
+//
+#define UFS_HC_UTRLCLR UINT32
+
+#if 0
+//
+// UFSHCI 2.0 Spec Section 5.4.5 Offset 60h: UTRLRSR - UTP Transfer Request List Run Stop Register
+//
+typedef struct {
+  UINT32 UtrlRsr:1;    // UTP Transfer Request List Run-Stop Register
+  UINT32 Rsvd1:31;
+} UFS_HC_UTRLRSR;
+#endif
+
+//
+// UFSHCI 2.0 Spec Section 5.5.1 Offset 70h: UTMRLBA - UTP Task Management Request List Base Address
+//
+typedef struct {
+  UINT32 Rsvd1:10;
+  UINT32 UtmrlBa:22;   // UTP Task Management Request List Base Address
+} UFS_HC_UTMRLBA;
+
+//
+// UFSHCI 2.0 Spec Section 5.5.2 Offset 74h: UTMRLBAU - UTP Task Management Request List Base Address Upper 32-bits
+//
+#define UFS_HC_UTMRLBAU UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.5.3 Offset 78h: UTMRLDBR - UTP Task Management Request List Door Bell Register
+//
+typedef struct {
+  UINT32 UtmrlDbr:8;   // UTP Task Management Request List Door bell Register
+  UINT32 Rsvd1:24;
+} UFS_HC_UTMRLDBR;
+
+//
+// UFSHCI 2.0 Spec Section 5.5.4 Offset 7Ch: UTMRLCLR - UTP Task Management Request List CLear Register
+//
+typedef struct {
+  UINT32 UtmrlClr:8;   // UTP Task Management List Clear Register
+  UINT32 Rsvd1:24;
+} UFS_HC_UTMRLCLR;
+
+#if 0
+//
+// UFSHCI 2.0 Spec Section 5.5.5 Offset 80h: UTMRLRSR - UTP Task Management Request List Run Stop Register
+//
+typedef struct {
+  UINT32 UtmrlRsr:1;   // UTP Task Management Request List Run-Stop Register
+  UINT32 Rsvd1:31;
+} UFS_HC_UTMRLRSR;
+#endif
+
+//
+// UFSHCI 2.0 Spec Section 5.6.1 Offset 90h: UICCMD - UIC Command
+//
+typedef struct {
+  UINT32 CmdOp:8;      // Command Opcode
+  UINT32 Rsvd1:24;
+} UFS_HC_UICCMD;
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 94h: UICCMDARG1 - UIC Command Argument 1
+//
+#define UFS_HC_UICCMD_ARG1 UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 98h: UICCMDARG2 - UIC Command Argument 2
+//
+#define UFS_HC_UICCMD_ARG2 UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 9ch: UICCMDARG3 - UIC Command Argument 3
+//
+#define UFS_HC_UICCMD_ARG3 UINT32
+
+//
+// UIC command opcodes
+//
+typedef enum {
+  UfsUicDmeGet            = 0x01,
+  UfsUicDmeSet            = 0x02,
+  UfsUicDmePeerGet        = 0x03,
+  UfsUicDmePeerSet        = 0x04,
+  UfsUicDmePwrOn          = 0x10,
+  UfsUicDmePwrOff         = 0x11,
+  UfsUicDmeEnable         = 0x12,
+  UfsUicDmeReset          = 0x14,
+  UfsUicDmeEndpointReset  = 0x15,
+  UfsUicDmeLinkStartup    = 0x16,
+  UfsUicDmeHibernateEnter = 0x17,
+  UfsUicDmeHibernateExit  = 0x18,
+  UfsUicDmeTestMode       = 0x1A
+} UFS_UIC_OPCODE;
+
+//
+// UTP Transfer Request Descriptor
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:24;
+  UINT32 Int:1;               /* Interrupt */
+  UINT32 Dd:2;                /* Data Direction */
+  UINT32 Rsvd2:1;
+  UINT32 Ct:4;                /* Command Type */
+
+  //
+  // DW1
+  //
+  UINT32 Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT32 Ocs:8;               /* Overall Command Status */
+  UINT32 Rsvd4:24;
+
+  //
+  // DW3
+  //
+  UINT32 Rsvd5;
+
+  //
+  // DW4
+  //
+  UINT32 Rsvd6:7;
+  UINT32 UcdBa:25;            /* UTP Command Descriptor Base Address */
+  
+  //
+  // DW5
+  //
+  UINT32 UcdBaU;              /* UTP Command Descriptor Base Address Upper 32-bits */
+ 
+  //
+  // DW6
+  //
+  UINT16 RuL;                 /* Response UPIU Length */  
+  UINT16 RuO;                 /* Response UPIU Offset */
+
+  //
+  // DW7
+  //
+  UINT16 PrdtL;               /* PRDT Length */  
+  UINT16 PrdtO;               /* PRDT Offset */
+} UTP_TRD;
+
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:2;
+  UINT32 DbAddr:30;           /* Data Base Address */
+  
+  //
+  // DW1
+  //
+  UINT32 DbAddrU;             /* Data Base Address Upper 32-bits */
+ 
+  //
+  // DW2
+  //
+  UINT32 Rsvd2;
+
+  //
+  // DW3
+  //
+  UINT32 DbCount:18;          /* Data Byte Count */
+  UINT32 Rsvd3:14;
+} UTP_TR_PRD;
+
+//
+// UFS 2.0 Spec Section 10.5.3 - UTP Command UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x01*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  CmdSet:4;            /* Command Set Type */
+  UINT8  Rsvd1:4;
+  UINT8  Rsvd2;
+  UINT8  Rsvd3;
+  UINT8  Rsvd4;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd5;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 ExpDataTranLen;      /* Expected Data Transfer Length - Big Endian */
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Cdb[16];
+} UTP_COMMAND_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.4 - UTP Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x21*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  CmdSet:4;            /* Command Set Type */
+  UINT8  Rsvd1:4;
+  UINT8  Rsvd2;
+  UINT8  Response;            /* Response */
+  UINT8  Status;              /* Status */
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 ResTranCount;        /* Residual Transfer Count - Big Endian */
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Rsvd3[16];
+
+  //
+  // Data Segment - Sense Data
+  //
+  UINT16 SenseDataLen;        /* Sense Data Length - Big Endian */
+  UINT8  SenseData[18];       /* Sense Data */
+} UTP_RESPONSE_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.5 - UTP Data-Out UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x02*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be sent out
+  //
+  //UINT8  Data[];            /* Data to be sent out, maximum is 65535 bytes */
+} UTP_DATA_OUT_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.6 - UTP Data-In UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x22*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be read
+  //
+  //UINT8  Data[];            /* Data to be read, maximum is 65535 bytes */
+} UTP_DATA_IN_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.7 - UTP Ready-To-Transfer UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x31*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be read
+  //
+  //UINT8  Data[];            /* Data to be read, maximum is 65535 bytes */
+} UTP_RDY_TO_TRAN_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.8 - UTP Task Management Request UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x04*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1;
+  UINT8  TskManFunc;          /* Task Management Function */
+  UINT8  Rsvd2[2];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 InputParam1;         /* Input Parameter 1 - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 InputParam2;         /* Input Parameter 2 - Big Endian */
+
+  //
+  // DW5
+  //
+  UINT32 InputParam3;         /* Input Parameter 3 - Big Endian */
+
+  //
+  // DW6 - DW7
+  //
+  UINT8  Rsvd4[8];
+} UTP_TM_REQ_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.9 - UTP Task Management Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x24*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[2];
+  UINT8  Resp;                /* Response */
+  UINT8  Rsvd2;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 OutputParam1;        /* Output Parameter 1 - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 OutputParam2;        /* Output Parameter 2 - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd4[12];
+} UTP_TM_RESP_UPIU;
+
+//
+// UTP Task Management Request Descriptor
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:24;
+  UINT32 Int:1;               /* Interrupt */
+  UINT32 Rsvd2:7;
+
+  //
+  // DW1
+  //
+  UINT32 Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT32 Ocs:8;               /* Overall Command Status */
+  UINT32 Rsvd4:24;
+
+  //
+  // DW3
+  //
+  UINT32 Rsvd5;
+
+  //
+  // DW4 - DW11
+  //
+  UTP_TM_REQ_UPIU TmReq;      /* Task Management Request UPIU */
+  
+  //
+  // DW12 - DW19
+  //
+  UTP_TM_RESP_UPIU TmResp;    /* Task Management Response UPIU */
+} UTP_TMRD;
+
+
+typedef struct {
+  UINT8  Opcode;
+  UINT8  DescId;
+  UINT8  Index;
+  UINT8  Selector;
+  UINT16 Rsvd1;
+  UINT16 Length;
+  UINT32 Value;
+  UINT32 Rsvd2;
+} UTP_UPIU_TSF;
+
+//
+// UFS 2.0 Spec Section 10.5.10 - UTP Query Request UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8        TransCode:6;   /* Transaction Type - 0x16*/
+  UINT8        Dd:1;
+  UINT8        Hd:1;
+  UINT8        Flags;
+  UINT8        Rsvd1;
+  UINT8        TaskTag;       /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8        Rsvd2;
+  UINT8        QueryFunc;     /* Query Function */
+  UINT8        Rsvd3[2];
+
+  //
+  // DW2
+  //
+  UINT8        EhsLen;        /* Total EHS Length - 0x00 */
+  UINT8        Rsvd4;
+  UINT16       DataSegLen;    /* Data Segment Length - Big Endian */
+
+  //
+  // DW3 - 6
+  //
+  UTP_UPIU_TSF Tsf;           /* Transaction Specific Fields */
+
+  //
+  // DW7
+  //
+  UINT8        Rsvd5[4];
+
+  //
+  // Data Segment - Data to be transferred
+  //
+  //UINT8  Data[];            /* Data to be transferred, maximum is 65535 bytes */
+} UTP_QUERY_REQ_UPIU;
+
+#define QUERY_FUNC_STD_READ_REQ     0x01
+#define QUERY_FUNC_STD_WRITE_REQ    0x81
+
+typedef enum {
+  UtpQueryFuncOpcodeNop      = 0x00,
+  UtpQueryFuncOpcodeRdDesc   = 0x01,
+  UtpQueryFuncOpcodeWrDesc   = 0x02,
+  UtpQueryFuncOpcodeRdAttr   = 0x03,
+  UtpQueryFuncOpcodeWrAttr   = 0x04,
+  UtpQueryFuncOpcodeRdFlag   = 0x05,
+  UtpQueryFuncOpcodeSetFlag  = 0x06,
+  UtpQueryFuncOpcodeClrFlag  = 0x07,
+  UtpQueryFuncOpcodeTogFlag  = 0x08
+} UTP_QUERY_FUNC_OPCODE;
+
+//
+// UFS 2.0 Spec Section 10.5.11 - UTP Query Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8        TransCode:6;   /* Transaction Type - 0x36*/
+  UINT8        Dd:1;
+  UINT8        Hd:1;
+  UINT8        Flags;
+  UINT8        Rsvd1;
+  UINT8        TaskTag;       /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8        Rsvd2;
+  UINT8        QueryFunc;     /* Query Function */
+  UINT8        QueryResp;     /* Query Response */
+  UINT8        Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT8        EhsLen;        /* Total EHS Length - 0x00 */
+  UINT8        DevInfo;       /* Device Information */
+  UINT16       DataSegLen;    /* Data Segment Length - Big Endian */
+
+  //
+  // DW3 - 6
+  //
+  UTP_UPIU_TSF Tsf;           /* Transaction Specific Fields */
+
+  //
+  // DW7
+  //
+  UINT8        Rsvd4[4];
+
+  //
+  // Data Segment - Data to be transferred
+  //
+  //UINT8      Data[];        /* Data to be transferred, maximum is 65535 bytes */
+} UTP_QUERY_RESP_UPIU;
+
+typedef enum {
+  UfsUtpQueryResponseSuccess             = 0x00,
+  UfsUtpQueryResponseParamNotReadable    = 0xF6,
+  UfsUtpQueryResponseParamNotWriteable   = 0xF7,  
+  UfsUtpQueryResponseParamAlreadyWritten = 0xF8,
+  UfsUtpQueryResponseInvalidLen          = 0xF9,
+  UfsUtpQueryResponseInvalidVal          = 0xFA,
+  UfsUtpQueryResponseInvalidSelector     = 0xFB,
+  UfsUtpQueryResponseInvalidIndex        = 0xFC,
+  UfsUtpQueryResponseInvalidIdn          = 0xFD,
+  UfsUtpQueryResponseInvalidOpc          = 0xFE,
+  UfsUtpQueryResponseGeneralFailure      = 0xFF
+} UTP_QUERY_RESP_CODE;
+
+//
+// UFS 2.0 Spec Section 10.5.12 - UTP Reject UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x3F*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[2];
+  UINT8  Response;            /* Response - 0x01 */
+  UINT8  Rsvd2;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information - 0x00 */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT8  HdrSts;              /* Basic Header Status */
+  UINT8  Rsvd3;
+  UINT8  E2ESts;              /* End-to-End Status */
+  UINT8  Rsvd4;
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Rsvd5[16];
+} UTP_REJ_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.13 - UTP NOP OUT UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x00*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Rsvd1;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd2[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3 - DW7
+  //
+  UINT8  Rsvd4[20];
+} UTP_NOP_OUT_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.14 - UTP NOP IN UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x20*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Rsvd1;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd2[2];
+  UINT8  Resp;                /* Response - 0x00 */
+  UINT8  Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information - 0x00 */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3 - DW7
+  //
+  UINT8  Rsvd4[20];
+} UTP_NOP_IN_UPIU;
+
+//
+// UFS Descriptors
+//
+typedef enum {
+  UfsDeviceDesc     = 0x00,
+  UfsConfigDesc     = 0x01,
+  UfsUnitDesc       = 0x02,
+  UfsInterConnDesc  = 0x04,
+  UfsStringDesc     = 0x05,
+  UfsGeometryDesc   = 0x07,
+  UfsPowerDesc      = 0x08
+} UFS_DESC_IDN;
+
+//
+// UFS 2.0 Spec Section 14.1.6.2 - Device Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  Device;
+  UINT8  DevClass;
+  UINT8  DevSubClass;
+  UINT8  Protocol;
+  UINT8  NumLun;
+  UINT8  NumWLun;
+  UINT8  BootEn;
+  UINT8  DescAccessEn;
+  UINT8  InitPowerMode;
+  UINT8  HighPriorityLun;
+  UINT8  SecureRemovalType;
+  UINT8  SecurityLun;
+  UINT8  BgOpsTermLat;
+  UINT8  InitActiveIccLevel;
+  UINT16 SpecVersion;
+  UINT16 ManufactureDate;
+  UINT8  ManufacturerName;
+  UINT8  ProductName;
+  UINT8  SerialName;
+  UINT8  OemId;
+  UINT16 ManufacturerId;
+  UINT8  Ud0BaseOffset;
+  UINT8  Ud0ConfParamLen;
+  UINT8  DevRttCap;
+  UINT16 PeriodicRtcUpdate;
+  UINT8  Rsvd1[17];
+  UINT8  Rsvd2[16];
+} UFS_DEV_DESC;
+
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  Rsvd1;
+  UINT8  BootEn;
+  UINT8  DescAccessEn;
+  UINT8  InitPowerMode;
+  UINT8  HighPriorityLun;
+  UINT8  SecureRemovalType;
+  UINT8  InitActiveIccLevel;
+  UINT16 PeriodicRtcUpdate;
+  UINT8  Rsvd2[5];
+} UFS_CONFIG_DESC_GEN_HEADER;
+
+typedef struct {
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  MemType;
+  UINT32 NumAllocUnits;
+  UINT8  DataReliability;
+  UINT8  LogicBlkSize;
+  UINT8  ProvisionType;
+  UINT16 CtxCap;
+  UINT8  Rsvd1[3];
+} UFS_UNIT_DESC_CONFIG_PARAMS;
+
+//
+// UFS 2.0 Spec Section 14.1.6.3 - Configuration Descriptor
+//
+typedef struct {
+  UFS_CONFIG_DESC_GEN_HEADER  Header;
+  UFS_UNIT_DESC_CONFIG_PARAMS UnitDescConfParams[8];
+} UFS_CONFIG_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.4 - Geometry Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  MediaTech;
+  UINT8  Rsvd1;
+  UINT64 TotalRawDevCapacity;
+  UINT8  Rsvd2;
+  UINT32 SegSize;
+  UINT8  AllocUnitSize;
+  UINT8  MinAddrBlkSize;
+  UINT8  OptReadBlkSize;
+  UINT8  OptWriteBlkSize;
+  UINT8  MaxInBufSize;
+  UINT8  MaxOutBufSize;
+  UINT8  RpmbRwSize;
+  UINT8  Rsvd3;
+  UINT8  DataOrder;
+  UINT8  MaxCtxIdNum;
+  UINT8  SysDataTagUnitSize;
+  UINT8  SysDataResUnitSize;
+  UINT8  SupSecRemovalTypes;
+  UINT16 SupMemTypes;
+  UINT32 SysCodeMaxNumAllocUnits;
+  UINT16 SupCodeCapAdjFac;
+  UINT32 NonPersMaxNumAllocUnits;
+  UINT16 NonPersCapAdjFac;
+  UINT32 Enhance1MaxNumAllocUnits;
+  UINT16 Enhance1CapAdjFac;
+  UINT32 Enhance2MaxNumAllocUnits;
+  UINT16 Enhance2CapAdjFac;
+  UINT32 Enhance3MaxNumAllocUnits;
+  UINT16 Enhance3CapAdjFac;
+  UINT32 Enhance4MaxNumAllocUnits;
+  UINT16 Enhance4CapAdjFac;
+} UFS_GEOMETRY_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.5 - Unit Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  UnitIdx;
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  LunQueueDep;
+  UINT8  Rsvd1;
+  UINT8  MemType;
+  UINT8  DataReliability;
+  UINT8  LogicBlkSize;
+  UINT64 LogicBlkCount;
+  UINT32 EraseBlkSize;
+  UINT8  ProvisionType;
+  UINT64 PhyMemResCount;
+  UINT16 CtxCap;
+  UINT8  LargeUnitGranularity;
+} UFS_UNIT_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.6 - RPMB Unit Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  UnitIdx;
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  LunQueueDep;
+  UINT8  Rsvd1;
+  UINT8  MemType;
+  UINT8  Rsvd2;
+  UINT8  LogicBlkSize;
+  UINT64 LogicBlkCount;
+  UINT32 EraseBlkSize;
+  UINT8  ProvisionType;
+  UINT64 PhyMemResCount;
+  UINT8  Rsvd3[3];
+} UFS_RPMB_UNIT_DESC;
+
+typedef struct {
+  UINT16 Value:10;
+  UINT16 Rsvd1:4;
+  UINT16 Unit:2;
+} UFS_POWER_PARAM_ELEMENT;
+
+//
+// UFS 2.0 Spec Section 14.1.6.7 - Power Parameter Descriptor
+//
+typedef struct {
+  UINT8                    Length;
+  UINT8                    DescType;
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVcc[16];
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVccQ[16];
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVccQ2[16];
+} UFS_POWER_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.8 - InterConnect Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT16 UniProVer;
+  UINT16 MphyVer;
+} UFS_INTER_CONNECT_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.9 - 14.1.6.12 - String Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  CHAR16 Unicode[126];
+} UFS_STRING_DESC;
+
+//
+// UFS 2.0 Spec Section 14.2 - Flags
+//
+typedef enum {
+  UfsFlagDevInit         = 0x01,
+  UfsFlagPermWpEn        = 0x02,
+  UfsFlagPowerOnWpEn     = 0x03,
+  UfsFlagBgOpsEn         = 0x04,
+  UfsFlagPurgeEn         = 0x06,
+  UfsFlagPhyResRemoval   = 0x08,
+  UfsFlagBusyRtc         = 0x09,
+  UfsFlagPermDisFwUpdate = 0x0B    
+} UFS_FLAGS_IDN;
+
+//
+// UFS 2.0 Spec Section 14.2 - Attributes
+//
+typedef enum {
+  UfsAttrBootLunEn         = 0x00,
+  UfsAttrCurPowerMode      = 0x02,
+  UfsAttrActiveIccLevel    = 0x03,
+  UfsAttrOutOfOrderDataEn  = 0x04,
+  UfsAttrBgOpStatus        = 0x05,
+  UfsAttrPurgeStatus       = 0x06,
+  UfsAttrMaxDataInSize     = 0x07,
+  UfsAttrMaxDataOutSize    = 0x08,
+  UfsAttrDynCapNeeded      = 0x09,
+  UfsAttrRefClkFreq        = 0x0a,
+  UfsAttrConfigDescLock    = 0x0b,
+  UfsAttrMaxNumOfRtt       = 0x0c,
+  UfsAttrExceptionEvtCtrl  = 0x0d,
+  UfsAttrExceptionEvtSts   = 0x0e,
+  UfsAttrSecondsPassed     = 0x0f,
+  UfsAttrContextConf       = 0x10,
+  UfsAttrCorrPrgBlkNum     = 0x11
+} UFS_ATTR_IDN;
+
+typedef enum {
+  UfsNoData                = 0,
+  UfsDataOut               = 1,
+  UfsDataIn                = 2,
+  UfsDdReserved
+} UFS_DATA_DIRECTION;
+
+
+#pragma pack()
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.c
new file mode 100644
index 0000000000..55c4a537fa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.c
@@ -0,0 +1,401 @@
+/** @file
+BOT Transportation implementation.
+
+Copyright (c) 2006, 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 "UsbBotPeim.h"
+#include "BotPeim.h"
+#include "PeiUsbLib.h"
+
+/**
+  Reset the given usb device.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+
+  @retval EFI_INVALID_PARAMETER  Can not get usb io ppi.
+  @retval EFI_SUCCESS            Failed to reset the given usb device.
+
+**/
+EFI_STATUS
+BotRecoveryReset (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_BOT_DEVICE            *PeiBotDev
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  UINT32                  Timeout;
+  PEI_USB_IO_PPI          *UsbIoPpi;
+  UINT8                   EndpointAddr;
+  EFI_STATUS              Status;
+
+  UsbIoPpi = PeiBotDev->UsbIoPpi;
+
+  if (UsbIoPpi == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  DevReq.RequestType  = 0x21;
+  DevReq.Request      = 0xFF;
+  DevReq.Value        = 0;
+  DevReq.Index        = 0;
+  DevReq.Length       = 0;
+
+  Timeout             = 3000;
+
+  Status = UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DevReq,
+                      EfiUsbNoData,
+                      Timeout,
+                      NULL,
+                      0
+                      );
+
+  //
+  // clear bulk in endpoint stall feature
+  //
+  EndpointAddr = (PeiBotDev->BulkInEndpoint)->EndpointAddress;
+  PeiUsbClearEndpointHalt (PeiServices, UsbIoPpi, EndpointAddr);
+
+  //
+  // clear bulk out endpoint stall feature
+  //
+  EndpointAddr = (PeiBotDev->BulkOutEndpoint)->EndpointAddress;
+  PeiUsbClearEndpointHalt (PeiServices, UsbIoPpi, EndpointAddr);
+
+  return Status;
+}
+
+/**
+  Send the command to the device using Bulk-Out endpoint.
+
+  This function sends the command to the device using Bulk-Out endpoint.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Command phase.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  Command                The command to transfer to device.
+  @param  CommandSize            The length of the command.
+  @param  DataTransferLength     The expected length of the data.
+  @param  Direction              The direction of the data.
+  @param  Timeout                Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to send the command to device.
+  @retval EFI_SUCCESS            Failed to send the command to device.
+
+**/
+EFI_STATUS
+BotCommandPhase (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_BOT_DEVICE            *PeiBotDev,
+  IN  VOID                      *Command,
+  IN  UINT8                     CommandSize,
+  IN  UINT32                    DataTransferLength,
+  IN  EFI_USB_DATA_DIRECTION    Direction,
+  IN  UINT16                    Timeout
+  )
+{
+  CBW             Cbw;
+  EFI_STATUS      Status;
+  PEI_USB_IO_PPI  *UsbIoPpi;
+  UINTN           DataSize;
+
+  UsbIoPpi = PeiBotDev->UsbIoPpi;
+
+  ZeroMem (&Cbw, sizeof (CBW));
+
+  //
+  // Fill the command block, detailed see BOT spec
+  //
+  Cbw.Signature           = CBWSIG;
+  Cbw.Tag                 = 0x01;
+  Cbw.DataTransferLength  = DataTransferLength;
+  Cbw.Flags               = (UINT8) ((Direction == EfiUsbDataIn) ? 0x80 : 0);
+  Cbw.Lun                 = 0;
+  Cbw.CmdLen              = CommandSize;
+
+  CopyMem (Cbw.CmdBlock, Command, CommandSize);
+
+  DataSize = sizeof (CBW);
+
+  Status = UsbIoPpi->UsbBulkTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      (PeiBotDev->BulkOutEndpoint)->EndpointAddress,
+                      (UINT8 *) &Cbw,
+                      &DataSize,
+                      Timeout
+                      );
+  if (EFI_ERROR (Status)) {
+    //
+    // Command phase fail, we need to recovery reset this device
+    //
+    BotRecoveryReset (PeiServices, PeiBotDev);
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Transfer the data between the device and host.
+
+  This function transfers the data between the device and host.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Data phase.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  DataSize               The length of the data.
+  @param  DataBuffer             The pointer to the data.
+  @param  Direction              The direction of the data.
+  @param  Timeout                Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to send the data to device.
+  @retval EFI_SUCCESS            Failed to send the data to device.
+
+**/
+EFI_STATUS
+BotDataPhase (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_BOT_DEVICE            *PeiBotDev,
+  IN  UINT32                    *DataSize,
+  IN  OUT VOID                  *DataBuffer,
+  IN  EFI_USB_DATA_DIRECTION    Direction,
+  IN  UINT16                    Timeout
+  )
+{
+  EFI_STATUS      Status;
+  PEI_USB_IO_PPI  *UsbIoPpi;
+  UINT8           EndpointAddr;
+  UINTN           Remain;
+  UINTN           Increment;
+  UINT32          MaxPacketLen;
+  UINT8           *BufferPtr;
+  UINTN           TransferredSize;
+
+  UsbIoPpi        = PeiBotDev->UsbIoPpi;
+
+  Remain          = *DataSize;
+  BufferPtr       = (UINT8 *) DataBuffer;
+  TransferredSize = 0;
+
+  //
+  // retrieve the the max packet length of the given endpoint
+  //
+  if (Direction == EfiUsbDataIn) {
+    MaxPacketLen  = (PeiBotDev->BulkInEndpoint)->MaxPacketSize;
+    EndpointAddr  = (PeiBotDev->BulkInEndpoint)->EndpointAddress;
+  } else {
+    MaxPacketLen  = (PeiBotDev->BulkOutEndpoint)->MaxPacketSize;
+    EndpointAddr  = (PeiBotDev->BulkOutEndpoint)->EndpointAddress;
+  }
+
+  while (Remain > 0) {
+    //
+    // Using 15 packets to avoid Bitstuff error
+    //
+    if (Remain > 16 * MaxPacketLen) {
+      Increment = 16 * MaxPacketLen;
+    } else {
+      Increment = Remain;
+    }
+
+    Status = UsbIoPpi->UsbBulkTransfer (
+                        PeiServices,
+                        UsbIoPpi,
+                        EndpointAddr,
+                        BufferPtr,
+                        &Increment,
+                        Timeout
+                        );
+
+    TransferredSize += Increment;
+
+    if (EFI_ERROR (Status)) {
+      PeiUsbClearEndpointHalt (PeiServices, UsbIoPpi, EndpointAddr);
+      return Status;
+    }
+
+    BufferPtr += Increment;
+    Remain -= Increment;
+  }
+
+  *DataSize = (UINT32) TransferredSize;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the command execution status from device.
+
+  This function gets the command execution status from device.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Status phase.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  TransferStatus         The status of the transaction.
+  @param  Timeout                Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to get the status of device.
+  @retval EFI_SUCCESS            Failed to get the status of device.
+
+**/
+EFI_STATUS
+BotStatusPhase (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_BOT_DEVICE            *PeiBotDev,
+  OUT UINT8                     *TransferStatus,
+  IN  UINT16                    Timeout
+  )
+{
+  CSW             Csw;
+  EFI_STATUS      Status;
+  PEI_USB_IO_PPI  *UsbIoPpi;
+  UINT8           EndpointAddr;
+  UINTN           DataSize;
+
+  UsbIoPpi = PeiBotDev->UsbIoPpi;
+
+  ZeroMem (&Csw, sizeof (CSW));
+
+  EndpointAddr  = (PeiBotDev->BulkInEndpoint)->EndpointAddress;
+
+  DataSize      = sizeof (CSW);
+
+  //
+  // Get the status field from bulk transfer
+  //
+  Status = UsbIoPpi->UsbBulkTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      EndpointAddr,
+                      &Csw,
+                      &DataSize,
+                      Timeout
+                      );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Csw.Signature == CSWSIG) {
+    *TransferStatus = Csw.Status;
+  } else {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send ATAPI command using BOT protocol.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  Command                The command to be sent to ATAPI device.
+  @param  CommandSize            The length of the data to be sent.
+  @param  DataBuffer             The pointer to the data.
+  @param  BufferLength           The length of the data.
+  @param  Direction              The direction of the data.
+  @param  TimeOutInMilliSeconds  Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to get the status of device.
+  @retval EFI_SUCCESS            Failed to get the status of device.
+
+**/
+EFI_STATUS
+PeiAtapiCommand (
+  IN  EFI_PEI_SERVICES            **PeiServices,
+  IN  PEI_BOT_DEVICE              *PeiBotDev,
+  IN  VOID                        *Command,
+  IN  UINT8                       CommandSize,
+  IN  VOID                        *DataBuffer,
+  IN  UINT32                      BufferLength,
+  IN  EFI_USB_DATA_DIRECTION      Direction,
+  IN  UINT16                      TimeOutInMilliSeconds
+  )
+{
+  EFI_STATUS  Status;
+  EFI_STATUS  BotDataStatus;
+  UINT8       TransferStatus;
+  UINT32      BufferSize;
+
+  BotDataStatus = EFI_SUCCESS;
+  //
+  // First send ATAPI command through Bot
+  //
+  Status = BotCommandPhase (
+            PeiServices,
+            PeiBotDev,
+            Command,
+            CommandSize,
+            BufferLength,
+            Direction,
+            TimeOutInMilliSeconds
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Send/Get Data if there is a Data Stage
+  //
+  switch (Direction) {
+  case EfiUsbDataIn:
+  case EfiUsbDataOut:
+    BufferSize = BufferLength;
+
+    BotDataStatus = BotDataPhase (
+                      PeiServices,
+                      PeiBotDev,
+                      &BufferSize,
+                      DataBuffer,
+                      Direction,
+                      TimeOutInMilliSeconds
+                      );
+    break;
+
+  case EfiUsbNoData:
+    break;
+  }
+  //
+  // Status Phase
+  //
+  Status = BotStatusPhase (
+            PeiServices,
+            PeiBotDev,
+            &TransferStatus,
+            TimeOutInMilliSeconds
+            );
+  if (EFI_ERROR (Status)) {
+    BotRecoveryReset (PeiServices, PeiBotDev);
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (TransferStatus == 0x01) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return BotDataStatus;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.h
new file mode 100644
index 0000000000..07235c397b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.h
@@ -0,0 +1,224 @@
+/** @file
+BOT Transportation implementation.
+
+Copyright (c) 2006, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PEI_BOT_PEIM_H_
+#define _PEI_BOT_PEIM_H_
+
+
+#include <PiPei.h>
+
+#include <Ppi/UsbIo.h>
+#include <Ppi/UsbHostController.h>
+#include <Ppi/BlockIo.h>
+
+//#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+
+#include <IndustryStandard/Atapi.h>
+
+#pragma pack(1)
+//
+// Bulk Only device protocol
+//
+typedef struct {
+  UINT32  Signature;
+  UINT32  Tag;
+  UINT32  DataTransferLength;
+  UINT8   Flags;
+  UINT8   Lun;
+  UINT8   CmdLen;
+  UINT8   CmdBlock[16];
+} CBW;
+
+typedef struct {
+  UINT32  Signature;
+  UINT32  Tag;
+  UINT32  DataResidue;
+  UINT8   Status;
+} CSW;
+
+#pragma pack()
+//
+// Status code, see Usb Bot device spec
+//
+#define CSWSIG  0x53425355
+#define CBWSIG  0x43425355
+
+/**
+  Sends out ATAPI Inquiry Packet Command to the specified device. This command will
+  return INQUIRY data of the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS       Inquiry command completes successfully.
+  @retval EFI_DEVICE_ERROR  Inquiry command failed.
+
+**/
+EFI_STATUS
+PeiUsbInquiry (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  );
+
+/**
+  Sends out ATAPI Test Unit Ready Packet Command to the specified device
+  to find out whether device is accessible.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS        TestUnit command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed TestUnit command successfully.
+
+**/
+EFI_STATUS
+PeiUsbTestUnitReady (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  );
+
+/**
+  Sends out ATAPI Request Sense Packet Command to the specified device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+  @param SenseCounts    Length of sense buffer.
+  @param SenseKeyBuffer Pointer to sense buffer.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbRequestSense (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice,
+  OUT UINTN             *SenseCounts,
+  IN  UINT8             *SenseKeyBuffer
+  );
+
+/**
+  Sends out ATAPI Read Capacity Packet Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbReadCapacity (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  );
+
+/**
+  Sends out ATAPI Read Format Capacity Data Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbReadFormattedCapacity (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  );
+
+/**
+  Execute Read(10) ATAPI command on a specific SCSI target.
+
+  Executes the ATAPI Read(10) command on the ATAPI target specified by PeiBotDevice.
+
+  @param PeiServices       The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice      The pointer to PEI_BOT_DEVICE instance.
+  @param Buffer            The pointer to data buffer.
+  @param Lba               The start logic block address of reading.
+  @param NumberOfBlocks    The block number of reading.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbRead10 (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice,
+  IN  VOID              *Buffer,
+  IN  EFI_PEI_LBA       Lba,
+  IN  UINTN             NumberOfBlocks
+  );
+
+/**  
+  Check if there is media according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    No media
+  @retval FALSE   Media exists
+
+**/
+BOOLEAN
+IsNoMedia (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if there is media error according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    Media error
+  @retval FALSE   No media error
+
+**/
+BOOLEAN
+IsMediaError (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if media is changed according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    There is media change event.
+  @retval FALSE   media is NOT changed.
+
+**/
+BOOLEAN
+IsMediaChange (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiAtapi.c b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiAtapi.c
new file mode 100644
index 0000000000..eaad626d61
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiAtapi.c
@@ -0,0 +1,654 @@
+/** @file
+Pei USB ATATPI command implementations.
+
+Copyright (c) 1999 - 2015, 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 "UsbBotPeim.h"
+#include "BotPeim.h"
+
+#define MAXSENSEKEY 5
+
+/**
+  Sends out ATAPI Inquiry Packet Command to the specified device. This command will
+  return INQUIRY data of the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS       Inquiry command completes successfully.
+  @retval EFI_DEVICE_ERROR  Inquiry command failed.
+
+**/
+EFI_STATUS
+PeiUsbInquiry (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  )
+{
+  ATAPI_PACKET_COMMAND  Packet;
+  EFI_STATUS            Status;
+  ATAPI_INQUIRY_DATA          Idata;
+
+  //
+  // fill command packet
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  ZeroMem (&Idata, sizeof (ATAPI_INQUIRY_DATA));
+
+  Packet.Inquiry.opcode             = ATA_CMD_INQUIRY;
+  Packet.Inquiry.page_code          = 0;
+  Packet.Inquiry.allocation_length  = 36;
+
+  //
+  // Send scsi INQUIRY command packet.
+  // According to SCSI Primary Commands-2 spec, host only needs to
+  // retrieve the first 36 bytes for standard INQUIRY data.
+  //
+  Status = PeiAtapiCommand (
+            PeiServices,
+            PeiBotDevice,
+            &Packet,
+            (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+            &Idata,
+            36,
+            EfiUsbDataIn,
+            2000
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((Idata.peripheral_type & 0x1f) == 0x05) {
+    PeiBotDevice->DeviceType      = USBCDROM;
+    PeiBotDevice->Media.BlockSize = 0x800;
+    PeiBotDevice->Media2.ReadOnly       = TRUE;
+    PeiBotDevice->Media2.RemovableMedia = TRUE;
+    PeiBotDevice->Media2.BlockSize      = 0x800;
+  } else {
+    PeiBotDevice->DeviceType      = USBFLOPPY;
+    PeiBotDevice->Media.BlockSize = 0x200;
+    PeiBotDevice->Media2.ReadOnly       = FALSE;
+    PeiBotDevice->Media2.RemovableMedia = TRUE;
+    PeiBotDevice->Media2.BlockSize      = 0x200;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sends out ATAPI Test Unit Ready Packet Command to the specified device
+  to find out whether device is accessible.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS        TestUnit command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed TestUnit command successfully.
+
+**/
+EFI_STATUS
+PeiUsbTestUnitReady (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  )
+{
+  ATAPI_PACKET_COMMAND  Packet;
+  EFI_STATUS            Status;
+
+  //
+  // fill command packet
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Packet.TestUnitReady.opcode = ATA_CMD_TEST_UNIT_READY;
+
+  //
+  // send command packet
+  //
+  Status = PeiAtapiCommand (
+            PeiServices,
+            PeiBotDevice,
+            &Packet,
+            (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+            NULL,
+            0,
+            EfiUsbNoData,
+            2000
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sends out ATAPI Request Sense Packet Command to the specified device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+  @param SenseCounts    Length of sense buffer.
+  @param SenseKeyBuffer Pointer to sense buffer.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbRequestSense (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice,
+  OUT UINTN             *SenseCounts,
+  IN  UINT8             *SenseKeyBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  ATAPI_PACKET_COMMAND        Packet;
+  UINT8                       *Ptr;
+  BOOLEAN                     SenseReq;
+  ATAPI_REQUEST_SENSE_DATA    *Sense;
+
+  *SenseCounts = 0;
+
+  //
+  // fill command packet for Request Sense Packet Command
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Packet.RequestSence.opcode            = ATA_CMD_REQUEST_SENSE;
+  Packet.RequestSence.allocation_length = (UINT8) sizeof (ATAPI_REQUEST_SENSE_DATA);
+
+  Ptr = SenseKeyBuffer;
+
+  SenseReq = TRUE;
+
+  //
+  //  request sense data from device continuously
+  //  until no sense data exists in the device.
+  //
+  while (SenseReq) {
+    Sense = (ATAPI_REQUEST_SENSE_DATA *) Ptr;
+
+    //
+    // send out Request Sense Packet Command and get one Sense
+    // data form device.
+    //
+    Status = PeiAtapiCommand (
+              PeiServices,
+              PeiBotDevice,
+              &Packet,
+              (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+              (VOID *) Ptr,
+              sizeof (ATAPI_REQUEST_SENSE_DATA),
+              EfiUsbDataIn,
+              2000
+              );
+
+    //
+    // failed to get Sense data
+    //
+    if (EFI_ERROR (Status)) {
+      if (*SenseCounts == 0) {
+        return EFI_DEVICE_ERROR;
+      } else {
+        return EFI_SUCCESS;
+      }
+    }
+
+    if (Sense->sense_key != ATA_SK_NO_SENSE) {
+
+      Ptr += sizeof (ATAPI_REQUEST_SENSE_DATA);
+      //
+      // Ptr is byte based pointer
+      //
+      (*SenseCounts)++;
+
+      if (*SenseCounts == MAXSENSEKEY) {
+        break;
+      }
+
+    } else {
+      //
+      // when no sense key, skip out the loop
+      //
+      SenseReq = FALSE;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sends out ATAPI Read Capacity Packet Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbReadCapacity (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  )
+{
+  EFI_STATUS                  Status;
+  ATAPI_PACKET_COMMAND        Packet;
+  ATAPI_READ_CAPACITY_DATA    Data;
+  UINT32                      LastBlock;
+
+  ZeroMem (&Data, sizeof (ATAPI_READ_CAPACITY_DATA));
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+
+  Packet.Inquiry.opcode = ATA_CMD_READ_CAPACITY;
+
+  //
+  // send command packet
+  //
+  Status = PeiAtapiCommand (
+            PeiServices,
+            PeiBotDevice,
+            &Packet,
+            (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+            (VOID *) &Data,
+            sizeof (ATAPI_READ_CAPACITY_DATA),
+            EfiUsbDataIn,
+            2000
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+  LastBlock = (Data.LastLba3 << 24) | (Data.LastLba2 << 16) | (Data.LastLba1 << 8) | Data.LastLba0;
+
+  if (LastBlock == 0xFFFFFFFF) {
+    DEBUG ((EFI_D_INFO, "The usb device LBA count is larger than 0xFFFFFFFF!\n"));
+  }
+
+  PeiBotDevice->Media.LastBlock    = LastBlock;
+  PeiBotDevice->Media.MediaPresent = TRUE;
+
+  PeiBotDevice->Media2.LastBlock    = LastBlock;
+  PeiBotDevice->Media2.MediaPresent = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sends out ATAPI Read Format Capacity Data Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbReadFormattedCapacity (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  )
+{
+  EFI_STATUS                      Status;
+  ATAPI_PACKET_COMMAND            Packet;
+  ATAPI_READ_FORMAT_CAPACITY_DATA FormatData;
+  UINT32                          LastBlock;
+
+  ZeroMem (&FormatData, sizeof (ATAPI_READ_FORMAT_CAPACITY_DATA));
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+
+  Packet.ReadFormatCapacity.opcode                = ATA_CMD_READ_FORMAT_CAPACITY;
+  Packet.ReadFormatCapacity.allocation_length_lo  = 12;
+
+  //
+  // send command packet
+  //
+  Status = PeiAtapiCommand (
+            PeiServices,
+            PeiBotDevice,
+            &Packet,
+            (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+            (VOID *) &FormatData,
+            sizeof (ATAPI_READ_FORMAT_CAPACITY_DATA),
+            EfiUsbDataIn,
+            2000
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (FormatData.DesCode == 3) {
+    //
+    // Media is not present
+    //
+    PeiBotDevice->Media.MediaPresent  = FALSE;
+    PeiBotDevice->Media.LastBlock     = 0;
+    PeiBotDevice->Media2.MediaPresent  = FALSE;
+    PeiBotDevice->Media2.LastBlock     = 0;
+
+  } else {
+    LastBlock = (FormatData.LastLba3 << 24) | (FormatData.LastLba2 << 16) | (FormatData.LastLba1 << 8) | FormatData.LastLba0;
+    if (LastBlock == 0xFFFFFFFF) {
+      DEBUG ((EFI_D_INFO, "The usb device LBA count is larger than 0xFFFFFFFF!\n"));
+    }
+
+    PeiBotDevice->Media.LastBlock = LastBlock;
+
+    PeiBotDevice->Media.LastBlock--;
+
+    PeiBotDevice->Media.MediaPresent = TRUE;
+
+    PeiBotDevice->Media2.MediaPresent = TRUE;
+    PeiBotDevice->Media2.LastBlock    = PeiBotDevice->Media.LastBlock;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Execute Read(10) ATAPI command on a specific SCSI target.
+
+  Executes the ATAPI Read(10) command on the ATAPI target specified by PeiBotDevice.
+
+  @param PeiServices       The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice      The pointer to PEI_BOT_DEVICE instance.
+  @param Buffer            The pointer to data buffer.
+  @param Lba               The start logic block address of reading.
+  @param NumberOfBlocks    The block number of reading.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbRead10 (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice,
+  IN  VOID              *Buffer,
+  IN  EFI_PEI_LBA       Lba,
+  IN  UINTN             NumberOfBlocks
+  )
+{
+  ATAPI_PACKET_COMMAND  Packet;
+  ATAPI_READ10_CMD      *Read10Packet;
+  UINT16                MaxBlock;
+  UINT16                BlocksRemaining;
+  UINT16                SectorCount;
+  UINT32                Lba32;
+  UINT32                BlockSize;
+  UINT32                ByteCount;
+  VOID                  *PtrBuffer;
+  EFI_STATUS            Status;
+  UINT16                TimeOut;
+
+  //
+  // prepare command packet for the Inquiry Packet Command.
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Read10Packet    = &Packet.Read10;
+  Lba32           = (UINT32) Lba;
+  PtrBuffer       = Buffer;
+
+  BlockSize       = (UINT32) PeiBotDevice->Media.BlockSize;
+
+  MaxBlock        = (UINT16) (65535 / BlockSize);
+  BlocksRemaining = (UINT16) NumberOfBlocks;
+
+  Status          = EFI_SUCCESS;
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+
+      SectorCount = BlocksRemaining;
+
+    } else {
+
+      SectorCount = MaxBlock;
+    }
+    //
+    // fill the Packet data structure
+    //
+    Read10Packet->opcode = ATA_CMD_READ_10;
+
+    //
+    // Lba0 ~ Lba3 specify the start logical block address of the data transfer.
+    // Lba0 is MSB, Lba3 is LSB
+    //
+    Read10Packet->Lba3  = (UINT8) (Lba32 & 0xff);
+    Read10Packet->Lba2  = (UINT8) (Lba32 >> 8);
+    Read10Packet->Lba1  = (UINT8) (Lba32 >> 16);
+    Read10Packet->Lba0  = (UINT8) (Lba32 >> 24);
+
+    //
+    // TranLen0 ~ TranLen1 specify the transfer length in block unit.
+    // TranLen0 is MSB, TranLen is LSB
+    //
+    Read10Packet->TranLen1  = (UINT8) (SectorCount & 0xff);
+    Read10Packet->TranLen0  = (UINT8) (SectorCount >> 8);
+
+    ByteCount               = SectorCount * BlockSize;
+
+    TimeOut                 = (UINT16) (SectorCount * 2000);
+
+    //
+    // send command packet
+    //
+    Status = PeiAtapiCommand (
+              PeiServices,
+              PeiBotDevice,
+              &Packet,
+              (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+              (VOID *) PtrBuffer,
+              ByteCount,
+              EfiUsbDataIn,
+              TimeOut
+              );
+
+    if (Status != EFI_SUCCESS) {
+      return Status;
+    }
+
+    Lba32 += SectorCount;
+    PtrBuffer       = (UINT8 *) PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining = (UINT16) (BlocksRemaining - SectorCount);
+  }
+
+  return Status;
+}
+
+/**  
+  Check if there is media according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    No media
+  @retval FALSE   Media exists
+
+**/
+BOOLEAN
+IsNoMedia (
+  IN  ATAPI_REQUEST_SENSE_DATA *SenseData,
+  IN  UINTN                    SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   NoMedia;
+
+  NoMedia   = FALSE;
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->sense_key) {
+
+    case ATA_SK_NOT_READY:
+      switch (SensePtr->addnl_sense_code) {
+      //
+      // if no media, fill IdeDev parameter with specific info.
+      //
+      case ATA_ASC_NO_MEDIA:
+        NoMedia = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return NoMedia;
+}
+
+/**  
+  Check if there is media error according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    Media error
+  @retval FALSE   No media error
+
+**/
+BOOLEAN
+IsMediaError (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                       SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   Error;
+
+  SensePtr  = SenseData;
+  Error     = FALSE;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->sense_key) {
+    //
+    // Medium error case
+    //
+    case ATA_SK_MEDIUM_ERROR:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_ERR1:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR2:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR3:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR4:
+        Error = TRUE;
+        break;
+
+      default:
+        break;
+      }
+
+      break;
+
+    //
+    // Medium upside-down case
+    //
+    case ATA_SK_NOT_READY:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_UPSIDE_DOWN:
+        Error = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return Error;
+}
+
+/**  
+  Check if media is changed according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    There is media change event.
+  @retval FALSE   media is NOT changed.
+
+**/
+BOOLEAN
+IsMediaChange (
+  IN  ATAPI_REQUEST_SENSE_DATA *SenseData,
+  IN  UINTN                    SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   MediaChange;
+
+  MediaChange = FALSE;
+
+  SensePtr    = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    //
+    // catch media change sense key and addition sense data
+    //
+    switch (SensePtr->sense_key) {
+    case ATA_SK_UNIT_ATTENTION:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_CHANGE:
+        MediaChange = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return MediaChange;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.c b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.c
new file mode 100644
index 0000000000..46c1a06444
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.c
@@ -0,0 +1,331 @@
+/** @file
+Common Libarary  for PEI USB.
+
+Copyright (c) 2006 - 2014, 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 "UsbPeim.h"
+#include "PeiUsbLib.h"
+
+/**
+  Get a given usb descriptor.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  Value              Request Value.
+  @param  Index              Request Index.
+  @param  DescriptorLength   Request descriptor Length.
+  @param  Descriptor         Request descriptor.
+
+
+  @retval EFI_SUCCESS        Usb descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the usb descriptor due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetDescriptor (
+  IN  EFI_PEI_SERVICES         **PeiServices,
+  IN  PEI_USB_IO_PPI           *UsbIoPpi,
+  IN  UINT16                   Value,
+  IN  UINT16                   Index,
+  IN  UINT16                   DescriptorLength,
+  OUT VOID                     *Descriptor
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  DevReq.RequestType  = USB_DEV_GET_DESCRIPTOR_REQ_TYPE;
+  DevReq.Request      = USB_DEV_GET_DESCRIPTOR;
+  DevReq.Value        = Value;
+  DevReq.Index        = Index;
+  DevReq.Length       = DescriptorLength;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbDataIn,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     Descriptor,
+                     DescriptorLength
+                     );
+}
+
+/**
+  Set a usb device with a specified address.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  AddressValue       The address to assign.
+
+  @retval EFI_SUCCESS        Usb device address is set successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb address due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetDeviceAddress (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT16                   AddressValue
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  DevReq.RequestType  = USB_DEV_SET_ADDRESS_REQ_TYPE;
+  DevReq.Request      = USB_DEV_SET_ADDRESS;
+  DevReq.Value        = AddressValue;
+  DevReq.Index        = 0;
+  DevReq.Length       = 0;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear a given usb feature.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  Recipient         The recipient of ClearFeature Request, should be one of Device/Interface/Endpoint.
+  @param  Value             Request Value.
+  @param  Target            Request Index.
+
+  @retval EFI_SUCCESS       Usb feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the usb feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearDeviceFeature (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN EFI_USB_RECIPIENT        Recipient,
+  IN UINT16                   Value,
+  IN UINT16                   Target
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  switch (Recipient) {
+  case EfiUsbDevice:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_D;
+    break;
+
+  case EfiUsbInterface:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_I;
+    break;
+
+  case EfiUsbEndpoint:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_E;
+    break;
+  }
+
+  DevReq.Request      = USB_DEV_CLEAR_FEATURE;
+  DevReq.Value        = Value;
+  DevReq.Index        = Target;
+  DevReq.Length       = 0;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Configure a usb device to Configuration 1.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       Usb device is set to use Configuration 1 successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the usb device due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetConfiguration (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  DevReq.RequestType  = USB_DEV_SET_CONFIGURATION_REQ_TYPE;
+  DevReq.Request      = USB_DEV_SET_CONFIGURATION;
+  DevReq.Value        = 1;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear Endpoint Halt.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  EndpointAddress   The endpoint address.
+
+  @retval EFI_SUCCESS       Endpoint halt is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the endpoint halt status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearEndpointHalt (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT8                    EndpointAddress
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  *InterfaceDesc;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *EndpointDescriptor;
+  UINT8                         EndpointIndex;
+
+
+  //
+  // Check its interface
+  //
+  Status = UsbIoPpi->UsbGetInterfaceDescriptor (
+                      PeiServices,
+                      UsbIoPpi,
+                      &InterfaceDesc
+                      );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  for (EndpointIndex = 0; EndpointIndex < InterfaceDesc->NumEndpoints; EndpointIndex++) {
+    Status = UsbIoPpi->UsbGetEndpointDescriptor (PeiServices, UsbIoPpi, EndpointIndex, &EndpointDescriptor);
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (EndpointDescriptor->EndpointAddress == EndpointAddress) {
+      break;
+    }
+  }
+
+  if (EndpointIndex == InterfaceDesc->NumEndpoints) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PeiUsbClearDeviceFeature (
+            PeiServices,
+            UsbIoPpi,
+            EfiUsbEndpoint,
+            EfiUsbEndpointHalt,
+            EndpointAddress
+            );
+
+  return Status;
+}
+
+/**
+  Judge if the port is connected with a usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A usb device is connected with the port.
+  @retval FALSE       No usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortConnect (
+  IN UINT16  PortStatus
+  )
+{
+  //
+  // return the bit 0 value of PortStatus
+  //
+  if ((PortStatus & USB_PORT_STAT_CONNECTION) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Judge if the port is connected with a low-speed usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A low-speed usb device is connected with the port.
+  @retval FALSE       No low-speed usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortLowSpeedDeviceAttached (
+  IN UINT16  PortStatus
+  )
+{
+  //
+  // return the bit 9 value of PortStatus
+  //
+  if ((PortStatus & USB_PORT_STAT_LOW_SPEED) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Judge if the port is in "connection change" status or not.
+
+  @param  PortChangeStatus  The usb port change status gotten.
+
+  @retval TRUE              The port is in "connection change" status.
+  @retval FALSE             The port is NOT in "connection change" status.
+
+**/
+BOOLEAN
+IsPortConnectChange (
+  IN UINT16  PortChangeStatus
+  )
+{
+  //
+  // return the bit 0 value of PortChangeStatus
+  //
+  if ((PortChangeStatus & USB_PORT_STAT_C_CONNECTION) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.h b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.h
new file mode 100644
index 0000000000..eafccfdbf8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.h
@@ -0,0 +1,248 @@
+/** @file
+Common Libarary  for PEI USB.
+
+Copyright (c) 1999 - 2010, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PEI_USB_LIB_H_
+#define _PEI_USB_LIB_H_
+//
+// Standard device request and request type
+// By [Spec-USB20/Chapter-9.4]
+//
+#define USB_DEV_GET_STATUS                  0x00
+#define USB_DEV_GET_STATUS_REQ_TYPE_D       0x80 // Receiver : Device
+#define USB_DEV_GET_STATUS_REQ_TYPE_I       0x81 // Receiver : Interface
+#define USB_DEV_GET_STATUS_REQ_TYPE_E       0x82 // Receiver : Endpoint
+
+#define USB_DEV_CLEAR_FEATURE               0x01
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_D    0x00 // Receiver : Device
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_I    0x01 // Receiver : Interface
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_E    0x02 // Receiver : Endpoint
+
+#define USB_DEV_SET_FEATURE                 0x03
+#define USB_DEV_SET_FEATURE_REQ_TYPE_D      0x00 // Receiver : Device
+#define USB_DEV_SET_FEATURE_REQ_TYPE_I      0x01 // Receiver : Interface
+#define USB_DEV_SET_FEATURE_REQ_TYPE_E      0x02 // Receiver : Endpoint
+ 
+#define USB_DEV_SET_ADDRESS                 0x05
+#define USB_DEV_SET_ADDRESS_REQ_TYPE        0x00
+
+#define USB_DEV_GET_DESCRIPTOR              0x06
+#define USB_DEV_GET_DESCRIPTOR_REQ_TYPE     0x80
+
+#define USB_DEV_SET_DESCRIPTOR              0x07
+#define USB_DEV_SET_DESCRIPTOR_REQ_TYPE     0x00
+
+#define USB_DEV_GET_CONFIGURATION           0x08
+#define USB_DEV_GET_CONFIGURATION_REQ_TYPE  0x80
+
+#define USB_DEV_SET_CONFIGURATION           0x09
+#define USB_DEV_SET_CONFIGURATION_REQ_TYPE  0x00
+
+#define USB_DEV_GET_INTERFACE               0x0A
+#define USB_DEV_GET_INTERFACE_REQ_TYPE      0x81
+
+#define USB_DEV_SET_INTERFACE               0x0B
+#define USB_DEV_SET_INTERFACE_REQ_TYPE      0x01
+
+#define USB_DEV_SYNCH_FRAME                 0x0C
+#define USB_DEV_SYNCH_FRAME_REQ_TYPE        0x82
+
+//
+// USB Descriptor types
+//
+#define USB_DT_DEVICE     0x01
+#define USB_DT_CONFIG     0x02
+#define USB_DT_STRING     0x03
+#define USB_DT_INTERFACE  0x04
+#define USB_DT_ENDPOINT   0x05
+#define USB_DT_HUB        0x29
+#define USB_DT_HID        0x21
+
+//
+// USB request type
+//
+#define USB_TYPE_STANDARD (0x00 << 5)
+#define USB_TYPE_CLASS    (0x01 << 5)
+#define USB_TYPE_VENDOR   (0x02 << 5)
+#define USB_TYPE_RESERVED (0x03 << 5)
+
+//
+// USB request targer device
+//
+#define USB_RECIP_DEVICE    0x00
+#define USB_RECIP_INTERFACE 0x01
+#define USB_RECIP_ENDPOINT  0x02
+#define USB_RECIP_OTHER     0x03
+
+typedef enum {
+  EfiUsbEndpointHalt,
+  EfiUsbDeviceRemoteWakeup
+} EFI_USB_STANDARD_FEATURE_SELECTOR;
+
+//
+// Usb Data recipient type
+//
+typedef enum {
+  EfiUsbDevice,
+  EfiUsbInterface,
+  EfiUsbEndpoint
+} EFI_USB_RECIPIENT;
+
+/**
+  Get a given usb descriptor.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  Value              Request Value.
+  @param  Index              Request Index.
+  @param  DescriptorLength   Request descriptor Length.
+  @param  Descriptor         Request descriptor.
+
+
+  @retval EFI_SUCCESS       Usb descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the usb descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetDescriptor (
+  IN  EFI_PEI_SERVICES         **PeiServices,
+  IN  PEI_USB_IO_PPI           *UsbIoPpi,
+  IN  UINT16                   Value,
+  IN  UINT16                   Index,
+  IN  UINT16                   DescriptorLength,
+  OUT VOID                     *Descriptor
+  );
+
+/**
+  Set a usb device with a specified address.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  AddressValue       The address to assign.
+
+  @retval EFI_SUCCESS        Usb device address is set successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb address due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetDeviceAddress (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT16                   AddressValue
+  );
+
+/**
+  Clear a given usb feature.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  Recipient         The recipient of ClearFeature Request, should be one of Device/Interface/Endpoint.
+  @param  Value             Request Value.
+  @param  Target            Request Index.
+
+  @retval EFI_SUCCESS       Usb feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the usb feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearDeviceFeature (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN EFI_USB_RECIPIENT        Recipient,
+  IN UINT16                   Value,
+  IN UINT16                   Target
+  );
+
+/**
+  Configure a usb device to Configuration 1.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS        Usb device is set to use Configuration 1 successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb device due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetConfiguration (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi
+  );
+
+/**
+  Clear Endpoint Halt.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  EndpointAddress   The endpoint address.
+
+  @retval EFI_SUCCESS       Endpoint halt is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the endpoint halt status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearEndpointHalt (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT8                    EndpointAddress
+  );
+
+/**
+  Judge if the port is connected with a usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A usb device is connected with the port.
+  @retval FALSE       No usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortConnect (
+  IN UINT16  PortStatus
+  );
+
+/**
+  Judge if the port is connected with a low-speed usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A low-speed usb device is connected with the port.
+  @retval FALSE       No low-speed usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortLowSpeedDeviceAttached (
+  IN UINT16  PortStatus
+  );
+
+/**
+  Judge if the port is in "connection change" status or not.
+
+  @param  PortChangeStatus  The usb port change status gotten.
+
+  @retval TRUE              The port is in "connection change" status.
+  @retval FALSE             The port is NOT in "connection change" status.
+
+**/
+BOOLEAN
+IsPortConnectChange (
+  IN UINT16  PortChangeStatus
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.inf b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.inf
new file mode 100644
index 0000000000..977bef5c8b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.inf
@@ -0,0 +1,69 @@
+## @file
+# The Usb mass storage device Peim driver is used to support recovery from USB device.
+#
+# Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions
+# of the BSD License which accompanies this distribution.  The
+# full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbBotPei
+  MODULE_UNI_FILE                = UsbBotPei.uni
+  FILE_GUID                      = 8401A046-6F70-4505-8471-7015B40355E3
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = PeimInitializeUsbBot
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  PeiUsbLib.c
+  PeiAtapi.c
+  BotPeim.c
+  UsbBotPeim.c
+  UsbPeim.h
+  UsbBotPeim.h
+  PeiUsbLib.h
+  BotPeim.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  BaseMemoryLib
+  PeiServicesLib
+  PeimEntryPoint
+  DebugLib
+  PcdLib
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUsbTransferTimeoutValue  ## CONSUMES
+
+[Ppis]
+  gEfiPeiVirtualBlockIoPpiGuid                  ## PRODUCES
+  gEfiPeiVirtualBlockIo2PpiGuid                 ## PRODUCES
+  ## CONSUMES
+  ## NOTIFY
+  gPeiUsbIoPpiGuid
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gPeiUsbIoPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbBotPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.uni b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.uni
new file mode 100644
index 0000000000..3f7125ab57
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeiExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeiExtra.uni
new file mode 100644
index 0000000000..2e2cdf5a7b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.c
new file mode 100644
index 0000000000..5e18306652
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.c
@@ -0,0 +1,922 @@
+/** @file
+
+Copyright (c) 2006 - 2015, 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 "UsbBotPeim.h"
+#include "BotPeim.h"
+
+//
+// Global function
+//
+EFI_PEI_NOTIFY_DESCRIPTOR        mNotifyList = {
+  EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
+  &gPeiUsbIoPpiGuid,
+  NotifyOnUsbIoPpi
+};
+
+EFI_PEI_RECOVERY_BLOCK_IO_PPI    mRecoveryBlkIoPpi = {
+  BotGetNumberOfBlockDevices,
+  BotGetMediaInfo,
+  BotReadBlocks
+};
+
+EFI_PEI_RECOVERY_BLOCK_IO2_PPI   mRecoveryBlkIo2Ppi = {
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI_REVISION,
+  BotGetNumberOfBlockDevices2,
+  BotGetMediaInfo2,
+  BotReadBlocks2
+};
+
+EFI_PEI_PPI_DESCRIPTOR           mPpiList[2] = {
+  {
+    EFI_PEI_PPI_DESCRIPTOR_PPI,
+    &gEfiPeiVirtualBlockIoPpiGuid,
+    NULL
+  },
+  {
+    EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
+    &gEfiPeiVirtualBlockIo2PpiGuid,
+    NULL
+  }
+};
+
+/**
+  Detect whether the removable media is present and whether it has changed.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM.
+  @param[in]  PeiBotDev     Indicates the PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS       The media status is successfully checked.
+  @retval Other             Failed to detect media.
+
+**/
+EFI_STATUS
+PeiBotDetectMedia (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  PEI_BOT_DEVICE                            *PeiBotDev
+  );
+
+/**
+  Initializes the Usb Bot. 
+  
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            Usb bot driver is successfully initialized.
+  @retval EFI_OUT_OF_RESOURCES   Can't initialize the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+PeimInitializeUsbBot (
+  IN EFI_PEI_FILE_HANDLE       FileHandle,
+  IN CONST EFI_PEI_SERVICES    **PeiServices
+  )
+{
+  EFI_STATUS                  Status;
+  UINTN                       UsbIoPpiInstance;
+  EFI_PEI_PPI_DESCRIPTOR      *TempPpiDescriptor;
+  PEI_USB_IO_PPI              *UsbIoPpi;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // locate all usb io PPIs
+  //
+  for (UsbIoPpiInstance = 0; UsbIoPpiInstance < PEI_FAT_MAX_USB_IO_PPI; UsbIoPpiInstance++) {
+
+    Status = PeiServicesLocatePpi (
+                              &gPeiUsbIoPpiGuid,
+                              UsbIoPpiInstance,
+                              &TempPpiDescriptor,
+                              (VOID **) &UsbIoPpi
+                              );
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  }
+  //
+  // Register a notify function
+  //
+  return PeiServicesNotifyPpi (&mNotifyList);
+}
+
+/**
+  UsbIo installation notification function. 
+  
+  This function finds out all the current USB IO PPIs in the system and add them
+  into private data.
+
+  @param  PeiServices      Indirect reference to the PEI Services Table.
+  @param  NotifyDesc       Address of the notification descriptor data structure.
+  @param  InvokePpi        Address of the PPI that was invoked.
+
+  @retval EFI_SUCCESS      The function completes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+NotifyOnUsbIoPpi (
+  IN  EFI_PEI_SERVICES                              **PeiServices,
+  IN  EFI_PEI_NOTIFY_DESCRIPTOR                     *NotifyDesc,
+  IN  VOID                                          *InvokePpi
+  )
+{
+  PEI_USB_IO_PPI  *UsbIoPpi;
+
+  UsbIoPpi = (PEI_USB_IO_PPI *) InvokePpi;
+
+  InitUsbBot (PeiServices, UsbIoPpi);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the usb bot device.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM.
+  @param[in]  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       The usb bot device is initialized successfully.
+  @retval Other             Failed to initialize media.
+
+**/
+EFI_STATUS
+InitUsbBot (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  PEI_USB_IO_PPI                            *UsbIoPpi
+  )
+{
+  PEI_BOT_DEVICE                *PeiBotDevice;
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  *InterfaceDesc;
+  UINTN                         MemPages;
+  EFI_PHYSICAL_ADDRESS          AllocateAddress;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *EndpointDesc;
+  UINT8                         Index;
+
+  //
+  // Check its interface
+  //
+  Status = UsbIoPpi->UsbGetInterfaceDescriptor (
+                      PeiServices,
+                      UsbIoPpi,
+                      &InterfaceDesc
+                      );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Check if it is the BOT device we support
+  //
+  if ((InterfaceDesc->InterfaceClass != 0x08) || (InterfaceDesc->InterfaceProtocol != 0x50)) {
+
+    return EFI_NOT_FOUND;
+  }
+
+  MemPages = sizeof (PEI_BOT_DEVICE) / EFI_PAGE_SIZE + 1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             MemPages,
+             &AllocateAddress
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PeiBotDevice                  = (PEI_BOT_DEVICE *) ((UINTN) AllocateAddress);
+
+  PeiBotDevice->Signature       = PEI_BOT_DEVICE_SIGNATURE;
+  PeiBotDevice->UsbIoPpi        = UsbIoPpi;
+  PeiBotDevice->AllocateAddress = (UINTN) AllocateAddress;
+  PeiBotDevice->BotInterface    = InterfaceDesc;
+
+  //
+  // Default value
+  //
+  PeiBotDevice->Media.DeviceType  = UsbMassStorage;
+  PeiBotDevice->Media.BlockSize   = 0x200;
+  PeiBotDevice->Media2.InterfaceType = MSG_USB_DP;
+  PeiBotDevice->Media2.BlockSize     = 0x200;
+  PeiBotDevice->Media2.RemovableMedia = FALSE;
+  PeiBotDevice->Media2.ReadOnly       = FALSE;
+
+  //
+  // Check its Bulk-in/Bulk-out endpoint
+  //
+  for (Index = 0; Index < 2; Index++) {
+    Status = UsbIoPpi->UsbGetEndpointDescriptor (
+                        PeiServices,
+                        UsbIoPpi,
+                        Index,
+                        &EndpointDesc
+                        );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if ((EndpointDesc->EndpointAddress & 0x80) != 0) {
+      PeiBotDevice->BulkInEndpoint = EndpointDesc;
+    } else {
+      PeiBotDevice->BulkOutEndpoint = EndpointDesc;
+    }
+  }
+
+  CopyMem (
+    &(PeiBotDevice->BlkIoPpi),
+    &mRecoveryBlkIoPpi,
+    sizeof (EFI_PEI_RECOVERY_BLOCK_IO_PPI)
+    );
+  CopyMem (
+    &(PeiBotDevice->BlkIo2Ppi),
+    &mRecoveryBlkIo2Ppi,
+    sizeof (EFI_PEI_RECOVERY_BLOCK_IO2_PPI)
+    );
+  CopyMem (
+    &(PeiBotDevice->BlkIoPpiList),
+    &mPpiList[0],
+    sizeof (EFI_PEI_PPI_DESCRIPTOR)
+    );
+  CopyMem (
+    &(PeiBotDevice->BlkIo2PpiList),
+    &mPpiList[1],
+    sizeof (EFI_PEI_PPI_DESCRIPTOR)
+    );
+  PeiBotDevice->BlkIoPpiList.Ppi  = &PeiBotDevice->BlkIoPpi;
+  PeiBotDevice->BlkIo2PpiList.Ppi = &PeiBotDevice->BlkIo2Ppi;
+
+  Status                          = PeiUsbInquiry (PeiServices, PeiBotDevice);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             1,
+             &AllocateAddress
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PeiBotDevice->SensePtr = (ATAPI_REQUEST_SENSE_DATA *) ((UINTN) AllocateAddress);
+
+  Status = PeiServicesInstallPpi (&PeiBotDevice->BlkIoPpiList);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetNumberOfBlockDevices (
+  IN  EFI_PEI_SERVICES                         **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI            *This,
+  OUT UINTN                                    *NumberBlockDevices
+  )
+{
+  //
+  // For Usb devices, this value should be always 1
+  //
+  *NumberBlockDevices = 1;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetMediaInfo (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI             *This,
+  IN  UINTN                                     DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO_MEDIA                    *MediaInfo
+  )
+{
+  PEI_BOT_DEVICE  *PeiBotDev;
+  EFI_STATUS      Status;
+
+  PeiBotDev = PEI_BOT_DEVICE_FROM_THIS (This);
+
+  //
+  // First test unit ready
+  //
+  PeiUsbTestUnitReady (
+    PeiServices,
+    PeiBotDev
+    );
+
+  Status = PeiBotDetectMedia (
+            PeiServices,
+            PeiBotDev
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  CopyMem (
+    MediaInfo,
+    &(PeiBotDev->Media),
+    sizeof (EFI_PEI_BLOCK_IO_MEDIA)
+    );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+BotReadBlocks (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI             *This,
+  IN  UINTN                                     DeviceIndex,
+  IN  EFI_PEI_LBA                               StartLBA,
+  IN  UINTN                                     BufferSize,
+  OUT VOID                                      *Buffer
+  )
+{
+  PEI_BOT_DEVICE  *PeiBotDev;
+  EFI_STATUS      Status;
+  UINTN           BlockSize;
+  UINTN           NumberOfBlocks;
+
+  Status    = EFI_SUCCESS;
+  PeiBotDev = PEI_BOT_DEVICE_FROM_THIS (This);
+
+  //
+  // Check parameters
+  //
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (!PeiBotDev->Media.MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  BlockSize = PeiBotDev->Media.BlockSize;
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+  }
+
+  if (StartLBA > PeiBotDev->Media2.LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  NumberOfBlocks = BufferSize / (PeiBotDev->Media.BlockSize);
+
+  if (Status == EFI_SUCCESS) {
+
+    Status = PeiUsbTestUnitReady (
+              PeiServices,
+              PeiBotDev
+              );
+    if (Status == EFI_SUCCESS) {
+      Status = PeiUsbRead10 (
+                PeiServices,
+                PeiBotDev,
+                Buffer,
+                StartLBA,
+                1
+                );
+    }
+  } else {
+    //
+    // To generate sense data for DetectMedia use.
+    //
+    PeiUsbTestUnitReady (
+      PeiServices,
+      PeiBotDev
+      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    //
+    // if any error encountered, detect what happened to the media and
+    // update the media info accordingly.
+    //
+    Status = PeiBotDetectMedia (
+              PeiServices,
+              PeiBotDev
+              );
+    if (Status != EFI_SUCCESS) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    NumberOfBlocks = BufferSize / PeiBotDev->Media.BlockSize;
+
+    if (!(PeiBotDev->Media.MediaPresent)) {
+      return EFI_NO_MEDIA;
+    }
+
+    if (BufferSize % (PeiBotDev->Media.BlockSize) != 0) {
+      return EFI_BAD_BUFFER_SIZE;
+    }
+
+    if (StartLBA > PeiBotDev->Media2.LastBlock) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((StartLBA + NumberOfBlocks - 1) > PeiBotDev->Media2.LastBlock) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Status = PeiUsbRead10 (
+              PeiServices,
+              PeiBotDev,
+              Buffer,
+              StartLBA,
+              NumberOfBlocks
+              );
+
+    switch (Status) {
+
+    case EFI_SUCCESS:
+      return EFI_SUCCESS;
+
+    default:
+      return EFI_DEVICE_ERROR;
+    }
+  } else {
+    StartLBA += 1;
+    NumberOfBlocks -= 1;
+    Buffer = (UINT8 *) Buffer + PeiBotDev->Media.BlockSize;
+
+    if (NumberOfBlocks == 0) {
+      return EFI_SUCCESS;
+    }
+
+    Status = PeiUsbRead10 (
+              PeiServices,
+              PeiBotDev,
+              Buffer,
+              StartLBA,
+              NumberOfBlocks
+              );
+    switch (Status) {
+
+    case EFI_SUCCESS:
+      return EFI_SUCCESS;
+
+    default:
+      return EFI_DEVICE_ERROR;
+
+    }
+  }
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process.
+  To the PEI legacy floppy driver, it returns the number of all the legacy
+  devices it finds during its enumeration process. If no device is detected,
+  then the function will return zero.
+
+  @param[in]  PeiServices          General-purpose services that are available
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetNumberOfBlockDevices2 (
+  IN  EFI_PEI_SERVICES                         **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI           *This,
+  OUT UINTN                                    *NumberBlockDevices
+  )
+{
+  //
+  // For Usb devices, this value should be always 1
+  //
+  *NumberBlockDevices = 1;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media
+  information. If the media changes, calling this function will update the media
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.
+                            The caller is responsible for the ownership of this
+                            data structure.
+
+  @retval EFI_SUCCESS        Media information about the specified block device
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetMediaInfo2 (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI            *This,
+  IN  UINTN                                     DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA                   *MediaInfo
+  )
+{
+  PEI_BOT_DEVICE  *PeiBotDev;
+  EFI_STATUS      Status;
+
+  PeiBotDev = PEI_BOT_DEVICE2_FROM_THIS (This);
+
+  Status = BotGetMediaInfo (
+             PeiServices,
+             &PeiBotDev->BlkIoPpi,
+             DeviceIndex,
+             &PeiBotDev->Media
+             );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CopyMem (
+    MediaInfo,
+    &(PeiBotDev->Media2),
+    sizeof (EFI_PEI_BLOCK_IO2_MEDIA)
+    );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the device
+                            index that was assigned during the enumeration process.
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+BotReadBlocks2 (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI            *This,
+  IN  UINTN                                     DeviceIndex,
+  IN  EFI_PEI_LBA                               StartLBA,
+  IN  UINTN                                     BufferSize,
+  OUT VOID                                      *Buffer
+  )
+{
+  PEI_BOT_DEVICE  *PeiBotDev;
+  EFI_STATUS      Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status    = EFI_SUCCESS;
+  PeiBotDev = PEI_BOT_DEVICE2_FROM_THIS (This);
+
+  Status = BotReadBlocks (
+             PeiServices,
+             &PeiBotDev->BlkIoPpi,
+             DeviceIndex,
+             StartLBA,
+             BufferSize,
+             Buffer
+             );
+
+  return Status;
+}
+
+/**
+  Detect whether the removable media is present and whether it has changed.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM.
+  @param[in]  PeiBotDev     Indicates the PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS       The media status is successfully checked.
+  @retval Other             Failed to detect media.
+
+**/
+EFI_STATUS
+PeiBotDetectMedia (
+  IN  EFI_PEI_SERVICES        **PeiServices,
+  IN  PEI_BOT_DEVICE          *PeiBotDev
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_STATUS                  FloppyStatus;
+  UINTN                       SenseCounts;
+  BOOLEAN                     NeedReadCapacity;
+  EFI_PHYSICAL_ADDRESS        AllocateAddress;
+  ATAPI_REQUEST_SENSE_DATA    *SensePtr;
+  UINTN                       Retry;
+
+  //
+  // if there is no media present,or media not changed,
+  // the request sense command will detect faster than read capacity command.
+  // read capacity command can be bypassed, thus improve performance.
+  //
+  SenseCounts       = 0;
+  NeedReadCapacity  = TRUE;
+
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             1,
+             &AllocateAddress
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SensePtr = PeiBotDev->SensePtr;
+  ZeroMem (SensePtr, EFI_PAGE_SIZE);
+
+  Status = PeiUsbRequestSense (
+            PeiServices,
+            PeiBotDev,
+            &SenseCounts,
+            (UINT8 *) SensePtr
+            );
+
+  if (Status == EFI_SUCCESS) {
+    //
+    // No Media
+    //
+    if (IsNoMedia (SensePtr, SenseCounts)) {
+      NeedReadCapacity              = FALSE;
+      PeiBotDev->Media.MediaPresent = FALSE;
+      PeiBotDev->Media.LastBlock    = 0;
+      PeiBotDev->Media2.MediaPresent = FALSE;
+      PeiBotDev->Media2.LastBlock    = 0;
+    } else {
+      //
+      // Media Changed
+      //
+      if (IsMediaChange (SensePtr, SenseCounts)) {
+        PeiBotDev->Media.MediaPresent  = TRUE;
+        PeiBotDev->Media2.MediaPresent = TRUE;
+      }
+      //
+      // Media Error
+      //
+      if (IsMediaError (SensePtr, SenseCounts)) {
+        //
+        // if media error encountered, make it look like no media present.
+        //
+        PeiBotDev->Media.MediaPresent = FALSE;
+        PeiBotDev->Media.LastBlock    = 0;
+        PeiBotDev->Media2.MediaPresent = FALSE;
+        PeiBotDev->Media2.LastBlock    = 0;
+      }
+
+    }
+
+  }
+
+  if (NeedReadCapacity) {
+    //
+    // Retry at most 4 times to detect media info
+    //
+    for (Retry = 0; Retry < 4; Retry++) {
+      switch (PeiBotDev->DeviceType) {
+      case USBCDROM:
+        Status = PeiUsbReadCapacity (
+                  PeiServices,
+                  PeiBotDev
+                  );
+        break;
+
+      case USBFLOPPY2:
+        Status = PeiUsbReadFormattedCapacity (
+                  PeiServices,
+                  PeiBotDev
+                  );
+          if (EFI_ERROR(Status)||
+              !PeiBotDev->Media.MediaPresent) {
+          //
+          // retry the ReadCapacity command
+          //
+          PeiBotDev->DeviceType = USBFLOPPY;
+          Status = EFI_DEVICE_ERROR;
+        }
+        break;
+
+      case USBFLOPPY:
+        Status = PeiUsbReadCapacity (
+                  PeiServices,
+                  PeiBotDev
+                  );
+        if (EFI_ERROR (Status)) {
+          //
+          // retry the ReadFormatCapacity command
+          //
+          PeiBotDev->DeviceType = USBFLOPPY2;
+        }
+        break;
+
+      default:
+        return EFI_INVALID_PARAMETER;
+      }
+
+      SenseCounts = 0;
+      ZeroMem (SensePtr, EFI_PAGE_SIZE);
+
+      if (Status == EFI_SUCCESS) {
+        break;
+      }
+
+      FloppyStatus = PeiUsbRequestSense (
+                      PeiServices,
+                      PeiBotDev,
+                      &SenseCounts,
+                      (UINT8 *) SensePtr
+                      );
+
+      //
+      // If Request Sense data failed,retry.
+      //
+      if (EFI_ERROR (FloppyStatus)) {
+        continue;
+      }
+      //
+      // No Media
+      //
+      if (IsNoMedia (SensePtr, SenseCounts)) {
+        PeiBotDev->Media.MediaPresent = FALSE;
+        PeiBotDev->Media.LastBlock    = 0;
+        PeiBotDev->Media2.MediaPresent = FALSE;
+        PeiBotDev->Media2.LastBlock    = 0;
+        break;
+      }
+
+      if (IsMediaError (SensePtr, SenseCounts)) {
+        //
+        // if media error encountered, make it look like no media present.
+        //
+        PeiBotDev->Media.MediaPresent = FALSE;
+        PeiBotDev->Media.LastBlock    = 0;
+        PeiBotDev->Media2.MediaPresent = FALSE;
+        PeiBotDev->Media2.LastBlock    = 0;
+        break;
+      }
+    }
+    //
+    // ENDFOR
+    //
+    // tell whether the readcapacity process is successful or not
+    // ("Status" variable record the latest status returned
+    // by ReadCapacity )
+    //
+    if (Status != EFI_SUCCESS) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.h
new file mode 100644
index 0000000000..3ae8c67b35
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.h
@@ -0,0 +1,346 @@
+/** @file
+Usb BOT Peim definition.
+
+Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PEI_USB_BOT_PEIM_H_
+#define _PEI_USB_BOT_PEIM_H_
+
+#include <PiPei.h>
+
+#include <Ppi/UsbIo.h>
+#include <Ppi/UsbHostController.h>
+#include <Ppi/BlockIo.h>
+#include <Ppi/BlockIo2.h>
+
+#include <Library/DebugLib.h>
+
+#include <IndustryStandard/Usb.h>
+#include <IndustryStandard/Atapi.h>
+
+#define PEI_FAT_MAX_USB_IO_PPI  127
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetNumberOfBlockDevices (
+  IN  EFI_PEI_SERVICES                         **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI            *This,
+  OUT UINTN                                    *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetMediaInfo (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI             *This,
+  IN  UINTN                                     DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO_MEDIA                    *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+BotReadBlocks (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI             *This,
+  IN  UINTN                                     DeviceIndex,
+  IN  EFI_PEI_LBA                               StartLBA,
+  IN  UINTN                                     BufferSize,
+  OUT VOID                                      *Buffer
+  );
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process.
+  To the PEI legacy floppy driver, it returns the number of all the legacy
+  devices it finds during its enumeration process. If no device is detected,
+  then the function will return zero.
+
+  @param[in]  PeiServices          General-purpose services that are available
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetNumberOfBlockDevices2 (
+  IN  EFI_PEI_SERVICES                         **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI           *This,
+  OUT UINTN                                    *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media
+  information. If the media changes, calling this function will update the media
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.
+                            The caller is responsible for the ownership of this
+                            data structure.
+
+  @retval EFI_SUCCESS        Media information about the specified block device
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetMediaInfo2 (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI            *This,
+  IN  UINTN                                     DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA                   *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the device
+                            index that was assigned during the enumeration process.
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+BotReadBlocks2 (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI            *This,
+  IN  UINTN                                     DeviceIndex,
+  IN  EFI_PEI_LBA                               StartLBA,
+  IN  UINTN                                     BufferSize,
+  OUT VOID                                      *Buffer
+  );
+
+/**
+  UsbIo installation notification function.
+
+  This function finds out all the current USB IO PPIs in the system and add them
+  into private data.
+
+  @param  PeiServices      Indirect reference to the PEI Services Table.
+  @param  NotifyDesc       Address of the notification descriptor data structure.
+  @param  InvokePpi        Address of the PPI that was invoked.
+
+  @retval EFI_SUCCESS      The function completes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+NotifyOnUsbIoPpi (
+  IN  EFI_PEI_SERVICES                              **PeiServices,
+  IN  EFI_PEI_NOTIFY_DESCRIPTOR                     *NotifyDesc,
+  IN  VOID                                          *InvokePpi
+  );
+
+/**
+  Initialize the usb bot device.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM.
+  @param[in]  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       The usb bot device is initialized successfully.
+  @retval Other             Failed to initialize media.
+
+**/
+EFI_STATUS
+InitUsbBot (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  PEI_USB_IO_PPI                            *UsbIoPpi
+  );
+
+#define USBCDROM    1 // let the device type value equal to USBCDROM, which is defined by PI spec.
+                      // Therefore the CdExpressPei module can do recovery on UsbCdrom.
+#define USBFLOPPY   2 // for those that use ReadCapacity(0x25) command to retrieve media capacity
+#define USBFLOPPY2  3 // for those that use ReadFormatCapacity(0x23) command to retrieve media capacity
+
+//
+// Bot device structure
+//
+#define PEI_BOT_DEVICE_SIGNATURE  SIGNATURE_32 ('U', 'B', 'O', 'T')
+typedef struct {
+  UINTN                           Signature;
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI   BlkIoPpi;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI  BlkIo2Ppi;
+  EFI_PEI_PPI_DESCRIPTOR          BlkIoPpiList;
+  EFI_PEI_PPI_DESCRIPTOR          BlkIo2PpiList;
+  EFI_PEI_BLOCK_IO_MEDIA          Media;
+  EFI_PEI_BLOCK_IO2_MEDIA         Media2;
+  PEI_USB_IO_PPI                  *UsbIoPpi;
+  EFI_USB_INTERFACE_DESCRIPTOR    *BotInterface;
+  EFI_USB_ENDPOINT_DESCRIPTOR     *BulkInEndpoint;
+  EFI_USB_ENDPOINT_DESCRIPTOR     *BulkOutEndpoint;
+  UINTN                           AllocateAddress;
+  UINTN                           DeviceType;
+  ATAPI_REQUEST_SENSE_DATA        *SensePtr;
+} PEI_BOT_DEVICE;
+
+#define PEI_BOT_DEVICE_FROM_THIS(a) CR (a, PEI_BOT_DEVICE, BlkIoPpi, PEI_BOT_DEVICE_SIGNATURE)
+#define PEI_BOT_DEVICE2_FROM_THIS(a) CR (a, PEI_BOT_DEVICE, BlkIo2Ppi, PEI_BOT_DEVICE_SIGNATURE)
+
+/**
+  Send ATAPI command using BOT protocol.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  Command                The command to be sent to ATAPI device.
+  @param  CommandSize            The length of the data to be sent.
+  @param  DataBuffer             The pointer to the data.
+  @param  BufferLength           The length of the data.
+  @param  Direction              The direction of the data.
+  @param  TimeOutInMilliSeconds  Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to get the status of device.
+  @retval EFI_SUCCESS            Failed to get the status of device.
+
+**/
+EFI_STATUS
+PeiAtapiCommand (
+  IN  EFI_PEI_SERVICES            **PeiServices,
+  IN  PEI_BOT_DEVICE              *PeiBotDev,
+  IN  VOID                        *Command,
+  IN  UINT8                       CommandSize,
+  IN  VOID                        *DataBuffer,
+  IN  UINT32                      BufferLength,
+  IN  EFI_USB_DATA_DIRECTION      Direction,
+  IN  UINT16                      TimeOutInMilliSeconds
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbPeim.h
new file mode 100644
index 0000000000..c62b99d4a1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbPeim.h
@@ -0,0 +1,33 @@
+/** @file
+Usb Peim definition.
+
+Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PEI_USB_PEIM_H_
+#define _PEI_USB_PEIM_H_
+
+
+#include <PiPei.h>
+
+#include <Ppi/UsbIo.h>
+#include <Ppi/BlockIo.h>
+#include <Ppi/BlockIo2.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PcdLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/ComponentName.c
new file mode 100644
index 0000000000..dc00208632
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/ComponentName.c
@@ -0,0 +1,309 @@
+/** @file
+
+  UEFI Component Name(2) protocol implementation for Usb Bus driver.
+
+Copyright (c) 2004 - 2011, 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 <Uefi.h>
+
+
+#include <Library/UefiLib.h>
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  mUsbBusComponentName = {
+  UsbBusComponentNameGetDriverName,
+  UsbBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL mUsbBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbBusComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUsbBusDriverNameTable[] = {
+  { "eng;en", L"Usb Bus Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &mUsbBusComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is not a valid EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.c
new file mode 100644
index 0000000000..ef002f53bb
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.c
@@ -0,0 +1,1532 @@
+/** @file
+
+    Usb Bus Driver Binding and Bus IO Protocol.
+
+Copyright (c) 2004 - 2013, 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 "UsbBus.h"
+
+EFI_USB_IO_PROTOCOL mUsbIoProtocol = {
+  UsbIoControlTransfer,
+  UsbIoBulkTransfer,
+  UsbIoAsyncInterruptTransfer,
+  UsbIoSyncInterruptTransfer,
+  UsbIoIsochronousTransfer,
+  UsbIoAsyncIsochronousTransfer,
+  UsbIoGetDeviceDescriptor,
+  UsbIoGetActiveConfigDescriptor,
+  UsbIoGetInterfaceDescriptor,
+  UsbIoGetEndpointDescriptor,
+  UsbIoGetStringDescriptor,
+  UsbIoGetSupportedLanguages,
+  UsbIoPortReset
+};
+
+EFI_DRIVER_BINDING_PROTOCOL mUsbBusDriverBinding = {
+  UsbBusControllerDriverSupported,
+  UsbBusControllerDriverStart,
+  UsbBusControllerDriverStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  USB_IO function to execute a control transfer. This
+  function will execute the USB transfer. If transfer
+  successes, it will sync the internal state of USB bus
+  with device state.
+
+  @param  This                   The USB_IO instance
+  @param  Request                The control transfer request
+  @param  Direction              Direction for data stage
+  @param  Timeout                The time to wait before timeout
+  @param  Data                   The buffer holding the data
+  @param  DataLength             Then length of the data
+  @param  UsbStatus              USB result
+
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid
+  @retval EFI_SUCCESS            The control transfer succeeded.
+  @retval Others                 Failed to execute the transfer
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoControlTransfer (
+  IN  EFI_USB_IO_PROTOCOL     *This,
+  IN  EFI_USB_DEVICE_REQUEST  *Request,
+  IN  EFI_USB_DATA_DIRECTION  Direction,
+  IN  UINT32                  Timeout,
+  IN  OUT VOID                *Data,      OPTIONAL
+  IN  UINTN                   DataLength, OPTIONAL
+  OUT UINT32                  *UsbStatus
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  USB_ENDPOINT_DESC       *EpDesc;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+
+  if (UsbStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  Status = UsbHcControlTransfer (
+             Dev->Bus,
+             Dev->Address,
+             Dev->Speed,
+             Dev->MaxPacket0,
+             Request,
+             Direction,
+             Data,
+             &DataLength,
+             (UINTN) Timeout,
+             &Dev->Translator,
+             UsbStatus
+             );
+
+  if (EFI_ERROR (Status) || (*UsbStatus != EFI_USB_NOERROR)) {
+    //
+    // Clear TT buffer when CTRL/BULK split transaction failes
+    // Clear the TRANSLATOR TT buffer, not parent's buffer
+    //
+    ASSERT (Dev->Translator.TranslatorHubAddress < Dev->Bus->MaxDevices);
+    if (Dev->Translator.TranslatorHubAddress != 0) {
+      UsbHubCtrlClearTTBuffer (
+        Dev->Bus->Devices[Dev->Translator.TranslatorHubAddress],
+        Dev->Translator.TranslatorPortNumber,
+        Dev->Address,
+        0,
+        USB_ENDPOINT_CONTROL
+        );
+    }
+
+    goto ON_EXIT;
+  }
+
+  //
+  // Some control transfer will change the device's internal
+  // status, such as Set_Configuration and Set_Interface.
+  // We must synchronize the bus driver's status with that in
+  // device. We ignore the Set_Descriptor request because it's
+  // hardly used by any device, especially in pre-boot environment
+  //
+
+  //
+  // Reset the endpoint toggle when endpoint stall is cleared
+  //
+  if ((Request->Request     == USB_REQ_CLEAR_FEATURE) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD,
+                                                 USB_TARGET_ENDPOINT)) &&
+      (Request->Value       == USB_FEATURE_ENDPOINT_HALT)) {
+
+    EpDesc = UsbGetEndpointDesc (UsbIf, (UINT8) Request->Index);
+
+    if (EpDesc != NULL) {
+      EpDesc->Toggle = 0;
+    }
+  }
+
+  //
+  // Select a new configuration. This is a dangerous action. Upper driver
+  // should stop use its current UsbIo after calling this driver. The old
+  // UsbIo will be uninstalled and new UsbIo be installed. We can't use
+  // ReinstallProtocol since interfaces in different configuration may be
+  // completely irrelevant.
+  //
+  if ((Request->Request == USB_REQ_SET_CONFIG) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD,
+                                                 USB_TARGET_DEVICE))) {
+    //
+    // Don't re-create the USB interfaces if configuration isn't changed.
+    //
+    if ((Dev->ActiveConfig != NULL) &&
+        (Request->Value == Dev->ActiveConfig->Desc.ConfigurationValue)) {
+
+      goto ON_EXIT;
+    }
+    DEBUG ((EFI_D_INFO, "UsbIoControlTransfer: configure changed!!! Do NOT use old UsbIo!!!\n"));
+
+    if (Dev->ActiveConfig != NULL) {
+      UsbRemoveConfig (Dev);
+    }
+
+    if (Request->Value != 0) {
+      Status = UsbSelectConfig (Dev, (UINT8) Request->Value);
+    }
+
+    //
+    // Exit now, Old USB_IO is invalid now
+    //
+    goto ON_EXIT;
+  }
+
+  //
+  // A new alternative setting is selected for the interface.
+  // No need to reinstall UsbIo in this case because only
+  // underlying communication endpoints are changed. Functionality
+  // should remains the same.
+  //
+  if ((Request->Request     == USB_REQ_SET_INTERFACE) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD,
+                                                 USB_TARGET_INTERFACE)) &&
+      (Request->Index       == UsbIf->IfSetting->Desc.InterfaceNumber)) {
+
+    Status = UsbSelectSetting (UsbIf->IfDesc, (UINT8) Request->Value);
+
+    if (!EFI_ERROR (Status)) {
+      ASSERT (UsbIf->IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);
+      UsbIf->IfSetting = UsbIf->IfDesc->Settings[UsbIf->IfDesc->ActiveIndex];
+    }
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Execute a bulk transfer to the device endpoint.
+
+  @param  This                   The USB IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  Timeout                Time to wait before timeout.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_SUCCESS            The bulk transfer is OK.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to execute transfer, reason returned in
+                                 UsbStatus.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoBulkTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               Endpoint,
+  IN  OUT VOID            *Data,
+  IN  OUT UINTN           *DataLength,
+  IN  UINTN               Timeout,
+  OUT UINT32              *UsbStatus
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  USB_ENDPOINT_DESC       *EpDesc;
+  UINT8                   BufNum;
+  UINT8                   Toggle;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+
+  if ((USB_ENDPOINT_ADDR (Endpoint) == 0) || (USB_ENDPOINT_ADDR(Endpoint) > 15) ||
+      (UsbStatus == NULL)) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf   = USB_INTERFACE_FROM_USBIO (This);
+  Dev     = UsbIf->Device;
+
+  EpDesc  = UsbGetEndpointDesc (UsbIf, Endpoint);
+
+  if ((EpDesc == NULL) || (USB_ENDPOINT_TYPE (&EpDesc->Desc) != USB_ENDPOINT_BULK)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  BufNum  = 1;
+  Toggle  = EpDesc->Toggle;
+  Status  = UsbHcBulkTransfer (
+              Dev->Bus,
+              Dev->Address,
+              Endpoint,
+              Dev->Speed,
+              EpDesc->Desc.MaxPacketSize,
+              BufNum,
+              &Data,
+              DataLength,
+              &Toggle,
+              Timeout,
+              &Dev->Translator,
+              UsbStatus
+              );
+
+  EpDesc->Toggle = Toggle;
+
+  if (EFI_ERROR (Status) || (*UsbStatus != EFI_USB_NOERROR)) {
+    //
+    // Clear TT buffer when CTRL/BULK split transaction failes.
+    // Clear the TRANSLATOR TT buffer, not parent's buffer
+    //
+    ASSERT (Dev->Translator.TranslatorHubAddress < Dev->Bus->MaxDevices);
+    if (Dev->Translator.TranslatorHubAddress != 0) {
+      UsbHubCtrlClearTTBuffer (
+        Dev->Bus->Devices[Dev->Translator.TranslatorHubAddress],
+        Dev->Translator.TranslatorPortNumber,
+        Dev->Address,
+        0,
+        USB_ENDPOINT_BULK
+        );
+    }
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Execute a synchronous interrupt transfer.
+
+  @param  This                   The USB IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  Timeout                Time to wait before timeout.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_SUCCESS            The synchronous interrupt transfer is OK.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to execute transfer, reason returned in
+                                 UsbStatus.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoSyncInterruptTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               Endpoint,
+  IN  OUT VOID            *Data,
+  IN  OUT UINTN           *DataLength,
+  IN  UINTN               Timeout,
+  OUT UINT32              *UsbStatus
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  USB_ENDPOINT_DESC       *EpDesc;
+  EFI_TPL                 OldTpl;
+  UINT8                   Toggle;
+  EFI_STATUS              Status;
+
+  if ((USB_ENDPOINT_ADDR (Endpoint) == 0) || (USB_ENDPOINT_ADDR(Endpoint) > 15) ||
+      (UsbStatus == NULL)) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf   = USB_INTERFACE_FROM_USBIO (This);
+  Dev     = UsbIf->Device;
+
+  EpDesc  = UsbGetEndpointDesc (UsbIf, Endpoint);
+
+  if ((EpDesc == NULL) || (USB_ENDPOINT_TYPE (&EpDesc->Desc) != USB_ENDPOINT_INTERRUPT)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Toggle = EpDesc->Toggle;
+  Status = UsbHcSyncInterruptTransfer (
+             Dev->Bus,
+             Dev->Address,
+             Endpoint,
+             Dev->Speed,
+             EpDesc->Desc.MaxPacketSize,
+             Data,
+             DataLength,
+             &Toggle,
+             Timeout,
+             &Dev->Translator,
+             UsbStatus
+             );
+
+  EpDesc->Toggle = Toggle;
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Queue a new asynchronous interrupt transfer, or remove the old
+  request if (IsNewTransfer == FALSE).
+
+  @param  This                   The USB_IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  IsNewTransfer          Whether this is a new request, if it's old, remove
+                                 the request.
+  @param  PollInterval           The interval to poll the transfer result, (in ms).
+  @param  DataLength             The length of perodic data transfer.
+  @param  Callback               The function to call periodicaly when transfer is
+                                 ready.
+  @param  Context                The context to the callback.
+
+  @retval EFI_SUCCESS            New transfer is queued or old request is removed.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to queue the new request or remove the old
+                                 request.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoAsyncInterruptTransfer (
+  IN EFI_USB_IO_PROTOCOL              *This,
+  IN UINT8                            Endpoint,
+  IN BOOLEAN                          IsNewTransfer,
+  IN UINTN                            PollInterval,       OPTIONAL
+  IN UINTN                            DataLength,         OPTIONAL
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  Callback,           OPTIONAL
+  IN VOID                             *Context            OPTIONAL
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  USB_ENDPOINT_DESC       *EpDesc;
+  EFI_TPL                 OldTpl;
+  UINT8                   Toggle;
+  EFI_STATUS              Status;
+
+  if ((USB_ENDPOINT_ADDR (Endpoint) == 0) || (USB_ENDPOINT_ADDR (Endpoint) > 15)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (USB_BUS_TPL);
+  UsbIf   = USB_INTERFACE_FROM_USBIO (This);
+  Dev     = UsbIf->Device;
+
+  EpDesc  = UsbGetEndpointDesc (UsbIf, Endpoint);
+
+  if ((EpDesc == NULL) || (USB_ENDPOINT_TYPE (&EpDesc->Desc) != USB_ENDPOINT_INTERRUPT)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Toggle  = EpDesc->Toggle;
+  Status  = UsbHcAsyncInterruptTransfer (
+              Dev->Bus,
+              Dev->Address,
+              Endpoint,
+              Dev->Speed,
+              EpDesc->Desc.MaxPacketSize,
+              IsNewTransfer,
+              &Toggle,
+              PollInterval,
+              DataLength,
+              &Dev->Translator,
+              Callback,
+              Context
+              );
+
+  EpDesc->Toggle = Toggle;
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Execute a synchronous isochronous transfer.
+
+  @param  This                   The USB IO instance.
+  @param  DeviceEndpoint         The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_UNSUPPORTED        Currently isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoIsochronousTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               DeviceEndpoint,
+  IN  OUT VOID            *Data,
+  IN  UINTN               DataLength,
+  OUT UINT32              *Status
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Queue an asynchronous isochronous transfer.
+
+  @param  This                   The USB_IO instance.
+  @param  DeviceEndpoint         The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of perodic data transfer.
+  @param  IsochronousCallBack    The function to call periodicaly when transfer is
+                                 ready.
+  @param  Context                The context to the callback.
+
+  @retval EFI_UNSUPPORTED        Currently isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoAsyncIsochronousTransfer (
+  IN EFI_USB_IO_PROTOCOL              *This,
+  IN UINT8                            DeviceEndpoint,
+  IN OUT VOID                         *Data,
+  IN UINTN                            DataLength,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  IsochronousCallBack,
+  IN VOID                             *Context              OPTIONAL
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Retrieve the device descriptor of the device.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The variable to receive the device descriptor.
+
+  @retval EFI_SUCCESS            The device descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  The parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetDeviceDescriptor (
+  IN  EFI_USB_IO_PROTOCOL       *This,
+  OUT EFI_USB_DEVICE_DESCRIPTOR *Descriptor
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  EFI_TPL                 OldTpl;
+
+  if (Descriptor == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  CopyMem (Descriptor, &Dev->DevDesc->Desc, sizeof (EFI_USB_DEVICE_DESCRIPTOR));
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Return the configuration descriptor of the current active configuration.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The USB configuration descriptor.
+
+  @retval EFI_SUCCESS            The active configuration descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+  @retval EFI_NOT_FOUND          Currently no active configuration is selected.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetActiveConfigDescriptor (
+  IN  EFI_USB_IO_PROTOCOL       *This,
+  OUT EFI_USB_CONFIG_DESCRIPTOR *Descriptor
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  if (Descriptor == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EFI_SUCCESS;
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  if (Dev->ActiveConfig == NULL) {
+    Status = EFI_NOT_FOUND;
+    goto ON_EXIT;
+  }
+
+  CopyMem (Descriptor, &(Dev->ActiveConfig->Desc), sizeof (EFI_USB_CONFIG_DESCRIPTOR));
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Retrieve the active interface setting descriptor for this USB IO instance.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The variable to receive active interface setting.
+
+  @retval EFI_SUCCESS            The active interface setting is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetInterfaceDescriptor (
+  IN  EFI_USB_IO_PROTOCOL           *This,
+  OUT EFI_USB_INTERFACE_DESCRIPTOR  *Descriptor
+  )
+{
+  USB_INTERFACE           *UsbIf;
+  EFI_TPL                 OldTpl;
+
+  if (Descriptor == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  CopyMem (Descriptor, &(UsbIf->IfSetting->Desc), sizeof (EFI_USB_INTERFACE_DESCRIPTOR));
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieve the endpoint descriptor from this interface setting.
+
+  @param  This                   The USB IO instance.
+  @param  Index                  The index (start from zero) of the endpoint to
+                                 retrieve.
+  @param  Descriptor             The variable to receive the descriptor.
+
+  @retval EFI_SUCCESS            The endpoint descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetEndpointDescriptor (
+  IN  EFI_USB_IO_PROTOCOL         *This,
+  IN  UINT8                       Index,
+  OUT EFI_USB_ENDPOINT_DESCRIPTOR *Descriptor
+  )
+{
+  USB_INTERFACE           *UsbIf;
+  EFI_TPL                 OldTpl;
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+
+  if ((Descriptor == NULL) || (Index > 15)) {
+    gBS->RestoreTPL (OldTpl);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Index >= UsbIf->IfSetting->Desc.NumEndpoints) {
+    gBS->RestoreTPL (OldTpl);
+    return EFI_NOT_FOUND;
+  }
+
+  CopyMem (
+    Descriptor,
+    &(UsbIf->IfSetting->Endpoints[Index]->Desc),
+    sizeof (EFI_USB_ENDPOINT_DESCRIPTOR)
+    );
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieve the supported language ID table from the device.
+
+  @param  This                   The USB IO instance.
+  @param  LangIDTable            The table to return the language IDs.
+  @param  TableSize              The size, in bytes, of the table LangIDTable.
+
+  @retval EFI_SUCCESS            The language ID is return.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetSupportedLanguages (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  OUT UINT16              **LangIDTable,
+  OUT UINT16              *TableSize
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  EFI_TPL                 OldTpl;
+
+  OldTpl        = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf         = USB_INTERFACE_FROM_USBIO (This);
+  Dev           = UsbIf->Device;
+
+  *LangIDTable  = Dev->LangId;
+  *TableSize    = (UINT16) (Dev->TotalLangId * sizeof (UINT16));
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieve an indexed string in the language of LangID.
+
+  @param  This                   The USB IO instance.
+  @param  LangID                 The language ID of the string to retrieve.
+  @param  StringIndex            The index of the string.
+  @param  String                 The variable to receive the string.
+
+  @retval EFI_SUCCESS            The string is returned.
+  @retval EFI_NOT_FOUND          No such string existed.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetStringDescriptor (
+  IN  EFI_USB_IO_PROTOCOL   *This,
+  IN  UINT16                LangID,
+  IN  UINT8                 StringIndex,
+  OUT CHAR16                **String
+  )
+{
+  USB_DEVICE                *Dev;
+  USB_INTERFACE             *UsbIf;
+  EFI_USB_STRING_DESCRIPTOR *StrDesc;
+  EFI_TPL                   OldTpl;
+  UINT8                     *Buf;
+  UINT8                     Index;
+  EFI_STATUS                Status;
+
+  if ((StringIndex == 0) || (LangID == 0)) {
+    return EFI_NOT_FOUND;
+  }
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  //
+  // Check whether language ID is supported
+  //
+  Status = EFI_NOT_FOUND;
+
+  for (Index = 0; Index < Dev->TotalLangId; Index++) {
+    ASSERT (Index < USB_MAX_LANG_ID);
+    if (Dev->LangId[Index] == LangID) {
+      break;
+    }
+  }
+
+  if (Index == Dev->TotalLangId) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Retrieve the string descriptor then allocate a buffer
+  // to hold the string itself.
+  //
+  StrDesc = UsbGetOneString (Dev, StringIndex, LangID);
+
+  if (StrDesc == NULL) {
+    goto ON_EXIT;
+  }
+
+  if (StrDesc->Length <= 2) {
+    goto FREE_STR;
+  }
+
+  Buf = AllocateZeroPool (StrDesc->Length);
+
+  if (Buf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto FREE_STR;
+  }
+
+  CopyMem (Buf, StrDesc->String, StrDesc->Length - 2);
+  *String = (CHAR16 *) Buf;
+  Status  = EFI_SUCCESS;
+
+FREE_STR:
+  gBS->FreePool (StrDesc);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Reset the device, then if that succeeds, reconfigure the
+  device with its address and current active configuration.
+
+  @param  This                   The USB IO instance.
+
+  @retval EFI_SUCCESS            The device is reset and configured.
+  @retval Others                 Failed to reset the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoPortReset (
+  IN EFI_USB_IO_PROTOCOL  *This
+  )
+{
+  USB_INTERFACE           *UsbIf;
+  USB_INTERFACE           *HubIf;
+  USB_DEVICE              *Dev;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  UINT8                   DevAddress;
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  if (UsbIf->IsHub) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  HubIf  = Dev->ParentIf;
+  Status = HubIf->HubApi->ResetPort (HubIf, Dev->ParentPort);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbIoPortReset: failed to reset hub port %d@hub  %d, %r \n",
+                Dev->ParentPort, Dev->ParentAddr, Status));
+
+    goto ON_EXIT;
+  }
+
+  HubIf->HubApi->ClearPortChange (HubIf, Dev->ParentPort);
+
+  //
+  // Reset the device to its current address. The device now has an address
+  // of ZERO after port reset, so need to set Dev->Address to the device again for
+  // host to communicate with it.
+  //
+  DevAddress   = Dev->Address;
+  Dev->Address = 0;
+  Status  = UsbSetAddress (Dev, DevAddress);
+  Dev->Address = DevAddress;
+
+  gBS->Stall (USB_SET_DEVICE_ADDRESS_STALL);
+  
+  if (EFI_ERROR (Status)) {
+    //
+    // It may fail due to device disconnection or other reasons.
+    //
+    DEBUG (( EFI_D_ERROR, "UsbIoPortReset: failed to set address for device %d - %r\n",
+                Dev->Address, Status));
+
+    goto ON_EXIT;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbIoPortReset: device is now ADDRESSED at %d\n", Dev->Address));
+
+  //
+  // Reset the current active configure, after this device
+  // is in CONFIGURED state.
+  //
+  if (Dev->ActiveConfig != NULL) {
+    Status = UsbSetConfig (Dev, Dev->ActiveConfig->Desc.ConfigurationValue);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG (( EFI_D_ERROR, "UsbIoPortReset: failed to set configure for device %d - %r\n",
+                  Dev->Address, Status));
+    }
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Install Usb Bus Protocol on host controller, and start the Usb bus.
+
+  @param This                    The USB bus driver binding instance.
+  @param Controller              The controller to check.
+  @param RemainingDevicePath     The remaining device patch.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb bus.
+  @retval EFI_ALREADY_STARTED    The controller is already controlled by the usb bus.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusBuildProtocol (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  USB_BUS                 *UsbBus;
+  USB_DEVICE              *RootHub;
+  USB_INTERFACE           *RootIf;
+  EFI_STATUS              Status;
+  EFI_STATUS              Status2;
+
+  UsbBus = AllocateZeroPool (sizeof (USB_BUS));
+
+  if (UsbBus == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  UsbBus->Signature  = USB_BUS_SIGNATURE;
+  UsbBus->HostHandle = Controller;
+  UsbBus->MaxDevices = USB_MAX_DEVICES;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &UsbBus->DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to open device path %r\n", Status));
+
+    FreePool (UsbBus);
+    return Status;
+  }
+
+  //
+  // Get USB_HC2/USB_HC host controller protocol (EHCI/UHCI).
+  // This is for backward compatibility with EFI 1.x. In UEFI
+  // 2.x, USB_HC2 replaces USB_HC. We will open both USB_HC2
+  // and USB_HC because EHCI driver will install both protocols
+  // (for the same reason). If we don't consume both of them,
+  // the unconsumed one may be opened by others.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &(UsbBus->Usb2Hc),
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  Status2 = gBS->OpenProtocol (
+                   Controller,
+                   &gEfiUsbHcProtocolGuid,
+                   (VOID **) &(UsbBus->UsbHc),
+                   This->DriverBindingHandle,
+                   Controller,
+                   EFI_OPEN_PROTOCOL_BY_DRIVER
+                   );
+
+  if (EFI_ERROR (Status) && EFI_ERROR (Status2)) {
+    DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to open USB_HC/USB2_HC %r\n", Status));
+
+    Status = EFI_DEVICE_ERROR;
+    goto CLOSE_HC;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // The EFI_USB2_HC_PROTOCOL is produced for XHCI support.
+    // Then its max supported devices are 256. Otherwise it's 128.
+    //
+    ASSERT (UsbBus->Usb2Hc != NULL);
+    if (UsbBus->Usb2Hc->MajorRevision == 0x3) {
+      UsbBus->MaxDevices = 256;
+    }
+  }
+
+  UsbHcReset (UsbBus, EFI_USB_HC_RESET_GLOBAL);
+  UsbHcSetState (UsbBus, EfiUsbHcStateOperational);
+
+  //
+  // Install an EFI_USB_BUS_PROTOCOL to host controller to identify it.
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiCallerIdGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &UsbBus->BusId
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to install bus protocol %r\n", Status));
+    goto CLOSE_HC;
+  }
+
+  //
+  // Initial the wanted child device path list, and add first RemainingDevicePath
+  //
+  InitializeListHead (&UsbBus->WantedUsbIoDPList);
+  Status = UsbBusAddWantedUsbIoDP (&UsbBus->BusId, RemainingDevicePath);
+  ASSERT (!EFI_ERROR (Status));
+  //
+  // Create a fake usb device for root hub
+  //
+  RootHub = AllocateZeroPool (sizeof (USB_DEVICE));
+
+  if (RootHub == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto UNINSTALL_USBBUS;
+  }
+
+  RootIf = AllocateZeroPool (sizeof (USB_INTERFACE));
+
+  if (RootIf == NULL) {
+    FreePool (RootHub);
+    Status = EFI_OUT_OF_RESOURCES;
+    goto FREE_ROOTHUB;
+  }
+
+  RootHub->Bus            = UsbBus;
+  RootHub->NumOfInterface = 1;
+  RootHub->Interfaces[0]  = RootIf;
+  RootHub->Tier           = 0;
+  RootIf->Signature       = USB_INTERFACE_SIGNATURE;
+  RootIf->Device          = RootHub;
+  RootIf->DevicePath      = UsbBus->DevicePath;
+  
+  //
+  // Report Status Code here since we will enumerate the USB devices
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_USB | EFI_IOB_PC_DETECT),
+    UsbBus->DevicePath
+    );
+  
+  Status                  = mUsbRootHubApi.Init (RootIf);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to init root hub %r\n", Status));
+    goto FREE_ROOTHUB;
+  }
+
+  UsbBus->Devices[0] = RootHub;
+
+  DEBUG ((EFI_D_INFO, "UsbBusStart: usb bus started on %p, root hub %p\n", Controller, RootIf));
+  return EFI_SUCCESS;
+
+FREE_ROOTHUB:
+  if (RootIf != NULL) {
+    FreePool (RootIf);
+  }
+  if (RootHub != NULL) {
+    FreePool (RootHub);
+  }
+
+UNINSTALL_USBBUS:
+  gBS->UninstallProtocolInterface (Controller, &gEfiCallerIdGuid, &UsbBus->BusId);
+
+CLOSE_HC:
+  if (UsbBus->Usb2Hc != NULL) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsb2HcProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+  if (UsbBus->UsbHc != NULL) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbHcProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiDevicePathProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  FreePool (UsbBus);
+
+  DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to start bus driver %r\n", Status));
+  return Status;
+}
+
+
+/**
+  The USB bus driver entry pointer.
+
+  @param ImageHandle       The driver image handle.
+  @param SystemTable       The system table.
+
+  @return EFI_SUCCESS      The component name protocol is installed.
+  @return Others           Failed to init the usb driver.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &mUsbBusDriverBinding,
+           ImageHandle,
+           &mUsbBusComponentName,
+           &mUsbBusComponentName2
+           );
+}
+
+
+/**
+  Check whether USB bus driver support this device.
+
+  @param  This                   The USB bus driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The bus supports this controller.
+  @retval EFI_UNSUPPORTED        This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_DEV_PATH_PTR          DevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_USB2_HC_PROTOCOL      *Usb2Hc;
+  EFI_USB_HC_PROTOCOL       *UsbHc;
+  EFI_STATUS                Status;
+
+  //
+  // Check whether device path is valid
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node, 
+    // if yes, go on checking other conditions
+    //
+    if (!IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node,
+      // check its validation
+      //
+      DevicePathNode.DevPath = RemainingDevicePath;
+      
+      if ((DevicePathNode.DevPath->Type    != MESSAGING_DEVICE_PATH) ||
+          (DevicePathNode.DevPath->SubType != MSG_USB_DP &&
+           DevicePathNode.DevPath->SubType != MSG_USB_CLASS_DP
+           && DevicePathNode.DevPath->SubType != MSG_USB_WWID_DP
+           )) {
+      
+        return EFI_UNSUPPORTED;
+      }
+    }
+  }
+
+  //
+  // Check whether USB_HC2 protocol is installed
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    //
+    // If failed to open USB_HC2, fall back to USB_HC
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiUsbHcProtocolGuid,
+                    (VOID **) &UsbHc,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+    if (Status == EFI_ALREADY_STARTED) {
+      return EFI_SUCCESS;
+    }
+  
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Close the USB_HC used to perform the supported test
+    //
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbHcProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+  } else {
+
+    //
+    // Close the USB_HC2 used to perform the supported test
+    //
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiUsb2HcProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+ 
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Close protocol, don't use device path protocol in the Support() function
+    //
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    return EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+
+/**
+  Start to process the controller.
+
+  @param  This                   The USB bus driver binding instance.
+  @param  Controller             The controller to check.
+  @param  RemainingDevicePath    The remaining device patch.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb bus.
+  @retval EFI_ALREADY_STARTED    The controller is already controlled by the usb
+                                 bus.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_USB_BUS_PROTOCOL          *UsbBusId;
+  EFI_STATUS                    Status;
+  EFI_DEVICE_PATH_PROTOCOL      *ParentDevicePath;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Report Status Code here since we will initialize the host controller
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_USB | EFI_IOB_PC_INIT),
+    ParentDevicePath
+    );
+
+  //
+  // Locate the USB bus protocol, if it is found, USB bus
+  // is already started on this controller.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiCallerIdGuid,
+                  (VOID **) &UsbBusId,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // If first start, build the bus execute environment and install bus protocol
+    //
+    REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_IO_BUS_USB | EFI_P_PC_ENABLE));
+    Status = UsbBusBuildProtocol (This, Controller, RemainingDevicePath);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Try get the Usb Bus protocol interface again
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &UsbBusId,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    ASSERT (!EFI_ERROR (Status));
+  } else {
+    //
+    // USB Bus driver need to control the recursive connect policy of the bus, only those wanted
+    // usb child device will be recursively connected.
+    // The RemainingDevicePath indicate the child usb device which user want to fully recursively connecte this time.
+    // All wanted usb child devices will be remembered by the usb bus driver itself.
+    // If RemainingDevicePath == NULL, all the usb child devices in the usb bus are wanted devices.
+    //
+    // Save the passed in RemainingDevicePath this time
+    //
+    if (RemainingDevicePath != NULL) {
+      if (IsDevicePathEnd (RemainingDevicePath)) {
+        //
+        // If RemainingDevicePath is the End of Device Path Node,
+        // skip enumerate any device and return EFI_SUCESSS
+        // 
+        return EFI_SUCCESS;
+      }
+    }
+
+    Status = UsbBusAddWantedUsbIoDP (UsbBusId, RemainingDevicePath);
+    ASSERT (!EFI_ERROR (Status));
+    //
+    // Ensure all wanted child usb devices are fully recursively connected
+    //
+    Status = UsbBusRecursivelyConnectWantedUsbIo (UsbBusId);
+    ASSERT (!EFI_ERROR (Status));
+  }
+
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Stop handle the controller by this USB bus driver.
+
+  @param  This                   The USB bus driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The child of USB bus that opened controller
+                                 BY_CHILD.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The controller or children are stopped.
+  @retval EFI_DEVICE_ERROR       Failed to stop the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  USB_BUS               *Bus;
+  USB_DEVICE            *RootHub;
+  USB_DEVICE            *UsbDev;
+  USB_INTERFACE         *RootIf;
+  USB_INTERFACE         *UsbIf;
+  EFI_USB_BUS_PROTOCOL  *BusId;
+  EFI_USB_IO_PROTOCOL   *UsbIo;
+  EFI_TPL               OldTpl;
+  UINTN                 Index;
+  EFI_STATUS            Status;
+  EFI_STATUS            ReturnStatus;
+
+  Status  = EFI_SUCCESS;
+
+  if (NumberOfChildren > 0) {
+    //
+    // BugBug: Raise TPL to callback level instead of USB_BUS_TPL to avoid TPL conflict
+    //
+    OldTpl   = gBS->RaiseTPL (TPL_CALLBACK);
+
+    ReturnStatus = EFI_SUCCESS;
+    for (Index = 0; Index < NumberOfChildren; Index++) {
+      Status = gBS->OpenProtocol (
+                      ChildHandleBuffer[Index],
+                      &gEfiUsbIoProtocolGuid,
+                      (VOID **) &UsbIo,
+                      This->DriverBindingHandle,
+                      Controller,
+                      EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                      );
+
+      if (EFI_ERROR (Status)) {
+        //
+        // It is possible that the child has already been released:
+        // 1. For combo device, free one device will release others.
+        // 2. If a hub is released, all devices on its down facing
+        //    ports are released also.
+        //
+        continue;
+      }
+
+      UsbIf   = USB_INTERFACE_FROM_USBIO (UsbIo);
+      UsbDev  = UsbIf->Device;
+
+      ReturnStatus = UsbRemoveDevice (UsbDev);
+    }
+
+    gBS->RestoreTPL (OldTpl);
+    return ReturnStatus;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbBusStop: usb bus stopped on %p\n", Controller));
+
+  //
+  // Locate USB_BUS for the current host controller
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiCallerIdGuid,
+                  (VOID **) &BusId,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Bus = USB_BUS_FROM_THIS (BusId);
+
+  //
+  // Stop the root hub, then free all the devices
+  //
+  // BugBug: Raise TPL to callback level instead of USB_BUS_TPL to avoid TPL conflict
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+
+  RootHub = Bus->Devices[0];
+  RootIf  = RootHub->Interfaces[0];
+
+  ASSERT (Bus->MaxDevices <= 256);
+  ReturnStatus = EFI_SUCCESS;
+  for (Index = 1; Index < Bus->MaxDevices; Index++) {
+    if (Bus->Devices[Index] != NULL) {
+      Status = UsbRemoveDevice (Bus->Devices[Index]);
+      if (EFI_ERROR (Status)) {
+        ReturnStatus = Status;
+      }
+    }
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  if (!EFI_ERROR (ReturnStatus)) {
+    mUsbRootHubApi.Release (RootIf);
+    gBS->FreePool   (RootIf);
+    gBS->FreePool   (RootHub);
+
+    Status = UsbBusFreeUsbDPList (&Bus->WantedUsbIoDPList);
+    ASSERT (!EFI_ERROR (Status));
+
+    //
+    // Uninstall the bus identifier and close USB_HC/USB2_HC protocols
+    //
+    gBS->UninstallProtocolInterface (Controller, &gEfiCallerIdGuid, &Bus->BusId);
+
+    if (Bus->Usb2Hc != NULL) {
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      &gEfiUsb2HcProtocolGuid,
+                      This->DriverBindingHandle,
+                      Controller
+                      );
+    }
+
+    if (Bus->UsbHc != NULL) {
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      &gEfiUsbHcProtocolGuid,
+                      This->DriverBindingHandle,
+                      Controller
+                      );
+    }
+
+    if (!EFI_ERROR (Status)) {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiDevicePathProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+
+      gBS->FreePool (Bus);
+    }
+  }
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.h
new file mode 100644
index 0000000000..9ede83ab7e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.h
@@ -0,0 +1,770 @@
+/** @file
+
+    Usb Bus Driver Binding and Bus IO Protocol.
+
+Copyright (c) 2004 - 2012, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_USB_BUS_H_
+#define _EFI_USB_BUS_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/Usb2HostController.h>
+#include <Protocol/UsbHostController.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+
+#include <IndustryStandard/Usb.h>
+
+typedef struct _USB_DEVICE     USB_DEVICE;
+typedef struct _USB_INTERFACE  USB_INTERFACE;
+typedef struct _USB_BUS        USB_BUS;
+typedef struct _USB_HUB_API    USB_HUB_API;
+
+
+#include "UsbUtility.h"
+#include "UsbDesc.h"
+#include "UsbHub.h"
+#include "UsbEnumer.h"
+
+//
+// USB bus timeout experience values
+//
+
+#define USB_MAX_LANG_ID           16
+#define USB_MAX_INTERFACE         16
+#define USB_MAX_DEVICES           128
+
+#define USB_BUS_1_MILLISECOND     1000
+
+//
+// Roothub and hub's polling interval, set by experience,
+// The unit of roothub is 100us, means 100ms as interval, and
+// the unit of hub is 1ms, means 64ms as interval.
+//
+#define USB_ROOTHUB_POLL_INTERVAL (100 * 10000U)
+#define USB_HUB_POLL_INTERVAL     64
+
+//
+// Wait for port stable to work, refers to specification
+// [USB20-9.1.2]
+//
+#define USB_WAIT_PORT_STABLE_STALL  (100 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for port statue reg change, set by experience
+//
+#define USB_WAIT_PORT_STS_CHANGE_STALL (100)
+
+//
+// Wait for set device address, refers to specification
+// [USB20-9.2.6.3, it says 2ms]
+//
+#define USB_SET_DEVICE_ADDRESS_STALL   (2 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for retry max packet size, set by experience
+//
+#define USB_RETRY_MAX_PACK_SIZE_STALL  (100 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for hub port power-on, refers to specification
+// [USB20-11.23.2]
+//
+#define USB_SET_PORT_POWER_STALL       (2 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for port reset, refers to specification
+// [USB20-7.1.7.5, it says 10ms for hub and 50ms for
+// root hub]
+//
+// According to USB2.0, Chapter 11.5.1.5 Resetting,
+// the worst case for TDRST is 20ms
+//
+#define USB_SET_PORT_RESET_STALL       (20 * USB_BUS_1_MILLISECOND)
+#define USB_SET_ROOT_PORT_RESET_STALL  (50 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for port recovery to accept SetAddress, refers to specification
+// [USB20-7.1.7.5, it says 10 ms for TRSTRCY]
+//
+#define USB_SET_PORT_RECOVERY_STALL    (10 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for clear roothub port reset, set by experience
+//
+#define USB_CLR_ROOT_PORT_RESET_STALL  (20 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for set roothub port enable, set by experience
+//
+#define USB_SET_ROOT_PORT_ENABLE_STALL (20 * USB_BUS_1_MILLISECOND)
+
+//
+// Send general device request timeout.
+// 
+// The USB Specification 2.0, section 11.24.1 recommends a value of
+// 50 milliseconds.  We use a value of 500 milliseconds to work
+// around slower hubs and devices.
+//
+#define USB_GENERAL_DEVICE_REQUEST_TIMEOUT 500
+
+//
+// Send clear feature request timeout, set by experience
+//
+#define USB_CLEAR_FEATURE_REQUEST_TIMEOUT  10
+
+//
+// Bus raises TPL to TPL_NOTIFY to serialize all its operations
+// to protect shared data structures.
+//
+#define  USB_BUS_TPL               TPL_NOTIFY
+
+#define  USB_INTERFACE_SIGNATURE   SIGNATURE_32 ('U', 'S', 'B', 'I')
+#define  USB_BUS_SIGNATURE         SIGNATURE_32 ('U', 'S', 'B', 'B')
+
+#define USB_BIT(a)                  ((UINTN)(1 << (a)))
+#define USB_BIT_IS_SET(Data, Bit)   ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define USB_INTERFACE_FROM_USBIO(a) \
+          CR(a, USB_INTERFACE, UsbIo, USB_INTERFACE_SIGNATURE)
+
+#define USB_BUS_FROM_THIS(a) \
+          CR(a, USB_BUS, BusId, USB_BUS_SIGNATURE)
+
+//
+// Used to locate USB_BUS
+// UsbBusProtocol is the private protocol.
+// gEfiCallerIdGuid will be used as its protocol guid.
+//
+typedef struct _EFI_USB_BUS_PROTOCOL {
+  UINT64                    Reserved;
+} EFI_USB_BUS_PROTOCOL;
+
+
+//
+// Stands for the real USB device. Each device may
+// has several seperately working interfaces.
+//
+struct _USB_DEVICE {
+  USB_BUS                   *Bus;
+
+  //
+  // Configuration information
+  //
+  UINT8                     Speed;
+  UINT8                     Address;
+  UINT32                    MaxPacket0;
+
+  //
+  // The device's descriptors and its configuration
+  //
+  USB_DEVICE_DESC           *DevDesc;
+  USB_CONFIG_DESC           *ActiveConfig;
+
+  UINT16                    LangId [USB_MAX_LANG_ID];
+  UINT16                    TotalLangId;
+
+  UINT8                     NumOfInterface;
+  USB_INTERFACE             *Interfaces [USB_MAX_INTERFACE];
+
+  //
+  // Parent child relationship
+  //
+  EFI_USB2_HC_TRANSACTION_TRANSLATOR Translator;
+
+  UINT8                     ParentAddr;
+  USB_INTERFACE             *ParentIf;
+  UINT8                     ParentPort;       // Start at 0
+  UINT8                     Tier;
+  BOOLEAN                   DisconnectFail;
+};
+
+//
+// Stands for different functions of USB device
+//
+struct _USB_INTERFACE {
+  UINTN                     Signature;
+  USB_DEVICE                *Device;
+  USB_INTERFACE_DESC        *IfDesc;
+  USB_INTERFACE_SETTING     *IfSetting;
+
+  //
+  // Handles and protocols
+  //
+  EFI_HANDLE                Handle;
+  EFI_USB_IO_PROTOCOL       UsbIo;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  BOOLEAN                   IsManaged;
+
+  //
+  // Hub device special data
+  //
+  BOOLEAN                   IsHub;
+  USB_HUB_API               *HubApi;
+  UINT8                     NumOfPort;
+  EFI_EVENT                 HubNotify;
+
+  //
+  // Data used only by normal hub devices
+  //
+  USB_ENDPOINT_DESC         *HubEp;
+  UINT8                     *ChangeMap;
+
+  //
+  // Data used only by root hub to hand over device to
+  // companion UHCI driver if low/full speed devices are
+  // connected to EHCI.
+  //
+  UINT8                     MaxSpeed;
+};
+
+//
+// Stands for the current USB Bus
+//
+struct _USB_BUS {
+  UINTN                     Signature;
+  EFI_USB_BUS_PROTOCOL      BusId;
+
+  //
+  // Managed USB host controller
+  //
+  EFI_HANDLE                HostHandle;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_USB2_HC_PROTOCOL      *Usb2Hc;
+  EFI_USB_HC_PROTOCOL       *UsbHc;
+
+  //
+  // Recorded the max supported usb devices.
+  // XHCI can support up to 255 devices.
+  // EHCI/UHCI/OHCI supports up to 127 devices.
+  //
+  UINT32                    MaxDevices;
+  //
+  // An array of device that is on the bus. Devices[0] is
+  // for root hub. Device with address i is at Devices[i].
+  //
+  USB_DEVICE                *Devices[256];
+
+  //
+  // USB Bus driver need to control the recursive connect policy of the bus, only those wanted
+  // usb child device will be recursively connected.
+  //
+  // WantedUsbIoDPList tracks the Usb child devices which user want to recursivly fully connecte,
+  // every wanted child device is stored in a item of the WantedUsbIoDPList, whose structrure is
+  // DEVICE_PATH_LIST_ITEM
+  //
+  LIST_ENTRY                WantedUsbIoDPList;
+
+};
+
+//
+// USB Hub Api
+//
+struct _USB_HUB_API{
+  USB_HUB_INIT                Init;
+  USB_HUB_GET_PORT_STATUS     GetPortStatus;
+  USB_HUB_CLEAR_PORT_CHANGE   ClearPortChange;
+  USB_HUB_SET_PORT_FEATURE    SetPortFeature;
+  USB_HUB_CLEAR_PORT_FEATURE  ClearPortFeature;
+  USB_HUB_RESET_PORT          ResetPort;
+  USB_HUB_RELEASE             Release;
+};
+
+#define USB_US_LAND_ID   0x0409
+
+#define DEVICE_PATH_LIST_ITEM_SIGNATURE     SIGNATURE_32('d','p','l','i')
+typedef struct _DEVICE_PATH_LIST_ITEM{
+  UINTN                                 Signature;
+  LIST_ENTRY                            Link;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+} DEVICE_PATH_LIST_ITEM;
+
+typedef struct {
+  USB_CLASS_DEVICE_PATH           UsbClass;
+  EFI_DEVICE_PATH_PROTOCOL        End;
+} USB_CLASS_FORMAT_DEVICE_PATH;
+
+/**
+  Free a DEVICE_PATH_LIST_ITEM list.
+
+  @param  UsbIoDPList            a DEVICE_PATH_LIST_ITEM list pointer.
+
+  @retval EFI_INVALID_PARAMETER  If parameters are invalid, return this value.
+  @retval EFI_SUCCESS            If free operation is successful, return this value.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusFreeUsbDPList (
+  IN     LIST_ENTRY                                 *UsbIoDPList
+  );
+
+/**
+  Store a wanted usb child device info (its Usb part of device path) which is indicated by
+  RemainingDevicePath in a Usb bus which  is indicated by UsbBusId.
+
+  @param  UsbBusId               Point to EFI_USB_BUS_PROTOCOL interface.
+  @param  RemainingDevicePath    The remaining device patch.
+
+  @retval EFI_SUCCESS            Add operation is successful.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusAddWantedUsbIoDP (
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Check whether a usb child device is the wanted device in a bus.
+
+  @param  Bus     The Usb bus's private data pointer.
+  @param  UsbIf   The usb child device inferface.
+
+  @retval True    If a usb child device is the wanted device in a bus.
+  @retval False   If a usb child device is *NOT* the wanted device in a bus.
+
+**/
+BOOLEAN
+EFIAPI
+UsbBusIsWantedUsbIO (
+  IN USB_BUS                 *Bus,
+  IN USB_INTERFACE           *UsbIf
+  );
+
+/**
+  Recursively connnect every wanted usb child device to ensure they all fully connected.
+  Check all the child Usb IO handles in this bus, recursively connecte if it is wanted usb child device.
+
+  @param  UsbBusId                  Point to EFI_USB_BUS_PROTOCOL interface.
+
+  @retval EFI_SUCCESS               Connect is done successfully.
+  @retval EFI_INVALID_PARAMETER     The parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusRecursivelyConnectWantedUsbIo (
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId
+  );
+
+/**
+  USB_IO function to execute a control transfer. This
+  function will execute the USB transfer. If transfer
+  successes, it will sync the internal state of USB bus
+  with device state.
+
+  @param  This                   The USB_IO instance
+  @param  Request                The control transfer request
+  @param  Direction              Direction for data stage
+  @param  Timeout                The time to wait before timeout
+  @param  Data                   The buffer holding the data
+  @param  DataLength             Then length of the data
+  @param  UsbStatus              USB result
+
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid
+  @retval EFI_SUCCESS            The control transfer succeded.
+  @retval Others                 Failed to execute the transfer
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoControlTransfer (
+  IN  EFI_USB_IO_PROTOCOL     *This,
+  IN  EFI_USB_DEVICE_REQUEST  *Request,
+  IN  EFI_USB_DATA_DIRECTION  Direction,
+  IN  UINT32                  Timeout,
+  IN  OUT VOID                *Data,      OPTIONAL
+  IN  UINTN                   DataLength, OPTIONAL
+  OUT UINT32                  *UsbStatus
+  );
+
+/**
+  Execute a bulk transfer to the device endpoint.
+
+  @param  This                   The USB IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  Timeout                Time to wait before timeout.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_SUCCESS            The bulk transfer is OK.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to execute transfer, reason returned in
+                                 UsbStatus.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoBulkTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               Endpoint,
+  IN  OUT VOID            *Data,
+  IN  OUT UINTN           *DataLength,
+  IN  UINTN               Timeout,
+  OUT UINT32              *UsbStatus
+  );
+
+/**
+  Execute a synchronous interrupt transfer.
+
+  @param  This                   The USB IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  Timeout                Time to wait before timeout.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_SUCCESS            The synchronous interrupt transfer is OK.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to execute transfer, reason returned in
+                                 UsbStatus.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoSyncInterruptTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               Endpoint,
+  IN  OUT VOID            *Data,
+  IN  OUT UINTN           *DataLength,
+  IN  UINTN               Timeout,
+  OUT UINT32              *UsbStatus
+  );
+
+/**
+  Queue a new asynchronous interrupt transfer, or remove the old
+  request if (IsNewTransfer == FALSE).
+
+  @param  This                   The USB_IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  IsNewTransfer          Whether this is a new request, if it's old, remove
+                                 the request.
+  @param  PollInterval           The interval to poll the transfer result, (in ms).
+  @param  DataLength             The length of perodic data transfer.
+  @param  Callback               The function to call periodicaly when transfer is
+                                 ready.
+  @param  Context                The context to the callback.
+
+  @retval EFI_SUCCESS            New transfer is queued or old request is removed.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to queue the new request or remove the old
+                                 request.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoAsyncInterruptTransfer (
+  IN EFI_USB_IO_PROTOCOL              *This,
+  IN UINT8                            Endpoint,
+  IN BOOLEAN                          IsNewTransfer,
+  IN UINTN                            PollInterval,       OPTIONAL
+  IN UINTN                            DataLength,         OPTIONAL
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  Callback,           OPTIONAL
+  IN VOID                             *Context            OPTIONAL
+  );
+
+/**
+  Execute a synchronous isochronous transfer.
+
+  @param  This                   The USB IO instance.
+  @param  DeviceEndpoint         The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_UNSUPPORTED        Currently isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoIsochronousTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               DeviceEndpoint,
+  IN  OUT VOID            *Data,
+  IN  UINTN               DataLength,
+  OUT UINT32              *Status
+  );
+
+/**
+  Queue an asynchronous isochronous transfer.
+
+  @param  This                   The USB_IO instance.
+  @param  DeviceEndpoint         The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of perodic data transfer.
+  @param  IsochronousCallBack    The function to call periodicaly when transfer is
+                                 ready.
+  @param  Context                The context to the callback.
+
+  @retval EFI_UNSUPPORTED        Currently isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoAsyncIsochronousTransfer (
+  IN EFI_USB_IO_PROTOCOL              *This,
+  IN UINT8                            DeviceEndpoint,
+  IN OUT VOID                         *Data,
+  IN UINTN                            DataLength,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  IsochronousCallBack,
+  IN VOID                             *Context              OPTIONAL
+  );
+
+/**
+  Retrieve the device descriptor of the device.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The variable to receive the device descriptor.
+
+  @retval EFI_SUCCESS            The device descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  The parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetDeviceDescriptor (
+  IN  EFI_USB_IO_PROTOCOL       *This,
+  OUT EFI_USB_DEVICE_DESCRIPTOR *Descriptor
+  );
+
+/**
+  Return the configuration descriptor of the current active configuration.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The USB configuration descriptor.
+
+  @retval EFI_SUCCESS            The active configuration descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+  @retval EFI_NOT_FOUND          Currently no active configuration is selected.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetActiveConfigDescriptor (
+  IN  EFI_USB_IO_PROTOCOL       *This,
+  OUT EFI_USB_CONFIG_DESCRIPTOR *Descriptor
+  );
+
+/**
+  Retrieve the active interface setting descriptor for this USB IO instance.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The variable to receive active interface setting.
+
+  @retval EFI_SUCCESS            The active interface setting is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetInterfaceDescriptor (
+  IN  EFI_USB_IO_PROTOCOL           *This,
+  OUT EFI_USB_INTERFACE_DESCRIPTOR  *Descriptor
+  );
+
+/**
+  Retrieve the endpoint descriptor from this interface setting.
+
+  @param  This                   The USB IO instance.
+  @param  Index                  The index (start from zero) of the endpoint to
+                                 retrieve.
+  @param  Descriptor             The variable to receive the descriptor.
+
+  @retval EFI_SUCCESS            The endpoint descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetEndpointDescriptor (
+  IN  EFI_USB_IO_PROTOCOL         *This,
+  IN  UINT8                       Index,
+  OUT EFI_USB_ENDPOINT_DESCRIPTOR *Descriptor
+  );
+
+/**
+  Retrieve the supported language ID table from the device.
+
+  @param  This                   The USB IO instance.
+  @param  LangIDTable            The table to return the language IDs.
+  @param  TableSize              The size, in bytes, of the table LangIDTable.
+
+  @retval EFI_SUCCESS            The language ID is return.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetSupportedLanguages (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  OUT UINT16              **LangIDTable,
+  OUT UINT16              *TableSize
+  );
+
+/**
+  Retrieve an indexed string in the language of LangID.
+
+  @param  This                   The USB IO instance.
+  @param  LangID                 The language ID of the string to retrieve.
+  @param  StringIndex            The index of the string.
+  @param  String                 The variable to receive the string.
+
+  @retval EFI_SUCCESS            The string is returned.
+  @retval EFI_NOT_FOUND          No such string existed.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetStringDescriptor (
+  IN  EFI_USB_IO_PROTOCOL   *This,
+  IN  UINT16                LangID,
+  IN  UINT8                 StringIndex,
+  OUT CHAR16                **String
+  );
+
+/**
+  Reset the device, then if that succeeds, reconfigure the
+  device with its address and current active configuration.
+
+  @param  This                   The USB IO instance.
+
+  @retval EFI_SUCCESS            The device is reset and configured.
+  @retval Others                 Failed to reset the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoPortReset (
+  IN EFI_USB_IO_PROTOCOL  *This
+  );
+
+/**
+  Install Usb Bus Protocol on host controller, and start the Usb bus.
+
+  @param This                    The USB bus driver binding instance.
+  @param Controller              The controller to check.
+  @param RemainingDevicePath     The remaining device patch.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb bus.
+  @retval EFI_ALREADY_STARTED    The controller is already controlled by the usb bus.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusBuildProtocol (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  The USB bus driver entry pointer.
+
+  @param ImageHandle       The driver image handle.
+  @param SystemTable       The system table.
+
+  @return EFI_SUCCESS      The component name protocol is installed.
+  @return Others           Failed to init the usb driver.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  );
+
+/**
+  Check whether USB bus driver support this device.
+
+  @param  This                   The USB bus driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The bus supports this controller.
+  @retval EFI_UNSUPPORTED        This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Start to process the controller.
+
+  @param  This                   The USB bus driver binding instance.
+  @param  Controller             The controller to check.
+  @param  RemainingDevicePath    The remaining device patch.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb bus.
+  @retval EFI_ALREADY_STARTED    The controller is already controlled by the usb
+                                 bus.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stop handle the controller by this USB bus driver.
+
+  @param  This                   The USB bus driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The child of USB bus that opened controller
+                                 BY_CHILD.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The controller or children are stopped.
+  @retval EFI_DEVICE_ERROR       Failed to stop the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+extern EFI_USB_IO_PROTOCOL            mUsbIoProtocol;
+extern EFI_DRIVER_BINDING_PROTOCOL    mUsbBusDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL    mUsbBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL   mUsbBusComponentName2;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.inf
new file mode 100644
index 0000000000..b7b30cc937
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.inf
@@ -0,0 +1,79 @@
+## @file
+#  The Usb Bus Dxe driver is used to enumerate and manage all attached usb devices.
+#
+#  Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbBusDxe
+  MODULE_UNI_FILE                = UsbBusDxe.uni
+  FILE_GUID                      = 240612B7-A063-11d4-9A3A-0090273FC14D
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = UsbBusDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  mUsbBusDriverBinding                        
+#  COMPONENT_NAME                =  mUsbBusComponentName
+#  COMPONENT_NAME2               =  mUsbBusComponentName2
+#
+
+[Sources]
+  UsbDesc.c
+  UsbEnumer.c
+  UsbEnumer.h
+  UsbBus.c
+  UsbHub.c
+  ComponentName.c
+  UsbUtility.h
+  UsbHub.h
+  UsbUtility.c
+  UsbDesc.h
+  UsbBus.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+
+[LibraryClasses]
+  MemoryAllocationLib
+  DevicePathLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  ReportStatusCodeLib
+
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## BY_START
+  ## TO_START
+  ## BY_START
+  gEfiDevicePathProtocolGuid                    
+  gEfiUsb2HcProtocolGuid                        ## TO_START
+  gEfiUsbHcProtocolGuid                         ## TO_START
+
+# [Event]
+#
+# EVENT_TYPE_PERIODIC_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.uni
new file mode 100644
index 0000000000..70f9e62436
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxeExtra.uni
new file mode 100644
index 0000000000..305ab09c3e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.c
new file mode 100644
index 0000000000..9687eb0bca
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.c
@@ -0,0 +1,978 @@
+/** @file
+
+    Manage Usb Descriptor List
+
+Copyright (c) 2007 - 2013, 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 "UsbBus.h"
+
+
+/**
+  Free the interface setting descriptor.
+
+  @param  Setting               The descriptor to free.
+
+**/
+VOID
+UsbFreeInterfaceDesc (
+  IN USB_INTERFACE_SETTING  *Setting
+  )
+{
+  USB_ENDPOINT_DESC       *Ep;
+  UINTN                   Index;
+
+  if (Setting->Endpoints != NULL) {
+    //
+    // Each interface setting may have several endpoints, free them first.
+    //
+    for (Index = 0; Index < Setting->Desc.NumEndpoints; Index++) {
+      Ep = Setting->Endpoints[Index];
+
+      if (Ep != NULL) {
+        FreePool (Ep);
+      }
+    }
+
+    //
+    // Only call FreePool() if NumEndpoints > 0.
+    //
+    if (Setting->Desc.NumEndpoints > 0) {
+      FreePool (Setting->Endpoints);
+    }
+  }
+
+  FreePool (Setting);
+}
+
+
+/**
+  Free a configuration descriptor with its interface
+  descriptors. It may be initialized partially.
+
+  @param  Config                The configuration descriptor to free.
+
+**/
+VOID
+UsbFreeConfigDesc (
+  IN USB_CONFIG_DESC      *Config
+  )
+{
+  USB_INTERFACE_DESC      *Interface;
+  UINTN                   Index;
+  UINTN                   SetIndex;
+
+  if (Config->Interfaces != NULL) {
+    //
+    // A configuration may have several interfaces, free the interface
+    //
+    for (Index = 0; Index < Config->Desc.NumInterfaces; Index++) {
+      Interface = Config->Interfaces[Index];
+
+      if (Interface == NULL) {
+        continue;
+      }
+
+      //
+      // Each interface may have several settings, free the settings
+      //
+      for (SetIndex = 0; SetIndex < Interface->NumOfSetting; SetIndex++) {
+        if (Interface->Settings[SetIndex] != NULL) {
+          UsbFreeInterfaceDesc (Interface->Settings[SetIndex]);
+        }
+      }
+
+      FreePool (Interface);
+    }
+
+    FreePool (Config->Interfaces);
+  }
+
+  FreePool (Config);
+
+}
+
+
+/**
+  Free a device descriptor with its configurations.
+
+  @param  DevDesc               The device descriptor.
+
+**/
+VOID
+UsbFreeDevDesc (
+  IN USB_DEVICE_DESC      *DevDesc
+  )
+{
+  UINTN                   Index;
+
+  if (DevDesc->Configs != NULL) {
+    for (Index = 0; Index < DevDesc->Desc.NumConfigurations; Index++) {
+      if (DevDesc->Configs[Index] != NULL) {
+        UsbFreeConfigDesc (DevDesc->Configs[Index]);
+      }
+    }
+
+    FreePool (DevDesc->Configs);
+  }
+
+  FreePool (DevDesc);
+}
+
+
+/**
+  Create a descriptor.
+
+  @param  DescBuf               The buffer of raw descriptor.
+  @param  Len                   The length of the raw descriptor buffer.
+  @param  Type                  The type of descriptor to create.
+  @param  Consumed              Number of bytes consumed.
+
+  @return Created descriptor or NULL.
+
+**/
+VOID *
+UsbCreateDesc (
+  IN  UINT8               *DescBuf,
+  IN  UINTN               Len,
+  IN  UINT8               Type,
+  OUT UINTN               *Consumed
+  )
+{
+  USB_DESC_HEAD           *Head;
+  UINTN                   DescLen;
+  UINTN                   CtrlLen;
+  UINTN                   Offset;
+  VOID                    *Desc;
+
+  DescLen   = 0;
+  CtrlLen   = 0;
+  *Consumed = 0;
+
+  switch (Type) {
+  case USB_DESC_TYPE_DEVICE:
+    DescLen = sizeof (EFI_USB_DEVICE_DESCRIPTOR);
+    CtrlLen = sizeof (USB_DEVICE_DESC);
+    break;
+
+  case USB_DESC_TYPE_CONFIG:
+    DescLen = sizeof (EFI_USB_CONFIG_DESCRIPTOR);
+    CtrlLen = sizeof (USB_CONFIG_DESC);
+    break;
+
+  case USB_DESC_TYPE_INTERFACE:
+    DescLen = sizeof (EFI_USB_INTERFACE_DESCRIPTOR);
+    CtrlLen = sizeof (USB_INTERFACE_SETTING);
+    break;
+
+  case USB_DESC_TYPE_ENDPOINT:
+    DescLen = sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);
+    CtrlLen = sizeof (USB_ENDPOINT_DESC);
+    break;
+  }
+
+  //
+  // All the descriptor has a common LTV (Length, Type, Value)
+  // format. Skip the descriptor that isn't of this Type
+  //
+  Offset = 0;
+  Head   = (USB_DESC_HEAD*)DescBuf;
+
+  while ((Offset < Len) && (Head->Type != Type)) {
+    Offset += Head->Len;
+    if (Len <= Offset) {
+      DEBUG (( EFI_D_ERROR, "UsbCreateDesc: met mal-format descriptor, Beyond boundary!\n"));
+      return NULL;
+    }
+    Head    = (USB_DESC_HEAD*)(DescBuf + Offset);
+    if (Head->Len == 0) {
+      DEBUG (( EFI_D_ERROR, "UsbCreateDesc: met mal-format descriptor, Head->Len = 0!\n"));
+      return NULL;
+    }
+  }
+
+  if ((Len <= Offset)      || (Len < Offset + DescLen) ||
+      (Head->Type != Type) || (Head->Len != DescLen)) {
+    DEBUG (( EFI_D_ERROR, "UsbCreateDesc: met mal-format descriptor\n"));
+    return NULL;
+  }
+
+  Desc = AllocateZeroPool ((UINTN) CtrlLen);
+  if (Desc == NULL) {
+    return NULL;
+  }
+
+  CopyMem (Desc, Head, (UINTN) DescLen);
+
+  *Consumed = Offset + Head->Len;
+
+  return Desc;
+}
+
+
+/**
+  Parse an interface descriptor and its endpoints.
+
+  @param  DescBuf               The buffer of raw descriptor.
+  @param  Len                   The length of the raw descriptor buffer.
+  @param  Consumed              The number of raw descriptor consumed.
+
+  @return The create interface setting or NULL if failed.
+
+**/
+USB_INTERFACE_SETTING *
+UsbParseInterfaceDesc (
+  IN  UINT8               *DescBuf,
+  IN  UINTN               Len,
+  OUT UINTN               *Consumed
+  )
+{
+  USB_INTERFACE_SETTING   *Setting;
+  USB_ENDPOINT_DESC       *Ep;
+  UINTN                   Index;
+  UINTN                   NumEp;
+  UINTN                   Used;
+  UINTN                   Offset;
+
+  *Consumed = 0;
+  Setting   = UsbCreateDesc (DescBuf, Len, USB_DESC_TYPE_INTERFACE, &Used);
+
+  if (Setting == NULL) {
+    DEBUG (( EFI_D_ERROR, "UsbParseInterfaceDesc: failed to create interface descriptor\n"));
+    return NULL;
+  }
+
+  Offset = Used;
+
+  //
+  // Create an array to hold the interface's endpoints
+  //
+  NumEp  = Setting->Desc.NumEndpoints;
+
+  DEBUG (( EFI_D_INFO, "UsbParseInterfaceDesc: interface %d(setting %d) has %d endpoints\n",
+              Setting->Desc.InterfaceNumber, Setting->Desc.AlternateSetting, (UINT32)NumEp));
+
+  if (NumEp == 0) {
+    goto ON_EXIT;
+  }
+
+  Setting->Endpoints  = AllocateZeroPool (sizeof (USB_ENDPOINT_DESC *) * NumEp);
+
+  if (Setting->Endpoints == NULL) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Create the endpoints for this interface
+  //
+  for (Index = 0; (Index < NumEp) && (Offset < Len); Index++) {
+    Ep = UsbCreateDesc (DescBuf + Offset, Len - Offset, USB_DESC_TYPE_ENDPOINT, &Used);
+
+    if (Ep == NULL) {
+      DEBUG (( EFI_D_ERROR, "UsbParseInterfaceDesc: failed to create endpoint(index %d)\n", (UINT32)Index));
+      goto ON_ERROR;
+    }
+
+    Setting->Endpoints[Index]  = Ep;
+    Offset                    += Used;
+  }
+
+
+ON_EXIT:
+  *Consumed = Offset;
+  return Setting;
+
+ON_ERROR:
+  UsbFreeInterfaceDesc (Setting);
+  return NULL;
+}
+
+
+/**
+  Parse the configuration descriptor and its interfaces.
+
+  @param  DescBuf               The buffer of raw descriptor.
+  @param  Len                   The length of the raw descriptor buffer.
+
+  @return The created configuration descriptor.
+
+**/
+USB_CONFIG_DESC *
+UsbParseConfigDesc (
+  IN UINT8                *DescBuf,
+  IN UINTN                Len
+  )
+{
+  USB_CONFIG_DESC         *Config;
+  USB_INTERFACE_SETTING   *Setting;
+  USB_INTERFACE_DESC      *Interface;
+  UINTN                   Index;
+  UINTN                   NumIf;
+  UINTN                   Consumed;
+
+  ASSERT (DescBuf != NULL);
+
+  Config = UsbCreateDesc (DescBuf, Len, USB_DESC_TYPE_CONFIG, &Consumed);
+
+  if (Config == NULL) {
+    return NULL;
+  }
+
+  //
+  // Initialize an array of setting for the configuration's interfaces.
+  //
+  NumIf               = Config->Desc.NumInterfaces;
+  Config->Interfaces  = AllocateZeroPool (sizeof (USB_INTERFACE_DESC *) * NumIf);
+
+  if (Config->Interfaces == NULL) {
+    goto ON_ERROR;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbParseConfigDesc: config %d has %d interfaces\n",
+                Config->Desc.ConfigurationValue, (UINT32)NumIf));
+
+  for (Index = 0; Index < NumIf; Index++) {
+    Interface = AllocateZeroPool (sizeof (USB_INTERFACE_DESC));
+
+    if (Interface == NULL) {
+      goto ON_ERROR;
+    }
+
+    Config->Interfaces[Index] = Interface;
+  }
+
+  //
+  // If a configuration has several interfaces, these interfaces are
+  // numbered from zero to n. If a interface has several settings,
+  // these settings are also number from zero to m. The interface
+  // setting must be organized as |interface 0, setting 0|interface 0
+  // setting 1|interface 1, setting 0|interface 2, setting 0|. Check
+  // USB2.0 spec, page 267.
+  //
+  DescBuf += Consumed;
+  Len     -= Consumed;
+
+  //
+  // Make allowances for devices that return extra data at the 
+  // end of their config descriptors
+  //
+  while (Len >= sizeof (EFI_USB_INTERFACE_DESCRIPTOR)) {
+    Setting = UsbParseInterfaceDesc (DescBuf, Len, &Consumed);
+
+    if (Setting == NULL) {
+      DEBUG (( EFI_D_ERROR, "UsbParseConfigDesc: warning: failed to get interface setting, stop parsing now.\n"));
+      break;
+
+    } else if (Setting->Desc.InterfaceNumber >= NumIf) {
+      DEBUG (( EFI_D_ERROR, "UsbParseConfigDesc: mal-formated interface descriptor\n"));
+
+      UsbFreeInterfaceDesc (Setting);
+      goto ON_ERROR;
+    }
+
+    //
+    // Insert the descriptor to the corresponding set.
+    //
+    Interface = Config->Interfaces[Setting->Desc.InterfaceNumber];
+
+    if (Interface->NumOfSetting >= USB_MAX_INTERFACE_SETTING) {
+      goto ON_ERROR;
+    }
+
+    Interface->Settings[Interface->NumOfSetting] = Setting;
+    Interface->NumOfSetting++;
+
+    DescBuf += Consumed;
+    Len     -= Consumed;
+  }
+
+  return Config;
+
+ON_ERROR:
+  UsbFreeConfigDesc (Config);
+  return NULL;
+}
+
+
+/**
+  USB standard control transfer support routine. This
+  function is used by USB device. It is possible that
+  the device's interfaces are still waiting to be
+  enumerated.
+
+  @param  UsbDev                The usb device.
+  @param  Direction             The direction of data transfer.
+  @param  Type                  Standard / class specific / vendor specific.
+  @param  Target                The receiving target.
+  @param  Request               Which request.
+  @param  Value                 The wValue parameter of the request.
+  @param  Index                 The wIndex parameter of the request.
+  @param  Buf                   The buffer to receive data into / transmit from.
+  @param  Length                The length of the buffer.
+
+  @retval EFI_SUCCESS           The control request is executed.
+  @retval EFI_DEVICE_ERROR      Failed to execute the control transfer.
+
+**/
+EFI_STATUS
+UsbCtrlRequest (
+  IN USB_DEVICE             *UsbDev,
+  IN EFI_USB_DATA_DIRECTION Direction,
+  IN UINTN                  Type,
+  IN UINTN                  Target,
+  IN UINTN                  Request,
+  IN UINT16                 Value,
+  IN UINT16                 Index,
+  IN OUT VOID               *Buf,
+  IN UINTN                  Length
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  EFI_STATUS              Status;
+  UINT32                  Result;
+  UINTN                   Len;
+
+  ASSERT ((UsbDev != NULL) && (UsbDev->Bus != NULL));
+
+  DevReq.RequestType  = USB_REQUEST_TYPE (Direction, Type, Target);
+  DevReq.Request      = (UINT8) Request;
+  DevReq.Value        = Value;
+  DevReq.Index        = Index;
+  DevReq.Length       = (UINT16) Length;
+
+  Len                 = Length;
+  Status = UsbHcControlTransfer (
+             UsbDev->Bus,
+             UsbDev->Address,
+             UsbDev->Speed,
+             UsbDev->MaxPacket0,
+             &DevReq,
+             Direction,
+             Buf,
+             &Len,
+             USB_GENERAL_DEVICE_REQUEST_TIMEOUT,
+             &UsbDev->Translator,
+             &Result
+             );
+
+  return Status;
+}
+
+
+/**
+  Get the standard descriptors.
+
+  @param  UsbDev                The USB device to read descriptor from.
+  @param  DescType              The type of descriptor to read.
+  @param  DescIndex             The index of descriptor to read.
+  @param  LangId                Language ID, only used to get string, otherwise set
+                                it to 0.
+  @param  Buf                   The buffer to hold the descriptor read.
+  @param  Length                The length of the buffer.
+
+  @retval EFI_SUCCESS           The descriptor is read OK.
+  @retval Others                Failed to retrieve the descriptor.
+
+**/
+EFI_STATUS
+UsbCtrlGetDesc (
+  IN  USB_DEVICE          *UsbDev,
+  IN  UINTN               DescType,
+  IN  UINTN               DescIndex,
+  IN  UINT16              LangId,
+  OUT VOID                *Buf,
+  IN  UINTN               Length
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             UsbDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_STANDARD,
+             USB_TARGET_DEVICE,
+             USB_REQ_GET_DESCRIPTOR,
+             (UINT16) ((DescType << 8) | DescIndex),
+             LangId,
+             Buf,
+             Length
+             );
+
+  return Status;
+}
+
+
+/**
+  Return the max packet size for endpoint zero. This function
+  is the first function called to get descriptors during bus
+  enumeration.
+
+  @param  UsbDev                The usb device.
+
+  @retval EFI_SUCCESS           The max packet size of endpoint zero is retrieved.
+  @retval EFI_DEVICE_ERROR      Failed to retrieve it.
+
+**/
+EFI_STATUS
+UsbGetMaxPacketSize0 (
+  IN USB_DEVICE           *UsbDev
+  )
+{
+  EFI_USB_DEVICE_DESCRIPTOR DevDesc;
+  EFI_STATUS                Status;
+  UINTN                     Index;
+
+
+  //
+  // Get the first 8 bytes of the device descriptor which contains
+  // max packet size for endpoint 0, which is at least 8.
+  //
+  for (Index = 0; Index < 3; Index++) {
+    Status = UsbCtrlGetDesc (UsbDev, USB_DESC_TYPE_DEVICE, 0, 0, &DevDesc, 8);
+
+    if (!EFI_ERROR (Status)) {
+      if ((DevDesc.BcdUSB == 0x0300) && (DevDesc.MaxPacketSize0 == 9)) {
+        UsbDev->MaxPacket0 = 1 << 9;
+        return EFI_SUCCESS;
+      }
+      UsbDev->MaxPacket0 = DevDesc.MaxPacketSize0;
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (USB_RETRY_MAX_PACK_SIZE_STALL);
+  }
+
+  return EFI_DEVICE_ERROR;
+}
+
+
+/**
+  Get the device descriptor for the device.
+
+  @param  UsbDev                The Usb device to retrieve descriptor from.
+
+  @retval EFI_SUCCESS           The device descriptor is returned.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory.
+
+**/
+EFI_STATUS
+UsbGetDevDesc (
+  IN USB_DEVICE           *UsbDev
+  )
+{
+  USB_DEVICE_DESC         *DevDesc;
+  EFI_STATUS              Status;
+
+  DevDesc = AllocateZeroPool (sizeof (USB_DEVICE_DESC));
+
+  if (DevDesc == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status  = UsbCtrlGetDesc (
+              UsbDev,
+              USB_DESC_TYPE_DEVICE,
+              0,
+              0,
+              DevDesc,
+              sizeof (EFI_USB_DEVICE_DESCRIPTOR)
+              );
+
+  if (EFI_ERROR (Status)) {
+    gBS->FreePool (DevDesc);
+  } else {
+    UsbDev->DevDesc = DevDesc;
+  }
+
+  return Status;
+}
+
+
+/**
+  Retrieve the indexed string for the language. It requires two
+  steps to get a string, first to get the string's length. Then
+  the string itself.
+
+  @param  UsbDev                The usb device.
+  @param  Index                 The index the string to retrieve.
+  @param  LangId                Language ID.
+
+  @return The created string descriptor or NULL.
+
+**/
+EFI_USB_STRING_DESCRIPTOR *
+UsbGetOneString (
+  IN     USB_DEVICE       *UsbDev,
+  IN     UINT8            Index,
+  IN     UINT16           LangId
+  )
+{
+  EFI_USB_STRING_DESCRIPTOR Desc;
+  EFI_STATUS                Status;
+  UINT8                     *Buf;
+
+  //
+  // First get two bytes which contains the string length.
+  //
+  Status = UsbCtrlGetDesc (UsbDev, USB_DESC_TYPE_STRING, Index, LangId, &Desc, 2);
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Buf = AllocateZeroPool (Desc.Length);
+
+  if (Buf == NULL) {
+    return NULL;
+  }
+
+  Status = UsbCtrlGetDesc (
+             UsbDev,
+             USB_DESC_TYPE_STRING,
+             Index,
+             LangId,
+             Buf,
+             Desc.Length
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    return NULL;
+  }
+
+  return (EFI_USB_STRING_DESCRIPTOR *) Buf;
+}
+
+
+/**
+  Build the language ID table for string descriptors.
+
+  @param  UsbDev                The Usb device.
+
+  @retval EFI_UNSUPPORTED       This device doesn't support string table.
+
+**/
+EFI_STATUS
+UsbBuildLangTable (
+  IN USB_DEVICE           *UsbDev
+  )
+{
+  EFI_USB_STRING_DESCRIPTOR *Desc;
+  EFI_STATUS                Status;
+  UINTN                     Index;
+  UINTN                     Max;
+  UINT16                    *Point;
+
+  //
+  // The string of language ID zero returns the supported languages
+  //
+  Desc = UsbGetOneString (UsbDev, 0, 0);
+
+  if (Desc == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Desc->Length < 4) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+  }
+
+  Status = EFI_SUCCESS;
+
+  Max   = (Desc->Length - 2) / 2;
+  Max   = MIN(Max, USB_MAX_LANG_ID);
+  
+  Point = Desc->String;
+  for (Index = 0; Index < Max; Index++) {
+    UsbDev->LangId[Index] = *Point;
+    Point++;
+  }
+
+  UsbDev->TotalLangId = (UINT16)Max;
+
+ON_EXIT:
+  gBS->FreePool (Desc);
+  return Status;
+}
+
+
+/**
+  Retrieve the indexed configure for the device. USB device
+  returns the configuration together with the interfaces for
+  this configuration. Configuration descriptor is also of
+  variable length.
+
+  @param  UsbDev                The Usb interface.
+  @param  Index                 The index of the configuration.
+
+  @return The created configuration descriptor.
+
+**/
+EFI_USB_CONFIG_DESCRIPTOR *
+UsbGetOneConfig (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                Index
+  )
+{
+  EFI_USB_CONFIG_DESCRIPTOR Desc;
+  EFI_STATUS                Status;
+  VOID                      *Buf;
+
+  //
+  // First get four bytes which contains the total length
+  // for this configuration.
+  //
+  Status = UsbCtrlGetDesc (UsbDev, USB_DESC_TYPE_CONFIG, Index, 0, &Desc, 8);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbGetOneConfig: failed to get descript length(%d) %r\n",
+                Desc.TotalLength, Status));
+
+    return NULL;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbGetOneConfig: total length is %d\n", Desc.TotalLength));
+
+  Buf = AllocateZeroPool (Desc.TotalLength);
+
+  if (Buf == NULL) {
+    return NULL;
+  }
+
+  Status = UsbCtrlGetDesc (UsbDev, USB_DESC_TYPE_CONFIG, Index, 0, Buf, Desc.TotalLength);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbGetOneConfig: failed to get full descript %r\n", Status));
+
+    FreePool (Buf);
+    return NULL;
+  }
+
+  return Buf;
+}
+
+
+/**
+  Build the whole array of descriptors. This function must
+  be called after UsbGetMaxPacketSize0 returns the max packet
+  size correctly for endpoint 0.
+
+  @param  UsbDev                The Usb device.
+
+  @retval EFI_SUCCESS           The descriptor table is build.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the descriptor.
+
+**/
+EFI_STATUS
+UsbBuildDescTable (
+  IN USB_DEVICE           *UsbDev
+  )
+{
+  EFI_USB_CONFIG_DESCRIPTOR *Config;
+  USB_DEVICE_DESC           *DevDesc;
+  USB_CONFIG_DESC           *ConfigDesc;
+  UINT8                     NumConfig;
+  EFI_STATUS                Status;
+  UINT8                     Index;
+
+  //
+  // Get the device descriptor, then allocate the configure
+  // descriptor pointer array to hold configurations.
+  //
+  Status = UsbGetDevDesc (UsbDev);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbBuildDescTable: failed to get device descriptor - %r\n", Status));
+    return Status;
+  }
+
+  DevDesc   = UsbDev->DevDesc;
+  NumConfig = DevDesc->Desc.NumConfigurations;
+  if (NumConfig == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  DevDesc->Configs = AllocateZeroPool (NumConfig * sizeof (USB_CONFIG_DESC *));
+  if (DevDesc->Configs == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbBuildDescTable: device has %d configures\n", NumConfig));
+
+  //
+  // Read each configurations, then parse them
+  //
+  for (Index = 0; Index < NumConfig; Index++) {
+    Config = UsbGetOneConfig (UsbDev, Index);
+
+    if (Config == NULL) {
+      DEBUG (( EFI_D_ERROR, "UsbBuildDescTable: failed to get configure (index %d)\n", Index));
+
+      //
+      // If we can get the default descriptor, it is likely that the
+      // device is still operational.
+      //
+      if (Index == 0) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      break;
+    }
+
+    ConfigDesc = UsbParseConfigDesc ((UINT8 *) Config, Config->TotalLength);
+
+    FreePool (Config);
+
+    if (ConfigDesc == NULL) {
+      DEBUG (( EFI_D_ERROR, "UsbBuildDescTable: failed to parse configure (index %d)\n", Index));
+
+      //
+      // If we can get the default descriptor, it is likely that the
+      // device is still operational.
+      //
+      if (Index == 0) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      break;
+    }
+
+    DevDesc->Configs[Index] = ConfigDesc;
+  }
+
+  //
+  // Don't return error even this function failed because
+  // it is possible for the device to not support strings.
+  //
+  Status = UsbBuildLangTable (UsbDev);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_INFO, "UsbBuildDescTable: get language ID table %r\n", Status));
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Set the device's address.
+
+  @param  UsbDev                The device to set address to.
+  @param  Address               The address to set.
+
+  @retval EFI_SUCCESS           The device is set to the address.
+  @retval Others                Failed to set the device address.
+
+**/
+EFI_STATUS
+UsbSetAddress (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                Address
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             UsbDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_STANDARD,
+             USB_TARGET_DEVICE,
+             USB_REQ_SET_ADDRESS,
+             Address,
+             0,
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Set the device's configuration. This function changes
+  the device's internal state. UsbSelectConfig changes
+  the Usb bus's internal state.
+
+  @param  UsbDev                The USB device to set configure to.
+  @param  ConfigIndex           The configure index to set.
+
+  @retval EFI_SUCCESS           The device is configured now.
+  @retval Others                Failed to set the device configure.
+
+**/
+EFI_STATUS
+UsbSetConfig (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                ConfigIndex
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             UsbDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_STANDARD,
+             USB_TARGET_DEVICE,
+             USB_REQ_SET_CONFIG,
+             ConfigIndex,
+             0,
+             NULL,
+             0
+            );
+
+  return Status;
+}
+
+
+/**
+  Usb UsbIo interface to clear the feature. This is should
+  only be used by HUB which is considered a device driver
+  on top of the UsbIo interface.
+
+  @param  UsbIo                 The UsbIo interface.
+  @param  Target                The target of the transfer: endpoint/device.
+  @param  Feature               The feature to clear.
+  @param  Index                 The wIndex parameter.
+
+  @retval EFI_SUCCESS           The device feature is cleared.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbIoClearFeature (
+  IN  EFI_USB_IO_PROTOCOL *UsbIo,
+  IN  UINTN               Target,
+  IN  UINT16              Feature,
+  IN  UINT16              Index
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  UINT32                  UsbResult;
+  EFI_STATUS              Status;
+
+  DevReq.RequestType  = USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, Target);
+  DevReq.Request      = USB_REQ_CLEAR_FEATURE;
+  DevReq.Value        = Feature;
+  DevReq.Index        = Index;
+  DevReq.Length       = 0;
+
+  Status = UsbIo->UsbControlTransfer (
+                    UsbIo,
+                    &DevReq,
+                    EfiUsbNoData,
+                    USB_CLEAR_FEATURE_REQUEST_TIMEOUT,
+                    NULL,
+                    0,
+                    &UsbResult
+                    );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.h
new file mode 100644
index 0000000000..482a71f338
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.h
@@ -0,0 +1,233 @@
+/** @file
+
+    Manage Usb Descriptor List
+
+Copyright (c) 2007 - 2014, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _USB_DESCRIPTOR_H_
+#define _USB_DESCRIPTOR_H_
+
+#define USB_MAX_INTERFACE_SETTING  256
+
+//
+// The RequestType in EFI_USB_DEVICE_REQUEST is composed of
+// three fields: One bit direction, 2 bit type, and 5 bit
+// target.
+//
+#define USB_REQUEST_TYPE(Dir, Type, Target) \
+          ((UINT8)((((Dir) == EfiUsbDataIn ? 0x01 : 0) << 7) | (Type) | (Target)))
+
+//
+// A common header for usb standard descriptor.
+// Each stand descriptor has a length and type.
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Len;
+  UINT8                   Type;
+} USB_DESC_HEAD;
+#pragma pack()
+
+
+//
+// Each USB device has a device descriptor. Each device may
+// have several configures. Each configure contains several
+// interfaces. Each interface may have several settings. Each
+// setting has several endpoints.
+//
+// EFI_USB_..._DESCRIPTOR must be the first member of the
+// structure.
+//
+typedef struct {
+  EFI_USB_ENDPOINT_DESCRIPTOR   Desc;
+  UINT8                         Toggle;
+} USB_ENDPOINT_DESC;
+
+typedef struct {
+  EFI_USB_INTERFACE_DESCRIPTOR  Desc;
+  USB_ENDPOINT_DESC             **Endpoints;
+} USB_INTERFACE_SETTING;
+
+//
+// An interface may have several settings. Use a
+// fixed max number of settings to simplify code.
+// It should sufice in most environments.
+//
+typedef struct {
+  USB_INTERFACE_SETTING*        Settings[USB_MAX_INTERFACE_SETTING];
+  UINTN                         NumOfSetting;
+  UINTN                         ActiveIndex;  // Index of active setting
+} USB_INTERFACE_DESC;
+
+typedef struct {
+  EFI_USB_CONFIG_DESCRIPTOR     Desc;
+  USB_INTERFACE_DESC            **Interfaces;
+} USB_CONFIG_DESC;
+
+typedef struct {
+  EFI_USB_DEVICE_DESCRIPTOR     Desc;
+  USB_CONFIG_DESC               **Configs;
+} USB_DEVICE_DESC;
+
+/**
+  USB standard control transfer support routine. This
+  function is used by USB device. It is possible that
+  the device's interfaces are still waiting to be
+  enumerated.
+
+  @param  UsbDev                The usb device.
+  @param  Direction             The direction of data transfer.
+  @param  Type                  Standard / class specific / vendor specific.
+  @param  Target                The receiving target.
+  @param  Request               Which request.
+  @param  Value                 The wValue parameter of the request.
+  @param  Index                 The wIndex parameter of the request.
+  @param  Buf                   The buffer to receive data into / transmit from.
+  @param  Length                The length of the buffer.
+
+  @retval EFI_SUCCESS           The control request is executed.
+  @retval EFI_DEVICE_ERROR      Failed to execute the control transfer.
+
+**/
+EFI_STATUS
+UsbCtrlRequest (
+  IN USB_DEVICE             *UsbDev,
+  IN EFI_USB_DATA_DIRECTION Direction,
+  IN UINTN                  Type,
+  IN UINTN                  Target,
+  IN UINTN                  Request,
+  IN UINT16                 Value,
+  IN UINT16                 Index,
+  IN OUT VOID               *Buf,
+  IN UINTN                  Length
+  );
+
+/**
+  Return the max packet size for endpoint zero. This function
+  is the first function called to get descriptors during bus
+  enumeration.
+
+  @param  UsbDev                The usb device.
+
+  @retval EFI_SUCCESS           The max packet size of endpoint zero is retrieved.
+  @retval EFI_DEVICE_ERROR      Failed to retrieve it.
+
+**/
+EFI_STATUS
+UsbGetMaxPacketSize0 (
+  IN USB_DEVICE           *UsbDev
+  );
+
+/**
+  Free a device descriptor with its configurations.
+
+  @param  DevDesc               The device descriptor.
+
+  @return None.
+
+**/
+VOID
+UsbFreeDevDesc (
+  IN USB_DEVICE_DESC      *DevDesc
+  );
+
+/**
+  Retrieve the indexed string for the language. It requires two
+  steps to get a string, first to get the string's length. Then
+  the string itself.
+
+  @param  UsbDev                The usb device.
+  @param  StringIndex           The index of the string to retrieve.
+  @param  LangId                Language ID.
+
+  @return The created string descriptor or NULL.
+
+**/
+EFI_USB_STRING_DESCRIPTOR*
+UsbGetOneString (
+  IN     USB_DEVICE       *UsbDev,
+  IN     UINT8            StringIndex,
+  IN     UINT16           LangId
+  );
+
+/**
+  Build the whole array of descriptors. This function must
+  be called after UsbGetMaxPacketSize0 returns the max packet
+  size correctly for endpoint 0.
+
+  @param  UsbDev                The Usb device.
+
+  @retval EFI_SUCCESS           The descriptor table is build.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the descriptor.
+
+**/
+EFI_STATUS
+UsbBuildDescTable (
+  IN USB_DEVICE           *UsbDev
+  );
+
+/**
+  Set the device's address.
+
+  @param  UsbDev                The device to set address to.
+  @param  Address               The address to set.
+
+  @retval EFI_SUCCESS           The device is set to the address.
+  @retval Others                Failed to set the device address.
+
+**/
+EFI_STATUS
+UsbSetAddress (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                Address
+  );
+
+/**
+  Set the device's configuration. This function changes
+  the device's internal state. UsbSelectConfig changes
+  the Usb bus's internal state.
+
+  @param  UsbDev                The USB device to set configure to.
+  @param  ConfigIndex           The configure index to set.
+
+  @retval EFI_SUCCESS           The device is configured now.
+  @retval Others                Failed to set the device configure.
+
+**/
+EFI_STATUS
+UsbSetConfig (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                ConfigIndex
+  );
+
+/**
+  Usb UsbIo interface to clear the feature. This is should
+  only be used by HUB which is considered a device driver
+  on top of the UsbIo interface.
+
+  @param  UsbIo                 The UsbIo interface.
+  @param  Target                The target of the transfer: endpoint/device.
+  @param  Feature               The feature to clear.
+  @param  Index                 The wIndex parameter.
+
+  @retval EFI_SUCCESS           The device feature is cleared.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbIoClearFeature (
+  IN  EFI_USB_IO_PROTOCOL *UsbIo,
+  IN  UINTN               Target,
+  IN  UINT16              Feature,
+  IN  UINT16              Index
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.c
new file mode 100644
index 0000000000..79453fed26
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.c
@@ -0,0 +1,1065 @@
+/** @file
+
+    Usb bus enumeration support.
+
+Copyright (c) 2007 - 2014, 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 "UsbBus.h"
+
+/**
+  Return the endpoint descriptor in this interface.
+
+  @param  UsbIf                 The interface to search in.
+  @param  EpAddr                The address of the endpoint to return.
+
+  @return The endpoint descriptor or NULL.
+
+**/
+USB_ENDPOINT_DESC *
+UsbGetEndpointDesc (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                EpAddr
+  )
+{
+  USB_ENDPOINT_DESC       *EpDesc;
+  UINT8                   Index;
+  UINT8                   NumEndpoints;
+  
+  NumEndpoints = UsbIf->IfSetting->Desc.NumEndpoints;
+  
+  for (Index = 0; Index < NumEndpoints; Index++) {
+    EpDesc = UsbIf->IfSetting->Endpoints[Index];
+
+    if (EpDesc->Desc.EndpointAddress == EpAddr) {
+      return EpDesc;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Free the resource used by USB interface.
+
+  @param  UsbIf                 The USB interface to free.
+
+**/
+VOID
+UsbFreeInterface (
+  IN USB_INTERFACE        *UsbIf
+  )
+{
+  UsbCloseHostProtoByChild (UsbIf->Device->Bus, UsbIf->Handle);
+
+  gBS->UninstallMultipleProtocolInterfaces (
+         UsbIf->Handle,
+         &gEfiDevicePathProtocolGuid,
+         UsbIf->DevicePath,
+         &gEfiUsbIoProtocolGuid,
+         &UsbIf->UsbIo,
+         NULL
+         );
+
+  if (UsbIf->DevicePath != NULL) {
+    FreePool (UsbIf->DevicePath);
+  }
+
+  FreePool (UsbIf);
+}
+
+
+/**
+  Create an interface for the descriptor IfDesc. Each
+  device's configuration can have several interfaces.
+
+  @param  Device                The device has the interface descriptor.
+  @param  IfDesc                The interface descriptor.
+
+  @return The created USB interface for the descriptor, or NULL.
+
+**/
+USB_INTERFACE *
+UsbCreateInterface (
+  IN USB_DEVICE           *Device,
+  IN USB_INTERFACE_DESC   *IfDesc
+  )
+{
+  USB_DEVICE_PATH         UsbNode;
+  USB_INTERFACE           *UsbIf;
+  USB_INTERFACE           *HubIf;
+  EFI_STATUS              Status;
+
+  UsbIf = AllocateZeroPool (sizeof (USB_INTERFACE));
+
+  if (UsbIf == NULL) {
+    return NULL;
+  }
+
+  UsbIf->Signature  = USB_INTERFACE_SIGNATURE;
+  UsbIf->Device     = Device;
+  UsbIf->IfDesc     = IfDesc;
+  ASSERT (IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);
+  UsbIf->IfSetting  = IfDesc->Settings[IfDesc->ActiveIndex];
+
+  CopyMem (
+    &(UsbIf->UsbIo),
+    &mUsbIoProtocol,
+    sizeof (EFI_USB_IO_PROTOCOL)
+    );
+
+  //
+  // Install protocols for USBIO and device path
+  //
+  UsbNode.Header.Type       = MESSAGING_DEVICE_PATH;
+  UsbNode.Header.SubType    = MSG_USB_DP;
+  UsbNode.ParentPortNumber  = Device->ParentPort;
+  UsbNode.InterfaceNumber   = UsbIf->IfSetting->Desc.InterfaceNumber;
+
+  SetDevicePathNodeLength (&UsbNode.Header, sizeof (UsbNode));
+
+  HubIf = Device->ParentIf;
+  ASSERT (HubIf != NULL);
+
+  UsbIf->DevicePath = AppendDevicePathNode (HubIf->DevicePath, &UsbNode.Header);
+
+  if (UsbIf->DevicePath == NULL) {
+    DEBUG ((EFI_D_ERROR, "UsbCreateInterface: failed to create device path\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &UsbIf->Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  UsbIf->DevicePath,
+                  &gEfiUsbIoProtocolGuid,
+                  &UsbIf->UsbIo,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbCreateInterface: failed to install UsbIo - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  //
+  // Open USB Host Controller Protocol by Child
+  //
+  Status = UsbOpenHostProtoByChild (Device->Bus, UsbIf->Handle);
+
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           &UsbIf->Handle,
+           &gEfiDevicePathProtocolGuid,
+           UsbIf->DevicePath,
+           &gEfiUsbIoProtocolGuid,
+           &UsbIf->UsbIo,
+           NULL
+           );
+
+    DEBUG ((EFI_D_ERROR, "UsbCreateInterface: failed to open host for child - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  return UsbIf;
+
+ON_ERROR:
+  if (UsbIf->DevicePath != NULL) {
+    FreePool (UsbIf->DevicePath);
+  }
+
+  FreePool (UsbIf);
+  return NULL;
+}
+
+
+/**
+  Free the resource used by this USB device.
+
+  @param  Device                The USB device to free.
+
+**/
+VOID
+UsbFreeDevice (
+  IN USB_DEVICE           *Device
+  )
+{
+  if (Device->DevDesc != NULL) {
+    UsbFreeDevDesc (Device->DevDesc);
+  }
+
+  gBS->FreePool (Device);
+}
+
+
+/**
+  Create a device which is on the parent's ParentPort port.
+
+  @param  ParentIf              The parent HUB interface.
+  @param  ParentPort            The port on the HUB this device is connected to.
+
+  @return Created USB device, Or NULL.
+
+**/
+USB_DEVICE *
+UsbCreateDevice (
+  IN USB_INTERFACE        *ParentIf,
+  IN UINT8                ParentPort
+  )
+{
+  USB_DEVICE              *Device;
+
+  ASSERT (ParentIf != NULL);
+
+  Device = AllocateZeroPool (sizeof (USB_DEVICE));
+
+  if (Device == NULL) {
+    return NULL;
+  }
+
+  Device->Bus         = ParentIf->Device->Bus;
+  Device->MaxPacket0  = 8;
+  Device->ParentAddr  = ParentIf->Device->Address;
+  Device->ParentIf    = ParentIf;
+  Device->ParentPort  = ParentPort;
+  Device->Tier        = (UINT8)(ParentIf->Device->Tier + 1);
+  return Device;
+}
+
+
+/**
+  Connect the USB interface with its driver. EFI USB bus will
+  create a USB interface for each separate interface descriptor.
+
+  @param  UsbIf             The interface to connect driver to.
+
+  @return EFI_SUCCESS       Interface is managed by some driver.
+  @return Others            Failed to locate a driver for this interface.
+
+**/
+EFI_STATUS
+UsbConnectDriver (
+  IN USB_INTERFACE        *UsbIf
+  )
+{
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Hub is maintained by the USB bus driver. Otherwise try to
+  // connect drivers with this interface
+  //
+  if (UsbIsHubInterface (UsbIf)) {
+    DEBUG ((EFI_D_INFO, "UsbConnectDriver: found a hub device\n"));
+    Status = mUsbHubApi.Init (UsbIf);
+
+  } else {
+    //
+    // This function is called in both UsbIoControlTransfer and
+    // the timer callback in hub enumeration. So, at least it is
+    // called at TPL_CALLBACK. Some driver sitting on USB has
+    // twisted TPL used. It should be no problem for us to connect
+    // or disconnect at CALLBACK.
+    //
+    
+    //
+    // Only recursively wanted usb child device
+    //
+    if (UsbBusIsWantedUsbIO (UsbIf->Device->Bus, UsbIf)) {
+      OldTpl            = UsbGetCurrentTpl ();
+      DEBUG ((EFI_D_INFO, "UsbConnectDriver: TPL before connect is %d, %p\n", (UINT32)OldTpl, UsbIf->Handle));
+
+      gBS->RestoreTPL (TPL_CALLBACK);
+
+      Status            = gBS->ConnectController (UsbIf->Handle, NULL, NULL, TRUE);
+      UsbIf->IsManaged  = (BOOLEAN)!EFI_ERROR (Status);
+
+      DEBUG ((EFI_D_INFO, "UsbConnectDriver: TPL after connect is %d\n", (UINT32)UsbGetCurrentTpl()));
+      ASSERT (UsbGetCurrentTpl () == TPL_CALLBACK);
+
+      gBS->RaiseTPL (OldTpl);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Select an alternate setting for the interface.
+  Each interface can have several mutually exclusive
+  settings. Only one setting is active. It will
+  also reset its endpoints' toggle to zero.
+
+  @param  IfDesc                The interface descriptor to set.
+  @param  Alternate             The alternate setting number to locate.
+
+  @retval EFI_NOT_FOUND         There is no setting with this alternate index.
+  @retval EFI_SUCCESS           The interface is set to Alternate setting.
+
+**/
+EFI_STATUS
+UsbSelectSetting (
+  IN USB_INTERFACE_DESC   *IfDesc,
+  IN UINT8                Alternate
+  )
+{
+  USB_INTERFACE_SETTING   *Setting;
+  UINTN                   Index;
+
+  //
+  // Locate the active alternate setting
+  //
+  Setting = NULL;
+
+  for (Index = 0; Index < IfDesc->NumOfSetting; Index++) {
+    ASSERT (Index < USB_MAX_INTERFACE_SETTING);
+    Setting = IfDesc->Settings[Index];
+
+    if (Setting->Desc.AlternateSetting == Alternate) {
+      break;
+    }
+  }
+
+  if (Index == IfDesc->NumOfSetting) {
+    return EFI_NOT_FOUND;
+  }
+
+  IfDesc->ActiveIndex = Index;
+
+  ASSERT (Setting != NULL);
+  DEBUG ((EFI_D_INFO, "UsbSelectSetting: setting %d selected for interface %d\n",
+              Alternate, Setting->Desc.InterfaceNumber));
+
+  //
+  // Reset the endpoint toggle to zero
+  //
+  for (Index = 0; Index < Setting->Desc.NumEndpoints; Index++) {
+    Setting->Endpoints[Index]->Toggle = 0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Select a new configuration for the device. Each
+  device may support several configurations.
+
+  @param  Device                The device to select configuration.
+  @param  ConfigValue           The index of the configuration ( != 0).
+
+  @retval EFI_NOT_FOUND         There is no configuration with the index.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource.
+  @retval EFI_SUCCESS           The configuration is selected.
+
+**/
+EFI_STATUS
+UsbSelectConfig (
+  IN USB_DEVICE           *Device,
+  IN UINT8                ConfigValue
+  )
+{
+  USB_DEVICE_DESC         *DevDesc;
+  USB_CONFIG_DESC         *ConfigDesc;
+  USB_INTERFACE_DESC      *IfDesc;
+  USB_INTERFACE           *UsbIf;
+  EFI_STATUS              Status;
+  UINT8                   Index;
+
+  //
+  // Locate the active config, then set the device's pointer
+  //
+  DevDesc     = Device->DevDesc;
+  ConfigDesc  = NULL;
+
+  for (Index = 0; Index < DevDesc->Desc.NumConfigurations; Index++) {
+    ConfigDesc = DevDesc->Configs[Index];
+
+    if (ConfigDesc->Desc.ConfigurationValue == ConfigValue) {
+      break;
+    }
+  }
+
+  if (Index == DevDesc->Desc.NumConfigurations) {
+    return EFI_NOT_FOUND;
+  }
+
+  Device->ActiveConfig = ConfigDesc;
+
+  DEBUG ((EFI_D_INFO, "UsbSelectConfig: config %d selected for device %d\n",
+              ConfigValue, Device->Address));
+
+  //
+  // Create interfaces for each USB interface descriptor.
+  //
+  for (Index = 0; Index < ConfigDesc->Desc.NumInterfaces; Index++) {
+    //
+    // First select the default interface setting, and reset
+    // the endpoint toggles to zero for its endpoints.
+    //
+    IfDesc = ConfigDesc->Interfaces[Index];
+    UsbSelectSetting (IfDesc, IfDesc->Settings[0]->Desc.AlternateSetting);
+
+    //
+    // Create a USB_INTERFACE and install USB_IO and other protocols
+    //
+    UsbIf = UsbCreateInterface (Device, ConfigDesc->Interfaces[Index]);
+
+    if (UsbIf == NULL) {
+      Device->NumOfInterface = Index;
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ASSERT (Index < USB_MAX_INTERFACE);
+    Device->Interfaces[Index] = UsbIf;
+
+    //
+    // Connect the device to drivers, if it failed, ignore
+    // the error. Don't let the unsupported interfaces to block
+    // the supported interfaces.
+    //
+    Status = UsbConnectDriver (UsbIf);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "UsbSelectConfig: failed to connect driver %r, ignored\n", Status));
+    }
+  }
+
+  Device->NumOfInterface = Index;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Disconnect the USB interface with its driver.
+
+  @param  UsbIf                 The interface to disconnect driver from.
+
+**/
+EFI_STATUS
+UsbDisconnectDriver (
+  IN USB_INTERFACE        *UsbIf
+  )
+{
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+
+  //
+  // Release the hub if it's a hub controller, otherwise
+  // disconnect the driver if it is managed by other drivers.
+  //
+  Status = EFI_SUCCESS;
+  if (UsbIf->IsHub) {
+    Status = UsbIf->HubApi->Release (UsbIf);
+
+  } else if (UsbIf->IsManaged) {
+    //
+    // This function is called in both UsbIoControlTransfer and
+    // the timer callback in hub enumeration. So, at least it is
+    // called at TPL_CALLBACK. Some driver sitting on USB has
+    // twisted TPL used. It should be no problem for us to connect
+    // or disconnect at CALLBACK.
+    //
+    OldTpl           = UsbGetCurrentTpl ();
+    DEBUG ((EFI_D_INFO, "UsbDisconnectDriver: old TPL is %d, %p\n", (UINT32)OldTpl, UsbIf->Handle));
+
+    gBS->RestoreTPL (TPL_CALLBACK);
+
+    Status = gBS->DisconnectController (UsbIf->Handle, NULL, NULL);
+    if (!EFI_ERROR (Status)) {
+      UsbIf->IsManaged = FALSE;
+    }
+    
+    DEBUG (( EFI_D_INFO, "UsbDisconnectDriver: TPL after disconnect is %d, %d\n", (UINT32)UsbGetCurrentTpl(), Status));
+    ASSERT (UsbGetCurrentTpl () == TPL_CALLBACK);
+
+    gBS->RaiseTPL (OldTpl);
+  }
+  
+  return Status;
+}
+
+
+/**
+  Remove the current device configuration.
+
+  @param  Device                The USB device to remove configuration from.
+
+**/
+EFI_STATUS
+UsbRemoveConfig (
+  IN USB_DEVICE           *Device
+  )
+{
+  USB_INTERFACE           *UsbIf;
+  UINTN                   Index;
+  EFI_STATUS              Status;
+  EFI_STATUS              ReturnStatus;
+
+  //
+  // Remove each interface of the device
+  //
+  ReturnStatus = EFI_SUCCESS;
+  for (Index = 0; Index < Device->NumOfInterface; Index++) {    
+    ASSERT (Index < USB_MAX_INTERFACE);
+    UsbIf = Device->Interfaces[Index];
+
+    if (UsbIf == NULL) {
+      continue;
+    }
+
+    Status = UsbDisconnectDriver (UsbIf);
+    if (!EFI_ERROR (Status)) {
+      UsbFreeInterface (UsbIf);
+      Device->Interfaces[Index] = NULL;
+    } else {
+      ReturnStatus = Status;
+    }
+  }
+
+  Device->ActiveConfig    = NULL;
+  return ReturnStatus;
+}
+
+
+/**
+  Remove the device and all its children from the bus.
+
+  @param  Device                The device to remove.
+
+  @retval EFI_SUCCESS           The device is removed.
+
+**/
+EFI_STATUS
+UsbRemoveDevice (
+  IN USB_DEVICE           *Device
+  )
+{
+  USB_BUS                 *Bus;
+  USB_DEVICE              *Child;
+  EFI_STATUS              Status;
+  EFI_STATUS              ReturnStatus;
+  UINTN                   Index;
+
+  Bus = Device->Bus;
+
+  //
+  // Remove all the devices on its downstream ports. Search from devices[1].
+  // Devices[0] is the root hub.
+  //
+  ReturnStatus = EFI_SUCCESS;
+  for (Index = 1; Index < Bus->MaxDevices; Index++) {
+    Child = Bus->Devices[Index];
+
+    if ((Child == NULL) || (Child->ParentAddr != Device->Address)) {
+      continue;
+    }
+
+    Status = UsbRemoveDevice (Child);
+
+    if (!EFI_ERROR (Status)) {
+      Bus->Devices[Index] = NULL;
+    } else {
+      Bus->Devices[Index]->DisconnectFail = TRUE;
+      ReturnStatus = Status;
+      DEBUG ((EFI_D_INFO, "UsbRemoveDevice: failed to remove child %p at parent %p\n", Child, Device));
+    }
+  }
+
+  if (EFI_ERROR (ReturnStatus)) {
+    return ReturnStatus;
+  }
+
+  Status = UsbRemoveConfig (Device);
+
+  if (!EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_INFO, "UsbRemoveDevice: device %d removed\n", Device->Address));
+
+    ASSERT (Device->Address < Bus->MaxDevices);
+    Bus->Devices[Device->Address] = NULL;
+    UsbFreeDevice (Device);
+  } else {
+    Bus->Devices[Device->Address]->DisconnectFail = TRUE;
+  }
+  return Status;
+}
+
+
+/**
+  Find the child device on the hub's port.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port of the hub this child is connected to.
+
+  @return The device on the hub's port, or NULL if there is none.
+
+**/
+USB_DEVICE *
+UsbFindChild (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  USB_DEVICE              *Device;
+  USB_BUS                 *Bus;
+  UINTN                   Index;
+
+  Bus = HubIf->Device->Bus;
+
+  //
+  // Start checking from device 1, device 0 is the root hub
+  //
+  for (Index = 1; Index < Bus->MaxDevices; Index++) {
+    Device = Bus->Devices[Index];
+
+    if ((Device != NULL) && (Device->ParentAddr == HubIf->Device->Address) &&
+        (Device->ParentPort == Port)) {
+
+      return Device;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Enumerate and configure the new device on the port of this HUB interface.
+
+  @param  HubIf                 The HUB that has the device connected.
+  @param  Port                  The port index of the hub (started with zero).
+
+  @retval EFI_SUCCESS           The device is enumerated (added or removed).
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the device.
+  @retval Others                Failed to enumerate the device.
+
+**/
+EFI_STATUS
+UsbEnumerateNewDev (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  USB_BUS                 *Bus;
+  USB_HUB_API             *HubApi;
+  USB_DEVICE              *Child;
+  USB_DEVICE              *Parent;
+  EFI_USB_PORT_STATUS     PortState;
+  UINTN                   Address;
+  UINT8                   Config;
+  EFI_STATUS              Status;
+
+  Parent  = HubIf->Device;
+  Bus     = Parent->Bus;
+  HubApi  = HubIf->HubApi;  
+  Address = Bus->MaxDevices;
+
+  gBS->Stall (USB_WAIT_PORT_STABLE_STALL);
+  
+  //
+  // Hub resets the device for at least 10 milliseconds.
+  // Host learns device speed. If device is of low/full speed
+  // and the hub is a EHCI root hub, ResetPort will release
+  // the device to its companion UHCI and return an error.
+  //
+  Status = HubApi->ResetPort (HubIf, Port);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to reset port %d - %r\n", Port, Status));
+
+    return Status;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: hub port %d is reset\n", Port));
+
+  Child = UsbCreateDevice (HubIf, Port);
+
+  if (Child == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // OK, now identify the device speed. After reset, hub
+  // fully knows the actual device speed.
+  //
+  Status = HubApi->GetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to get speed of port %d\n", Port));
+    goto ON_ERROR;
+  }
+
+  if (!USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_CONNECTION)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: No device present at port %d\n", Port));
+    goto ON_ERROR;
+  } else if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_SUPER_SPEED)){
+    Child->Speed      = EFI_USB_SPEED_SUPER;
+    Child->MaxPacket0 = 512;
+  } else if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_HIGH_SPEED)) {
+    Child->Speed      = EFI_USB_SPEED_HIGH;
+    Child->MaxPacket0 = 64;
+  } else if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_LOW_SPEED)) {
+    Child->Speed      = EFI_USB_SPEED_LOW;
+    Child->MaxPacket0 = 8;
+  } else {
+    Child->Speed      = EFI_USB_SPEED_FULL;
+    Child->MaxPacket0 = 8;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: device is of %d speed\n", Child->Speed));
+
+  if (((Child->Speed == EFI_USB_SPEED_LOW) || (Child->Speed == EFI_USB_SPEED_FULL)) &&
+      (Parent->Speed == EFI_USB_SPEED_HIGH)) {
+    //
+    // If the child is a low or full speed device, it is necessary to
+    // set the transaction translator. Port TT is 1-based.
+    // This is quite simple:
+    //  1. if parent is of high speed, then parent is our translator
+    //  2. otherwise use parent's translator.
+    //
+    Child->Translator.TranslatorHubAddress  = Parent->Address;
+    Child->Translator.TranslatorPortNumber  = (UINT8) (Port + 1);
+  } else {
+    Child->Translator = Parent->Translator;
+  }
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: device uses translator (%d, %d)\n",
+           Child->Translator.TranslatorHubAddress,
+           Child->Translator.TranslatorPortNumber));
+
+  //
+  // After port is reset, hub establishes a signal path between
+  // the device and host (DEFALUT state). Device's registers are
+  // reset, use default address 0 (host enumerates one device at
+  // a time) , and ready to respond to control transfer at EP 0.
+  //
+
+  //
+  // Host assigns an address to the device. Device completes the
+  // status stage with default address, then switches to new address.
+  // ADDRESS state. Address zero is reserved for root hub.
+  //
+  ASSERT (Bus->MaxDevices <= 256);
+  for (Address = 1; Address < Bus->MaxDevices; Address++) {
+    if (Bus->Devices[Address] == NULL) {
+      break;
+    }
+  }
+
+  if (Address >= Bus->MaxDevices) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: address pool is full for port %d\n", Port));
+
+    Status = EFI_ACCESS_DENIED;
+    goto ON_ERROR;
+  }
+
+  Status                = UsbSetAddress (Child, (UINT8)Address);
+  Child->Address        = (UINT8)Address;
+  Bus->Devices[Address] = Child;
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to set device address - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  gBS->Stall (USB_SET_DEVICE_ADDRESS_STALL);
+
+  DEBUG ((EFI_D_INFO, "UsbEnumerateNewDev: device is now ADDRESSED at %d\n", Address));
+
+  //
+  // Host sends a Get_Descriptor request to learn the max packet
+  // size of default pipe (only part of the device's descriptor).
+  //
+  Status = UsbGetMaxPacketSize0 (Child);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to get max packet for EP 0 - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: max packet size for EP 0 is %d\n", Child->MaxPacket0));
+
+  //
+  // Host learns about the device's abilities by requesting device's
+  // entire descriptions.
+  //
+  Status = UsbBuildDescTable (Child);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to build descriptor table - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  //
+  // Select a default configuration: UEFI must set the configuration
+  // before the driver can connect to the device.
+  //
+  Config = Child->DevDesc->Configs[0]->Desc.ConfigurationValue;
+  Status = UsbSetConfig (Child, Config);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to set configure %d - %r\n", Config, Status));
+    goto ON_ERROR;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: device %d is now in CONFIGED state\n", Address));
+
+  //
+  // Host assigns and loads a device driver.
+  //
+  Status = UsbSelectConfig (Child, Config);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to create interfaces - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  //
+  // Report Status Code to indicate USB device has been detected by hotplug
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_USB | EFI_IOB_PC_HOTPLUG),
+    Bus->DevicePath
+    );
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  //
+  // If reach here, it means the enumeration process on a given port is interrupted due to error.
+  // The s/w resources, including the assigned address(Address) and the allocated usb device data
+  // structure(Bus->Devices[Address]), will NOT be freed here. These resources will be freed when
+  // the device is unplugged from the port or DriverBindingStop() is invoked.
+  //
+  // This way is used to co-work with the lower layer EDKII UHCI/EHCI/XHCI host controller driver.
+  // It's mainly because to keep UEFI spec unchanged EDKII XHCI driver have to maintain a state machine
+  // to keep track of the mapping between actual address and request address. If the request address
+  // (Address) is freed here, the Address value will be used by next enumerated device. Then EDKII XHCI
+  // host controller driver will have wrong information, which will cause further transaction error.
+  //
+  // EDKII UHCI/EHCI doesn't get impacted as it's make sense to reserve s/w resource till it gets unplugged.
+  //
+  return Status;
+}
+
+
+/**
+  Process the events on the port.
+
+  @param  HubIf                 The HUB that has the device connected.
+  @param  Port                  The port index of the hub (started with zero).
+
+  @retval EFI_SUCCESS           The device is enumerated (added or removed).
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the device.
+  @retval Others                Failed to enumerate the device.
+
+**/
+EFI_STATUS
+UsbEnumeratePort (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  USB_HUB_API             *HubApi;
+  USB_DEVICE              *Child;
+  EFI_USB_PORT_STATUS     PortState;
+  EFI_STATUS              Status;
+
+  Child   = NULL;
+  HubApi  = HubIf->HubApi;
+
+  //
+  // Host learns of the new device by polling the hub for port changes.
+  //
+  Status = HubApi->GetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumeratePort: failed to get state of port %d\n", Port));
+    return Status;
+  }
+
+  //
+  // Only handle connection/enable/overcurrent/reset change.
+  // Usb super speed hub may report other changes, such as warm reset change. Ignore them.
+  //
+  if ((PortState.PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumeratePort: port %d state - %02x, change - %02x on %p\n",
+              Port, PortState.PortStatus, PortState.PortChangeStatus, HubIf));
+
+  //
+  // This driver only process two kinds of events now: over current and
+  // connect/disconnect. Other three events are: ENABLE, SUSPEND, RESET.
+  // ENABLE/RESET is used to reset port. SUSPEND isn't supported.
+  //
+  
+  if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_OVERCURRENT)) {     
+
+    if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_OVERCURRENT)) {
+      //
+      // Case1:
+      //   Both OverCurrent and OverCurrentChange set, means over current occurs, 
+      //   which probably is caused by short circuit. It has to wait system hardware
+      //   to perform recovery.
+      //
+      DEBUG (( EFI_D_ERROR, "UsbEnumeratePort: Critical Over Current\n", Port));
+      return EFI_DEVICE_ERROR;
+      
+    } 
+    //
+    // Case2:
+    //   Only OverCurrentChange set, means system has been recoveried from 
+    //   over current. As a result, all ports are nearly power-off, so
+    //   it's necessary to detach and enumerate all ports again. 
+    //
+    DEBUG (( EFI_D_ERROR, "UsbEnumeratePort: 2.0 device Recovery Over Current\n", Port)); 
+  }
+
+  if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_ENABLE)) {  
+    //
+    // Case3:
+    //   1.1 roothub port reg doesn't reflect over-current state, while its counterpart
+    //   on 2.0 roothub does. When over-current has influence on 1.1 device, the port 
+    //   would be disabled, so it's also necessary to detach and enumerate again.
+    //
+    DEBUG (( EFI_D_ERROR, "UsbEnumeratePort: 1.1 device Recovery Over Current\n", Port));
+  }
+  
+  if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_CONNECTION)) {
+    //
+    // Case4:
+    //   Device connected or disconnected normally. 
+    //
+    DEBUG ((EFI_D_INFO, "UsbEnumeratePort: Device Connect/Disconnect Normally\n", Port));
+  }
+
+  // 
+  // Following as the above cases, it's safety to remove and create again.
+  //
+  Child = UsbFindChild (HubIf, Port);
+  
+  if (Child != NULL) {
+    DEBUG (( EFI_D_INFO, "UsbEnumeratePort: device at port %d removed from root hub %p\n", Port, HubIf));
+    UsbRemoveDevice (Child);
+  }
+  
+  if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_CONNECTION)) {
+    //
+    // Now, new device connected, enumerate and configure the device 
+    //
+    DEBUG (( EFI_D_INFO, "UsbEnumeratePort: new device connected at port %d\n", Port));
+    Status = UsbEnumerateNewDev (HubIf, Port);
+  
+  } else {
+    DEBUG (( EFI_D_INFO, "UsbEnumeratePort: device disconnected event on port %d\n", Port));
+  }
+  
+  HubApi->ClearPortChange (HubIf, Port);
+  return Status;
+}
+
+
+/**
+  Enumerate all the changed hub ports.
+
+  @param  Event                 The event that is triggered.
+  @param  Context               The context to the event.
+
+**/
+VOID
+EFIAPI
+UsbHubEnumeration (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  USB_INTERFACE           *HubIf;
+  UINT8                   Byte;
+  UINT8                   Bit;
+  UINT8                   Index;
+  USB_DEVICE              *Child;
+  
+  ASSERT (Context != NULL);
+
+  HubIf = (USB_INTERFACE *) Context;
+
+  for (Index = 0; Index < HubIf->NumOfPort; Index++) {
+    Child = UsbFindChild (HubIf, Index);
+    if ((Child != NULL) && (Child->DisconnectFail == TRUE)) {
+      DEBUG (( EFI_D_INFO, "UsbEnumeratePort: The device disconnect fails at port %d from hub %p, try again\n", Index, HubIf));
+      UsbRemoveDevice (Child);
+    }
+  }
+
+  if (HubIf->ChangeMap == NULL) {
+    return ;
+  }
+
+  //
+  // HUB starts its port index with 1.
+  //
+  Byte  = 0;
+  Bit   = 1;
+
+  for (Index = 0; Index < HubIf->NumOfPort; Index++) {
+    if (USB_BIT_IS_SET (HubIf->ChangeMap[Byte], USB_BIT (Bit))) {
+      UsbEnumeratePort (HubIf, Index);
+    }
+
+    USB_NEXT_BIT (Byte, Bit);
+  }
+
+  UsbHubAckHubStatus (HubIf->Device);
+
+  gBS->FreePool (HubIf->ChangeMap);
+  HubIf->ChangeMap = NULL;
+  return ;
+}
+
+
+/**
+  Enumerate all the changed hub ports.
+
+  @param  Event                 The event that is triggered.
+  @param  Context               The context to the event.
+
+**/
+VOID
+EFIAPI
+UsbRootHubEnumeration (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  USB_INTERFACE           *RootHub;
+  UINT8                   Index;
+  USB_DEVICE              *Child;
+
+  RootHub = (USB_INTERFACE *) Context;
+
+  for (Index = 0; Index < RootHub->NumOfPort; Index++) {
+    Child = UsbFindChild (RootHub, Index);
+    if ((Child != NULL) && (Child->DisconnectFail == TRUE)) {
+      DEBUG (( EFI_D_INFO, "UsbEnumeratePort: The device disconnect fails at port %d from root hub %p, try again\n", Index, RootHub));
+      UsbRemoveDevice (Child);
+    }
+    
+    UsbEnumeratePort (RootHub, Index);
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.h
new file mode 100644
index 0000000000..ef7d440917
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.h
@@ -0,0 +1,203 @@
+/** @file
+
+    USB bus enumeration interface.
+
+Copyright (c) 2007, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _USB_ENUMERATION_H_
+#define _USB_ENUMERATION_H_
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define USB_NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+
+//
+// Common interface used by usb bus enumeration process.
+// This interface is defined to mask the difference between
+// the root hub and normal hub. So, bus enumeration code
+// can be shared by both root hub and normal hub
+//
+typedef
+EFI_STATUS
+(*USB_HUB_INIT) (
+  IN USB_INTERFACE        *UsbIf
+  );
+
+//
+// Get the port status. This function is required to
+// ACK the port change bits although it will return
+// the port changes in PortState. Bus enumeration code
+// doesn't need to ACK the port change bits.
+//
+typedef
+EFI_STATUS
+(*USB_HUB_GET_PORT_STATUS) (
+  IN  USB_INTERFACE       *UsbIf,
+  IN  UINT8               Port,
+  OUT EFI_USB_PORT_STATUS *PortState
+  );
+
+typedef
+VOID
+(*USB_HUB_CLEAR_PORT_CHANGE) (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  );
+
+typedef
+EFI_STATUS
+(*USB_HUB_SET_PORT_FEATURE) (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  );
+
+typedef
+EFI_STATUS
+(*USB_HUB_CLEAR_PORT_FEATURE) (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  );
+
+typedef
+EFI_STATUS
+(*USB_HUB_RESET_PORT) (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                Port
+  );
+
+typedef
+EFI_STATUS
+(*USB_HUB_RELEASE) (
+  IN USB_INTERFACE        *UsbIf
+  );
+
+/**
+  Return the endpoint descriptor in this interface.
+
+  @param  UsbIf                 The interface to search in.
+  @param  EpAddr                The address of the endpoint to return.
+
+  @return The endpoint descriptor or NULL.
+
+**/
+USB_ENDPOINT_DESC*
+UsbGetEndpointDesc (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                EpAddr
+  );
+
+/**
+  Select an alternate setting for the interface.
+  Each interface can have several mutually exclusive
+  settings. Only one setting is active. It will
+  also reset its endpoints' toggle to zero.
+
+  @param  IfDesc                The interface descriptor to set.
+  @param  Alternate             The alternate setting number to locate.
+
+  @retval EFI_NOT_FOUND         There is no setting with this alternate index.
+  @retval EFI_SUCCESS           The interface is set to Alternate setting.
+
+**/
+EFI_STATUS
+UsbSelectSetting (
+  IN USB_INTERFACE_DESC   *IfDesc,
+  IN UINT8                Alternate
+  );
+
+/**
+  Select a new configuration for the device. Each
+  device may support several configurations.
+
+  @param  Device                The device to select configuration.
+  @param  ConfigIndex           The index of the configuration ( != 0).
+
+  @retval EFI_NOT_FOUND         There is no configuration with the index.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource.
+  @retval EFI_SUCCESS           The configuration is selected.
+
+**/
+EFI_STATUS
+UsbSelectConfig (
+  IN USB_DEVICE           *Device,
+  IN UINT8                ConfigIndex
+  );
+
+/**
+  Remove the current device configuration.
+
+  @param  Device                The USB device to remove configuration from.
+
+  @return None.
+
+**/
+EFI_STATUS
+UsbRemoveConfig (
+  IN USB_DEVICE           *Device
+  );
+
+/**
+  Remove the device and all its children from the bus.
+
+  @param  Device                The device to remove.
+
+  @retval EFI_SUCCESS           The device is removed.
+
+**/
+EFI_STATUS
+UsbRemoveDevice (
+  IN USB_DEVICE           *Device
+  );
+
+/**
+  Enumerate all the changed hub ports.
+
+  @param  Event                 The event that is triggered.
+  @param  Context               The context to the event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+UsbHubEnumeration (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+
+/**
+  Enumerate all the changed hub ports.
+
+  @param  Event                 The event that is triggered.
+  @param  Context               The context to the event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+UsbRootHubEnumeration (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.c
new file mode 100644
index 0000000000..e3752d1f83
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.c
@@ -0,0 +1,1411 @@
+/** @file
+
+    Unified interface for RootHub and Hub.
+
+Copyright (c) 2007 - 2012, 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 "UsbBus.h"
+
+//
+// Array that maps the change bit to feature value which is
+// used to clear these change bit. USB HUB API will clear
+// these change bit automatically. For non-root hub, these
+// bits determine whether hub will report the port in changed
+// bit maps.
+//
+USB_CHANGE_FEATURE_MAP  mHubFeatureMap[] = {
+  {USB_PORT_STAT_C_CONNECTION,  EfiUsbPortConnectChange},
+  {USB_PORT_STAT_C_ENABLE,      EfiUsbPortEnableChange},
+  {USB_PORT_STAT_C_SUSPEND,     EfiUsbPortSuspendChange},
+  {USB_PORT_STAT_C_OVERCURRENT, EfiUsbPortOverCurrentChange},
+  {USB_PORT_STAT_C_RESET,       EfiUsbPortResetChange}
+};
+
+USB_CHANGE_FEATURE_MAP  mRootHubFeatureMap[] = {
+  {USB_PORT_STAT_C_CONNECTION,  EfiUsbPortConnectChange},
+  {USB_PORT_STAT_C_ENABLE,      EfiUsbPortEnableChange},
+  {USB_PORT_STAT_C_SUSPEND,     EfiUsbPortSuspendChange},
+  {USB_PORT_STAT_C_OVERCURRENT, EfiUsbPortOverCurrentChange},
+  {USB_PORT_STAT_C_RESET,       EfiUsbPortResetChange},
+};
+
+//
+// USB hub class specific requests. Although USB hub
+// is related to an interface, these requests are sent
+// to the control endpoint of the device.
+//
+/**
+  USB hub control transfer to set the hub depth.
+
+  @param  HubDev                The device of the hub.
+  @param  Depth                 The depth to set.
+
+  @retval EFI_SUCCESS           Depth of the hub is set.
+  @retval Others                Failed to set the depth.
+
+**/
+EFI_STATUS
+UsbHubCtrlSetHubDepth (
+  IN  USB_DEVICE          *HubDev,
+  IN  UINT16              Depth
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_SET_DEPTH,
+             Depth,
+             0,
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+/**
+  USB hub control transfer to clear the hub feature.
+
+  @param  HubDev                The device of the hub.
+  @param  Feature               The feature to clear.
+
+  @retval EFI_SUCCESS           Feature of the hub is cleared.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbHubCtrlClearHubFeature (
+  IN USB_DEVICE           *HubDev,
+  IN UINT16               Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_CLEAR_FEATURE,
+             Feature,
+             0,
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Clear the feature of the device's port.
+
+  @param  HubDev                The hub device.
+  @param  Port                  The port to clear feature.
+  @param  Feature               The feature to clear.
+
+  @retval EFI_SUCCESS           The feature of the port is cleared.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbHubCtrlClearPortFeature (
+  IN USB_DEVICE           *HubDev,
+  IN UINT8                Port,
+  IN UINT16               Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // In USB bus, all the port index starts from 0. But HUB
+  // indexes its port from 1. So, port number is added one.
+  //
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_PORT,
+             USB_HUB_REQ_CLEAR_FEATURE,
+             Feature,
+             (UINT16) (Port + 1),
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Clear the transaction translate buffer if full/low
+  speed control/bulk transfer failed and the transfer
+  uses this hub as translator.Remember to clear the TT
+  buffer of transaction translator, not that of the
+  parent.
+
+  @param  HubDev                The hub device.
+  @param  Port                  The port of the hub.
+  @param  DevAddr               Address of the failed transaction.
+  @param  EpNum                 The endpoint number of the failed transaction.
+  @param  EpType                The type of failed transaction.
+
+  @retval EFI_SUCCESS           The TT buffer is cleared.
+  @retval Others                Failed to clear the TT buffer.
+
+**/
+EFI_STATUS
+UsbHubCtrlClearTTBuffer (
+  IN USB_DEVICE           *HubDev,
+  IN UINT8                Port,
+  IN UINT16               DevAddr,
+  IN UINT16               EpNum,
+  IN UINT16               EpType
+  )
+{
+  EFI_STATUS              Status;
+  UINT16                  Value;
+
+  //
+  // Check USB2.0 spec page 424 for wValue's encoding
+  //
+  Value = (UINT16) ((EpNum & 0x0F) | (DevAddr << 4) |
+          ((EpType & 0x03) << 11) | ((EpNum & 0x80) << 15));
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_PORT,
+             USB_HUB_REQ_CLEAR_TT,
+             Value,
+             (UINT16) (Port + 1),
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+/**
+  Usb hub control transfer to get the super speed hub descriptor.
+
+  @param  HubDev                The hub device.
+  @param  Buf                   The buffer to hold the descriptor.
+
+  @retval EFI_SUCCESS           The hub descriptor is retrieved.
+  @retval Others                Failed to retrieve the hub descriptor.
+
+**/
+EFI_STATUS
+UsbHubCtrlGetSuperSpeedHubDesc (
+  IN  USB_DEVICE          *HubDev,
+  OUT VOID                *Buf
+  )
+{
+  EFI_STATUS              Status;
+  
+  Status = EFI_INVALID_PARAMETER;
+  
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_GET_DESC,
+             (UINT16) (USB_DESC_TYPE_HUB_SUPER_SPEED << 8),
+             0,
+             Buf,
+             32
+             );
+
+  return Status;
+}
+
+/**
+  Usb hub control transfer to get the hub descriptor.
+
+  @param  HubDev                The hub device.
+  @param  Buf                   The buffer to hold the descriptor.
+  @param  Len                   The length to retrieve.
+
+  @retval EFI_SUCCESS           The hub descriptor is retrieved.
+  @retval Others                Failed to retrieve the hub descriptor.
+
+**/
+EFI_STATUS
+UsbHubCtrlGetHubDesc (
+  IN  USB_DEVICE          *HubDev,
+  OUT VOID                *Buf,
+  IN  UINTN               Len
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_GET_DESC,
+             (UINT16) (USB_DESC_TYPE_HUB << 8),
+             0,
+             Buf,
+             Len
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to get the hub status.
+
+  @param  HubDev                The hub device.
+  @param  State                 The variable to return the status.
+
+  @retval EFI_SUCCESS           The hub status is returned in State.
+  @retval Others                Failed to get the hub status.
+
+**/
+EFI_STATUS
+UsbHubCtrlGetHubStatus (
+  IN  USB_DEVICE          *HubDev,
+  OUT UINT32              *State
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_GET_STATUS,
+             0,
+             0,
+             State,
+             4
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to get the port status.
+
+  @param  HubDev                The hub device.
+  @param  Port                  The port of the hub.
+  @param  State                 Variable to return the hub port state.
+
+  @retval EFI_SUCCESS           The port state is returned in State.
+  @retval Others                Failed to retrieve the port state.
+
+**/
+EFI_STATUS
+UsbHubCtrlGetPortStatus (
+  IN  USB_DEVICE          *HubDev,
+  IN  UINT8               Port,
+  OUT VOID                *State
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // In USB bus, all the port index starts from 0. But HUB
+  // indexes its port from 1. So, port number is added one.
+  // No need to convert the hub bit to UEFI definition, they
+  // are the same
+  //
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_PORT,
+             USB_HUB_REQ_GET_STATUS,
+             0,
+             (UINT16) (Port + 1),
+             State,
+             4
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to reset the TT (Transaction Transaltor).
+
+  @param  HubDev                The hub device.
+  @param  Port                  The port of the hub.
+
+  @retval EFI_SUCCESS           The TT of the hub is reset.
+  @retval Others                Failed to reset the port.
+
+**/
+EFI_STATUS
+UsbHubCtrlResetTT (
+  IN  USB_DEVICE          *HubDev,
+  IN  UINT8               Port
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_RESET_TT,
+             0,
+             (UINT16) (Port + 1),
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to set the hub feature.
+
+  @param  HubDev                The hub device.
+  @param  Feature               The feature to set.
+
+  @retval EFI_SUCESS            The feature is set for the hub.
+  @retval Others                Failed to set the feature.
+
+**/
+EFI_STATUS
+UsbHubCtrlSetHubFeature (
+  IN  USB_DEVICE          *HubDev,
+  IN  UINT8               Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_SET_FEATURE,
+             Feature,
+             0,
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to set the port feature.
+
+  @param  HubDev                The Usb hub device.
+  @param  Port                  The Usb port to set feature for.
+  @param  Feature               The feature to set.
+
+  @retval EFI_SUCCESS           The feature is set for the port.
+  @retval Others                Failed to set the feature.
+
+**/
+EFI_STATUS
+UsbHubCtrlSetPortFeature (
+  IN USB_DEVICE           *HubDev,
+  IN UINT8                Port,
+  IN UINT8                Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // In USB bus, all the port index starts from 0. But HUB
+  // indexes its port from 1. So, port number is added one.
+  //
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_PORT,
+             USB_HUB_REQ_SET_FEATURE,
+             Feature,
+             (UINT16) (Port + 1),
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Read the whole usb hub descriptor. It is necessary
+  to do it in two steps because hub descriptor is of
+  variable length.
+
+  @param  HubDev                The hub device.
+  @param  HubDesc               The variable to return the descriptor.
+
+  @retval EFI_SUCCESS           The hub descriptor is read.
+  @retval Others                Failed to read the hub descriptor.
+
+**/
+EFI_STATUS
+UsbHubReadDesc (
+  IN  USB_DEVICE              *HubDev,
+  OUT EFI_USB_HUB_DESCRIPTOR  *HubDesc
+  )
+{
+  EFI_STATUS              Status;
+
+  if (HubDev->Speed == EFI_USB_SPEED_SUPER) {
+    //
+    // Get the super speed hub descriptor
+    //
+    Status = UsbHubCtrlGetSuperSpeedHubDesc (HubDev, HubDesc);
+  } else {
+
+    //
+    // First get the hub descriptor length
+    //
+    Status = UsbHubCtrlGetHubDesc (HubDev, HubDesc, 2);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the whole hub descriptor
+    //
+    Status = UsbHubCtrlGetHubDesc (HubDev, HubDesc, HubDesc->Length);
+  }
+
+  return Status;
+}
+
+
+
+/**
+  Ack the hub change bits. If these bits are not ACKed, Hub will
+  always return changed bit map from its interrupt endpoint.
+
+  @param  HubDev                The hub device.
+
+  @retval EFI_SUCCESS           The hub change status is ACKed.
+  @retval Others                Failed to ACK the hub status.
+
+**/
+EFI_STATUS
+UsbHubAckHubStatus (
+  IN  USB_DEVICE         *HubDev
+  )
+{
+  EFI_USB_PORT_STATUS     HubState;
+  EFI_STATUS              Status;
+
+  Status = UsbHubCtrlGetHubStatus (HubDev, (UINT32 *) &HubState);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (USB_BIT_IS_SET (HubState.PortChangeStatus, USB_HUB_STAT_C_LOCAL_POWER)) {
+    UsbHubCtrlClearHubFeature (HubDev, USB_HUB_C_HUB_LOCAL_POWER);
+  }
+
+  if (USB_BIT_IS_SET (HubState.PortChangeStatus, USB_HUB_STAT_C_OVER_CURRENT)) {
+    UsbHubCtrlClearHubFeature (HubDev, USB_HUB_C_HUB_OVER_CURRENT);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Test whether the interface is a hub interface.
+
+  @param  UsbIf                 The interface to test.
+
+  @retval TRUE                  The interface is a hub interface.
+  @retval FALSE                 The interface isn't a hub interface.
+
+**/
+BOOLEAN
+UsbIsHubInterface (
+  IN USB_INTERFACE        *UsbIf
+  )
+{
+  EFI_USB_INTERFACE_DESCRIPTOR  *Setting;
+
+  //
+  // If the hub is a high-speed hub with multiple TT,
+  // the hub will has a default setting of single TT.
+  //
+  Setting = &UsbIf->IfSetting->Desc;
+
+  if ((Setting->InterfaceClass == USB_HUB_CLASS_CODE) &&
+      (Setting->InterfaceSubClass == USB_HUB_SUBCLASS_CODE)) {
+
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  The callback function to the USB hub status change
+  interrupt endpoint. It is called periodically by
+  the underlying host controller.
+
+  @param  Data                  The data read.
+  @param  DataLength            The length of the data read.
+  @param  Context               The context.
+  @param  Result                The result of the last interrupt transfer.
+
+  @retval EFI_SUCCESS           The process is OK.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbOnHubInterrupt (
+  IN  VOID                *Data,
+  IN  UINTN               DataLength,
+  IN  VOID                *Context,
+  IN  UINT32              Result
+  )
+{
+  USB_INTERFACE               *HubIf;
+  EFI_USB_IO_PROTOCOL         *UsbIo;
+  EFI_USB_ENDPOINT_DESCRIPTOR *EpDesc;
+  EFI_STATUS                  Status;
+
+  HubIf   = (USB_INTERFACE *) Context;
+  UsbIo   = &(HubIf->UsbIo);
+  EpDesc  = &(HubIf->HubEp->Desc);
+
+  if (Result != EFI_USB_NOERROR) {
+    //
+    // If endpoint is stalled, clear the stall. Use UsbIo to access
+    // the control transfer so internal status are maintained.
+    //
+    if (USB_BIT_IS_SET (Result, EFI_USB_ERR_STALL)) {
+      UsbIoClearFeature (
+        UsbIo,
+        USB_TARGET_ENDPOINT,
+        USB_FEATURE_ENDPOINT_HALT,
+        EpDesc->EndpointAddress
+        );
+    }
+
+    //
+    // Delete and submit a new async interrupt
+    //
+    Status = UsbIo->UsbAsyncInterruptTransfer (
+                      UsbIo,
+                      EpDesc->EndpointAddress,
+                      FALSE,
+                      0,
+                      0,
+                      NULL,
+                      NULL
+                      );
+
+    if (EFI_ERROR (Status)) {
+      DEBUG (( EFI_D_ERROR, "UsbOnHubInterrupt: failed to remove async transfer - %r\n", Status));
+      return Status;
+    }
+
+    Status = UsbIo->UsbAsyncInterruptTransfer (
+                      UsbIo,
+                      EpDesc->EndpointAddress,
+                      TRUE,
+                      USB_HUB_POLL_INTERVAL,
+                      HubIf->NumOfPort / 8 + 1,
+                      UsbOnHubInterrupt,
+                      HubIf
+                      );
+
+    if (EFI_ERROR (Status)) {
+      DEBUG (( EFI_D_ERROR, "UsbOnHubInterrupt: failed to submit new async transfer - %r\n", Status));
+    }
+
+    return Status;
+  }
+
+  if ((DataLength == 0) || (Data == NULL)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // OK, actually something is changed, save the change map
+  // then signal the HUB to do enumeration. This is a good
+  // practise since UsbOnHubInterrupt is called in the context
+  // of host contrller's AsyncInterrupt monitor.
+  //
+  HubIf->ChangeMap = AllocateZeroPool (DataLength);
+
+  if (HubIf->ChangeMap == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CopyMem (HubIf->ChangeMap, Data, DataLength);
+  gBS->SignalEvent (HubIf->HubNotify);
+
+  return EFI_SUCCESS;
+}
+
+
+
+
+/**
+  Initialize the device for a non-root hub.
+
+  @param  HubIf                 The USB hub interface.
+
+  @retval EFI_SUCCESS           The hub is initialized.
+  @retval EFI_DEVICE_ERROR      Failed to initialize the hub.
+
+**/
+EFI_STATUS
+UsbHubInit (
+  IN USB_INTERFACE        *HubIf
+  )
+{
+  EFI_USB_HUB_DESCRIPTOR  HubDesc;
+  USB_ENDPOINT_DESC       *EpDesc;
+  USB_INTERFACE_SETTING   *Setting;
+  EFI_USB_IO_PROTOCOL     *UsbIo;
+  USB_DEVICE              *HubDev;
+  EFI_STATUS              Status;
+  UINT8                   Index;
+  UINT8                   NumEndpoints;
+  UINT16                  Depth;
+
+  //
+  // Locate the interrupt endpoint for port change map
+  //
+  HubIf->IsHub  = FALSE;
+  Setting       = HubIf->IfSetting;
+  HubDev        = HubIf->Device;
+  EpDesc        = NULL;
+  NumEndpoints  = Setting->Desc.NumEndpoints;
+
+  for (Index = 0; Index < NumEndpoints; Index++) {
+    ASSERT ((Setting->Endpoints != NULL) && (Setting->Endpoints[Index] != NULL));
+
+    EpDesc = Setting->Endpoints[Index];
+
+    if (USB_BIT_IS_SET (EpDesc->Desc.EndpointAddress, USB_ENDPOINT_DIR_IN) &&
+       (USB_ENDPOINT_TYPE (&EpDesc->Desc) == USB_ENDPOINT_INTERRUPT)) {
+      break;
+    }
+  }
+
+  if (Index == NumEndpoints) {
+    DEBUG (( EFI_D_ERROR, "UsbHubInit: no interrupt endpoint found for hub %d\n", HubDev->Address));
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = UsbHubReadDesc (HubDev, &HubDesc);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbHubInit: failed to read HUB descriptor %r\n", Status));
+    return Status;
+  }
+
+  HubIf->NumOfPort = HubDesc.NumPorts;
+
+  DEBUG (( EFI_D_INFO, "UsbHubInit: hub %d has %d ports\n", HubDev->Address,HubIf->NumOfPort));
+
+  //
+  // OK, set IsHub to TRUE. Now usb bus can handle this device
+  // as a working HUB. If failed eariler, bus driver will not
+  // recognize it as a hub. Other parts of the bus should be able
+  // to work.
+  //
+  HubIf->IsHub  = TRUE;
+  HubIf->HubApi = &mUsbHubApi;
+  HubIf->HubEp  = EpDesc;
+
+  if (HubIf->Device->Speed == EFI_USB_SPEED_SUPER) {
+    Depth = (UINT16)(HubIf->Device->Tier - 1);
+    DEBUG ((EFI_D_INFO, "UsbHubInit: Set Hub Depth as 0x%x\n", Depth));
+    UsbHubCtrlSetHubDepth (HubIf->Device, Depth);
+    
+    for (Index = 0; Index < HubDesc.NumPorts; Index++) {
+      UsbHubCtrlSetPortFeature (HubIf->Device, Index, USB_HUB_PORT_REMOTE_WAKE_MASK);
+    }    
+  } else {
+    //
+    // Feed power to all the hub ports. It should be ok
+    // for both gang/individual powered hubs.
+    //
+    for (Index = 0; Index < HubDesc.NumPorts; Index++) {
+      UsbHubCtrlSetPortFeature (HubIf->Device, Index, (EFI_USB_PORT_FEATURE) USB_HUB_PORT_POWER);
+    }
+
+    //
+    // Update for the usb hub has no power on delay requirement
+    //
+    if (HubDesc.PwrOn2PwrGood > 0) {
+      gBS->Stall (HubDesc.PwrOn2PwrGood * USB_SET_PORT_POWER_STALL);
+    }
+    UsbHubAckHubStatus (HubIf->Device);
+  }
+
+  //
+  // Create an event to enumerate the hub's port. On
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  UsbHubEnumeration,
+                  HubIf,
+                  &HubIf->HubNotify
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbHubInit: failed to create signal for hub %d - %r\n",
+                HubDev->Address, Status));
+
+    return Status;
+  }
+
+  //
+  // Create AsyncInterrupt to query hub port change endpoint
+  // periodically. If the hub ports are changed, hub will return
+  // changed port map from the interrupt endpoint. The port map
+  // must be able to hold (HubIf->NumOfPort + 1) bits (one bit for
+  // host change status).
+  //
+  UsbIo  = &HubIf->UsbIo;
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    EpDesc->Desc.EndpointAddress,
+                    TRUE,
+                    USB_HUB_POLL_INTERVAL,
+                    HubIf->NumOfPort / 8 + 1,
+                    UsbOnHubInterrupt,
+                    HubIf
+                    );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbHubInit: failed to queue interrupt transfer for hub %d - %r\n",
+                HubDev->Address, Status));
+
+    gBS->CloseEvent (HubIf->HubNotify);
+    HubIf->HubNotify = NULL;
+
+    return Status;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbHubInit: hub %d initialized\n", HubDev->Address));
+  return Status;
+}
+
+
+
+/**
+  Get the port status. This function is required to
+  ACK the port change bits although it will return
+  the port changes in PortState. Bus enumeration code
+  doesn't need to ACK the port change bits.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port of the hub to get state.
+  @param  PortState             Variable to return the port state.
+
+  @retval EFI_SUCCESS           The port status is successfully returned.
+  @retval Others                Failed to return the status.
+
+**/
+EFI_STATUS
+UsbHubGetPortStatus (
+  IN  USB_INTERFACE       *HubIf,
+  IN  UINT8               Port,
+  OUT EFI_USB_PORT_STATUS *PortState
+  )
+{
+  EFI_STATUS              Status;
+
+  Status  = UsbHubCtrlGetPortStatus (HubIf->Device, Port, PortState);
+
+  return Status;
+}
+
+
+
+/**
+  Clear the port change status.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The hub port.
+
+**/
+VOID
+UsbHubClearPortChange (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  EFI_USB_PORT_STATUS     PortState;
+  USB_CHANGE_FEATURE_MAP  *Map;
+  UINTN                   Index;
+  EFI_STATUS              Status;
+
+  Status = UsbHubGetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // OK, get the usb port status, now ACK the change bits.
+  // Don't return error when failed to clear the change bits.
+  // It may lead to extra port state report. USB bus should
+  // be able to handle this.
+  //
+  for (Index = 0; Index < sizeof (mHubFeatureMap) / sizeof (mHubFeatureMap[0]); Index++) {
+    Map = &mHubFeatureMap[Index];
+
+    if (USB_BIT_IS_SET (PortState.PortChangeStatus, Map->ChangedBit)) {
+      UsbHubCtrlClearPortFeature (HubIf->Device, Port, (UINT16) Map->Feature);
+    }
+  }
+}
+
+
+
+/**
+  Function to set the port feature for non-root hub.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port of the hub.
+  @param  Feature               The feature of the port to set.
+
+  @retval EFI_SUCCESS           The hub port feature is set.
+  @retval Others                Failed to set the port feature.
+
+**/
+EFI_STATUS
+UsbHubSetPortFeature (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbHubCtrlSetPortFeature (HubIf->Device, Port, (UINT8) Feature);
+
+  return Status;
+}
+
+
+/**
+  Interface function to clear the port feature for non-root hub.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port of the hub to clear feature for.
+  @param  Feature               The feature to clear.
+
+  @retval EFI_SUCCESS           The port feature is cleared.
+  @retval Others                Failed to clear the port feature.
+
+**/
+EFI_STATUS
+UsbHubClearPortFeature (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbHubCtrlClearPortFeature (HubIf->Device, Port, (UINT8) Feature);
+
+  return Status;
+}
+
+
+/**
+  Interface function to reset the port.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port to reset.
+
+  @retval EFI_SUCCESS           The hub port is reset.
+  @retval EFI_TIMEOUT           Failed to reset the port in time.
+  @retval Others                Failed to reset the port.
+
+**/
+EFI_STATUS
+UsbHubResetPort (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  EFI_USB_PORT_STATUS     PortState;
+  UINTN                   Index;
+  EFI_STATUS              Status;
+
+  Status = UsbHubGetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  } else if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_RESET)) {
+    DEBUG (( EFI_D_INFO, "UsbHubResetPort: skip reset on hub %p port %d\n", HubIf, Port));
+    return EFI_SUCCESS;
+  }
+
+  Status  = UsbHubSetPortFeature (HubIf, Port, (EFI_USB_PORT_FEATURE) USB_HUB_PORT_RESET);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Drive the reset signal for worst 20ms. Check USB 2.0 Spec
+  // section 7.1.7.5 for timing requirements.
+  //
+  gBS->Stall (USB_SET_PORT_RESET_STALL);
+
+  //
+  // Check USB_PORT_STAT_C_RESET bit to see if the resetting state is done.
+  //
+  ZeroMem (&PortState, sizeof (EFI_USB_PORT_STATUS));
+
+  for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
+    Status = UsbHubGetPortStatus (HubIf, Port, &PortState);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (!EFI_ERROR (Status) &&
+        USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_RESET)) {
+      gBS->Stall (USB_SET_PORT_RECOVERY_STALL);
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (USB_WAIT_PORT_STS_CHANGE_STALL);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+
+/**
+  Release the hub's control of the interface.
+
+  @param  HubIf                 The hub interface.
+
+  @retval EFI_SUCCESS           The interface is release of hub control.
+
+**/
+EFI_STATUS
+UsbHubRelease (
+  IN USB_INTERFACE        *HubIf
+  )
+{
+  EFI_USB_IO_PROTOCOL     *UsbIo;
+  EFI_STATUS              Status;
+
+  UsbIo  = &HubIf->UsbIo;
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    HubIf->HubEp->Desc.EndpointAddress,
+                    FALSE,
+                    USB_HUB_POLL_INTERVAL,
+                    0,
+                    NULL,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gBS->CloseEvent (HubIf->HubNotify);
+
+  HubIf->IsHub      = FALSE;
+  HubIf->HubApi     = NULL;
+  HubIf->HubEp      = NULL;
+  HubIf->HubNotify  = NULL;
+
+  DEBUG (( EFI_D_INFO, "UsbHubRelease: hub device %d released\n", HubIf->Device->Address));
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Initialize the interface for root hub.
+
+  @param  HubIf                 The root hub interface.
+
+  @retval EFI_SUCCESS           The interface is initialized for root hub.
+  @retval Others                Failed to initialize the hub.
+
+**/
+EFI_STATUS
+UsbRootHubInit (
+  IN USB_INTERFACE        *HubIf
+  )
+{
+  EFI_STATUS              Status;
+  UINT8                   MaxSpeed;
+  UINT8                   NumOfPort;
+  UINT8                   Support64;
+
+  Status = UsbHcGetCapability (HubIf->Device->Bus, &MaxSpeed, &NumOfPort, &Support64);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbRootHubInit: root hub %p - max speed %d, %d ports\n",
+              HubIf, MaxSpeed, NumOfPort));
+
+  HubIf->IsHub      = TRUE;
+  HubIf->HubApi     = &mUsbRootHubApi;
+  HubIf->HubEp      = NULL;
+  HubIf->MaxSpeed   = MaxSpeed;
+  HubIf->NumOfPort  = NumOfPort;
+  HubIf->HubNotify  = NULL;
+
+  //
+  // Create a timer to poll root hub ports periodically
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  UsbRootHubEnumeration,
+                  HubIf,
+                  &HubIf->HubNotify
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // It should signal the event immediately here, or device detection
+  // by bus enumeration might be delayed by the timer interval.
+  //
+  gBS->SignalEvent (HubIf->HubNotify);
+
+  Status = gBS->SetTimer (
+                  HubIf->HubNotify,
+                  TimerPeriodic,
+                  USB_ROOTHUB_POLL_INTERVAL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->CloseEvent (HubIf->HubNotify);
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the port status. This function is required to
+  ACK the port change bits although it will return
+  the port changes in PortState. Bus enumeration code
+  doesn't need to ACK the port change bits.
+
+  @param  HubIf                 The root hub interface.
+  @param  Port                  The root hub port to get the state.
+  @param  PortState             Variable to return the port state.
+
+  @retval EFI_SUCCESS           The port state is returned.
+  @retval Others                Failed to retrieve the port state.
+
+**/
+EFI_STATUS
+UsbRootHubGetPortStatus (
+  IN  USB_INTERFACE       *HubIf,
+  IN  UINT8               Port,
+  OUT EFI_USB_PORT_STATUS *PortState
+  )
+{
+  USB_BUS                 *Bus;
+  EFI_STATUS              Status;
+
+  Bus     = HubIf->Device->Bus;
+  Status  = UsbHcGetRootHubPortStatus (Bus, Port, PortState);
+
+  return Status;
+}
+
+
+/**
+  Clear the port change status.
+
+  @param  HubIf                 The root hub interface.
+  @param  Port                  The root hub port.
+
+**/
+VOID
+UsbRootHubClearPortChange (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  EFI_USB_PORT_STATUS     PortState;
+  USB_CHANGE_FEATURE_MAP  *Map;
+  UINTN                   Index;
+  EFI_STATUS              Status;
+
+  Status = UsbRootHubGetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // OK, get the usb port status, now ACK the change bits.
+  // Don't return error when failed to clear the change bits.
+  // It may lead to extra port state report. USB bus should
+  // be able to handle this.
+  //
+  for (Index = 0; Index < sizeof (mRootHubFeatureMap) / sizeof (mRootHubFeatureMap[0]); Index++) {
+    Map = &mRootHubFeatureMap[Index];
+
+    if (USB_BIT_IS_SET (PortState.PortChangeStatus, Map->ChangedBit)) {
+      UsbHcClearRootHubPortFeature (HubIf->Device->Bus, Port, (EFI_USB_PORT_FEATURE) Map->Feature);
+    }
+  }
+}
+
+
+/**
+  Set the root hub port feature.
+
+  @param  HubIf                 The Usb hub interface.
+  @param  Port                  The hub port.
+  @param  Feature               The feature to set.
+
+  @retval EFI_SUCCESS           The root hub port is set with the feature.
+  @retval Others                Failed to set the feature.
+
+**/
+EFI_STATUS
+UsbRootHubSetPortFeature (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status  = UsbHcSetRootHubPortFeature (HubIf->Device->Bus, Port, Feature);
+
+  return Status;
+}
+
+
+/**
+  Clear the root hub port feature.
+
+  @param  HubIf                 The root hub interface.
+  @param  Port                  The root hub port.
+  @param  Feature               The feature to clear.
+
+  @retval EFI_SUCCESS           The root hub port is cleared of the feature.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbRootHubClearPortFeature (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status  = UsbHcClearRootHubPortFeature (HubIf->Device->Bus, Port, Feature);
+
+  return Status;
+}
+
+
+/**
+  Interface function to reset the root hub port.
+
+  @param  RootIf                The root hub interface.
+  @param  Port                  The port to reset.
+
+  @retval EFI_SUCCESS           The hub port is reset.
+  @retval EFI_TIMEOUT           Failed to reset the port in time.
+  @retval EFI_NOT_FOUND         The low/full speed device connected to high  speed.
+                                root hub is released to the companion UHCI.
+  @retval Others                Failed to reset the port.
+
+**/
+EFI_STATUS
+UsbRootHubResetPort (
+  IN USB_INTERFACE        *RootIf,
+  IN UINT8                Port
+  )
+{
+  USB_BUS                 *Bus;
+  EFI_STATUS              Status;
+  EFI_USB_PORT_STATUS     PortState;
+  UINTN                   Index;
+
+  //
+  // Notice: although EHCI requires that ENABLED bit be cleared
+  // when reset the port, we don't need to care that here. It
+  // should be handled in the EHCI driver.
+  //
+  Bus     = RootIf->Device->Bus;
+
+  Status = UsbHcGetRootHubPortStatus (Bus, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  } else if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_RESET)) {
+    DEBUG (( EFI_D_INFO, "UsbRootHubResetPort: skip reset on root port %d\n", Port));
+    return EFI_SUCCESS;
+  }
+
+  Status  = UsbHcSetRootHubPortFeature (Bus, Port, EfiUsbPortReset);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbRootHubResetPort: failed to start reset on port %d\n", Port));
+    return Status;
+  }
+
+  //
+  // Drive the reset signal for at least 50ms. Check USB 2.0 Spec
+  // section 7.1.7.5 for timing requirements.
+  //
+  gBS->Stall (USB_SET_ROOT_PORT_RESET_STALL);
+
+  Status = UsbHcClearRootHubPortFeature (Bus, Port, EfiUsbPortReset);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbRootHubResetPort: failed to clear reset on port %d\n", Port));
+    return Status;
+  }
+
+  gBS->Stall (USB_CLR_ROOT_PORT_RESET_STALL);
+
+  //
+  // USB host controller won't clear the RESET bit until
+  // reset is actually finished.
+  //
+  ZeroMem (&PortState, sizeof (EFI_USB_PORT_STATUS));
+
+  for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
+    Status = UsbHcGetRootHubPortStatus (Bus, Port, &PortState);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (!USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_RESET)) {
+      break;
+    }
+
+    gBS->Stall (USB_WAIT_PORT_STS_CHANGE_STALL);
+  }
+
+  if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {
+    DEBUG ((EFI_D_ERROR, "UsbRootHubResetPort: reset not finished in time on port %d\n", Port));
+    return EFI_TIMEOUT;
+  }
+
+  if (!USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_ENABLE)) {
+    //
+    // OK, the port is reset. If root hub is of high speed and
+    // the device is of low/full speed, release the ownership to
+    // companion UHCI. If root hub is of full speed, it won't
+    // automatically enable the port, we need to enable it manually.
+    //
+    if (RootIf->MaxSpeed == EFI_USB_SPEED_HIGH) {
+      DEBUG (( EFI_D_ERROR, "UsbRootHubResetPort: release low/full speed device (%d) to UHCI\n", Port));
+
+      UsbRootHubSetPortFeature (RootIf, Port, EfiUsbPortOwner);
+      return EFI_NOT_FOUND;
+
+    } else {
+
+      Status = UsbRootHubSetPortFeature (RootIf, Port, EfiUsbPortEnable);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG (( EFI_D_ERROR, "UsbRootHubResetPort: failed to enable port %d for UHCI\n", Port));
+        return Status;
+      }
+
+      gBS->Stall (USB_SET_ROOT_PORT_ENABLE_STALL);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Release the root hub's control of the interface.
+
+  @param  HubIf                 The root hub interface.
+
+  @retval EFI_SUCCESS           The root hub's control of the interface is
+                                released.
+
+**/
+EFI_STATUS
+UsbRootHubRelease (
+  IN USB_INTERFACE        *HubIf
+  )
+{
+  DEBUG (( EFI_D_INFO, "UsbRootHubRelease: root hub released for hub %p\n", HubIf));
+
+  gBS->SetTimer (HubIf->HubNotify, TimerCancel, USB_ROOTHUB_POLL_INTERVAL);
+  gBS->CloseEvent (HubIf->HubNotify);
+
+  return EFI_SUCCESS;
+}
+
+USB_HUB_API mUsbHubApi = {
+  UsbHubInit,
+  UsbHubGetPortStatus,
+  UsbHubClearPortChange,
+  UsbHubSetPortFeature,
+  UsbHubClearPortFeature,
+  UsbHubResetPort,
+  UsbHubRelease
+};
+
+USB_HUB_API mUsbRootHubApi = {
+  UsbRootHubInit,
+  UsbRootHubGetPortStatus,
+  UsbRootHubClearPortChange,
+  UsbRootHubSetPortFeature,
+  UsbRootHubClearPortFeature,
+  UsbRootHubResetPort,
+  UsbRootHubRelease
+};
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.h
new file mode 100644
index 0000000000..4e5fcd85e0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.h
@@ -0,0 +1,199 @@
+/** @file
+
+    The definition for USB hub.
+
+Copyright (c) 2007 - 2010, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _USB_HUB_H_
+#define _USB_HUB_H_
+
+#include <IndustryStandard/Usb.h>
+
+#define USB_ENDPOINT_ADDR(EpAddr) ((EpAddr) & 0x7F)
+#define USB_ENDPOINT_TYPE(Desc)   ((Desc)->Attributes & USB_ENDPOINT_TYPE_MASK)
+
+
+#define USB_DESC_TYPE_HUB     0x29
+
+#define USB_DESC_TYPE_HUB_SUPER_SPEED  0x2a
+
+//
+// Hub class control transfer target
+//
+#define USB_HUB_TARGET_HUB    0
+#define USB_HUB_TARGET_PORT   3
+//
+// HUB class specific contrl transfer request type
+//
+#define USB_HUB_REQ_GET_STATUS      0
+#define USB_HUB_REQ_CLEAR_FEATURE   1
+#define USB_HUB_REQ_SET_FEATURE     3
+#define USB_HUB_REQ_GET_DESC        6
+#define USB_HUB_REQ_SET_DESC        7
+#define USB_HUB_REQ_CLEAR_TT        8
+#define USB_HUB_REQ_RESET_TT        9
+#define USB_HUB_REQ_GET_TT_STATE    10
+#define USB_HUB_REQ_STOP_TT         11
+
+#define USB_HUB_REQ_SET_DEPTH       12
+
+//
+// USB hub class feature selector
+//
+#define USB_HUB_C_HUB_LOCAL_POWER   0
+#define USB_HUB_C_HUB_OVER_CURRENT  1
+#define USB_HUB_PORT_CONNECTION     0
+#define USB_HUB_PORT_ENABLE         1
+#define USB_HUB_PORT_SUSPEND        2
+#define USB_HUB_PORT_OVER_CURRENT   3
+#define USB_HUB_PORT_RESET          4
+
+#define USB_HUB_PORT_LINK_STATE     5
+
+#define USB_HUB_PORT_POWER          8
+#define USB_HUB_PORT_LOW_SPEED      9
+#define USB_HUB_C_PORT_CONNECT      16
+#define USB_HUB_C_PORT_ENABLE       17
+#define USB_HUB_C_PORT_SUSPEND      18
+#define USB_HUB_C_PORT_OVER_CURRENT 19
+#define USB_HUB_C_PORT_RESET        20
+#define USB_HUB_PORT_TEST           21
+#define USB_HUB_PORT_INDICATOR      22
+
+#define USB_HUB_C_PORT_LINK_STATE     25
+#define USB_HUB_PORT_REMOTE_WAKE_MASK 27
+#define USB_HUB_BH_PORT_RESET         28
+#define USB_HUB_C_BH_PORT_RESET       29
+
+//
+// Constant value for Port Status & Port Change Status of SuperSpeed port
+//
+#define USB_SS_PORT_STAT_C_BH_RESET         0x0020
+#define USB_SS_PORT_STAT_C_PORT_LINK_STATE  0x0040
+//
+// USB hub power control method. In gang power control
+//
+#define USB_HUB_GANG_POWER_CTRL     0
+#define USB_HUB_PORT_POWER_CTRL     0x01
+//
+// USB hub status bits
+//
+#define USB_HUB_STAT_LOCAL_POWER    0x01
+#define USB_HUB_STAT_OVER_CURRENT   0x02
+#define USB_HUB_STAT_C_LOCAL_POWER  0x01
+#define USB_HUB_STAT_C_OVER_CURRENT 0x02
+
+#define USB_HUB_CLASS_CODE          0x09
+#define USB_HUB_SUBCLASS_CODE       0x00
+
+//
+// Host software return timeout if port status doesn't change 
+// after 500ms(LOOP * STALL = 5000 * 0.1ms), set by experience
+//
+#define USB_WAIT_PORT_STS_CHANGE_LOOP  5000
+
+#pragma pack(1)
+//
+// Hub descriptor, the last two fields are of variable lenght.
+//
+typedef struct {
+  UINT8           Length;
+  UINT8           DescType;
+  UINT8           NumPorts;
+  UINT16          HubCharacter;
+  UINT8           PwrOn2PwrGood;
+  UINT8           HubContrCurrent;
+  UINT8           Filler[16];
+} EFI_USB_HUB_DESCRIPTOR;
+
+typedef struct {
+  UINT8           Length;
+  UINT8           DescType;
+  UINT8           NumPorts;
+  UINT16          HubCharacter;
+  UINT8           PwrOn2PwrGood;
+  UINT8           HubContrCurrent;
+  UINT8           HubHdrDecLat;
+  UINT8           HubDelay;
+  UINT8           DeviceRemovable;
+} EFI_USB_SUPER_SPEED_HUB_DESCRIPTOR;
+
+#pragma pack()
+
+
+typedef struct {
+  UINT16                ChangedBit;
+  EFI_USB_PORT_FEATURE  Feature;
+} USB_CHANGE_FEATURE_MAP;
+
+
+/**
+  Clear the transaction translate buffer if full/low
+  speed control/bulk transfer failed and the transfer
+  uses this hub as translator.Remember to clear the TT
+  buffer of transaction translator, not that of the
+  parent.
+
+  @param  UsbDev                The Usb device.
+  @param  Port                  The port of the hub.
+  @param  DevAddr               Address of the failed transaction.
+  @param  EpNum                 The endpoint number of the failed transaction.
+  @param  EpType                The type of failed transaction.
+
+  @retval EFI_SUCCESS           The TT buffer is cleared.
+  @retval Others                Failed to clear the TT buffer.
+
+**/
+EFI_STATUS
+UsbHubCtrlClearTTBuffer (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                Port,
+  IN UINT16               DevAddr,
+  IN UINT16               EpNum,
+  IN UINT16               EpType
+  );
+
+
+/**
+  Test whether the interface is a hub interface.
+
+  @param  UsbIf                 The interface to test.
+
+  @retval TRUE                  The interface is a hub interface.
+  @retval FALSE                 The interface isn't a hub interface.
+
+**/
+BOOLEAN
+UsbIsHubInterface (
+  IN USB_INTERFACE        *UsbIf
+  );
+
+
+/**
+  Ack the hub change bits. If these bits are not ACKed, Hub will
+  always return changed bit map from its interrupt endpoint.
+
+  @param  UsbDev                The Usb device.
+
+  @retval EFI_SUCCESS           The hub change status is ACKed.
+  @retval Others                Failed to ACK the hub status.
+
+**/
+EFI_STATUS
+UsbHubAckHubStatus (
+  IN  USB_DEVICE         *UsbDev
+  );
+
+extern USB_HUB_API        mUsbHubApi;
+extern USB_HUB_API        mUsbRootHubApi;
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.c
new file mode 100644
index 0000000000..e67a88541b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.c
@@ -0,0 +1,1377 @@
+/** @file
+
+    Wrapper function for usb host controller interface.
+
+Copyright (c) 2007 - 2010, 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 "UsbBus.h"
+
+//
+// if RemainingDevicePath== NULL, then all Usb child devices in this bus are wanted.
+// Use a shor form Usb class Device Path, which could match any usb device, in WantedUsbIoDPList to indicate all Usb devices
+// are wanted Usb devices
+//
+USB_CLASS_FORMAT_DEVICE_PATH mAllUsbClassDevicePath = {
+  {
+    {
+      MESSAGING_DEVICE_PATH,
+      MSG_USB_CLASS_DP,
+      {
+        (UINT8) (sizeof (USB_CLASS_DEVICE_PATH)),
+        (UINT8) ((sizeof (USB_CLASS_DEVICE_PATH)) >> 8)
+      }
+    },
+    0xffff, // VendorId
+    0xffff, // ProductId
+    0xff,   // DeviceClass
+    0xff,   // DeviceSubClass
+    0xff    // DeviceProtocol
+  },
+
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      END_DEVICE_PATH_LENGTH,
+      0
+    }
+  }
+};
+
+
+/**
+  Get the capability of the host controller.
+
+  @param  UsbBus           The usb driver.
+  @param  MaxSpeed         The maximum speed this host controller supports.
+  @param  NumOfPort        The number of the root hub port.
+  @param  Is64BitCapable   Whether this controller support 64 bit addressing.
+
+  @retval EFI_SUCCESS      The host controller capability is returned.
+  @retval Others           Failed to retrieve the host controller capability.
+
+**/
+EFI_STATUS
+UsbHcGetCapability (
+  IN  USB_BUS             *UsbBus,
+  OUT UINT8               *MaxSpeed,
+  OUT UINT8               *NumOfPort,
+  OUT UINT8               *Is64BitCapable
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->GetCapability (
+                              UsbBus->Usb2Hc,
+                              MaxSpeed,
+                              NumOfPort,
+                              Is64BitCapable
+                              );
+
+  } else {
+    Status = UsbBus->UsbHc->GetRootHubPortNumber (UsbBus->UsbHc, NumOfPort);
+
+    *MaxSpeed       = EFI_USB_SPEED_FULL;
+    *Is64BitCapable = (UINT8) FALSE;
+  }
+
+  return Status;
+}
+
+
+/**
+  Reset the host controller.
+
+  @param  UsbBus                The usb bus driver.
+  @param  Attributes            The reset type, only global reset is used by this driver.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+**/
+EFI_STATUS
+UsbHcReset (
+  IN USB_BUS              *UsbBus,
+  IN UINT16               Attributes
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->Reset (UsbBus->Usb2Hc, Attributes);
+  } else {
+    Status = UsbBus->UsbHc->Reset (UsbBus->UsbHc, Attributes);
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the current operation state of the host controller.
+
+  @param  UsbBus           The USB bus driver.
+  @param  State            The host controller operation state.
+
+  @retval EFI_SUCCESS      The operation state is returned in State.
+  @retval Others           Failed to get the host controller state.
+
+**/
+EFI_STATUS
+UsbHcGetState (
+  IN  USB_BUS             *UsbBus,
+  OUT EFI_USB_HC_STATE    *State
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->GetState (UsbBus->Usb2Hc, State);
+  } else {
+    Status = UsbBus->UsbHc->GetState (UsbBus->UsbHc, State);
+  }
+
+  return Status;
+}
+
+
+/**
+  Set the host controller operation state.
+
+  @param  UsbBus           The USB bus driver.
+  @param  State            The state to set.
+
+  @retval EFI_SUCCESS      The host controller is now working at State.
+  @retval Others           Failed to set operation state.
+
+**/
+EFI_STATUS
+UsbHcSetState (
+  IN USB_BUS              *UsbBus,
+  IN EFI_USB_HC_STATE     State
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->SetState (UsbBus->Usb2Hc, State);
+  } else {
+    Status = UsbBus->UsbHc->SetState (UsbBus->UsbHc, State);
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the root hub port state.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The index of port.
+  @param  PortStatus       The variable to save port state.
+
+  @retval EFI_SUCCESS      The root port state is returned in.
+  @retval Others           Failed to get the root hub port state.
+
+**/
+EFI_STATUS
+UsbHcGetRootHubPortStatus (
+  IN  USB_BUS             *UsbBus,
+  IN  UINT8               PortIndex,
+  OUT EFI_USB_PORT_STATUS *PortStatus
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->GetRootHubPortStatus (UsbBus->Usb2Hc, PortIndex, PortStatus);
+  } else {
+    Status = UsbBus->UsbHc->GetRootHubPortStatus (UsbBus->UsbHc, PortIndex, PortStatus);
+  }
+
+  return Status;
+}
+
+
+/**
+  Set the root hub port feature.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The port index.
+  @param  Feature          The port feature to set.
+
+  @retval EFI_SUCCESS      The port feature is set.
+  @retval Others           Failed to set port feature.
+
+**/
+EFI_STATUS
+UsbHcSetRootHubPortFeature (
+  IN USB_BUS              *UsbBus,
+  IN UINT8                PortIndex,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->SetRootHubPortFeature (UsbBus->Usb2Hc, PortIndex, Feature);
+  } else {
+    Status = UsbBus->UsbHc->SetRootHubPortFeature (UsbBus->UsbHc, PortIndex, Feature);
+  }
+
+  return Status;
+}
+
+
+/**
+  Clear the root hub port feature.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The port index.
+  @param  Feature          The port feature to clear.
+
+  @retval EFI_SUCCESS      The port feature is clear.
+  @retval Others           Failed to clear port feature.
+
+**/
+EFI_STATUS
+UsbHcClearRootHubPortFeature (
+  IN USB_BUS              *UsbBus,
+  IN UINT8                PortIndex,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->ClearRootHubPortFeature (UsbBus->Usb2Hc, PortIndex, Feature);
+  } else {
+    Status = UsbBus->UsbHc->ClearRootHubPortFeature (UsbBus->UsbHc, PortIndex, Feature);
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute a control transfer to the device.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The device address.
+  @param  DevSpeed         The device speed.
+  @param  MaxPacket        Maximum packet size of endpoint 0.
+  @param  Request          The control transfer request.
+  @param  Direction        The direction of data stage.
+  @param  Data             The buffer holding data.
+  @param  DataLength       The length of the data.
+  @param  TimeOut          Timeout (in ms) to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of transfer.
+
+  @retval EFI_SUCCESS      The control transfer finished without error.
+  @retval Others           The control transfer failed, reason returned in UsbReslt.
+
+**/
+EFI_STATUS
+UsbHcControlTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  EFI_USB_DEVICE_REQUEST              *Request,
+  IN  EFI_USB_DATA_DIRECTION              Direction,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  )
+{
+  EFI_STATUS              Status;
+  BOOLEAN                 IsSlowDevice;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->ControlTransfer (
+                               UsbBus->Usb2Hc,
+                               DevAddr,
+                               DevSpeed,
+                               MaxPacket,
+                               Request,
+                               Direction,
+                               Data,
+                               DataLength,
+                               TimeOut,
+                               Translator,
+                               UsbResult
+                               );
+
+  } else {
+    IsSlowDevice = (BOOLEAN)(EFI_USB_SPEED_LOW == DevSpeed);
+    Status = UsbBus->UsbHc->ControlTransfer (
+                              UsbBus->UsbHc,
+                              DevAddr,
+                              IsSlowDevice,
+                              (UINT8) MaxPacket,
+                              Request,
+                              Direction,
+                              Data,
+                              DataLength,
+                              TimeOut,
+                              UsbResult
+                              );
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute a bulk transfer to the device's endpoint.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  DataToggle       On input, the initial data toggle to use, also  return
+                           the next toggle on output.
+  @param  TimeOut          The time to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_SUCCESS      The bulk transfer is finished without error.
+  @retval Others           Failed to execute bulk transfer, result in UsbResult.
+
+**/
+EFI_STATUS
+UsbHcBulkTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->BulkTransfer (
+                               UsbBus->Usb2Hc,
+                               DevAddr,
+                               EpAddr,
+                               DevSpeed,
+                               MaxPacket,
+                               BufferNum,
+                               Data,
+                               DataLength,
+                               DataToggle,
+                               TimeOut,
+                               Translator,
+                               UsbResult
+                               );
+  } else {
+    Status = UsbBus->UsbHc->BulkTransfer (
+                              UsbBus->UsbHc,
+                              DevAddr,
+                              EpAddr,
+                              (UINT8) MaxPacket,
+                              *Data,
+                              DataLength,
+                              DataToggle,
+                              TimeOut,
+                              UsbResult
+                              );
+  }
+
+  return Status;
+}
+
+
+/**
+  Queue or cancel an asynchronous interrupt transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  IsNewTransfer    Whether this is a new request. If not, cancel the old
+                           request.
+  @param  DataToggle       Data toggle to use on input, next toggle on output.
+  @param  PollingInterval  The interval to poll the interrupt transfer (in ms).
+  @param  DataLength       The length of periodical data receive.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  Callback         Function to call when data is received.
+  @param  Context          The context to the callback.
+
+  @retval EFI_SUCCESS      The asynchronous transfer is queued.
+  @retval Others           Failed to queue the transfer.
+
+**/
+EFI_STATUS
+UsbHcAsyncInterruptTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  BOOLEAN                             IsNewTransfer,
+  IN OUT UINT8                            *DataToggle,
+  IN  UINTN                               PollingInterval,
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     Callback,
+  IN  VOID                                *Context OPTIONAL
+  )
+{
+  EFI_STATUS              Status;
+  BOOLEAN                 IsSlowDevice;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->AsyncInterruptTransfer (
+                               UsbBus->Usb2Hc,
+                               DevAddr,
+                               EpAddr,
+                               DevSpeed,
+                               MaxPacket,
+                               IsNewTransfer,
+                               DataToggle,
+                               PollingInterval,
+                               DataLength,
+                               Translator,
+                               Callback,
+                               Context
+                               );
+  } else {
+    IsSlowDevice = (BOOLEAN)(EFI_USB_SPEED_LOW == DevSpeed);
+
+    Status = UsbBus->UsbHc->AsyncInterruptTransfer (
+                              UsbBus->UsbHc,
+                              DevAddr,
+                              EpAddr,
+                              IsSlowDevice,
+                              (UINT8) MaxPacket,
+                              IsNewTransfer,
+                              DataToggle,
+                              PollingInterval,
+                              DataLength,
+                              Callback,
+                              Context
+                              );
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute a synchronous interrupt transfer to the target endpoint.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  Data             Pointer to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  DataToggle       On input, the initial data toggle to use, also  return
+                           the next toggle on output.
+  @param  TimeOut          The time to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_SUCCESS      The synchronous interrupt transfer is OK.
+  @retval Others           Failed to execute the synchronous interrupt transfer.
+
+**/
+EFI_STATUS
+UsbHcSyncInterruptTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN OUT VOID                             *Data,
+  IN OUT UINTN                            *DataLength,
+  IN OUT UINT8                            *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  )
+{
+  EFI_STATUS              Status;
+  BOOLEAN                 IsSlowDevice;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->SyncInterruptTransfer (
+                               UsbBus->Usb2Hc,
+                               DevAddr,
+                               EpAddr,
+                               DevSpeed,
+                               MaxPacket,
+                               Data,
+                               DataLength,
+                               DataToggle,
+                               TimeOut,
+                               Translator,
+                               UsbResult
+                               );
+  } else {
+    IsSlowDevice = (BOOLEAN) ((EFI_USB_SPEED_LOW == DevSpeed) ? TRUE : FALSE);
+    Status = UsbBus->UsbHc->SyncInterruptTransfer (
+                              UsbBus->UsbHc,
+                              DevAddr,
+                              EpAddr,
+                              IsSlowDevice,
+                              (UINT8) MaxPacket,
+                              Data,
+                              DataLength,
+                              DataToggle,
+                              TimeOut,
+                              UsbResult
+                              );
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute a synchronous Isochronous USB transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_UNSUPPORTED  The isochronous transfer isn't supported now.
+
+**/
+EFI_STATUS
+UsbHcIsochronousTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Queue an asynchronous isochronous transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  Callback         The function to call when data is transferred.
+  @param  Context          The context to the callback function.
+
+  @retval EFI_UNSUPPORTED  The asynchronous isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+UsbHcAsyncIsochronousTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN OUT VOID                             *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     Callback,
+  IN  VOID                                *Context
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Open the USB host controller protocol BY_CHILD.
+
+  @param  Bus              The USB bus driver.
+  @param  Child            The child handle.
+
+  @return The open protocol return.
+
+**/
+EFI_STATUS
+UsbOpenHostProtoByChild (
+  IN USB_BUS              *Bus,
+  IN EFI_HANDLE           Child
+  )
+{
+  EFI_USB_HC_PROTOCOL     *UsbHc;
+  EFI_USB2_HC_PROTOCOL    *Usb2Hc;
+  EFI_STATUS              Status;
+
+  if (Bus->Usb2Hc != NULL) {
+    Status = gBS->OpenProtocol (
+                    Bus->HostHandle,
+                    &gEfiUsb2HcProtocolGuid,
+                    (VOID **) &Usb2Hc,
+                    mUsbBusDriverBinding.DriverBindingHandle,
+                    Child,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+
+  } else {
+    Status = gBS->OpenProtocol (
+                    Bus->HostHandle,
+                    &gEfiUsbHcProtocolGuid,
+                    (VOID **) &UsbHc,
+                    mUsbBusDriverBinding.DriverBindingHandle,
+                    Child,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+  }
+
+  return Status;
+}
+
+
+/**
+  Close the USB host controller protocol BY_CHILD.
+
+  @param  Bus              The USB bus driver.
+  @param  Child            The child handle.
+
+**/
+VOID
+UsbCloseHostProtoByChild (
+  IN USB_BUS              *Bus,
+  IN EFI_HANDLE           Child
+  )
+{
+  if (Bus->Usb2Hc != NULL) {
+    gBS->CloseProtocol (
+           Bus->HostHandle,
+           &gEfiUsb2HcProtocolGuid,
+           mUsbBusDriverBinding.DriverBindingHandle,
+           Child
+           );
+
+  } else {
+    gBS->CloseProtocol (
+           Bus->HostHandle,
+           &gEfiUsbHcProtocolGuid,
+           mUsbBusDriverBinding.DriverBindingHandle,
+           Child
+           );
+  }
+}
+
+
+/**
+  return the current TPL, copied from the EDKII glue lib.
+
+  @param  VOID.
+
+  @return Current TPL.
+
+**/
+EFI_TPL
+UsbGetCurrentTpl (
+  VOID
+  )
+{
+  EFI_TPL                 Tpl;
+
+  Tpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+  gBS->RestoreTPL (Tpl);
+
+  return Tpl;
+}
+
+/**
+  Create a new device path which only contain the first Usb part of the DevicePath.
+
+  @param DevicePath  A full device path which contain the usb nodes.
+
+  @return            A new device path which only contain the Usb part of the DevicePath.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+EFIAPI
+GetUsbDPFromFullDP (
+  IN EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL    *UsbDevicePathPtr;
+  EFI_DEVICE_PATH_PROTOCOL    *UsbDevicePathBeginPtr;
+  EFI_DEVICE_PATH_PROTOCOL    *UsbDevicePathEndPtr;
+  UINTN                       Size;
+
+  //
+  // Get the Usb part first Begin node in full device path
+  //
+  UsbDevicePathBeginPtr = DevicePath;
+  while ( (!IsDevicePathEnd (UsbDevicePathBeginPtr))&&
+         ((UsbDevicePathBeginPtr->Type != MESSAGING_DEVICE_PATH) ||
+         (UsbDevicePathBeginPtr->SubType != MSG_USB_DP &&
+          UsbDevicePathBeginPtr->SubType != MSG_USB_CLASS_DP
+          && UsbDevicePathBeginPtr->SubType != MSG_USB_WWID_DP
+          ))) {
+
+    UsbDevicePathBeginPtr = NextDevicePathNode(UsbDevicePathBeginPtr);
+  }
+
+  //
+  // Get the Usb part first End node in full device path
+  //
+  UsbDevicePathEndPtr = UsbDevicePathBeginPtr;
+  while ((!IsDevicePathEnd (UsbDevicePathEndPtr))&&
+         (UsbDevicePathEndPtr->Type == MESSAGING_DEVICE_PATH) &&
+         (UsbDevicePathEndPtr->SubType == MSG_USB_DP ||
+          UsbDevicePathEndPtr->SubType == MSG_USB_CLASS_DP
+          || UsbDevicePathEndPtr->SubType == MSG_USB_WWID_DP
+          )) {
+
+    UsbDevicePathEndPtr = NextDevicePathNode(UsbDevicePathEndPtr);
+  }
+
+  Size  = GetDevicePathSize (UsbDevicePathBeginPtr);
+  Size -= GetDevicePathSize (UsbDevicePathEndPtr);
+  if (Size ==0){
+    //
+    // The passed in DevicePath does not contain the usb nodes
+    //
+    return NULL;
+  }
+
+  //
+  // Create a new device path which only contain the above Usb part
+  //
+  UsbDevicePathPtr = AllocateZeroPool (Size + sizeof (EFI_DEVICE_PATH_PROTOCOL));
+  ASSERT (UsbDevicePathPtr != NULL);
+  CopyMem (UsbDevicePathPtr, UsbDevicePathBeginPtr, Size);
+  //
+  // Append end device path node
+  //
+  UsbDevicePathEndPtr = (EFI_DEVICE_PATH_PROTOCOL *) ((UINTN) UsbDevicePathPtr + Size);
+  SetDevicePathEndNode (UsbDevicePathEndPtr);
+  return UsbDevicePathPtr;
+}
+
+/**
+  Check whether a usb device path is in a DEVICE_PATH_LIST_ITEM list.
+
+  @param UsbDP       a usb device path of DEVICE_PATH_LIST_ITEM.
+  @param UsbIoDPList a DEVICE_PATH_LIST_ITEM list.
+
+  @retval TRUE       there is a DEVICE_PATH_LIST_ITEM in UsbIoDPList which contains the passed in UsbDP.
+  @retval FALSE      there is no DEVICE_PATH_LIST_ITEM in UsbIoDPList which contains the passed in UsbDP.
+
+**/
+BOOLEAN
+EFIAPI
+SearchUsbDPInList (
+  IN EFI_DEVICE_PATH_PROTOCOL     *UsbDP,
+  IN LIST_ENTRY                   *UsbIoDPList
+  )
+{
+  LIST_ENTRY                  *ListIndex;
+  DEVICE_PATH_LIST_ITEM       *ListItem;
+  BOOLEAN                     Found;
+  UINTN                       UsbDpDevicePathSize;
+
+  //
+  // Check that UsbDP and UsbIoDPList are valid
+  //
+  if ((UsbIoDPList == NULL) || (UsbDP == NULL)) {
+    return FALSE;
+  }
+
+  Found = FALSE;
+  ListIndex = UsbIoDPList->ForwardLink;
+  while (ListIndex != UsbIoDPList){
+    ListItem = CR(ListIndex, DEVICE_PATH_LIST_ITEM, Link, DEVICE_PATH_LIST_ITEM_SIGNATURE);
+    //
+    // Compare DEVICE_PATH_LIST_ITEM.DevicePath[]
+    //
+    ASSERT (ListItem->DevicePath != NULL);
+
+    UsbDpDevicePathSize  = GetDevicePathSize (UsbDP);
+    if (UsbDpDevicePathSize == GetDevicePathSize (ListItem->DevicePath)) {
+      if ((CompareMem (UsbDP, ListItem->DevicePath, UsbDpDevicePathSize)) == 0) {
+        Found = TRUE;
+        break;
+      }
+    }
+    ListIndex =  ListIndex->ForwardLink;
+  }
+
+  return Found;
+}
+
+/**
+  Add a usb device path into the DEVICE_PATH_LIST_ITEM list.
+
+  @param UsbDP                   a usb device path of DEVICE_PATH_LIST_ITEM.
+  @param UsbIoDPList             a DEVICE_PATH_LIST_ITEM list.
+
+  @retval EFI_INVALID_PARAMETER  If parameters are invalid, return this value.
+  @retval EFI_SUCCESS            If Add operation is successful, return this value.
+
+**/
+EFI_STATUS
+EFIAPI
+AddUsbDPToList (
+  IN EFI_DEVICE_PATH_PROTOCOL     *UsbDP,
+  IN LIST_ENTRY                   *UsbIoDPList
+  )
+{
+  DEVICE_PATH_LIST_ITEM       *ListItem;
+
+  //
+  // Check that UsbDP and UsbIoDPList are valid
+  //
+  if ((UsbIoDPList == NULL) || (UsbDP == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (SearchUsbDPInList (UsbDP, UsbIoDPList)){
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Prepare the usbio device path DEVICE_PATH_LIST_ITEM structure.
+  //
+  ListItem = AllocateZeroPool (sizeof (DEVICE_PATH_LIST_ITEM));
+  ASSERT (ListItem != NULL);
+  ListItem->Signature = DEVICE_PATH_LIST_ITEM_SIGNATURE;
+  ListItem->DevicePath = DuplicateDevicePath (UsbDP);
+
+  InsertTailList (UsbIoDPList, &ListItem->Link);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether usb device, whose interface is UsbIf, matches the usb class which indicated by
+  UsbClassDevicePathPtr whose is a short form usb class device path.
+
+  @param UsbClassDevicePathPtr    a short form usb class device path.
+  @param UsbIf                    a usb device interface.
+
+  @retval TRUE                    the usb device match the usb class.
+  @retval FALSE                   the usb device does not match the usb class.
+
+**/
+BOOLEAN
+EFIAPI
+MatchUsbClass (
+  IN USB_CLASS_DEVICE_PATH      *UsbClassDevicePathPtr,
+  IN USB_INTERFACE              *UsbIf
+  )
+{
+  USB_INTERFACE_DESC            *IfDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR  *ActIfDesc;
+  EFI_USB_DEVICE_DESCRIPTOR     *DevDesc;
+
+
+  if ((UsbClassDevicePathPtr->Header.Type != MESSAGING_DEVICE_PATH) ||
+      (UsbClassDevicePathPtr->Header.SubType != MSG_USB_CLASS_DP)){
+    ASSERT (0);
+    return FALSE;
+  }
+
+  IfDesc       = UsbIf->IfDesc;
+  ASSERT (IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);
+  ActIfDesc    = &(IfDesc->Settings[IfDesc->ActiveIndex]->Desc);
+  DevDesc      = &(UsbIf->Device->DevDesc->Desc);
+
+  //
+  // If connect class policy, determine whether to create device handle by the five fields
+  // in class device path node.
+  //
+  // In addtion, hub interface is always matched for this policy.
+  //
+  if ((ActIfDesc->InterfaceClass == USB_HUB_CLASS_CODE) &&
+      (ActIfDesc->InterfaceSubClass == USB_HUB_SUBCLASS_CODE)) {
+    return TRUE;
+  }
+
+  //
+  // If vendor id or product id is 0xffff, they will be ignored.
+  //
+  if ((UsbClassDevicePathPtr->VendorId == 0xffff || UsbClassDevicePathPtr->VendorId == DevDesc->IdVendor) &&
+      (UsbClassDevicePathPtr->ProductId == 0xffff || UsbClassDevicePathPtr->ProductId == DevDesc->IdProduct)) {
+
+    //
+    // If Class in Device Descriptor is set to 0, the counterparts in interface should be checked.
+    //
+    if (DevDesc->DeviceClass == 0) {
+      if ((UsbClassDevicePathPtr->DeviceClass == ActIfDesc->InterfaceClass ||
+                                          UsbClassDevicePathPtr->DeviceClass == 0xff) &&
+          (UsbClassDevicePathPtr->DeviceSubClass == ActIfDesc->InterfaceSubClass ||
+                                       UsbClassDevicePathPtr->DeviceSubClass == 0xff) &&
+          (UsbClassDevicePathPtr->DeviceProtocol == ActIfDesc->InterfaceProtocol ||
+                                       UsbClassDevicePathPtr->DeviceProtocol == 0xff)) {
+        return TRUE;
+      }
+
+    } else if ((UsbClassDevicePathPtr->DeviceClass == DevDesc->DeviceClass ||
+                                         UsbClassDevicePathPtr->DeviceClass == 0xff) &&
+               (UsbClassDevicePathPtr->DeviceSubClass == DevDesc->DeviceSubClass ||
+                                      UsbClassDevicePathPtr->DeviceSubClass == 0xff) &&
+               (UsbClassDevicePathPtr->DeviceProtocol == DevDesc->DeviceProtocol ||
+                                      UsbClassDevicePathPtr->DeviceProtocol == 0xff)) {
+
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Check whether usb device, whose interface is UsbIf, matches the usb WWID requirement which indicated by
+  UsbWWIDDevicePathPtr whose is a short form usb WWID device path.
+
+  @param UsbWWIDDevicePathPtr    a short form usb WWID device path.
+  @param UsbIf                   a usb device interface.
+
+  @retval TRUE                   the usb device match the usb WWID requirement.
+  @retval FALSE                  the usb device does not match the usb WWID requirement.
+
+**/
+BOOLEAN
+MatchUsbWwid (
+  IN USB_WWID_DEVICE_PATH       *UsbWWIDDevicePathPtr,
+  IN USB_INTERFACE              *UsbIf
+  )
+{
+  USB_INTERFACE_DESC            *IfDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR  *ActIfDesc;
+  EFI_USB_DEVICE_DESCRIPTOR     *DevDesc;
+  EFI_USB_STRING_DESCRIPTOR     *StrDesc;
+  UINT16                        Index;
+  CHAR16                        *CompareStr;
+  UINTN                         CompareLen;
+  UINTN                         Length;
+
+  if ((UsbWWIDDevicePathPtr->Header.Type != MESSAGING_DEVICE_PATH) ||
+     (UsbWWIDDevicePathPtr->Header.SubType != MSG_USB_WWID_DP )){
+    ASSERT (0);
+    return FALSE;
+  }
+
+  IfDesc       = UsbIf->IfDesc;
+  ASSERT (IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);
+  ActIfDesc    = &(IfDesc->Settings[IfDesc->ActiveIndex]->Desc);
+  DevDesc      = &(UsbIf->Device->DevDesc->Desc);
+
+  //
+  // In addition, Hub interface is always matched for this policy.
+  //
+  if ((ActIfDesc->InterfaceClass == USB_HUB_CLASS_CODE) &&
+      (ActIfDesc->InterfaceSubClass == USB_HUB_SUBCLASS_CODE)) {
+    return TRUE;
+  }
+
+  //
+  // Check Vendor Id, Product Id and Interface Number.
+  //
+  if ((DevDesc->IdVendor != UsbWWIDDevicePathPtr->VendorId) ||
+      (DevDesc->IdProduct != UsbWWIDDevicePathPtr->ProductId) ||
+      (ActIfDesc->InterfaceNumber != UsbWWIDDevicePathPtr->InterfaceNumber)) {
+    return FALSE;
+  }
+
+  //
+  // Check SerialNumber.
+  //
+  if (DevDesc->StrSerialNumber == 0) {
+    return FALSE;
+  }
+
+  //
+  // Serial number in USB WWID device path is the last 64-or-less UTF-16 characters.
+  //
+  CompareStr = (CHAR16 *) (UINTN) (UsbWWIDDevicePathPtr + 1);
+  CompareLen = (DevicePathNodeLength (UsbWWIDDevicePathPtr) - sizeof (USB_WWID_DEVICE_PATH)) / sizeof (CHAR16);
+  if (CompareStr[CompareLen - 1] == L'\0') {
+    CompareLen--;
+  }
+
+  //
+  // Compare serial number in each supported language.
+  //
+  for (Index = 0; Index < UsbIf->Device->TotalLangId; Index++) {
+    StrDesc = UsbGetOneString (UsbIf->Device, DevDesc->StrSerialNumber, UsbIf->Device->LangId[Index]);
+    if (StrDesc == NULL) {
+      continue;
+    }
+
+    Length = (StrDesc->Length - 2) / sizeof (CHAR16);
+    if ((Length >= CompareLen) &&
+        (CompareMem (StrDesc->String + Length - CompareLen, CompareStr, CompareLen * sizeof (CHAR16)) == 0)) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Free a DEVICE_PATH_LIST_ITEM list.
+
+  @param  UsbIoDPList            a DEVICE_PATH_LIST_ITEM list pointer.
+
+  @retval EFI_INVALID_PARAMETER  If parameters are invalid, return this value.
+  @retval EFI_SUCCESS            If free operation is successful, return this value.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusFreeUsbDPList (
+  IN LIST_ENTRY          *UsbIoDPList
+  )
+{
+  LIST_ENTRY                  *ListIndex;
+  DEVICE_PATH_LIST_ITEM       *ListItem;
+
+  //
+  // Check that ControllerHandle is a valid handle
+  //
+  if (UsbIoDPList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ListIndex = UsbIoDPList->ForwardLink;
+  while (ListIndex != UsbIoDPList){
+    ListItem = CR(ListIndex, DEVICE_PATH_LIST_ITEM, Link, DEVICE_PATH_LIST_ITEM_SIGNATURE);
+    //
+    // Free DEVICE_PATH_LIST_ITEM.DevicePath[]
+    //
+    if (ListItem->DevicePath != NULL){
+      FreePool(ListItem->DevicePath);
+    }
+    //
+    // Free DEVICE_PATH_LIST_ITEM itself
+    //
+    ListIndex =  ListIndex->ForwardLink;
+    RemoveEntryList (&ListItem->Link);
+    FreePool (ListItem);
+  }
+
+  InitializeListHead (UsbIoDPList);
+  return EFI_SUCCESS;
+}
+
+/**
+  Store a wanted usb child device info (its Usb part of device path) which is indicated by
+  RemainingDevicePath in a Usb bus which  is indicated by UsbBusId.
+
+  @param  UsbBusId               Point to EFI_USB_BUS_PROTOCOL interface.
+  @param  RemainingDevicePath    The remaining device patch.
+
+  @retval EFI_SUCCESS            Add operation is successful.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusAddWantedUsbIoDP (
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  USB_BUS                       *Bus;
+  EFI_STATUS                    Status;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePathPtr;
+
+  //
+  // Check whether remaining device path is valid
+  //
+  if (RemainingDevicePath != NULL && !IsDevicePathEnd (RemainingDevicePath)) {
+    if ((RemainingDevicePath->Type    != MESSAGING_DEVICE_PATH) ||
+        (RemainingDevicePath->SubType != MSG_USB_DP &&
+         RemainingDevicePath->SubType != MSG_USB_CLASS_DP
+         && RemainingDevicePath->SubType != MSG_USB_WWID_DP
+         )) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  if (UsbBusId == NULL){
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Bus = USB_BUS_FROM_THIS (UsbBusId);
+
+  if (RemainingDevicePath == NULL) {
+    //
+    // RemainingDevicePath == NULL means all Usb devices in this bus are wanted.
+    // Here use a Usb class Device Path in WantedUsbIoDPList to indicate all Usb devices
+    // are wanted Usb devices
+    //
+    Status = UsbBusFreeUsbDPList (&Bus->WantedUsbIoDPList);
+    ASSERT (!EFI_ERROR (Status));
+    DevicePathPtr = DuplicateDevicePath ((EFI_DEVICE_PATH_PROTOCOL *) &mAllUsbClassDevicePath);
+  } else if (!IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // If RemainingDevicePath isn't the End of Device Path Node, 
+    // Create new Usb device path according to the usb part in remaining device path
+    //
+    DevicePathPtr = GetUsbDPFromFullDP (RemainingDevicePath);
+  } else {
+    //
+    // If RemainingDevicePath is the End of Device Path Node,
+    // skip enumerate any device and return EFI_SUCESSS
+    // 
+    return EFI_SUCCESS;
+  }
+
+  ASSERT (DevicePathPtr != NULL);
+  Status = AddUsbDPToList (DevicePathPtr, &Bus->WantedUsbIoDPList);
+  ASSERT (!EFI_ERROR (Status));
+  FreePool (DevicePathPtr);
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether a usb child device is the wanted device in a bus.
+
+  @param  Bus     The Usb bus's private data pointer.
+  @param  UsbIf   The usb child device inferface.
+
+  @retval True    If a usb child device is the wanted device in a bus.
+  @retval False   If a usb child device is *NOT* the wanted device in a bus.
+
+**/
+BOOLEAN
+EFIAPI
+UsbBusIsWantedUsbIO (
+  IN USB_BUS                 *Bus,
+  IN USB_INTERFACE           *UsbIf
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePathPtr;
+  LIST_ENTRY                    *WantedUsbIoDPListPtr;
+  LIST_ENTRY                    *WantedListIndex;
+  DEVICE_PATH_LIST_ITEM         *WantedListItem;
+  BOOLEAN                       DoConvert;
+  UINTN                         FirstDevicePathSize;
+
+  //
+  // Check whether passed in parameters are valid
+  //
+  if ((UsbIf == NULL) || (Bus == NULL)) {
+    return FALSE;
+  }
+  //
+  // Check whether UsbIf is Hub
+  //
+  if (UsbIf->IsHub) {
+    return TRUE;
+  }
+
+  //
+  // Check whether all Usb devices in this bus are wanted
+  //
+  if (SearchUsbDPInList ((EFI_DEVICE_PATH_PROTOCOL *)&mAllUsbClassDevicePath, &Bus->WantedUsbIoDPList)){
+    return TRUE;
+  }
+
+  //
+  // Check whether the Usb device match any item in WantedUsbIoDPList
+  //
+  WantedUsbIoDPListPtr = &Bus->WantedUsbIoDPList;
+  //
+  // Create new Usb device path according to the usb part in UsbIo full device path
+  //
+  DevicePathPtr = GetUsbDPFromFullDP (UsbIf->DevicePath);
+  ASSERT (DevicePathPtr != NULL);
+
+  DoConvert = FALSE;
+  WantedListIndex = WantedUsbIoDPListPtr->ForwardLink;
+  while (WantedListIndex != WantedUsbIoDPListPtr){
+    WantedListItem = CR(WantedListIndex, DEVICE_PATH_LIST_ITEM, Link, DEVICE_PATH_LIST_ITEM_SIGNATURE);
+    ASSERT (WantedListItem->DevicePath->Type == MESSAGING_DEVICE_PATH);
+    switch (WantedListItem->DevicePath->SubType) {
+    case MSG_USB_DP:
+      FirstDevicePathSize = GetDevicePathSize (WantedListItem->DevicePath);
+      if (FirstDevicePathSize == GetDevicePathSize (DevicePathPtr)) {
+        if (CompareMem (
+              WantedListItem->DevicePath,
+              DevicePathPtr,
+              GetDevicePathSize (DevicePathPtr)) == 0
+            ) {
+          DoConvert = TRUE;
+        }
+      }
+      break;
+    case MSG_USB_CLASS_DP:
+      if (MatchUsbClass((USB_CLASS_DEVICE_PATH *)WantedListItem->DevicePath, UsbIf)) {
+        DoConvert = TRUE;
+      }
+      break;
+   case MSG_USB_WWID_DP:
+      if (MatchUsbWwid((USB_WWID_DEVICE_PATH *)WantedListItem->DevicePath, UsbIf)) {
+        DoConvert = TRUE;
+      }
+      break;
+    default:
+      ASSERT (0);
+      break;
+    }
+
+    if (DoConvert) {
+      break;
+    }
+
+    WantedListIndex =  WantedListIndex->ForwardLink;
+  }
+  gBS->FreePool (DevicePathPtr);
+
+  //
+  // Check whether the new Usb device path is wanted
+  //
+  if (DoConvert){
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Recursively connnect every wanted usb child device to ensure they all fully connected.
+  Check all the child Usb IO handles in this bus, recursively connecte if it is wanted usb child device.
+
+  @param  UsbBusId                  Point to EFI_USB_BUS_PROTOCOL interface.
+
+  @retval EFI_SUCCESS               Connect is done successfully.
+  @retval EFI_INVALID_PARAMETER     The parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusRecursivelyConnectWantedUsbIo (
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId
+  )
+{
+  USB_BUS                       *Bus;
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  USB_INTERFACE                 *UsbIf;
+  UINTN                         UsbIoHandleCount;
+  EFI_HANDLE                    *UsbIoBuffer;
+  EFI_DEVICE_PATH_PROTOCOL      *UsbIoDevicePath;
+
+  if (UsbBusId == NULL){
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Bus = USB_BUS_FROM_THIS (UsbBusId);
+
+  //
+  // Get all Usb IO handles in system
+  //
+  UsbIoHandleCount = 0;
+  Status = gBS->LocateHandleBuffer (ByProtocol, &gEfiUsbIoProtocolGuid, NULL, &UsbIoHandleCount, &UsbIoBuffer);
+  if (Status == EFI_NOT_FOUND || UsbIoHandleCount == 0) {
+    return EFI_SUCCESS;
+  }
+  ASSERT (!EFI_ERROR (Status));
+
+  for (Index = 0; Index < UsbIoHandleCount; Index++) {
+    //
+    // Check whether the USB IO handle is a child of this bus
+    // Note: The usb child handle maybe invalid because of hot plugged out during the loop
+    //
+    UsbIoDevicePath = NULL;
+    Status = gBS->HandleProtocol (UsbIoBuffer[Index], &gEfiDevicePathProtocolGuid, (VOID *) &UsbIoDevicePath);
+    if (EFI_ERROR (Status) || UsbIoDevicePath == NULL) {
+      continue;
+    }
+    if (CompareMem (
+            UsbIoDevicePath,
+            Bus->DevicePath,
+            (GetDevicePathSize (Bus->DevicePath) - sizeof (EFI_DEVICE_PATH_PROTOCOL))
+            ) != 0) {
+      continue;
+    }
+
+    //
+    // Get the child Usb IO interface
+    //
+    Status = gBS->HandleProtocol(
+                     UsbIoBuffer[Index],
+                     &gEfiUsbIoProtocolGuid,
+                     (VOID **) &UsbIo
+                     );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+    UsbIf   = USB_INTERFACE_FROM_USBIO (UsbIo);
+
+    if (UsbBusIsWantedUsbIO (Bus, UsbIf)) {
+      if (!UsbIf->IsManaged) {
+        //
+        // Recursively connect the wanted Usb Io handle
+        //
+        DEBUG ((EFI_D_INFO, "UsbConnectDriver: TPL before connect is %d\n", (UINT32)UsbGetCurrentTpl ()));
+        Status            = gBS->ConnectController (UsbIf->Handle, NULL, NULL, TRUE);
+        UsbIf->IsManaged  = (BOOLEAN)!EFI_ERROR (Status);
+        DEBUG ((EFI_D_INFO, "UsbConnectDriver: TPL after connect is %d\n", (UINT32)UsbGetCurrentTpl()));
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.h
new file mode 100644
index 0000000000..26709caa36
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.h
@@ -0,0 +1,398 @@
+/** @file
+
+    Manage Usb Port/Hc/Etc.
+
+Copyright (c) 2007, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_USB_UTILITY_H_
+#define _EFI_USB_UTILITY_H_
+
+/**
+  Get the capability of the host controller.
+
+  @param  UsbBus           The usb driver.
+  @param  MaxSpeed         The maximum speed this host controller supports.
+  @param  NumOfPort        The number of the root hub port.
+  @param  Is64BitCapable   Whether this controller support 64 bit addressing.
+
+  @retval EFI_SUCCESS      The host controller capability is returned.
+  @retval Others           Failed to retrieve the host controller capability.
+
+**/
+EFI_STATUS
+UsbHcGetCapability (
+  IN  USB_BUS             *UsbBus,
+  OUT UINT8               *MaxSpeed,
+  OUT UINT8               *NumOfPort,
+  OUT UINT8               *Is64BitCapable
+  );
+
+/**
+  Reset the host controller.
+
+  @param  UsbBus                The usb bus driver.
+  @param  Attributes            The reset type, only global reset is used by this driver.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+**/
+EFI_STATUS
+UsbHcReset (
+  IN USB_BUS              *UsbBus,
+  IN UINT16               Attributes
+  );
+
+/**
+  Get the current operation state of the host controller.
+
+  @param  UsbBus           The USB bus driver.
+  @param  State            The host controller operation state.
+
+  @retval EFI_SUCCESS      The operation state is returned in State.
+  @retval Others           Failed to get the host controller state.
+
+**/
+EFI_STATUS
+UsbHcGetState (
+  IN  USB_BUS             *UsbBus,
+  OUT EFI_USB_HC_STATE    *State
+  );
+
+/**
+  Set the host controller operation state.
+
+  @param  UsbBus           The USB bus driver.
+  @param  State            The state to set.
+
+  @retval EFI_SUCCESS      The host controller is now working at State.
+  @retval Others           Failed to set operation state.
+
+**/
+EFI_STATUS
+UsbHcSetState (
+  IN  USB_BUS             *UsbBus,
+  IN EFI_USB_HC_STATE     State
+  );
+
+/**
+  Get the root hub port state.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The index of port.
+  @param  PortStatus       The variable to save port state.
+
+  @retval EFI_SUCCESS      The root port state is returned in.
+  @retval Others           Failed to get the root hub port state.
+
+**/
+EFI_STATUS
+UsbHcGetRootHubPortStatus (
+  IN  USB_BUS             *UsbBus,
+  IN  UINT8               PortIndex,
+  OUT EFI_USB_PORT_STATUS *PortStatus
+  );
+
+/**
+  Set the root hub port feature.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The port index.
+  @param  Feature          The port feature to set.
+
+  @retval EFI_SUCCESS      The port feature is set.
+  @retval Others           Failed to set port feature.
+
+**/
+EFI_STATUS
+UsbHcSetRootHubPortFeature (
+  IN USB_BUS              *UsbBus,
+  IN UINT8                PortIndex,
+  IN EFI_USB_PORT_FEATURE Feature
+  );
+
+/**
+  Clear the root hub port feature.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The port index.
+  @param  Feature          The port feature to clear.
+
+  @retval EFI_SUCCESS      The port feature is clear.
+  @retval Others           Failed to clear port feature.
+
+**/
+EFI_STATUS
+UsbHcClearRootHubPortFeature (
+  IN USB_BUS              *UsbBus,
+  IN UINT8                PortIndex,
+  IN EFI_USB_PORT_FEATURE Feature
+  );
+
+/**
+  Execute a control transfer to the device.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The device address.
+  @param  DevSpeed         The device speed.
+  @param  MaxPacket        Maximum packet size of endpoint 0.
+  @param  Request          The control transfer request.
+  @param  Direction        The direction of data stage.
+  @param  Data             The buffer holding data.
+  @param  DataLength       The length of the data.
+  @param  TimeOut          Timeout (in ms) to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of transfer.
+
+  @retval EFI_SUCCESS      The control transfer finished without error.
+  @retval Others           The control transfer failed, reason returned in UsbReslt.
+
+**/
+EFI_STATUS
+UsbHcControlTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  EFI_USB_DEVICE_REQUEST              *Request,
+  IN  EFI_USB_DATA_DIRECTION              Direction,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  );
+
+/**
+  Execute a bulk transfer to the device's endpoint.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  DataToggle       On input, the initial data toggle to use, also  return
+                           the next toggle on output.
+  @param  TimeOut          The time to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_SUCCESS      The bulk transfer is finished without error.
+  @retval Others           Failed to execute bulk transfer, result in UsbResult.
+
+**/
+EFI_STATUS
+UsbHcBulkTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  );
+
+/**
+  Queue or cancel an asynchronous interrupt transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  IsNewTransfer    Whether this is a new request. If not, cancel the old
+                           request.
+  @param  DataToggle       Data toggle to use on input, next toggle on output.
+  @param  PollingInterval  The interval to poll the interrupt transfer (in ms).
+  @param  DataLength       The length of periodical data receive.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  Callback         Function to call when data is received.
+  @param  Context          The context to the callback.
+
+  @retval EFI_SUCCESS      The asynchronous transfer is queued.
+  @retval Others           Failed to queue the transfer.
+
+**/
+EFI_STATUS
+UsbHcAsyncInterruptTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  BOOLEAN                             IsNewTransfer,
+  IN OUT UINT8                            *DataToggle,
+  IN  UINTN                               PollingInterval,
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     Callback,
+  IN  VOID                                *Context OPTIONAL
+  );
+
+/**
+  Execute a synchronous interrupt transfer to the target endpoint.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  Data             Pointer to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  DataToggle       On input, the initial data toggle to use, also  return
+                           the next toggle on output.
+  @param  TimeOut          The time to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_SUCCESS      The synchronous interrupt transfer is OK.
+  @retval Others           Failed to execute the synchronous interrupt transfer.
+
+**/
+EFI_STATUS
+UsbHcSyncInterruptTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN OUT VOID                             *Data,
+  IN OUT UINTN                            *DataLength,
+  IN OUT UINT8                            *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  );
+
+/**
+  Execute a synchronous Isochronous USB transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_UNSUPPORTED  The isochronous transfer isn't supported now.
+
+**/
+EFI_STATUS
+UsbHcIsochronousTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  );
+
+/**
+  Queue an asynchronous isochronous transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  Callback         The function to call when data is transferred.
+  @param  Context          The context to the callback function.
+
+  @retval EFI_UNSUPPORTED  The asynchronous isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+UsbHcAsyncIsochronousTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN OUT VOID                             *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     Callback,
+  IN  VOID                                *Context
+  );
+
+/**
+  Open the USB host controller protocol BY_CHILD.
+
+  @param  Bus              The USB bus driver.
+  @param  Child            The child handle.
+
+  @return The open protocol return.
+
+**/
+EFI_STATUS
+UsbOpenHostProtoByChild (
+  IN USB_BUS              *Bus,
+  IN EFI_HANDLE           Child
+  );
+
+/**
+  Close the USB host controller protocol BY_CHILD.
+
+  @param  Bus              The USB bus driver.
+  @param  Child            The child handle.
+
+  @return None.
+
+**/
+VOID
+UsbCloseHostProtoByChild (
+  IN USB_BUS              *Bus,
+  IN EFI_HANDLE           Child
+  );
+
+/**
+  return the current TPL, copied from the EDKII glue lib.
+
+  @param  VOID.
+
+  @return Current TPL.
+
+**/
+EFI_TPL
+UsbGetCurrentTpl (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.c
new file mode 100644
index 0000000000..16a7b589c1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.c
@@ -0,0 +1,670 @@
+/** @file
+Usb Hub Request Support In PEI Phase
+
+Copyright (c) 2006 - 2014, 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 "UsbPeim.h"
+#include "HubPeim.h"
+#include "PeiUsbLib.h"
+
+/**
+  Get a given hub port status.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  PortStatus    Current Hub port status and change status.
+
+  @retval EFI_SUCCESS       Port status is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the port status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubGetPortStatus (
+  IN  EFI_PEI_SERVICES    **PeiServices,
+  IN  PEI_USB_IO_PPI      *UsbIoPpi,
+  IN  UINT8               Port,
+  OUT UINT32              *PortStatus
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_GET_PORT_STATUS_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_GET_PORT_STATUS;
+  DeviceRequest.Index       = Port;
+  DeviceRequest.Length      = (UINT16) sizeof (UINT32);
+
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbDataIn,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     PortStatus,
+                     sizeof (UINT32)
+                     );
+
+}
+
+/**
+  Set specified feature to a given hub port.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  Value         New feature value.
+
+  @retval EFI_SUCCESS       Port feature is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubSetPortFeature (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               Port,
+  IN UINT8               Value
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_SET_PORT_FEATURE_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_SET_PORT_FEATURE;
+  DeviceRequest.Value       = Value;
+  DeviceRequest.Index       = Port;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear specified feature on a given hub port.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  Value         Feature value that will be cleared from the hub port.
+
+  @retval EFI_SUCCESS       Port feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubClearPortFeature (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               Port,
+  IN UINT8               Value
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_CLEAR_FEATURE_PORT_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_CLEAR_FEATURE_PORT;
+  DeviceRequest.Value       = Value;
+  DeviceRequest.Index       = Port;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Get a given hub status.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  HubStatus     Current Hub status and change status.
+
+  @retval EFI_SUCCESS       Hub status is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubGetHubStatus (
+  IN  EFI_PEI_SERVICES    **PeiServices,
+  IN  PEI_USB_IO_PPI      *UsbIoPpi,
+  OUT UINT32              *HubStatus
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_GET_HUB_STATUS_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_GET_HUB_STATUS;
+  DeviceRequest.Length      = (UINT16) sizeof (UINT32);
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbDataIn,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     HubStatus,
+                     sizeof (UINT32)
+                     );
+}
+
+/**
+  Set specified feature to a given hub.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Value         New feature value.
+
+  @retval EFI_SUCCESS       Port feature is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubSetHubFeature (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               Value
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_SET_HUB_FEATURE_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_SET_HUB_FEATURE;
+  DeviceRequest.Value       = Value;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear specified feature on a given hub.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Value         Feature value that will be cleared from the hub port.
+
+  @retval EFI_SUCCESS       Hub feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the hub feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubClearHubFeature (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               Value
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_CLEAR_FEATURE_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_CLEAR_FEATURE;
+  DeviceRequest.Value       = Value;
+
+  return  UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DeviceRequest,
+                      EfiUsbNoData,
+                      PcdGet32 (PcdUsbTransferTimeoutValue),
+                      NULL,
+                      0
+                      );
+}
+
+/**
+  Get a given hub descriptor.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi       Indicates the PEI_USB_IO_PPI instance.
+  @param  DescriptorSize The length of Hub Descriptor buffer.
+  @param  HubDescriptor  Caller allocated buffer to store the hub descriptor if
+                         successfully returned.
+
+  @retval EFI_SUCCESS       Hub descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiGetHubDescriptor (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_USB_IO_PPI            *UsbIoPpi,
+  IN  UINTN                     DescriptorSize,
+  OUT EFI_USB_HUB_DESCRIPTOR    *HubDescriptor
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DevReq.RequestType = USB_RT_HUB | 0x80;
+  DevReq.Request     = USB_HUB_GET_DESCRIPTOR;
+  DevReq.Value       = USB_DT_HUB << 8;
+  DevReq.Length      = (UINT16)DescriptorSize;
+
+  return  UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DevReq,
+                      EfiUsbDataIn,
+                      PcdGet32 (PcdUsbTransferTimeoutValue),
+                      HubDescriptor,
+                      (UINT16)DescriptorSize
+                      );
+}
+
+/**
+  Get a given SuperSpeed hub descriptor.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  HubDescriptor     Caller allocated buffer to store the hub descriptor if
+                            successfully returned.
+
+  @retval EFI_SUCCESS       Hub descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiGetSuperSpeedHubDesc (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_USB_IO_PPI            *UsbIoPpi,
+  OUT EFI_USB_HUB_DESCRIPTOR    *HubDescriptor
+  )
+{
+  EFI_USB_DEVICE_REQUEST        DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DevReq.RequestType = USB_RT_HUB | 0x80;
+  DevReq.Request     = USB_HUB_GET_DESCRIPTOR;
+  DevReq.Value       = USB_DT_SUPERSPEED_HUB << 8;
+  DevReq.Length      = 12;
+
+  return  UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DevReq,
+                      EfiUsbDataIn,
+                      PcdGet32 (PcdUsbTransferTimeoutValue),
+                      HubDescriptor,
+                      12
+                      );
+}
+
+/**
+  Read the whole usb hub descriptor. It is necessary
+  to do it in two steps because hub descriptor is of
+  variable length.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  PeiUsbDevice      Indicates the hub controller device.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  HubDescriptor     Caller allocated buffer to store the hub descriptor if
+                            successfully returned.
+
+  @retval EFI_SUCCESS       Hub descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbHubReadDesc (
+  IN EFI_PEI_SERVICES           **PeiServices,
+  IN PEI_USB_DEVICE             *PeiUsbDevice,
+  IN PEI_USB_IO_PPI             *UsbIoPpi,
+  OUT EFI_USB_HUB_DESCRIPTOR    *HubDescriptor
+  )
+{
+  EFI_STATUS Status;
+
+  if (PeiUsbDevice->DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    //
+    // Get the super speed hub descriptor
+    //
+    Status = PeiGetSuperSpeedHubDesc (PeiServices, UsbIoPpi, HubDescriptor);
+  } else {
+
+    //
+    // First get the hub descriptor length
+    //
+    Status = PeiGetHubDescriptor (PeiServices, UsbIoPpi, 2, HubDescriptor);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the whole hub descriptor
+    //
+    Status = PeiGetHubDescriptor (PeiServices, UsbIoPpi, HubDescriptor->Length, HubDescriptor);
+  }
+
+  return Status;
+}
+
+/**
+  USB hub control transfer to set the hub depth.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  PeiUsbDevice      Indicates the hub controller device.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       Depth of the hub is set.
+  @retval Others            Failed to set the depth.
+
+**/
+EFI_STATUS
+PeiUsbHubCtrlSetHubDepth (
+  IN EFI_PEI_SERVICES           **PeiServices,
+  IN PEI_USB_DEVICE             *PeiUsbDevice,
+  IN PEI_USB_IO_PPI             *UsbIoPpi
+  )
+{
+  EFI_USB_DEVICE_REQUEST        DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DevReq.RequestType = USB_RT_HUB;
+  DevReq.Request     = USB_HUB_REQ_SET_DEPTH;
+  DevReq.Value       = PeiUsbDevice->Tier;
+  DevReq.Length      = 0;
+
+  return  UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DevReq,
+                      EfiUsbNoData,
+                      PcdGet32 (PcdUsbTransferTimeoutValue),
+                      NULL,
+                      0
+                      );
+}
+
+/**
+  Configure a given hub.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  PeiUsbDevice   Indicating the hub controller device that will be configured
+
+  @retval EFI_SUCCESS       Hub configuration is done successfully.
+  @retval EFI_DEVICE_ERROR  Cannot configure the hub due to a hardware error.
+
+**/
+EFI_STATUS
+PeiDoHubConfig (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_DEVICE      *PeiUsbDevice
+  )
+{
+  EFI_USB_HUB_DESCRIPTOR  HubDescriptor;
+  EFI_STATUS              Status;
+  EFI_USB_HUB_STATUS      HubStatus;
+  UINTN                   Index;
+  PEI_USB_IO_PPI          *UsbIoPpi;
+
+  ZeroMem (&HubDescriptor, sizeof (HubDescriptor));
+  UsbIoPpi = &PeiUsbDevice->UsbIoPpi;
+
+  //
+  // Get the hub descriptor 
+  //
+  Status = PeiUsbHubReadDesc (
+            PeiServices,
+            PeiUsbDevice,
+            UsbIoPpi,
+            &HubDescriptor
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  PeiUsbDevice->DownStreamPortNo = HubDescriptor.NbrPorts;
+
+  if (PeiUsbDevice->DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    DEBUG ((EFI_D_INFO, "PeiDoHubConfig: Set Hub Depth as 0x%x\n", PeiUsbDevice->Tier));
+    PeiUsbHubCtrlSetHubDepth (
+      PeiServices,
+      PeiUsbDevice,
+      UsbIoPpi
+      );
+  } else {
+    //
+    //  Power all the hub ports
+    //
+    for (Index = 0; Index < PeiUsbDevice->DownStreamPortNo; Index++) {
+      Status = PeiHubSetPortFeature (
+                PeiServices,
+                UsbIoPpi,
+                (UINT8) (Index + 1),
+                EfiUsbPortPower
+                );
+      if (EFI_ERROR (Status)) {
+        DEBUG (( EFI_D_ERROR, "PeiDoHubConfig: PeiHubSetPortFeature EfiUsbPortPower failed %x\n", Index));
+        continue;
+      }
+    }
+
+    DEBUG (( EFI_D_INFO, "PeiDoHubConfig: HubDescriptor.PwrOn2PwrGood: 0x%x\n", HubDescriptor.PwrOn2PwrGood));
+    if (HubDescriptor.PwrOn2PwrGood > 0) {
+      MicroSecondDelay (HubDescriptor.PwrOn2PwrGood * USB_SET_PORT_POWER_STALL);
+    }
+
+    //
+    // Clear Hub Status Change
+    //
+    Status = PeiHubGetHubStatus (
+              PeiServices,
+              UsbIoPpi,
+              (UINT32 *) &HubStatus
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    } else {
+      //
+      // Hub power supply change happens
+      //
+      if ((HubStatus.HubChangeStatus & HUB_CHANGE_LOCAL_POWER) != 0) {
+        PeiHubClearHubFeature (
+          PeiServices,
+          UsbIoPpi,
+          C_HUB_LOCAL_POWER
+          );
+      }
+      //
+      // Hub change overcurrent happens
+      //
+      if ((HubStatus.HubChangeStatus & HUB_CHANGE_OVERCURRENT) != 0) {
+        PeiHubClearHubFeature (
+          PeiServices,
+          UsbIoPpi,
+          C_HUB_OVER_CURRENT
+          );
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send reset signal over the given root hub port.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi       Indicates the PEI_USB_IO_PPI instance.
+  @param  PortNum        Usb hub port number (starting from 1).
+
+**/
+VOID
+PeiResetHubPort (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               PortNum
+  )
+{
+  EFI_STATUS          Status;
+  UINTN               Index;
+  EFI_USB_PORT_STATUS HubPortStatus;
+
+  MicroSecondDelay (100 * 1000);
+
+  //
+  // reset root port
+  //
+  PeiHubSetPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortReset
+    );
+
+  //
+  // Drive the reset signal for worst 20ms. Check USB 2.0 Spec
+  // section 7.1.7.5 for timing requirements.
+  //
+  MicroSecondDelay (USB_SET_PORT_RESET_STALL);
+
+  //
+  // Check USB_PORT_STAT_C_RESET bit to see if the resetting state is done.
+  //
+  ZeroMem (&HubPortStatus, sizeof (EFI_USB_PORT_STATUS));
+
+  for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
+    Status = PeiHubGetPortStatus (
+               PeiServices,
+               UsbIoPpi,
+               PortNum,
+               (UINT32 *) &HubPortStatus
+               );
+
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+
+    if (USB_BIT_IS_SET (HubPortStatus.PortChangeStatus, USB_PORT_STAT_C_RESET)) {
+      break;
+    }
+
+    MicroSecondDelay (USB_WAIT_PORT_STS_CHANGE_STALL);
+  }
+
+  if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {
+    DEBUG ((EFI_D_ERROR, "PeiResetHubPort: reset not finished in time on port %d\n", PortNum));
+    return;
+  }
+
+  //
+  // clear reset change root port
+  //
+  PeiHubClearPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortResetChange
+    );
+
+  MicroSecondDelay (1 * 1000);
+
+  PeiHubClearPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortConnectChange
+    );
+
+  //
+  // Set port enable
+  //
+  PeiHubSetPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortEnable
+    );
+
+  //
+  // Clear any change status
+  //
+
+  PeiHubClearPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortEnableChange
+    );
+
+  MicroSecondDelay (10 * 1000);
+
+  return;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.h
new file mode 100644
index 0000000000..f50bc63501
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.h
@@ -0,0 +1,281 @@
+/** @file
+Constants definitions for Usb Hub Peim
+
+Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PEI_HUB_PEIM_H_
+#define _PEI_HUB_PEIM_H_
+
+
+//
+// Hub feature numbers
+//
+#define C_HUB_LOCAL_POWER   0
+#define C_HUB_OVER_CURRENT  1
+
+//
+// Hub class code & sub class code
+//
+#define CLASS_CODE_HUB      0x09
+#define SUB_CLASS_CODE_HUB  0
+
+//
+// Hub Status & Hub Change bit masks
+//
+#define HUB_STATUS_LOCAL_POWER  0x0001
+#define HUB_STATUS_OVERCURRENT  0x0002
+
+#define HUB_CHANGE_LOCAL_POWER  0x0001
+#define HUB_CHANGE_OVERCURRENT  0x0002
+
+//
+// Hub Characteristics
+//
+#define HUB_CHAR_LPSM     0x0003
+#define HUB_CHAR_COMPOUND 0x0004
+#define HUB_CHAR_OCPM     0x0018
+
+//
+// Standard hub request and request type
+// By [Spec-USB20/Chapter-11.24]
+//
+#define USB_HUB_CLEAR_FEATURE               0x01
+#define USB_HUB_CLEAR_FEATURE_REQ_TYPE      0x20
+
+#define USB_HUB_CLEAR_FEATURE_PORT          0x01
+#define USB_HUB_CLEAR_FEATURE_PORT_REQ_TYPE 0x23
+
+#define USB_HUB_GET_BUS_STATE               0x02
+#define USB_HUB_GET_BUS_STATE_REQ_TYPE      0xA3
+
+#define USB_HUB_GET_DESCRIPTOR              0x06
+#define USB_HUB_GET_DESCRIPTOR_REQ_TYPE     0xA0
+
+#define USB_HUB_GET_HUB_STATUS              0x00
+#define USB_HUB_GET_HUB_STATUS_REQ_TYPE     0xA0
+
+#define USB_HUB_GET_PORT_STATUS             0x00
+#define USB_HUB_GET_PORT_STATUS_REQ_TYPE    0xA3
+
+#define USB_HUB_SET_DESCRIPTOR              0x07
+#define USB_HUB_SET_DESCRIPTOR_REQ_TYPE     0x20
+
+#define USB_HUB_SET_HUB_FEATURE             0x03
+#define USB_HUB_SET_HUB_FEATURE_REQ_TYPE    0x20
+
+#define USB_HUB_SET_PORT_FEATURE            0x03
+#define USB_HUB_SET_PORT_FEATURE_REQ_TYPE   0x23
+
+#define USB_RT_HUB        (USB_TYPE_CLASS | USB_RECIP_DEVICE)
+#define USB_RT_PORT       (USB_TYPE_CLASS | USB_RECIP_OTHER)
+
+#define USB_HUB_REQ_SET_DEPTH               12
+
+#define MAXBYTES  8
+#pragma pack(1)
+//
+// Hub descriptor, the last two fields are of variable lenght.
+//
+typedef struct {
+  UINT8 Length;
+  UINT8 DescriptorType;
+  UINT8 NbrPorts;
+  UINT8 HubCharacteristics[2];
+  UINT8 PwrOn2PwrGood;
+  UINT8 HubContrCurrent;
+  UINT8 Filler[MAXBYTES];
+} EFI_USB_HUB_DESCRIPTOR;
+
+typedef struct {
+  UINT16  HubStatus;
+  UINT16  HubChangeStatus;
+} EFI_USB_HUB_STATUS;
+
+#pragma pack()
+/**
+  Get a given hub port status.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  PortStatus    Current Hub port status and change status.
+
+  @retval EFI_SUCCESS       Port status is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the port status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubGetPortStatus (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Port,
+  OUT UINT32                *PortStatus
+  );
+
+/**
+  Set specified feature to a given hub port.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  Value         New feature value.
+
+  @retval EFI_SUCCESS       Port feature is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubSetPortFeature (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Port,
+  IN  UINT8                 Value
+  );
+
+/**
+  Set specified feature to a given hub.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Value         New feature value.
+
+  @retval EFI_SUCCESS       Port feature is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubSetHubFeature (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Value
+  );
+
+/**
+  Get a given hub status.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  HubStatus     Current Hub status and change status.
+
+  @retval EFI_SUCCESS       Hub status is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubGetHubStatus (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  OUT UINT32                *HubStatus
+  );
+
+/**
+  Clear specified feature on a given hub port.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  Value         Feature value that will be cleared from the hub port.
+
+  @retval EFI_SUCCESS       Port feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubClearPortFeature (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Port,
+  IN  UINT8                 Value
+  );
+
+/**
+  Clear specified feature on a given hub.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Value         Feature value that will be cleared from the hub port.
+
+  @retval EFI_SUCCESS       Hub feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the hub feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubClearHubFeature (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Value
+  );
+
+/**
+  Get a given hub descriptor.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi       Indicates the PEI_USB_IO_PPI instance.
+  @param  DescriptorSize The length of Hub Descriptor buffer.
+  @param  HubDescriptor  Caller allocated buffer to store the hub descriptor if
+                         successfully returned.
+
+  @retval EFI_SUCCESS       Hub descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiGetHubDescriptor (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINTN                    DescriptorSize,
+  OUT EFI_USB_HUB_DESCRIPTOR  *HubDescriptor
+  );
+
+/**
+  Configure a given hub.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  PeiUsbDevice   Indicating the hub controller device that will be configured
+
+  @retval EFI_SUCCESS       Hub configuration is done successfully.
+  @retval EFI_DEVICE_ERROR  Cannot configure the hub due to a hardware error.
+
+**/
+EFI_STATUS
+PeiDoHubConfig (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_DEVICE           *PeiUsbDevice
+  );
+
+/**
+  Send reset signal over the given root hub port.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi       Indicates the PEI_USB_IO_PPI instance.
+  @param  PortNum        Usb hub port number (starting from 1).
+
+**/
+VOID
+PeiResetHubPort (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               PortNum
+  );
+
+#endif
+
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.c b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.c
new file mode 100644
index 0000000000..42be13ac3b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.c
@@ -0,0 +1,269 @@
+/** @file
+Common Libarary for PEI USB
+
+Copyright (c) 2006 - 2014, 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 "UsbPeim.h"
+#include "PeiUsbLib.h"
+
+/**
+  Get a given usb descriptor.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  Value              Request Value.
+  @param  Index              Request Index.
+  @param  DescriptorLength   Request descriptor Length.
+  @param  Descriptor         Request descriptor.
+
+
+  @retval EFI_SUCCESS       Usb descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the usb descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetDescriptor (
+  IN  EFI_PEI_SERVICES         **PeiServices,
+  IN  PEI_USB_IO_PPI           *UsbIoPpi,
+  IN  UINT16                   Value,
+  IN  UINT16                   Index,
+  IN  UINT16                   DescriptorLength,
+  OUT VOID                     *Descriptor
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  DevReq.RequestType  = USB_DEV_GET_DESCRIPTOR_REQ_TYPE;
+  DevReq.Request      = USB_DEV_GET_DESCRIPTOR;
+  DevReq.Value        = Value;
+  DevReq.Index        = Index;
+  DevReq.Length       = DescriptorLength;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbDataIn,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     Descriptor,
+                     DescriptorLength
+                     );
+}
+
+/**
+  Set a usb device with a specified address.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  AddressValue       The address to assign.
+
+  @retval EFI_SUCCESS       Usb device address is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the usb address due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetDeviceAddress (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT16                   AddressValue
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  DevReq.RequestType  = USB_DEV_SET_ADDRESS_REQ_TYPE;
+  DevReq.Request      = USB_DEV_SET_ADDRESS;
+  DevReq.Value        = AddressValue;
+  DevReq.Index        = 0;
+  DevReq.Length       = 0;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear a given usb feature.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  Recipient         The recipient of ClearFeature Request, should be one of Device/Interface/Endpoint.
+  @param  Value             Request Value.
+  @param  Target            Request Index.
+
+  @retval EFI_SUCCESS       Usb feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the usb feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearDeviceFeature (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN EFI_USB_RECIPIENT        Recipient,
+  IN UINT16                   Value,
+  IN UINT16                   Target
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  switch (Recipient) {
+  case EfiUsbDevice:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_D;
+    break;
+
+  case EfiUsbInterface:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_I;
+    break;
+
+  case EfiUsbEndpoint:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_E;
+    break;
+  }
+
+  DevReq.Request      = USB_DEV_CLEAR_FEATURE;
+  DevReq.Value        = Value;
+  DevReq.Index        = Target;
+  DevReq.Length       = 0;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Configure a usb device to Configuration 1.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       Usb device is set to use Configuration 1 successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the usb device due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetConfiguration (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  DevReq.RequestType  = USB_DEV_SET_CONFIGURATION_REQ_TYPE;
+  DevReq.Request      = USB_DEV_SET_CONFIGURATION;
+  DevReq.Value        = 1;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Judge if the port is connected with a usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A usb device is connected with the port.
+  @retval FALSE       No usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortConnect (
+  IN UINT16  PortStatus
+  )
+{
+  //
+  // return the bit 0 value of PortStatus
+  //
+  if ((PortStatus & USB_PORT_STAT_CONNECTION) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Get device speed according to port status.
+
+  @param    PortStatus  The usb port status gotten.
+
+  @return   Device speed value.
+
+**/
+UINTN
+PeiUsbGetDeviceSpeed (
+  IN UINT16 PortStatus
+  )
+{
+  if ((PortStatus & USB_PORT_STAT_LOW_SPEED) != 0) {
+    return EFI_USB_SPEED_LOW;
+  } else if ((PortStatus & USB_PORT_STAT_HIGH_SPEED) != 0){
+    return EFI_USB_SPEED_HIGH;
+  } else if ((PortStatus & USB_PORT_STAT_SUPER_SPEED) != 0) {
+    return EFI_USB_SPEED_SUPER;
+  } else {
+    return EFI_USB_SPEED_FULL;
+  }
+}
+
+/**
+  Judge if the port is in "connection change" status or not.
+
+  @param  PortChangeStatus  The usb port change status gotten.
+
+  @retval TRUE              The port is in "connection change" status.
+  @retval FALSE             The port is NOT in "connection change" status.
+
+**/
+BOOLEAN
+IsPortConnectChange (
+  IN UINT16  PortChangeStatus
+  )
+{
+  //
+  // return the bit 0 value of PortChangeStatus
+  //
+  if ((PortChangeStatus & USB_PORT_STAT_C_CONNECTION) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.h b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.h
new file mode 100644
index 0000000000..1ace89fbc3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.h
@@ -0,0 +1,231 @@
+/** @file
+Common Libarary for PEI USB
+
+Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved. <BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PEI_USB_LIB_H_
+#define _PEI_USB_LIB_H_
+
+
+//
+// Standard device request and request type
+// By [Spec-USB20/Chapter-9.4]
+//
+#define USB_DEV_GET_STATUS                  0x00
+#define USB_DEV_GET_STATUS_REQ_TYPE_D       0x80 // Receiver : Device
+#define USB_DEV_GET_STATUS_REQ_TYPE_I       0x81 // Receiver : Interface
+#define USB_DEV_GET_STATUS_REQ_TYPE_E       0x82 // Receiver : Endpoint
+
+#define USB_DEV_CLEAR_FEATURE               0x01
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_D    0x00 // Receiver : Device
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_I    0x01 // Receiver : Interface
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_E    0x02 // Receiver : Endpoint
+
+#define USB_DEV_SET_FEATURE                 0x03
+#define USB_DEV_SET_FEATURE_REQ_TYPE_D      0x00 // Receiver : Device
+#define USB_DEV_SET_FEATURE_REQ_TYPE_I      0x01 // Receiver : Interface
+#define USB_DEV_SET_FEATURE_REQ_TYPE_E      0x02 // Receiver : Endpoint
+
+#define USB_DEV_SET_ADDRESS                 0x05
+#define USB_DEV_SET_ADDRESS_REQ_TYPE        0x00
+
+#define USB_DEV_GET_DESCRIPTOR              0x06
+#define USB_DEV_GET_DESCRIPTOR_REQ_TYPE     0x80
+
+#define USB_DEV_SET_DESCRIPTOR              0x07
+#define USB_DEV_SET_DESCRIPTOR_REQ_TYPE     0x00
+
+#define USB_DEV_GET_CONFIGURATION           0x08
+#define USB_DEV_GET_CONFIGURATION_REQ_TYPE  0x80
+
+#define USB_DEV_SET_CONFIGURATION           0x09
+#define USB_DEV_SET_CONFIGURATION_REQ_TYPE  0x00
+
+#define USB_DEV_GET_INTERFACE               0x0A
+#define USB_DEV_GET_INTERFACE_REQ_TYPE      0x81
+
+#define USB_DEV_SET_INTERFACE               0x0B
+#define USB_DEV_SET_INTERFACE_REQ_TYPE      0x01
+
+#define USB_DEV_SYNCH_FRAME                 0x0C
+#define USB_DEV_SYNCH_FRAME_REQ_TYPE        0x82
+
+//
+// USB Descriptor types
+//
+#define USB_DT_DEVICE     0x01
+#define USB_DT_CONFIG     0x02
+#define USB_DT_STRING     0x03
+#define USB_DT_INTERFACE  0x04
+#define USB_DT_ENDPOINT   0x05
+#define USB_DT_HUB        0x29
+#define USB_DT_SUPERSPEED_HUB 0x2A
+#define USB_DT_HID        0x21
+
+//
+// USB request type
+//
+#define USB_TYPE_STANDARD (0x00 << 5)
+#define USB_TYPE_CLASS    (0x01 << 5)
+#define USB_TYPE_VENDOR   (0x02 << 5)
+#define USB_TYPE_RESERVED (0x03 << 5)
+
+//
+// USB request targer device
+//
+#define USB_RECIP_DEVICE    0x00
+#define USB_RECIP_INTERFACE 0x01
+#define USB_RECIP_ENDPOINT  0x02
+#define USB_RECIP_OTHER     0x03
+
+typedef enum {
+  EfiUsbEndpointHalt,
+  EfiUsbDeviceRemoteWakeup
+} EFI_USB_STANDARD_FEATURE_SELECTOR;
+
+//
+// Usb Data recipient type
+//
+typedef enum {
+  EfiUsbDevice,
+  EfiUsbInterface,
+  EfiUsbEndpoint
+} EFI_USB_RECIPIENT;
+
+/**
+  Get a given usb descriptor.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  Value              Request Value.
+  @param  Index              Request Index.
+  @param  DescriptorLength   Request descriptor Length.
+  @param  Descriptor         Request descriptor.
+
+
+  @retval EFI_SUCCESS       Usb descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the usb descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetDescriptor (
+  IN  EFI_PEI_SERVICES         **PeiServices,
+  IN  PEI_USB_IO_PPI           *UsbIoPpi,
+  IN  UINT16                   Value,
+  IN  UINT16                   Index,
+  IN  UINT16                   DescriptorLength,
+  OUT VOID                     *Descriptor
+  );
+
+/**
+  Set a usb device with a specified address.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  AddressValue       The address to assign.
+
+  @retval EFI_SUCCESS        Usb device address is set successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb address due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetDeviceAddress (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT16                   AddressValue
+  );
+
+/**
+  Clear a given usb feature.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  Recipient         The recipient of ClearFeature Request, should be one of Device/Interface/Endpoint.
+  @param  Value             Request Value.
+  @param  Target            Request Index.
+
+  @retval EFI_SUCCESS       Usb feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the usb feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearDeviceFeature (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN EFI_USB_RECIPIENT        Recipient,
+  IN UINT16                   Value,
+  IN UINT16                   Target
+  );
+
+/**
+  Configure a usb device to Configuration 1.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS        Usb device is set to use Configuration 1 successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb device due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetConfiguration (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi
+  );
+
+/**
+  Judge if the port is connected with a usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A usb device is connected with the port.
+  @retval FALSE       No usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortConnect (
+  IN UINT16  PortStatus
+  );
+
+/**
+  Get device speed according to port status.
+
+  @param    PortStatus  The usb port status gotten.
+
+  @return   Device speed value.
+
+**/
+UINTN
+PeiUsbGetDeviceSpeed (
+  IN UINT16 PortStatus
+  );
+
+/**
+  Judge if the port is in "connection change" status or not.
+
+  @param  PortChangeStatus  The usb port change status gotten.
+
+  @retval TRUE              The port is in "connection change" status.
+  @retval FALSE             The port is NOT in "connection change" status.
+
+**/
+BOOLEAN
+IsPortConnectChange (
+  IN UINT16  PortChangeStatus
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.inf b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.inf
new file mode 100644
index 0000000000..734619a2fa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.inf
@@ -0,0 +1,67 @@
+## @file
+# The Usb Bus Peim driver is used to support recovery from usb device.
+#
+# Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions
+# of the BSD License which accompanies this distribution.  The
+# full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbBusPei
+  MODULE_UNI_FILE                = UsbBusPei.uni
+  FILE_GUID                      = 8401A045-6F70-4505-8471-7015B40355E3
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = PeimInitializeUsb
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  PeiUsbLib.c
+  HubPeim.c
+  UsbIoPeim.c
+  UsbPeim.c
+  UsbPeim.h
+  PeiUsbLib.h
+  HubPeim.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  TimerLib
+  BaseMemoryLib
+  PeiServicesLib
+  PeimEntryPoint
+  DebugLib
+  PcdLib
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUsbTransferTimeoutValue  ## CONSUMES
+  
+[Ppis]
+  gPeiUsbIoPpiGuid                              ## PRODUCES
+  gPeiUsbHostControllerPpiGuid                  ## SOMETIMES_CONSUMES
+  gPeiUsb2HostControllerPpiGuid                 ## SOMETIMES_CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid AND gPeiUsb2HostControllerPpiGuid OR gPeiUsbHostControllerPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbBusPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.uni b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.uni
new file mode 100644
index 0000000000..66a8c01eda
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPeiExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPeiExtra.uni
new file mode 100644
index 0000000000..3520fdba45
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbIoPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbIoPeim.c
new file mode 100644
index 0000000000..d13a7ee0a3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbIoPeim.c
@@ -0,0 +1,372 @@
+/** @file
+The module is used to implement Usb Io PPI interfaces.
+
+Copyright (c) 2006 - 2014, 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 "UsbPeim.h"
+#include "PeiUsbLib.h"
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_IO_PPI.
+  @param  Request                USB device request to send.
+  @param  Direction              Specifies the data direction for the data stage.
+  @param  Timeout                Indicates the maximum timeout, in millisecond. If Timeout
+                                 is 0, then the caller must wait for the function to be
+                                 completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbControlTransfer (
+  IN     EFI_PEI_SERVICES          **PeiServices,
+  IN     PEI_USB_IO_PPI            *This,
+  IN     EFI_USB_DEVICE_REQUEST    *Request,
+  IN     EFI_USB_DATA_DIRECTION    Direction,
+  IN     UINT32                    Timeout,
+  IN OUT VOID                      *Data,      OPTIONAL
+  IN     UINTN                     DataLength  OPTIONAL
+  )
+{
+  EFI_STATUS                  Status;
+  PEI_USB_DEVICE              *PeiUsbDev;
+  UINT32                      TransferResult;
+  EFI_USB_ENDPOINT_DESCRIPTOR *EndpointDescriptor;
+  UINT8                       EndpointIndex;
+
+  PeiUsbDev = PEI_USB_DEVICE_FROM_THIS (This);
+
+  EndpointDescriptor = NULL;
+  EndpointIndex = 0;
+
+  if ((Request->Request     == USB_REQ_CLEAR_FEATURE) &&
+      (Request->RequestType == USB_DEV_CLEAR_FEATURE_REQ_TYPE_E) &&
+      (Request->Value       == USB_FEATURE_ENDPOINT_HALT)) {
+    //
+    // Request->Index is the Endpoint Address, use it to get the Endpoint Index.
+    //
+    while (EndpointIndex < MAX_ENDPOINT) {
+      Status = PeiUsbGetEndpointDescriptor (PeiServices, This, EndpointIndex, &EndpointDescriptor);
+      if (EFI_ERROR (Status)) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      if (EndpointDescriptor->EndpointAddress == Request->Index) {
+        break;
+      }
+
+      EndpointIndex++;
+    }
+
+    if (EndpointIndex == MAX_ENDPOINT) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  if (PeiUsbDev->Usb2HcPpi != NULL) {
+    Status = PeiUsbDev->Usb2HcPpi->ControlTransfer (
+                        PeiServices,
+                        PeiUsbDev->Usb2HcPpi,
+                        PeiUsbDev->DeviceAddress,
+                        PeiUsbDev->DeviceSpeed,
+                        PeiUsbDev->MaxPacketSize0,
+                        Request,
+                        Direction,
+                        Data,
+                        &DataLength,
+                        Timeout,
+                        &(PeiUsbDev->Translator),
+                        &TransferResult
+                        );
+  } else {
+    Status = PeiUsbDev->UsbHcPpi->ControlTransfer (
+                        PeiServices,
+                        PeiUsbDev->UsbHcPpi,
+                        PeiUsbDev->DeviceAddress,
+                        PeiUsbDev->DeviceSpeed,
+                        (UINT8) PeiUsbDev->MaxPacketSize0,
+                        Request,
+                        Direction,
+                        Data,
+                        &DataLength,
+                        Timeout,
+                        &TransferResult
+                        );
+  }
+
+  //
+  // Reset the endpoint toggle when endpoint stall is cleared
+  //
+  if ((Request->Request     == USB_REQ_CLEAR_FEATURE) &&
+      (Request->RequestType == USB_DEV_CLEAR_FEATURE_REQ_TYPE_E) &&
+      (Request->Value       == USB_FEATURE_ENDPOINT_HALT)) {
+    if ((PeiUsbDev->DataToggle & (1 << EndpointIndex)) != 0) {
+      PeiUsbDev->DataToggle = (UINT16) (PeiUsbDev->DataToggle ^ (1 << EndpointIndex));
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "PeiUsbControlTransfer: %r\n", Status));
+  return Status;
+}
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_IO_PPI.
+  @param  DeviceEndpoint        Endpoint number and its direction in bit 7.
+  @param  Data                  A pointer to the buffer of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  Timeout               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete. If Timeout is 0, then
+                                the caller must wait for the function to be completed
+                                until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbBulkTransfer (
+  IN     EFI_PEI_SERVICES    **PeiServices,
+  IN     PEI_USB_IO_PPI      *This,
+  IN     UINT8               DeviceEndpoint,
+  IN OUT VOID                *Data,
+  IN OUT UINTN               *DataLength,
+  IN     UINTN               Timeout
+  )
+{
+  EFI_STATUS                  Status;
+  PEI_USB_DEVICE              *PeiUsbDev;
+  UINT32                      TransferResult;
+  UINTN                       MaxPacketLength;
+  UINT8                       DataToggle;
+  UINT8                       OldToggle;
+  EFI_USB_ENDPOINT_DESCRIPTOR *EndpointDescriptor;
+  UINT8                       EndpointIndex;
+  VOID                        *Data2[EFI_USB_MAX_BULK_BUFFER_NUM];
+
+  PeiUsbDev     = PEI_USB_DEVICE_FROM_THIS (This);
+
+  EndpointDescriptor = NULL;
+  EndpointIndex = 0;
+  Data2[0] = Data;
+  Data2[1] = NULL;
+
+  while (EndpointIndex < MAX_ENDPOINT) {
+    Status = PeiUsbGetEndpointDescriptor (PeiServices, This, EndpointIndex, &EndpointDescriptor);
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (EndpointDescriptor->EndpointAddress == DeviceEndpoint) {
+      break;
+    }
+
+    EndpointIndex++;
+  }
+
+  if (EndpointIndex == MAX_ENDPOINT) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MaxPacketLength = PeiUsbDev->EndpointDesc[EndpointIndex]->MaxPacketSize;
+  if ((PeiUsbDev->DataToggle & (1 << EndpointIndex)) != 0) {
+    DataToggle = 1;
+  } else {
+    DataToggle = 0;
+  }
+
+  OldToggle = DataToggle;
+
+  if (PeiUsbDev->Usb2HcPpi != NULL) {
+    Status = PeiUsbDev->Usb2HcPpi->BulkTransfer (
+                        PeiServices,
+                        PeiUsbDev->Usb2HcPpi,
+                        PeiUsbDev->DeviceAddress,
+                        DeviceEndpoint,
+                        PeiUsbDev->DeviceSpeed,
+                        MaxPacketLength,
+                        Data2,
+                        DataLength,
+                        &DataToggle,
+                        Timeout,
+                        &(PeiUsbDev->Translator),
+                        &TransferResult
+                        );
+  } else {
+    Status = PeiUsbDev->UsbHcPpi->BulkTransfer (
+                        PeiServices,
+                        PeiUsbDev->UsbHcPpi,
+                        PeiUsbDev->DeviceAddress,
+                        DeviceEndpoint,
+                        (UINT8) MaxPacketLength,
+                        Data,
+                        DataLength,
+                        &DataToggle,
+                        Timeout,
+                        &TransferResult
+                        );
+  }
+
+  if (OldToggle != DataToggle) {
+    PeiUsbDev->DataToggle = (UINT16) (PeiUsbDev->DataToggle ^ (1 << EndpointIndex));
+  }
+
+  DEBUG ((EFI_D_INFO, "PeiUsbBulkTransfer: %r\n", Status));
+  return Status;
+}
+
+/**
+  Get the usb interface descriptor.
+
+  @param  PeiServices          General-purpose services that are available to every PEIM.
+  @param  This                 Indicates the PEI_USB_IO_PPI instance.
+  @param  InterfaceDescriptor  Request interface descriptor.
+
+
+  @retval EFI_SUCCESS          Usb interface descriptor is obtained successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbGetInterfaceDescriptor (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  PEI_USB_IO_PPI                 *This,
+  OUT EFI_USB_INTERFACE_DESCRIPTOR   **InterfaceDescriptor
+  )
+{
+  PEI_USB_DEVICE  *PeiUsbDev;
+  PeiUsbDev             = PEI_USB_DEVICE_FROM_THIS (This);
+  *InterfaceDescriptor  = PeiUsbDev->InterfaceDesc;
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the usb endpoint descriptor.
+
+  @param  PeiServices          General-purpose services that are available to every PEIM.
+  @param  This                 Indicates the PEI_USB_IO_PPI instance.
+  @param  EndpointIndex        The valid index of the specified endpoint.
+  @param  EndpointDescriptor   Request endpoint descriptor.
+
+  @retval EFI_SUCCESS       Usb endpoint descriptor is obtained successfully.
+  @retval EFI_NOT_FOUND     Usb endpoint descriptor is NOT found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbGetEndpointDescriptor (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  PEI_USB_IO_PPI                 *This,
+  IN  UINT8                          EndpointIndex,
+  OUT EFI_USB_ENDPOINT_DESCRIPTOR    **EndpointDescriptor
+  )
+{
+  PEI_USB_DEVICE  *PeiUsbDev;
+
+  PeiUsbDev = PEI_USB_DEVICE_FROM_THIS (This);
+
+  ASSERT (EndpointDescriptor != NULL);
+
+  //
+  // The valid range of EndpointIndex is 0..15
+  // If EndpointIndex is lesser than 15 but larger than the number of interfaces,
+  // a EFI_NOT_FOUND should be returned
+  //
+  ASSERT (EndpointIndex <= 15);
+
+  if (EndpointIndex >= PeiUsbDev->InterfaceDesc->NumEndpoints) {
+    return EFI_NOT_FOUND;
+  }
+
+  *EndpointDescriptor = PeiUsbDev->EndpointDesc[EndpointIndex];
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reset the port and re-configure the usb device.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  This           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS    Usb device is reset and configured successfully.
+  @retval Others         Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbPortReset (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_IO_PPI                 *This
+  )
+{
+  PEI_USB_DEVICE  *PeiUsbDev;
+  EFI_STATUS      Status;
+  UINT8           Address;
+
+  PeiUsbDev = PEI_USB_DEVICE_FROM_THIS (This);
+
+  ResetRootPort (
+    PeiServices,
+    PeiUsbDev->UsbHcPpi,
+    PeiUsbDev->Usb2HcPpi,
+    PeiUsbDev->DeviceAddress,
+    0
+    );
+
+  //
+  // Set address
+  //
+  Address                   = PeiUsbDev->DeviceAddress;
+  PeiUsbDev->DeviceAddress  = 0;
+
+  Status = PeiUsbSetDeviceAddress (
+            PeiServices,
+            This,
+            Address
+            );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PeiUsbDev->DeviceAddress = Address;
+
+  //
+  // Set default configuration
+  //
+  Status = PeiUsbSetConfiguration (
+            PeiServices,
+            This
+            );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c
new file mode 100644
index 0000000000..f350021862
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c
@@ -0,0 +1,1231 @@
+/** @file
+The module to produce Usb Bus PPI.
+
+Copyright (c) 2006 - 2014, 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 "UsbPeim.h"
+#include "HubPeim.h"
+#include "PeiUsbLib.h"
+
+//
+// UsbIo PPI interface function
+//
+PEI_USB_IO_PPI         mUsbIoPpi = {
+  PeiUsbControlTransfer,
+  PeiUsbBulkTransfer,
+  PeiUsbGetInterfaceDescriptor,
+  PeiUsbGetEndpointDescriptor,
+  PeiUsbPortReset
+};
+
+EFI_PEI_PPI_DESCRIPTOR mUsbIoPpiList = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gPeiUsbIoPpiGuid,
+  NULL
+};
+
+/**
+  The enumeration routine to detect device change.
+  
+  @param  PeiServices            Describes the list of possible PEI Services.
+  @param  UsbHcPpi               The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.
+  @param  Usb2HcPpi              The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.
+
+  @retval EFI_SUCCESS            The usb is enumerated successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+  @retval Others                 Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbEnumeration (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *UsbHcPpi,
+  IN PEI_USB2_HOST_CONTROLLER_PPI    *Usb2HcPpi
+  );
+
+/**
+  Configure new detected usb device.
+  
+  @param  PeiServices            Describes the list of possible PEI Services.
+  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.
+  @param  Port                   The port to be configured.
+  @param  DeviceAddress          The device address to be configured.
+
+  @retval EFI_SUCCESS            The new detected usb device is configured successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+  @retval Others                 Other failure occurs.
+
+**/
+EFI_STATUS
+PeiConfigureUsbDevice (
+  IN     EFI_PEI_SERVICES    **PeiServices,
+  IN     PEI_USB_DEVICE      *PeiUsbDevice,
+  IN     UINT8               Port,
+  IN OUT UINT8               *DeviceAddress
+  );
+
+/**
+  Get all configurations from a detected usb device.
+  
+  @param  PeiServices            Describes the list of possible PEI Services.
+  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.
+
+  @retval EFI_SUCCESS            The new detected usb device is configured successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+  @retval Others                 Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetAllConfiguration (
+  IN EFI_PEI_SERVICES   **PeiServices,
+  IN PEI_USB_DEVICE     *PeiUsbDevice
+  );
+
+/**
+  Get the start position of next wanted descriptor.
+  
+  @param  Buffer            Buffer containing data to parse.
+  @param  Length            Buffer length.
+  @param  DescType          Descriptor type.
+  @param  DescLength        Descriptor length.
+  @param  ParsedBytes       Bytes has been parsed.
+
+  @retval EFI_SUCCESS       Get wanted descriptor successfully.
+  @retval EFI_DEVICE_ERROR  Error occurred.
+
+**/
+EFI_STATUS
+GetExpectedDescriptor (
+  IN  UINT8       *Buffer,
+  IN  UINTN       Length,
+  IN  UINT8       DescType,
+  IN  UINT8       DescLength,
+  OUT UINTN       *ParsedBytes
+  );
+
+/**
+  The entrypoint of the module, it will enumerate all HCs.
+  
+  @param  FileHandle             Handle of the file being invoked.
+  @param  PeiServices            Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            Usb initialization is done successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+  @retval EFI_UNSUPPORTED        Can't find required PPI.
+
+**/
+EFI_STATUS
+EFIAPI
+PeimInitializeUsb (
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS                   Status;
+  UINTN                        Index;
+  PEI_USB_HOST_CONTROLLER_PPI  *UsbHcPpi;
+  PEI_USB2_HOST_CONTROLLER_PPI *Usb2HcPpi;
+
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // gPeiUsbHostControllerPpiGuid and gPeiUsb2HostControllerPpiGuid should not 
+  // be produced at the same time
+  //
+  Index = 0;
+  while (TRUE) {
+    //
+    // Get UsbHcPpi at first.
+    //
+    Status = PeiServicesLocatePpi (
+               &gPeiUsbHostControllerPpiGuid,
+               Index,
+               NULL,
+               (VOID **) &UsbHcPpi
+               );
+    if (EFI_ERROR (Status)) {
+      //
+      // No more host controller, break out
+      //
+      break;
+    }
+    PeiUsbEnumeration ((EFI_PEI_SERVICES **) PeiServices, UsbHcPpi, NULL);
+    Index++;
+  }
+
+  if (Index == 0) {
+    //
+    // Then try to get Usb2HcPpi.
+    //
+    while (TRUE) {
+      Status = PeiServicesLocatePpi (
+                 &gPeiUsb2HostControllerPpiGuid,
+                 Index,
+                 NULL,
+                 (VOID **) &Usb2HcPpi
+                 );    
+      if (EFI_ERROR (Status)) {
+        //
+        // No more host controller, break out
+        //
+        break;
+      }   
+      PeiUsbEnumeration ((EFI_PEI_SERVICES **) PeiServices, NULL, Usb2HcPpi);   
+      Index++;
+    }
+  }
+  
+  if (Index == 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The Hub Enumeration just scans the hub ports one time. It also
+  doesn't support hot-plug.
+  
+  @param  PeiServices            Describes the list of possible PEI Services.
+  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.
+  @param  CurrentAddress         The DeviceAddress of usb device.
+
+  @retval EFI_SUCCESS            The usb hub is enumerated successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+  @retval Others                 Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubEnumeration (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_DEVICE                 *PeiUsbDevice,
+  IN UINT8                          *CurrentAddress
+  )
+{
+  UINTN                 Index;
+  EFI_STATUS            Status;
+  PEI_USB_IO_PPI        *UsbIoPpi;
+  EFI_USB_PORT_STATUS   PortStatus;
+  UINTN                 MemPages;
+  EFI_PHYSICAL_ADDRESS  AllocateAddress;
+  PEI_USB_DEVICE        *NewPeiUsbDevice;
+  UINTN                 InterfaceIndex;
+  UINTN                 EndpointIndex;
+
+
+  UsbIoPpi    = &PeiUsbDevice->UsbIoPpi;
+
+  DEBUG ((EFI_D_INFO, "PeiHubEnumeration: DownStreamPortNo: %x\n", PeiUsbDevice->DownStreamPortNo));
+
+  for (Index = 0; Index < PeiUsbDevice->DownStreamPortNo; Index++) {
+
+    Status = PeiHubGetPortStatus (
+              PeiServices,
+              UsbIoPpi,
+              (UINT8) (Index + 1),
+              (UINT32 *) &PortStatus
+              );
+
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    DEBUG ((EFI_D_INFO, "USB Status --- Port: %x ConnectChange[%04x] Status[%04x]\n", Index, PortStatus.PortChangeStatus, PortStatus.PortStatus));
+    //
+    // Only handle connection/enable/overcurrent/reset change.
+    //
+    if ((PortStatus.PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
+      continue;
+    } else {
+      if (IsPortConnect (PortStatus.PortStatus)) {
+        //
+        // Begin to deal with the new device
+        //
+        MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;
+        Status = PeiServicesAllocatePages (
+                   EfiBootServicesCode,
+                   MemPages,
+                   &AllocateAddress
+                   );
+        if (EFI_ERROR (Status)) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        NewPeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);
+        ZeroMem (NewPeiUsbDevice, sizeof (PEI_USB_DEVICE));
+
+        NewPeiUsbDevice->Signature        = PEI_USB_DEVICE_SIGNATURE;
+        NewPeiUsbDevice->DeviceAddress    = 0;
+        NewPeiUsbDevice->MaxPacketSize0   = 8;
+        NewPeiUsbDevice->DataToggle       = 0;
+        CopyMem (
+          &(NewPeiUsbDevice->UsbIoPpi),
+          &mUsbIoPpi,
+          sizeof (PEI_USB_IO_PPI)
+          );
+        CopyMem (
+          &(NewPeiUsbDevice->UsbIoPpiList),
+          &mUsbIoPpiList,
+          sizeof (EFI_PEI_PPI_DESCRIPTOR)
+          );
+        NewPeiUsbDevice->UsbIoPpiList.Ppi = &NewPeiUsbDevice->UsbIoPpi;
+        NewPeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;
+        NewPeiUsbDevice->UsbHcPpi         = PeiUsbDevice->UsbHcPpi;
+        NewPeiUsbDevice->Usb2HcPpi        = PeiUsbDevice->Usb2HcPpi;
+        NewPeiUsbDevice->Tier             = (UINT8) (PeiUsbDevice->Tier + 1);
+        NewPeiUsbDevice->IsHub            = 0x0;
+        NewPeiUsbDevice->DownStreamPortNo = 0x0;
+
+        if (((PortStatus.PortChangeStatus & USB_PORT_STAT_C_RESET) == 0) || 
+             ((PortStatus.PortStatus & (USB_PORT_STAT_CONNECTION | USB_PORT_STAT_ENABLE)) == 0)) {
+          //
+          // If the port already has reset change flag and is connected and enabled, skip the port reset logic. 
+          //
+          PeiResetHubPort (PeiServices, UsbIoPpi, (UINT8)(Index + 1));
+  
+          PeiHubGetPortStatus (
+             PeiServices,
+             UsbIoPpi,
+             (UINT8) (Index + 1),
+             (UINT32 *) &PortStatus
+             );
+        } else {
+          PeiHubClearPortFeature (
+            PeiServices,
+            UsbIoPpi,
+            (UINT8) (Index + 1),
+            EfiUsbPortResetChange
+            );
+        }
+
+        NewPeiUsbDevice->DeviceSpeed = (UINT8) PeiUsbGetDeviceSpeed (PortStatus.PortStatus);
+        DEBUG ((EFI_D_INFO, "Device Speed =%d\n", PeiUsbDevice->DeviceSpeed));
+
+        if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_SUPER_SPEED)){
+          NewPeiUsbDevice->MaxPacketSize0 = 512;
+        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_HIGH_SPEED)) {
+          NewPeiUsbDevice->MaxPacketSize0 = 64;
+        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_LOW_SPEED)) {
+          NewPeiUsbDevice->MaxPacketSize0 = 8;
+        } else {
+          NewPeiUsbDevice->MaxPacketSize0 = 8;
+        }
+
+        if(NewPeiUsbDevice->DeviceSpeed != EFI_USB_SPEED_HIGH) {
+          if (PeiUsbDevice->DeviceSpeed == EFI_USB_SPEED_HIGH) {
+            NewPeiUsbDevice->Translator.TranslatorPortNumber = (UINT8)Index;
+            NewPeiUsbDevice->Translator.TranslatorHubAddress = *CurrentAddress;
+          } else {
+            CopyMem(&(NewPeiUsbDevice->Translator), &(PeiUsbDevice->Translator), sizeof(EFI_USB2_HC_TRANSACTION_TRANSLATOR));
+          }
+        }
+
+        //
+        // Configure that Usb Device
+        //
+        Status = PeiConfigureUsbDevice (
+                  PeiServices,
+                  NewPeiUsbDevice,
+                  (UINT8) (Index + 1),
+                  CurrentAddress
+                  );
+
+        if (EFI_ERROR (Status)) {
+          continue;
+        }
+        DEBUG ((EFI_D_INFO, "PeiHubEnumeration: PeiConfigureUsbDevice Success\n"));
+
+        Status = PeiServicesInstallPpi (&NewPeiUsbDevice->UsbIoPpiList);
+
+        if (NewPeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {
+          NewPeiUsbDevice->IsHub  = 0x1;
+
+          Status = PeiDoHubConfig (PeiServices, NewPeiUsbDevice);
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+
+          PeiHubEnumeration (PeiServices, NewPeiUsbDevice, CurrentAddress);
+        }
+
+        for (InterfaceIndex = 1; InterfaceIndex < NewPeiUsbDevice->ConfigDesc->NumInterfaces; InterfaceIndex++) {
+          //
+          // Begin to deal with the new device
+          //
+          MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;
+          Status = PeiServicesAllocatePages (
+                     EfiBootServicesCode,
+                     MemPages,
+                     &AllocateAddress
+                     );
+          if (EFI_ERROR (Status)) {
+            return EFI_OUT_OF_RESOURCES;
+          }
+          CopyMem ((VOID *)(UINTN)AllocateAddress, NewPeiUsbDevice, sizeof (PEI_USB_DEVICE));
+          NewPeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);
+          NewPeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;
+          NewPeiUsbDevice->UsbIoPpiList.Ppi = &NewPeiUsbDevice->UsbIoPpi;
+          NewPeiUsbDevice->InterfaceDesc = NewPeiUsbDevice->InterfaceDescList[InterfaceIndex];
+          for (EndpointIndex = 0; EndpointIndex < NewPeiUsbDevice->InterfaceDesc->NumEndpoints; EndpointIndex++) {
+            NewPeiUsbDevice->EndpointDesc[EndpointIndex] = NewPeiUsbDevice->EndpointDescList[InterfaceIndex][EndpointIndex];
+          }
+
+          Status = PeiServicesInstallPpi (&NewPeiUsbDevice->UsbIoPpiList);
+
+          if (NewPeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {
+            NewPeiUsbDevice->IsHub  = 0x1;
+
+            Status = PeiDoHubConfig (PeiServices, NewPeiUsbDevice);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+
+            PeiHubEnumeration (PeiServices, NewPeiUsbDevice, CurrentAddress);
+          }
+        }
+      }
+    }
+  }
+
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The enumeration routine to detect device change.
+  
+  @param  PeiServices            Describes the list of possible PEI Services.
+  @param  UsbHcPpi               The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.
+  @param  Usb2HcPpi              The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.
+
+  @retval EFI_SUCCESS            The usb is enumerated successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+  @retval Others                 Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbEnumeration (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *UsbHcPpi,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *Usb2HcPpi
+  )
+{
+  UINT8                 NumOfRootPort;
+  EFI_STATUS            Status;
+  UINT8                 Index;
+  EFI_USB_PORT_STATUS   PortStatus;
+  PEI_USB_DEVICE        *PeiUsbDevice;
+  UINTN                 MemPages;
+  EFI_PHYSICAL_ADDRESS  AllocateAddress;
+  UINT8                 CurrentAddress;
+  UINTN                 InterfaceIndex;
+  UINTN                 EndpointIndex;
+
+  CurrentAddress = 0;
+  if (Usb2HcPpi != NULL) {
+    Usb2HcPpi->GetRootHubPortNumber (
+                PeiServices,
+                Usb2HcPpi,
+                (UINT8 *) &NumOfRootPort
+                );    
+  } else if (UsbHcPpi != NULL) {
+    UsbHcPpi->GetRootHubPortNumber (
+                PeiServices,
+                UsbHcPpi,
+                (UINT8 *) &NumOfRootPort
+                );
+  } else {
+    ASSERT (FALSE);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG ((EFI_D_INFO, "PeiUsbEnumeration: NumOfRootPort: %x\n", NumOfRootPort));
+
+  for (Index = 0; Index < NumOfRootPort; Index++) {
+    //
+    // First get root port status to detect changes happen
+    //
+    if (Usb2HcPpi != NULL) {
+      Usb2HcPpi->GetRootHubPortStatus (
+                  PeiServices,
+                  Usb2HcPpi,
+                  (UINT8) Index,
+                  &PortStatus
+                  );      
+    } else {
+      UsbHcPpi->GetRootHubPortStatus (
+                  PeiServices,
+                  UsbHcPpi,
+                  (UINT8) Index,
+                  &PortStatus
+                  );
+    }
+    DEBUG ((EFI_D_INFO, "USB Status --- Port: %x ConnectChange[%04x] Status[%04x]\n", Index, PortStatus.PortChangeStatus, PortStatus.PortStatus));
+    //
+    // Only handle connection/enable/overcurrent/reset change.
+    //
+    if ((PortStatus.PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
+      continue;
+    } else {
+      if (IsPortConnect (PortStatus.PortStatus)) {
+        MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;
+        Status = PeiServicesAllocatePages (
+                   EfiBootServicesCode,
+                   MemPages,
+                   &AllocateAddress
+                   );
+        if (EFI_ERROR (Status)) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        PeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);
+        ZeroMem (PeiUsbDevice, sizeof (PEI_USB_DEVICE));
+
+        PeiUsbDevice->Signature         = PEI_USB_DEVICE_SIGNATURE;
+        PeiUsbDevice->DeviceAddress     = 0;
+        PeiUsbDevice->MaxPacketSize0    = 8;
+        PeiUsbDevice->DataToggle        = 0;
+        CopyMem (
+          &(PeiUsbDevice->UsbIoPpi),
+          &mUsbIoPpi,
+          sizeof (PEI_USB_IO_PPI)
+          );
+        CopyMem (
+          &(PeiUsbDevice->UsbIoPpiList),
+          &mUsbIoPpiList,
+          sizeof (EFI_PEI_PPI_DESCRIPTOR)
+          );
+        PeiUsbDevice->UsbIoPpiList.Ppi  = &PeiUsbDevice->UsbIoPpi;
+        PeiUsbDevice->AllocateAddress   = (UINTN) AllocateAddress;
+        PeiUsbDevice->UsbHcPpi          = UsbHcPpi;
+        PeiUsbDevice->Usb2HcPpi         = Usb2HcPpi;
+        PeiUsbDevice->IsHub             = 0x0;
+        PeiUsbDevice->DownStreamPortNo  = 0x0;
+
+        if (((PortStatus.PortChangeStatus & USB_PORT_STAT_C_RESET) == 0) || 
+             ((PortStatus.PortStatus & (USB_PORT_STAT_CONNECTION | USB_PORT_STAT_ENABLE)) == 0)) {
+          //
+          // If the port already has reset change flag and is connected and enabled, skip the port reset logic. 
+          //
+          ResetRootPort (
+            PeiServices,
+            PeiUsbDevice->UsbHcPpi,
+            PeiUsbDevice->Usb2HcPpi,
+            Index,
+            0
+            );
+
+          if (Usb2HcPpi != NULL) {
+            Usb2HcPpi->GetRootHubPortStatus (
+                         PeiServices,
+                         Usb2HcPpi,
+                         (UINT8) Index,
+                         &PortStatus
+                         );
+          } else {
+            UsbHcPpi->GetRootHubPortStatus (
+                        PeiServices,
+                        UsbHcPpi,
+                        (UINT8) Index,
+                        &PortStatus
+                        );
+          }
+        } else {
+          if (Usb2HcPpi != NULL) {
+            Usb2HcPpi->ClearRootHubPortFeature (
+                        PeiServices,
+                        Usb2HcPpi,
+                        (UINT8) Index,
+                        EfiUsbPortResetChange
+                        );        
+          } else {
+            UsbHcPpi->ClearRootHubPortFeature (
+                        PeiServices,
+                        UsbHcPpi,
+                        (UINT8) Index,
+                        EfiUsbPortResetChange
+                        );
+          }
+        }
+
+        PeiUsbDevice->DeviceSpeed = (UINT8) PeiUsbGetDeviceSpeed (PortStatus.PortStatus);
+        DEBUG ((EFI_D_INFO, "Device Speed =%d\n", PeiUsbDevice->DeviceSpeed));
+
+        if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_SUPER_SPEED)){
+          PeiUsbDevice->MaxPacketSize0 = 512;
+        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_HIGH_SPEED)) {
+          PeiUsbDevice->MaxPacketSize0 = 64;
+        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_LOW_SPEED)) {
+          PeiUsbDevice->MaxPacketSize0 = 8;
+        } else {
+          PeiUsbDevice->MaxPacketSize0 = 8;
+        }
+
+        //
+        // Configure that Usb Device
+        //
+        Status = PeiConfigureUsbDevice (
+                  PeiServices,
+                  PeiUsbDevice,
+                  Index,
+                  &CurrentAddress
+                  );
+
+        if (EFI_ERROR (Status)) {
+          continue;
+        }
+        DEBUG ((EFI_D_INFO, "PeiUsbEnumeration: PeiConfigureUsbDevice Success\n"));
+
+        Status = PeiServicesInstallPpi (&PeiUsbDevice->UsbIoPpiList);
+
+        if (PeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {
+          PeiUsbDevice->IsHub = 0x1;
+
+          Status = PeiDoHubConfig (PeiServices, PeiUsbDevice);
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+
+          PeiHubEnumeration (PeiServices, PeiUsbDevice, &CurrentAddress);
+        }
+
+        for (InterfaceIndex = 1; InterfaceIndex < PeiUsbDevice->ConfigDesc->NumInterfaces; InterfaceIndex++) {
+          //
+          // Begin to deal with the new device
+          //
+          MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;
+          Status = PeiServicesAllocatePages (
+                     EfiBootServicesCode,
+                     MemPages,
+                     &AllocateAddress
+                     );
+          if (EFI_ERROR (Status)) {
+            return EFI_OUT_OF_RESOURCES;
+          }
+          CopyMem ((VOID *)(UINTN)AllocateAddress, PeiUsbDevice, sizeof (PEI_USB_DEVICE));
+          PeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);
+          PeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;
+          PeiUsbDevice->UsbIoPpiList.Ppi = &PeiUsbDevice->UsbIoPpi;
+          PeiUsbDevice->InterfaceDesc = PeiUsbDevice->InterfaceDescList[InterfaceIndex];
+          for (EndpointIndex = 0; EndpointIndex < PeiUsbDevice->InterfaceDesc->NumEndpoints; EndpointIndex++) {
+            PeiUsbDevice->EndpointDesc[EndpointIndex] = PeiUsbDevice->EndpointDescList[InterfaceIndex][EndpointIndex];
+          }
+
+          Status = PeiServicesInstallPpi (&PeiUsbDevice->UsbIoPpiList);
+
+          if (PeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {
+            PeiUsbDevice->IsHub = 0x1;
+
+            Status = PeiDoHubConfig (PeiServices, PeiUsbDevice);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+
+            PeiHubEnumeration (PeiServices, PeiUsbDevice, &CurrentAddress);
+          }
+        }
+      } else {
+        //
+        // Disconnect change happen, currently we don't support
+        //
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Configure new detected usb device.
+  
+  @param  PeiServices            Describes the list of possible PEI Services.
+  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.
+  @param  Port                   The port to be configured.
+  @param  DeviceAddress          The device address to be configured.
+
+  @retval EFI_SUCCESS            The new detected usb device is configured successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+  @retval Others                 Other failure occurs.
+
+**/
+EFI_STATUS
+PeiConfigureUsbDevice (
+  IN EFI_PEI_SERVICES   **PeiServices,
+  IN PEI_USB_DEVICE     *PeiUsbDevice,
+  IN UINT8              Port,
+  IN OUT UINT8          *DeviceAddress
+  )
+{
+  EFI_USB_DEVICE_DESCRIPTOR   DeviceDescriptor;
+  EFI_STATUS                  Status;
+  PEI_USB_IO_PPI              *UsbIoPpi;
+  UINT8                       Retry;
+
+  UsbIoPpi = &PeiUsbDevice->UsbIoPpi;
+  Status   = EFI_SUCCESS;
+  ZeroMem (&DeviceDescriptor, sizeof (EFI_USB_DEVICE_DESCRIPTOR));
+  //
+  // Get USB device descriptor
+  //
+
+  for (Retry = 0; Retry < 3; Retry ++) {
+    Status = PeiUsbGetDescriptor (
+               PeiServices,
+               UsbIoPpi,
+               (USB_DT_DEVICE << 8),
+               0,
+               8,
+               &DeviceDescriptor
+               );
+
+    if (!EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, "PeiUsbGet Device Descriptor the %d time Success\n", Retry));
+      break;
+    }
+  }
+
+  if (Retry == 3) {
+    DEBUG ((EFI_D_ERROR, "PeiUsbGet Device Descriptor fail: %x %r\n", Retry, Status));
+    return Status;
+  }
+
+  if ((DeviceDescriptor.BcdUSB == 0x0300) && (DeviceDescriptor.MaxPacketSize0 == 9)) {
+    PeiUsbDevice->MaxPacketSize0 = 1 << 9;
+  } else {
+    PeiUsbDevice->MaxPacketSize0 = DeviceDescriptor.MaxPacketSize0;
+  }
+
+  (*DeviceAddress) ++;
+
+  Status = PeiUsbSetDeviceAddress (
+            PeiServices,
+            UsbIoPpi,
+            *DeviceAddress
+            );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "PeiUsbSetDeviceAddress Failed: %r\n", Status));
+    return Status;
+  }
+  MicroSecondDelay (USB_SET_DEVICE_ADDRESS_STALL);
+
+  PeiUsbDevice->DeviceAddress = *DeviceAddress;
+
+  //
+  // Get whole USB device descriptor
+  //
+  Status = PeiUsbGetDescriptor (
+            PeiServices,
+            UsbIoPpi,
+            (USB_DT_DEVICE << 8),
+            0,
+            (UINT16) sizeof (EFI_USB_DEVICE_DESCRIPTOR),
+            &DeviceDescriptor
+            );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "PeiUsbGetDescriptor First Failed\n"));
+    return Status;
+  }
+
+  //
+  // Get its default configuration and its first interface
+  //
+  Status = PeiUsbGetAllConfiguration (
+            PeiServices,
+            PeiUsbDevice
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  MicroSecondDelay (USB_GET_CONFIG_DESCRIPTOR_STALL);
+
+  Status = PeiUsbSetConfiguration (
+            PeiServices,
+            UsbIoPpi
+            );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get all configurations from a detected usb device.
+  
+  @param  PeiServices            Describes the list of possible PEI Services.
+  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.
+
+  @retval EFI_SUCCESS            The new detected usb device is configured successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+  @retval Others                 Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetAllConfiguration (
+  IN EFI_PEI_SERVICES   **PeiServices,
+  IN PEI_USB_DEVICE     *PeiUsbDevice
+  )
+{
+  EFI_STATUS                Status;
+  EFI_USB_CONFIG_DESCRIPTOR *ConfigDesc;
+  PEI_USB_IO_PPI            *UsbIoPpi;
+  UINT16                    ConfigDescLength;
+  UINT8                     *Ptr;
+  UINTN                     SkipBytes;
+  UINTN                     LengthLeft;
+  UINTN                     InterfaceIndex;
+  UINTN                     Index;
+  UINTN                     NumOfEndpoint;
+
+  UsbIoPpi = &PeiUsbDevice->UsbIoPpi;
+
+  //
+  // First get its 4-byte configuration descriptor
+  //
+  Status = PeiUsbGetDescriptor (
+            PeiServices,
+            UsbIoPpi,
+            (USB_DT_CONFIG << 8), // Value
+            0,      // Index
+            4,      // Length
+            PeiUsbDevice->ConfigurationData
+            );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "PeiUsbGet Config Descriptor First Failed\n"));
+    return Status;
+  }
+  MicroSecondDelay (USB_GET_CONFIG_DESCRIPTOR_STALL);
+
+  ConfigDesc        = (EFI_USB_CONFIG_DESCRIPTOR *) PeiUsbDevice->ConfigurationData;
+  ConfigDescLength  = ConfigDesc->TotalLength;
+
+  //
+  // Then we get the total descriptors for this configuration
+  //
+  Status = PeiUsbGetDescriptor (
+            PeiServices,
+            UsbIoPpi,
+            (USB_DT_CONFIG << 8),
+            0,
+            ConfigDescLength,
+            PeiUsbDevice->ConfigurationData
+            );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "PeiUsbGet Config Descriptor all Failed\n"));
+    return Status;
+  }
+  //
+  // Parse this configuration descriptor
+  // First get the current config descriptor;
+  //
+  Status = GetExpectedDescriptor (
+            PeiUsbDevice->ConfigurationData,
+            ConfigDescLength,
+            USB_DT_CONFIG,
+            (UINT8) sizeof (EFI_USB_CONFIG_DESCRIPTOR),
+            &SkipBytes
+            );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Ptr                       = PeiUsbDevice->ConfigurationData + SkipBytes;
+  PeiUsbDevice->ConfigDesc  = (EFI_USB_CONFIG_DESCRIPTOR *) Ptr;
+
+  Ptr += sizeof (EFI_USB_CONFIG_DESCRIPTOR);
+  LengthLeft = ConfigDescLength - SkipBytes - sizeof (EFI_USB_CONFIG_DESCRIPTOR);
+
+  for (InterfaceIndex = 0; InterfaceIndex < PeiUsbDevice->ConfigDesc->NumInterfaces; InterfaceIndex++) {
+
+    //
+    // Get the interface descriptor
+    //
+    Status = GetExpectedDescriptor (
+              Ptr,
+              LengthLeft,
+              USB_DT_INTERFACE,
+              (UINT8) sizeof (EFI_USB_INTERFACE_DESCRIPTOR),
+              &SkipBytes
+              );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Ptr += SkipBytes;
+    if (InterfaceIndex == 0) {
+      PeiUsbDevice->InterfaceDesc = (EFI_USB_INTERFACE_DESCRIPTOR *) Ptr;
+    }
+    PeiUsbDevice->InterfaceDescList[InterfaceIndex] = (EFI_USB_INTERFACE_DESCRIPTOR *) Ptr;
+
+    Ptr += sizeof (EFI_USB_INTERFACE_DESCRIPTOR);
+    LengthLeft -= SkipBytes;
+    LengthLeft -= sizeof (EFI_USB_INTERFACE_DESCRIPTOR);
+
+    //
+    // Parse all the endpoint descriptor within this interface
+    //
+    NumOfEndpoint = PeiUsbDevice->InterfaceDescList[InterfaceIndex]->NumEndpoints;
+    ASSERT (NumOfEndpoint <= MAX_ENDPOINT);
+
+    for (Index = 0; Index < NumOfEndpoint; Index++) {
+      //
+      // Get the endpoint descriptor
+      //
+      Status = GetExpectedDescriptor (
+                Ptr,
+                LengthLeft,
+                USB_DT_ENDPOINT,
+                (UINT8) sizeof (EFI_USB_ENDPOINT_DESCRIPTOR),
+                &SkipBytes
+                );
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      Ptr += SkipBytes;
+      if (InterfaceIndex == 0) {
+        PeiUsbDevice->EndpointDesc[Index] = (EFI_USB_ENDPOINT_DESCRIPTOR *) Ptr;
+      }
+      PeiUsbDevice->EndpointDescList[InterfaceIndex][Index] = (EFI_USB_ENDPOINT_DESCRIPTOR *) Ptr;
+
+      Ptr += sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);
+      LengthLeft -= SkipBytes;
+      LengthLeft -= sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the start position of next wanted descriptor.
+  
+  @param  Buffer            Buffer containing data to parse.
+  @param  Length            Buffer length.
+  @param  DescType          Descriptor type.
+  @param  DescLength        Descriptor length.
+  @param  ParsedBytes       Bytes has been parsed.
+
+  @retval EFI_SUCCESS       Get wanted descriptor successfully.
+  @retval EFI_DEVICE_ERROR  Error occurred.
+
+**/
+EFI_STATUS
+GetExpectedDescriptor (
+  IN  UINT8       *Buffer,
+  IN  UINTN       Length,
+  IN  UINT8       DescType,
+  IN  UINT8       DescLength,
+  OUT UINTN       *ParsedBytes
+  )
+{
+  UINT16  DescriptorHeader;
+  UINT8   Len;
+  UINT8   *Ptr;
+  UINTN   Parsed;
+
+  Parsed  = 0;
+  Ptr     = Buffer;
+
+  while (TRUE) {
+    //
+    // Buffer length should not less than Desc length
+    //
+    if (Length < DescLength) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    DescriptorHeader  = (UINT16) (*Ptr + ((*(Ptr + 1)) << 8));
+
+    Len               = Buffer[0];
+
+    //
+    // Check to see if it is a start of expected descriptor
+    //
+    if (DescriptorHeader == ((DescType << 8) | DescLength)) {
+      break;
+    }
+
+    if ((UINT8) (DescriptorHeader >> 8) == DescType) {
+      if (Len > DescLength) {
+        return EFI_DEVICE_ERROR;
+      }
+    }
+    //
+    // Descriptor length should be at least 2
+    // and should not exceed the buffer length
+    //
+    if (Len < 2) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    if (Len > Length) {
+      return EFI_DEVICE_ERROR;
+    }
+    //
+    // Skip this mismatch descriptor
+    //
+    Length -= Len;
+    Ptr += Len;
+    Parsed += Len;
+  }
+
+  *ParsedBytes = Parsed;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send reset signal over the given root hub port.
+  
+  @param  PeiServices       Describes the list of possible PEI Services.
+  @param  UsbHcPpi          The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.
+  @param  Usb2HcPpi         The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.
+  @param  PortNum           The port to be reset.
+  @param  RetryIndex        The retry times.
+
+**/
+VOID
+ResetRootPort (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *UsbHcPpi,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *Usb2HcPpi,
+  IN UINT8                          PortNum,
+  IN UINT8                          RetryIndex
+  )
+{
+  EFI_STATUS             Status;
+  UINTN                  Index;
+  EFI_USB_PORT_STATUS    PortStatus;
+
+
+  if (Usb2HcPpi != NULL) {
+    MicroSecondDelay (200 * 1000);
+    
+    //
+    // reset root port
+    //
+    Status = Usb2HcPpi->SetRootHubPortFeature (
+                         PeiServices,
+                         Usb2HcPpi,
+                         PortNum,
+                         EfiUsbPortReset
+                         );
+    
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SetRootHubPortFeature EfiUsbPortReset Failed\n"));
+      return;
+    }
+    
+    //
+    // Drive the reset signal for at least 50ms. Check USB 2.0 Spec
+    // section 7.1.7.5 for timing requirements.
+    //
+    MicroSecondDelay (USB_SET_ROOT_PORT_RESET_STALL);
+
+    //
+    // clear reset root port
+    //
+    Status = Usb2HcPpi->ClearRootHubPortFeature (
+                         PeiServices,
+                         Usb2HcPpi,
+                         PortNum,
+                         EfiUsbPortReset
+                         );
+    
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "ClearRootHubPortFeature EfiUsbPortReset Failed\n"));
+      return;
+    }
+
+    MicroSecondDelay (USB_CLR_ROOT_PORT_RESET_STALL);
+
+    //
+    // USB host controller won't clear the RESET bit until
+    // reset is actually finished.
+    //
+    ZeroMem (&PortStatus, sizeof (EFI_USB_PORT_STATUS));
+
+    for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
+      Status = Usb2HcPpi->GetRootHubPortStatus (
+                            PeiServices,
+                            Usb2HcPpi,
+                            PortNum,
+                            &PortStatus
+                            ); 
+      if (EFI_ERROR (Status)) {
+        return;
+      }
+
+      if (!USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_RESET)) {
+        break;
+      }
+
+      MicroSecondDelay (USB_WAIT_PORT_STS_CHANGE_STALL);
+    }
+
+    if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {
+      DEBUG ((EFI_D_ERROR, "ResetRootPort: reset not finished in time on port %d\n", PortNum));
+      return;
+    }
+
+    Usb2HcPpi->ClearRootHubPortFeature (
+                PeiServices,
+                Usb2HcPpi,
+                PortNum,
+                EfiUsbPortResetChange
+                );
+
+    Usb2HcPpi->ClearRootHubPortFeature (
+                PeiServices,
+                Usb2HcPpi,
+                PortNum,
+                EfiUsbPortConnectChange
+                );
+    
+    //
+    // Set port enable
+    //
+    Usb2HcPpi->SetRootHubPortFeature(
+                PeiServices,
+                Usb2HcPpi,
+                PortNum,
+                EfiUsbPortEnable
+                );
+    
+    Usb2HcPpi->ClearRootHubPortFeature (
+                PeiServices,
+                Usb2HcPpi,
+                PortNum,
+                EfiUsbPortEnableChange
+                );
+    
+    MicroSecondDelay ((RetryIndex + 1) * 50 * 1000);
+  } else {
+    MicroSecondDelay (200 * 1000);
+    
+    //
+    // reset root port
+    //
+    Status = UsbHcPpi->SetRootHubPortFeature (
+                         PeiServices,
+                         UsbHcPpi,
+                         PortNum,
+                         EfiUsbPortReset
+                         );
+    
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SetRootHubPortFeature EfiUsbPortReset Failed\n"));
+      return;
+    }
+    
+    //
+    // Drive the reset signal for at least 50ms. Check USB 2.0 Spec
+    // section 7.1.7.5 for timing requirements.
+    //
+    MicroSecondDelay (USB_SET_ROOT_PORT_RESET_STALL);
+    
+    //
+    // clear reset root port
+    //
+    Status = UsbHcPpi->ClearRootHubPortFeature (
+                         PeiServices,
+                         UsbHcPpi,
+                         PortNum,
+                         EfiUsbPortReset
+                         );
+    
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "ClearRootHubPortFeature EfiUsbPortReset Failed\n"));
+      return;
+    }
+    
+    MicroSecondDelay (USB_CLR_ROOT_PORT_RESET_STALL);
+
+    //
+    // USB host controller won't clear the RESET bit until
+    // reset is actually finished.
+    //
+    ZeroMem (&PortStatus, sizeof (EFI_USB_PORT_STATUS));
+
+    for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
+      Status = UsbHcPpi->GetRootHubPortStatus (
+                           PeiServices,
+                           UsbHcPpi,
+                           PortNum,
+                           &PortStatus
+                           ); 
+      if (EFI_ERROR (Status)) {
+        return;
+      }
+
+      if (!USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_RESET)) {
+        break;
+      }
+
+      MicroSecondDelay (USB_WAIT_PORT_STS_CHANGE_STALL);
+    }
+
+    if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {
+      DEBUG ((EFI_D_ERROR, "ResetRootPort: reset not finished in time on port %d\n", PortNum));
+      return;
+    }
+
+    UsbHcPpi->ClearRootHubPortFeature (
+                PeiServices,
+                UsbHcPpi,
+                PortNum,
+                EfiUsbPortResetChange
+                );
+
+    UsbHcPpi->ClearRootHubPortFeature (
+                PeiServices,
+                UsbHcPpi,
+                PortNum,
+                EfiUsbPortConnectChange
+                );
+    
+    //
+    // Set port enable
+    //
+    UsbHcPpi->SetRootHubPortFeature(
+                PeiServices,
+                UsbHcPpi,
+                PortNum,
+                EfiUsbPortEnable
+                );
+    
+    UsbHcPpi->ClearRootHubPortFeature (
+                PeiServices,
+                UsbHcPpi,
+                PortNum,
+                EfiUsbPortEnableChange
+                );
+    
+    MicroSecondDelay ((RetryIndex + 1) * 50 * 1000);
+  }
+  return;
+}
+
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.h
new file mode 100644
index 0000000000..dff8eeb202
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.h
@@ -0,0 +1,253 @@
+/** @file
+Usb Peim definition.
+
+Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved. <BR>
+  
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PEI_USB_PEIM_H_
+#define _PEI_USB_PEIM_H_
+
+
+#include <PiPei.h>
+
+#include <Ppi/UsbHostController.h>
+#include <Ppi/Usb2HostController.h>
+#include <Ppi/UsbIo.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PcdLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#define MAX_INTERFACE             8
+#define MAX_ENDPOINT              16
+
+#define PEI_USB_DEVICE_SIGNATURE  SIGNATURE_32 ('U', 's', 'b', 'D')
+typedef struct {
+  UINTN                         Signature;
+  PEI_USB_IO_PPI                UsbIoPpi;
+  EFI_PEI_PPI_DESCRIPTOR        UsbIoPpiList;
+  UINT16                        MaxPacketSize0;
+  UINT16                        DataToggle;
+  UINT8                         DeviceAddress;
+  UINT8                         DeviceSpeed;
+  UINT8                         IsHub;
+  UINT8                         DownStreamPortNo;
+  UINTN                         AllocateAddress;
+  PEI_USB_HOST_CONTROLLER_PPI   *UsbHcPpi;
+  PEI_USB2_HOST_CONTROLLER_PPI  *Usb2HcPpi;
+  UINT8                         ConfigurationData[1024];
+  EFI_USB_CONFIG_DESCRIPTOR     *ConfigDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR  *InterfaceDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR  *InterfaceDescList[MAX_INTERFACE];
+  EFI_USB_ENDPOINT_DESCRIPTOR   *EndpointDesc[MAX_ENDPOINT];
+  EFI_USB_ENDPOINT_DESCRIPTOR   *EndpointDescList[MAX_INTERFACE][MAX_ENDPOINT];
+  EFI_USB2_HC_TRANSACTION_TRANSLATOR Translator;
+  UINT8                          Tier;
+} PEI_USB_DEVICE;
+
+#define PEI_USB_DEVICE_FROM_THIS(a) CR (a, PEI_USB_DEVICE, UsbIoPpi, PEI_USB_DEVICE_SIGNATURE)
+
+#define USB_BIT_IS_SET(Data, Bit)   ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define USB_BUS_1_MILLISECOND       1000
+
+//
+// Wait for port reset, refers to specification
+// [USB20-7.1.7.5, it says 10ms for hub and 50ms for
+// root hub]
+//
+// According to USB2.0, Chapter 11.5.1.5 Resetting,
+// the worst case for TDRST is 20ms
+//
+#define USB_SET_PORT_RESET_STALL        (20 * USB_BUS_1_MILLISECOND)
+#define USB_SET_ROOT_PORT_RESET_STALL   (50 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for clear roothub port reset, set by experience
+//
+#define USB_CLR_ROOT_PORT_RESET_STALL   (20 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for port statue reg change, set by experience
+//
+#define USB_WAIT_PORT_STS_CHANGE_STALL  (100)
+
+//
+// Host software return timeout if port status doesn't change 
+// after 500ms(LOOP * STALL = 5000 * 0.1ms), set by experience
+//
+#define USB_WAIT_PORT_STS_CHANGE_LOOP   5000
+
+//
+// Wait for hub port power-on, refers to specification
+// [USB20-11.23.2]
+//
+#define USB_SET_PORT_POWER_STALL        (2 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for set device address, refers to specification
+// [USB20-9.2.6.3, it says 2ms]
+//
+#define USB_SET_DEVICE_ADDRESS_STALL    (2 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for get configuration descriptor, set by experience
+//
+#define USB_GET_CONFIG_DESCRIPTOR_STALL (1 * USB_BUS_1_MILLISECOND)
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_IO_PPI.
+  @param  Request                USB device request to send.
+  @param  Direction              Specifies the data direction for the data stage.
+  @param  Timeout                Indicates the maximum timeout, in millisecond. If Timeout
+                                 is 0, then the caller must wait for the function to be
+                                 completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbControlTransfer (
+  IN     EFI_PEI_SERVICES          **PeiServices,
+  IN     PEI_USB_IO_PPI            *This,
+  IN     EFI_USB_DEVICE_REQUEST    *Request,
+  IN     EFI_USB_DATA_DIRECTION    Direction,
+  IN     UINT32                    Timeout,
+  IN OUT VOID                      *Data,      OPTIONAL
+  IN     UINTN                     DataLength  OPTIONAL
+  );
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_IO_PPI.
+  @param  DeviceEndpoint        Endpoint number and its direction in bit 7.
+  @param  Data                  A pointer to the buffer of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  Timeout               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete. If Timeout is 0, then
+                                the caller must wait for the function to be completed
+                                until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbBulkTransfer (
+  IN     EFI_PEI_SERVICES    **PeiServices,
+  IN     PEI_USB_IO_PPI      *This,
+  IN     UINT8               DeviceEndpoint,
+  IN OUT VOID                *Data,
+  IN OUT UINTN               *DataLength,
+  IN     UINTN               Timeout
+  );
+
+/**
+  Get the usb interface descriptor.
+
+  @param  PeiServices          General-purpose services that are available to every PEIM.
+  @param  This                 Indicates the PEI_USB_IO_PPI instance.
+  @param  InterfaceDescriptor  Request interface descriptor.
+
+
+  @retval EFI_SUCCESS          Usb interface descriptor is obtained successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbGetInterfaceDescriptor (
+  IN  EFI_PEI_SERVICES                **PeiServices,
+  IN  PEI_USB_IO_PPI                  *This,
+  OUT EFI_USB_INTERFACE_DESCRIPTOR    **InterfaceDescriptor
+  );
+
+/**
+  Get the usb endpoint descriptor.
+
+  @param  PeiServices          General-purpose services that are available to every PEIM.
+  @param  This                 Indicates the PEI_USB_IO_PPI instance.
+  @param  EndpointIndex        The valid index of the specified endpoint.
+  @param  EndpointDescriptor   Request endpoint descriptor.
+
+  @retval EFI_SUCCESS       Usb endpoint descriptor is obtained successfully.
+  @retval EFI_NOT_FOUND     Usb endpoint descriptor is NOT found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbGetEndpointDescriptor (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  PEI_USB_IO_PPI                 *This,
+  IN  UINT8                          EndpointIndex,
+  OUT EFI_USB_ENDPOINT_DESCRIPTOR    **EndpointDescriptor
+  );
+
+/**
+  Reset the port and re-configure the usb device.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  This           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS    Usb device is reset and configured successfully.
+  @retval Others         Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbPortReset (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *This
+  );
+
+/**
+  Send reset signal over the given root hub port.
+  
+  @param  PeiServices       Describes the list of possible PEI Services.
+  @param  UsbHcPpi          The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.
+  @param  Usb2HcPpi         The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.
+  @param  PortNum           The port to be reset.
+  @param  RetryIndex        The retry times.
+
+**/
+VOID
+ResetRootPort (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *UsbHcPpi,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *Usb2HcPpi,
+  IN UINT8                          PortNum,
+  IN UINT8                          RetryIndex
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/ComponentName.c
new file mode 100644
index 0000000000..186bdbe950
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/ComponentName.c
@@ -0,0 +1,223 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for USB Keyboard driver.
+
+Copyright (c) 2004 - 2011, 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 "KeyBoard.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUsbKeyboardComponentName = {
+  UsbKeyboardComponentNameGetDriverName,
+  UsbKeyboardComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUsbKeyboardComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbKeyboardComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbKeyboardComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUsbKeyboardDriverNameTable[] = {
+  { "eng;en", L"Usb Keyboard Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbKeyboardComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbKeyboardDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUsbKeyboardComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbKeyboardComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                  Status;
+  USB_KB_DEV                  *UsbKbDev;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL *SimpleTxtIn;
+  EFI_USB_IO_PROTOCOL         *UsbIoProtocol;
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIoProtocol,
+                  gUsbKeyboardDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiUsbIoProtocolGuid,
+           gUsbKeyboardDriverBinding.DriverBindingHandle,
+           ControllerHandle
+           );
+
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Status != EFI_ALREADY_STARTED) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleTextInProtocolGuid,
+                  (VOID **) &SimpleTxtIn,
+                  gUsbKeyboardDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UsbKbDev = USB_KB_DEV_FROM_THIS (SimpleTxtIn);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           UsbKbDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUsbKeyboardComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.c b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.c
new file mode 100644
index 0000000000..fb7558b730
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.c
@@ -0,0 +1,1172 @@
+/** @file
+  USB Keyboard Driver that manages USB keyboard and produces Simple Text Input
+  Protocol and Simple Text Input Ex Protocol.
+
+Copyright (c) 2004 - 2012, 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 "EfiKey.h"
+#include "KeyBoard.h"
+
+//
+// USB Keyboard Driver Global Variables
+//
+EFI_DRIVER_BINDING_PROTOCOL gUsbKeyboardDriverBinding = {
+  USBKeyboardDriverBindingSupported,
+  USBKeyboardDriverBindingStart,
+  USBKeyboardDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  Entrypoint of USB Keyboard Driver.
+
+  This function is the entrypoint of USB Keyboard Driver. It installs Driver Binding
+  Protocols together with Component Name Protocols.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUsbKeyboardDriverBinding,
+             ImageHandle,
+             &gUsbKeyboardComponentName,
+             &gUsbKeyboardComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether USB keyboard driver supports this device.
+
+  @param  This                   The USB keyboard driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS          Status;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+
+  //
+  // Check if USB I/O Protocol is attached on the controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Use the USB I/O Protocol interface to check whether Controller is
+  // a keyboard device that can be managed by this driver.
+  //
+  Status = EFI_SUCCESS;
+
+  if (!IsUSBKeyboard (UsbIo)) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+/**
+  Starts the keyboard device with this driver.
+
+  This function produces Simple Text Input Protocol and Simple Text Input Ex Protocol,
+  initializes the keyboard device, and submit Asynchronous Interrupt Transfer to manage
+  this keyboard device.
+
+  @param  This                   The USB keyboard driver binding instance.
+  @param  Controller             Handle of device to bind driver to.
+  @param  RemainingDevicePath    Optional parameter use to pick a specific child
+                                 device to start.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb keyboard driver.
+  @retval EFI_UNSUPPORTED        No interrupt endpoint can be found.
+  @retval Other                  This controller cannot be started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  USB_KB_DEV                    *UsbKeyboardDevice;
+  UINT8                         EndpointNumber;
+  EFI_USB_ENDPOINT_DESCRIPTOR   EndpointDescriptor;
+  UINT8                         Index;
+  UINT8                         EndpointAddr;
+  UINT8                         PollingInterval;
+  UINT8                         PacketSize;
+  BOOLEAN                       Found;
+  EFI_TPL                       OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  //
+  // Open USB I/O Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit1;
+  }
+
+  UsbKeyboardDevice = AllocateZeroPool (sizeof (USB_KB_DEV));
+  ASSERT (UsbKeyboardDevice != NULL);
+
+  //
+  // Get the Device Path Protocol on Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &UsbKeyboardDevice->DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+  //
+  // Report that the USB keyboard is being enabled
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_ENABLE),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  //
+  // This is pretty close to keyboard detection, so log progress
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_PRESENCE_DETECT),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  UsbKeyboardDevice->UsbIo = UsbIo;
+
+  //
+  // Get interface & endpoint descriptor
+  //
+  UsbIo->UsbGetInterfaceDescriptor (
+           UsbIo,
+           &UsbKeyboardDevice->InterfaceDescriptor
+           );
+
+  EndpointNumber = UsbKeyboardDevice->InterfaceDescriptor.NumEndpoints;
+
+  //
+  // Traverse endpoints to find interrupt endpoint
+  //
+  Found = FALSE;
+  for (Index = 0; Index < EndpointNumber; Index++) {
+
+    UsbIo->UsbGetEndpointDescriptor (
+             UsbIo,
+             Index,
+             &EndpointDescriptor
+             );
+
+    if ((EndpointDescriptor.Attributes & (BIT0 | BIT1)) == USB_ENDPOINT_INTERRUPT) {
+      //
+      // We only care interrupt endpoint here
+      //
+      CopyMem(&UsbKeyboardDevice->IntEndpointDescriptor, &EndpointDescriptor, sizeof(EndpointDescriptor));
+      Found = TRUE;
+      break;
+    }
+  }
+
+  if (!Found) {
+    //
+    // Report Status Code to indicate that there is no USB keyboard
+    //
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_KEYBOARD | EFI_P_EC_NOT_DETECTED)
+      );
+    //
+    // No interrupt endpoint found, then return unsupported.
+    //
+    Status = EFI_UNSUPPORTED;
+    goto ErrorExit;
+  }
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_DETECTED),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  UsbKeyboardDevice->Signature                  = USB_KB_DEV_SIGNATURE;
+  UsbKeyboardDevice->SimpleInput.Reset          = USBKeyboardReset;
+  UsbKeyboardDevice->SimpleInput.ReadKeyStroke  = USBKeyboardReadKeyStroke;
+
+  UsbKeyboardDevice->SimpleInputEx.Reset               = USBKeyboardResetEx;
+  UsbKeyboardDevice->SimpleInputEx.ReadKeyStrokeEx     = USBKeyboardReadKeyStrokeEx;
+  UsbKeyboardDevice->SimpleInputEx.SetState            = USBKeyboardSetState;
+  UsbKeyboardDevice->SimpleInputEx.RegisterKeyNotify   = USBKeyboardRegisterKeyNotify;
+  UsbKeyboardDevice->SimpleInputEx.UnregisterKeyNotify = USBKeyboardUnregisterKeyNotify;
+
+  InitializeListHead (&UsbKeyboardDevice->NotifyList);
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  USBKeyboardTimerHandler,
+                  UsbKeyboardDevice,
+                  &UsbKeyboardDevice->TimerEvent
+                  );
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->SetTimer (UsbKeyboardDevice->TimerEvent, TimerPeriodic, KEYBOARD_TIMER_INTERVAL);
+  }
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  USBKeyboardWaitForKey,
+                  UsbKeyboardDevice,
+                  &(UsbKeyboardDevice->SimpleInputEx.WaitForKeyEx)
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  USBKeyboardWaitForKey,
+                  UsbKeyboardDevice,
+                  &(UsbKeyboardDevice->SimpleInput.WaitForKey)
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Install Simple Text Input Protocol and Simple Text Input Ex Protocol
+  // for the USB keyboard device.
+  // USB keyboard is a hot plug device, and expected to work immediately
+  // when plugging into system, other conventional console devices could
+  // distinguish it by its device path.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiSimpleTextInProtocolGuid,
+                  &UsbKeyboardDevice->SimpleInput,
+                  &gEfiSimpleTextInputExProtocolGuid,
+                  &UsbKeyboardDevice->SimpleInputEx,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  UsbKeyboardDevice->ControllerHandle = Controller;
+  Status = InitKeyboardLayout (UsbKeyboardDevice);
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+      Controller,
+      &gEfiSimpleTextInProtocolGuid,
+      &UsbKeyboardDevice->SimpleInput,
+      &gEfiSimpleTextInputExProtocolGuid,
+      &UsbKeyboardDevice->SimpleInputEx,
+      NULL
+      );
+    goto ErrorExit;
+  }
+
+
+  //
+  // Reset USB Keyboard Device exhaustively.
+  //
+  Status = UsbKeyboardDevice->SimpleInputEx.Reset (
+                                            &UsbKeyboardDevice->SimpleInputEx,
+                                            TRUE
+                                            );
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Controller,
+           &gEfiSimpleTextInProtocolGuid,
+           &UsbKeyboardDevice->SimpleInput,
+           &gEfiSimpleTextInputExProtocolGuid,
+           &UsbKeyboardDevice->SimpleInputEx,
+           NULL
+           );
+    goto ErrorExit;
+  }
+
+  //
+  // Submit Asynchronous Interrupt Transfer to manage this device.
+  //
+  EndpointAddr    = UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress;
+  PollingInterval = UsbKeyboardDevice->IntEndpointDescriptor.Interval;
+  PacketSize      = (UINT8) (UsbKeyboardDevice->IntEndpointDescriptor.MaxPacketSize);
+
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    EndpointAddr,
+                    TRUE,
+                    PollingInterval,
+                    PacketSize,
+                    KeyboardHandler,
+                    UsbKeyboardDevice
+                    );
+
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Controller,
+           &gEfiSimpleTextInProtocolGuid,
+           &UsbKeyboardDevice->SimpleInput,
+           &gEfiSimpleTextInputExProtocolGuid,
+           &UsbKeyboardDevice->SimpleInputEx,
+           NULL
+           );
+    goto ErrorExit;
+  }
+
+  UsbKeyboardDevice->ControllerNameTable = NULL;
+  AddUnicodeString2 (
+    "eng",
+    gUsbKeyboardComponentName.SupportedLanguages,
+    &UsbKeyboardDevice->ControllerNameTable,
+    L"Generic Usb Keyboard",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gUsbKeyboardComponentName2.SupportedLanguages,
+    &UsbKeyboardDevice->ControllerNameTable,
+    L"Generic Usb Keyboard",
+    FALSE
+    );
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+
+//
+// Error handler
+//
+ErrorExit:
+  if (UsbKeyboardDevice != NULL) {
+    if (UsbKeyboardDevice->TimerEvent != NULL) {
+      gBS->CloseEvent (UsbKeyboardDevice->TimerEvent);
+    }
+    if (UsbKeyboardDevice->SimpleInput.WaitForKey != NULL) {
+      gBS->CloseEvent (UsbKeyboardDevice->SimpleInput.WaitForKey);
+    }
+    if (UsbKeyboardDevice->SimpleInputEx.WaitForKeyEx != NULL) {
+      gBS->CloseEvent (UsbKeyboardDevice->SimpleInputEx.WaitForKeyEx);
+    }
+    if (UsbKeyboardDevice->KeyboardLayoutEvent != NULL) {
+      ReleaseKeyboardLayoutResources (UsbKeyboardDevice);
+      gBS->CloseEvent (UsbKeyboardDevice->KeyboardLayoutEvent);
+    }
+    FreePool (UsbKeyboardDevice);
+    UsbKeyboardDevice = NULL;
+  }
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+ErrorExit1:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+
+}
+
+
+/**
+  Stop the USB keyboard device handled by this driver.
+
+  @param  This                   The USB keyboard driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Simple Text In Protocol or Simple Text In Ex Protocol
+                                 is not installed on Controller.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL *SimpleInput;
+  USB_KB_DEV                     *UsbKeyboardDevice;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSimpleTextInProtocolGuid,
+                  (VOID **) &SimpleInput,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSimpleTextInputExProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  UsbKeyboardDevice = USB_KB_DEV_FROM_THIS (SimpleInput);
+
+  //
+  // The key data input from this device will be disabled.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_DISABLE),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  //
+  // Delete the Asynchronous Interrupt Transfer from this device
+  //
+  UsbKeyboardDevice->UsbIo->UsbAsyncInterruptTransfer (
+                              UsbKeyboardDevice->UsbIo,
+                              UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress,
+                              FALSE,
+                              UsbKeyboardDevice->IntEndpointDescriptor.Interval,
+                              0,
+                              NULL,
+                              NULL
+                              );
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Controller,
+                  &gEfiSimpleTextInProtocolGuid,
+                  &UsbKeyboardDevice->SimpleInput,
+                  &gEfiSimpleTextInputExProtocolGuid,
+                  &UsbKeyboardDevice->SimpleInputEx,
+                  NULL
+                  );
+  //
+  // Free all resources.
+  //
+  gBS->CloseEvent (UsbKeyboardDevice->TimerEvent);
+  gBS->CloseEvent (UsbKeyboardDevice->RepeatTimer);
+  gBS->CloseEvent (UsbKeyboardDevice->DelayedRecoveryEvent);
+  gBS->CloseEvent (UsbKeyboardDevice->SimpleInput.WaitForKey);
+  gBS->CloseEvent (UsbKeyboardDevice->SimpleInputEx.WaitForKeyEx);
+  KbdFreeNotifyList (&UsbKeyboardDevice->NotifyList);
+
+  ReleaseKeyboardLayoutResources (UsbKeyboardDevice);
+  gBS->CloseEvent (UsbKeyboardDevice->KeyboardLayoutEvent);
+
+  if (UsbKeyboardDevice->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (UsbKeyboardDevice->ControllerNameTable);
+  }
+
+  DestroyQueue (&UsbKeyboardDevice->UsbKeyQueue);
+  DestroyQueue (&UsbKeyboardDevice->EfiKeyQueue);
+
+  FreePool (UsbKeyboardDevice);
+
+  return Status;
+}
+
+/**
+  Internal function to read the next keystroke from the keyboard buffer.
+
+  @param  UsbKeyboardDevice       USB keyboard's private structure.
+  @param  KeyData                 A pointer to buffer to hold the keystroke
+                                  data for the key that was pressed.
+
+  @retval EFI_SUCCESS             The keystroke information was returned.
+  @retval EFI_NOT_READY           There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR        The keystroke information was not returned due to
+                                  hardware errors.
+  @retval EFI_INVALID_PARAMETER   KeyData is NULL.
+  @retval Others                  Fail to translate keycode into EFI_INPUT_KEY
+
+**/
+EFI_STATUS
+USBKeyboardReadKeyStrokeWorker (
+  IN OUT USB_KB_DEV                 *UsbKeyboardDevice,
+     OUT EFI_KEY_DATA               *KeyData
+  )
+{
+  if (KeyData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsQueueEmpty (&UsbKeyboardDevice->EfiKeyQueue)) {
+    return EFI_NOT_READY;
+  }
+
+  Dequeue (&UsbKeyboardDevice->EfiKeyQueue, KeyData, sizeof (*KeyData));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reset the input device and optionally run diagnostics
+
+  There are 2 types of reset for USB keyboard.
+  For non-exhaustive reset, only keyboard buffer is cleared.
+  For exhaustive reset, in addition to clearance of keyboard buffer, the hardware status
+  is also re-initialized.
+
+  @param  This                 Protocol instance pointer.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReset (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL   *This,
+  IN  BOOLEAN                          ExtendedVerification
+  )
+{
+  EFI_STATUS          Status;
+  USB_KB_DEV          *UsbKeyboardDevice;
+
+  UsbKeyboardDevice = USB_KB_DEV_FROM_THIS (This);
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_RESET),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  //
+  // Non-exhaustive reset:
+  // only reset private data structures.
+  //
+  if (!ExtendedVerification) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_PERIPHERAL_KEYBOARD | EFI_P_KEYBOARD_PC_CLEAR_BUFFER),
+      UsbKeyboardDevice->DevicePath
+      );
+    //
+    // Clear the key buffer of this USB keyboard
+    //
+    InitQueue (&UsbKeyboardDevice->UsbKeyQueue, sizeof (USB_KEY));
+    InitQueue (&UsbKeyboardDevice->EfiKeyQueue, sizeof (EFI_KEY_DATA));
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Exhaustive reset
+  //
+  Status = InitUSBKeyboard (UsbKeyboardDevice);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reads the next keystroke from the input device.
+
+  @param  This                 The EFI_SIMPLE_TEXT_INPUT_PROTOCOL instance.
+  @param  Key                  A pointer to a buffer that is filled in with the keystroke
+                               information for the key that was pressed.
+
+  @retval EFI_SUCCESS          The keystroke information was returned.
+  @retval EFI_NOT_READY        There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR     The keystroke information was not returned due to
+                               hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReadKeyStroke (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL   *This,
+  OUT EFI_INPUT_KEY                    *Key
+  )
+{
+  USB_KB_DEV   *UsbKeyboardDevice;
+  EFI_STATUS   Status;
+  EFI_KEY_DATA KeyData;
+
+  UsbKeyboardDevice = USB_KB_DEV_FROM_THIS (This);
+
+  //
+  // Considering if the partial keystroke is enabled, there maybe a partial
+  // keystroke in the queue, so here skip the partial keystroke and get the
+  // next key from the queue
+  //
+  while (1) {
+    Status = USBKeyboardReadKeyStrokeWorker (UsbKeyboardDevice, &KeyData);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // SimpleTextIn Protocol doesn't support partial keystroke;
+    //
+    if (KeyData.Key.ScanCode == CHAR_NULL && KeyData.Key.UnicodeChar == SCAN_NULL) {
+      continue;
+    }
+    //
+    // Translate the CTRL-Alpha characters to their corresponding control value
+    // (ctrl-a = 0x0001 through ctrl-Z = 0x001A)
+    //
+    if ((KeyData.KeyState.KeyShiftState & (EFI_LEFT_CONTROL_PRESSED | EFI_RIGHT_CONTROL_PRESSED)) != 0) {
+      if (KeyData.Key.UnicodeChar >= L'a' && KeyData.Key.UnicodeChar <= L'z') {
+        KeyData.Key.UnicodeChar = (CHAR16) (KeyData.Key.UnicodeChar - L'a' + 1);
+      } else if (KeyData.Key.UnicodeChar >= L'A' && KeyData.Key.UnicodeChar <= L'Z') {
+        KeyData.Key.UnicodeChar = (CHAR16) (KeyData.Key.UnicodeChar - L'A' + 1);
+      }
+    }
+
+    CopyMem (Key, &KeyData.Key, sizeof (EFI_INPUT_KEY));
+    return EFI_SUCCESS;
+  }
+}
+
+
+/**
+  Event notification function registered for EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL.WaitForKeyEx
+  and EFI_SIMPLE_TEXT_INPUT_PROTOCOL.WaitForKey.
+
+  @param  Event        Event to be signaled when a key is pressed.
+  @param  Context      Points to USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardWaitForKey (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  )
+{
+  USB_KB_DEV  *UsbKeyboardDevice;
+  EFI_KEY_DATA KeyData;
+  EFI_TPL      OldTpl;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+
+  //
+  // Enter critical section
+  //  
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  
+  //
+  // WaitforKey doesn't suppor the partial key.
+  // Considering if the partial keystroke is enabled, there maybe a partial
+  // keystroke in the queue, so here skip the partial keystroke and get the
+  // next key from the queue
+  //
+  while (!IsQueueEmpty (&UsbKeyboardDevice->EfiKeyQueue)) {
+    //
+    // If there is pending key, signal the event.
+    //
+    CopyMem (
+      &KeyData,
+      UsbKeyboardDevice->EfiKeyQueue.Buffer[UsbKeyboardDevice->EfiKeyQueue.Head],
+      sizeof (EFI_KEY_DATA)
+      );
+    if (KeyData.Key.ScanCode == SCAN_NULL && KeyData.Key.UnicodeChar == CHAR_NULL) {
+      Dequeue (&UsbKeyboardDevice->EfiKeyQueue, &KeyData, sizeof (EFI_KEY_DATA));
+      continue;
+    }
+    gBS->SignalEvent (Event);
+    break;
+  }
+  //
+  // Leave critical section and return
+  //
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Timer handler to convert the key from USB.
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+**/
+VOID
+EFIAPI
+USBKeyboardTimerHandler (
+  IN  EFI_EVENT                 Event,
+  IN  VOID                      *Context
+  )
+{
+  EFI_STATUS                    Status;
+  USB_KB_DEV                    *UsbKeyboardDevice;
+  UINT8                         KeyCode;
+  EFI_KEY_DATA                  KeyData;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+
+  //
+  // Fetch raw data from the USB keyboard buffer,
+  // and translate it into USB keycode.
+  //
+  Status = USBParseKey (UsbKeyboardDevice, &KeyCode);
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  //
+  // Translate saved USB keycode into EFI_INPUT_KEY
+  //
+  Status = UsbKeyCodeToEfiInputKey (UsbKeyboardDevice, KeyCode, &KeyData);
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  //
+  // Insert to the EFI Key queue
+  //
+  Enqueue (&UsbKeyboardDevice->EfiKeyQueue, &KeyData, sizeof (KeyData));
+}
+
+/**
+  Free keyboard notify list.
+
+  @param  NotifyList              The keyboard notify list to free.
+
+  @retval EFI_SUCCESS             Free the notify list successfully.
+  @retval EFI_INVALID_PARAMETER   NotifyList is NULL.
+
+**/
+EFI_STATUS
+KbdFreeNotifyList (
+  IN OUT LIST_ENTRY           *NotifyList
+  )
+{
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY *NotifyNode;
+  LIST_ENTRY                    *Link;
+
+  if (NotifyList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  while (!IsListEmpty (NotifyList)) {
+    Link = GetFirstNode (NotifyList);
+    NotifyNode = CR (Link, KEYBOARD_CONSOLE_IN_EX_NOTIFY, NotifyEntry, USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE);
+    RemoveEntryList (Link);
+    FreePool (NotifyNode);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether the pressed key matches a registered key or not.
+
+  @param  RegsiteredData    A pointer to keystroke data for the key that was registered.
+  @param  InputData         A pointer to keystroke data for the key that was pressed.
+
+  @retval TRUE              Key pressed matches a registered key.
+  @retval FLASE             Key pressed does not matches a registered key.
+
+**/
+BOOLEAN
+IsKeyRegistered (
+  IN EFI_KEY_DATA  *RegsiteredData,
+  IN EFI_KEY_DATA  *InputData
+  )
+{
+  ASSERT (RegsiteredData != NULL && InputData != NULL);
+
+  if ((RegsiteredData->Key.ScanCode    != InputData->Key.ScanCode) ||
+      (RegsiteredData->Key.UnicodeChar != InputData->Key.UnicodeChar)) {
+    return FALSE;
+  }
+
+  //
+  // Assume KeyShiftState/KeyToggleState = 0 in Registered key data means these state could be ignored.
+  //
+  if (RegsiteredData->KeyState.KeyShiftState != 0 &&
+      RegsiteredData->KeyState.KeyShiftState != InputData->KeyState.KeyShiftState) {
+    return FALSE;
+  }
+  if (RegsiteredData->KeyState.KeyToggleState != 0 &&
+      RegsiteredData->KeyState.KeyToggleState != InputData->KeyState.KeyToggleState) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+//
+// Simple Text Input Ex protocol functions
+//
+/**
+  Resets the input device hardware.
+
+  The Reset() function resets the input device hardware. As part
+  of initialization process, the firmware/device will make a quick
+  but reasonable attempt to verify that the device is functioning.
+  If the ExtendedVerification flag is TRUE the firmware may take
+  an extended amount of time to verify the device is operating on
+  reset. Otherwise the reset operation is to occur as quickly as
+  possible. The hardware verification process is not defined by
+  this specification and is left up to the platform firmware or
+  driver to implement.
+
+  @param This                 A pointer to the EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL instance.
+
+  @param ExtendedVerification Indicates that the driver may perform a more exhaustive
+                              verification operation of the device during reset.
+
+  @retval EFI_SUCCESS         The device was reset.
+  @retval EFI_DEVICE_ERROR    The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardResetEx (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN BOOLEAN                            ExtendedVerification
+  )
+{
+  EFI_STATUS                Status;
+  USB_KB_DEV                *UsbKeyboardDevice;
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  Status = UsbKeyboardDevice->SimpleInput.Reset (&UsbKeyboardDevice->SimpleInput, ExtendedVerification);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  UsbKeyboardDevice->KeyState.KeyShiftState  = EFI_SHIFT_STATE_VALID;
+  UsbKeyboardDevice->KeyState.KeyToggleState = EFI_TOGGLE_STATE_VALID;
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Reads the next keystroke from the input device.
+
+  @param  This                   Protocol instance pointer.
+  @param  KeyData                A pointer to a buffer that is filled in with the keystroke
+                                 state data for the key that was pressed.
+
+  @retval EFI_SUCCESS            The keystroke information was returned.
+  @retval EFI_NOT_READY          There was no keystroke data available.
+  @retval EFI_DEVICE_ERROR       The keystroke information was not returned due to
+                                 hardware errors.
+  @retval EFI_INVALID_PARAMETER  KeyData is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReadKeyStrokeEx (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *This,
+  OUT EFI_KEY_DATA                      *KeyData
+  )
+{
+  USB_KB_DEV                        *UsbKeyboardDevice;
+
+  if (KeyData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  return USBKeyboardReadKeyStrokeWorker (UsbKeyboardDevice, KeyData);
+
+}
+
+/**
+  Set certain state for the input device.
+
+  @param  This                    Protocol instance pointer.
+  @param  KeyToggleState          A pointer to the EFI_KEY_TOGGLE_STATE to set the
+                                  state for the input device.
+
+  @retval EFI_SUCCESS             The device state was set appropriately.
+  @retval EFI_DEVICE_ERROR        The device is not functioning correctly and could
+                                  not have the setting adjusted.
+  @retval EFI_UNSUPPORTED         The device does not support the ability to have its state set.
+  @retval EFI_INVALID_PARAMETER   KeyToggleState is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardSetState (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_TOGGLE_STATE               *KeyToggleState
+  )
+{
+  USB_KB_DEV                        *UsbKeyboardDevice;
+
+  if (KeyToggleState == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  if (((UsbKeyboardDevice->KeyState.KeyToggleState & EFI_TOGGLE_STATE_VALID) != EFI_TOGGLE_STATE_VALID) ||
+      ((*KeyToggleState & EFI_TOGGLE_STATE_VALID) != EFI_TOGGLE_STATE_VALID)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Update the status light
+  //
+
+  UsbKeyboardDevice->ScrollOn   = FALSE;
+  UsbKeyboardDevice->NumLockOn  = FALSE;
+  UsbKeyboardDevice->CapsOn     = FALSE;
+  UsbKeyboardDevice->IsSupportPartialKey = FALSE;
+
+  if ((*KeyToggleState & EFI_SCROLL_LOCK_ACTIVE) == EFI_SCROLL_LOCK_ACTIVE) {
+    UsbKeyboardDevice->ScrollOn = TRUE;
+  }
+  if ((*KeyToggleState & EFI_NUM_LOCK_ACTIVE) == EFI_NUM_LOCK_ACTIVE) {
+    UsbKeyboardDevice->NumLockOn = TRUE;
+  }
+  if ((*KeyToggleState & EFI_CAPS_LOCK_ACTIVE) == EFI_CAPS_LOCK_ACTIVE) {
+    UsbKeyboardDevice->CapsOn = TRUE;
+  }
+  if ((*KeyToggleState & EFI_KEY_STATE_EXPOSED) == EFI_KEY_STATE_EXPOSED) {
+    UsbKeyboardDevice->IsSupportPartialKey = TRUE;
+  }
+
+  SetKeyLED (UsbKeyboardDevice);
+
+  UsbKeyboardDevice->KeyState.KeyToggleState = *KeyToggleState;
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Register a notification function for a particular keystroke for the input device.
+
+  @param  This                        Protocol instance pointer.
+  @param  KeyData                     A pointer to a buffer that is filled in with the keystroke
+                                      information data for the key that was pressed.
+  @param  KeyNotificationFunction     Points to the function to be called when the key
+                                      sequence is typed specified by KeyData.
+  @param  NotifyHandle                Points to the unique handle assigned to the registered notification.
+
+  @retval EFI_SUCCESS                 The notification function was registered successfully.
+  @retval EFI_OUT_OF_RESOURCES        Unable to allocate resources for necessary data structures.
+  @retval EFI_INVALID_PARAMETER       KeyData or NotifyHandle or KeyNotificationFunction is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardRegisterKeyNotify (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN  EFI_KEY_DATA                       *KeyData,
+  IN  EFI_KEY_NOTIFY_FUNCTION            KeyNotificationFunction,
+  OUT VOID                               **NotifyHandle
+  )
+{
+  USB_KB_DEV                        *UsbKeyboardDevice;
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY     *NewNotify;
+  LIST_ENTRY                        *Link;
+  LIST_ENTRY                        *NotifyList;
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY     *CurrentNotify;
+
+  if (KeyData == NULL || NotifyHandle == NULL || KeyNotificationFunction == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  //
+  // Return EFI_SUCCESS if the (KeyData, NotificationFunction) is already registered.
+  //
+  NotifyList = &UsbKeyboardDevice->NotifyList;
+
+  for (Link = GetFirstNode (NotifyList);
+       !IsNull (NotifyList, Link);
+       Link = GetNextNode (NotifyList, Link)) {
+    CurrentNotify = CR (
+                      Link,
+                      KEYBOARD_CONSOLE_IN_EX_NOTIFY,
+                      NotifyEntry,
+                      USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE
+                      );
+    if (IsKeyRegistered (&CurrentNotify->KeyData, KeyData)) {
+      if (CurrentNotify->KeyNotificationFn == KeyNotificationFunction) {
+        *NotifyHandle = CurrentNotify;
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  //
+  // Allocate resource to save the notification function
+  //
+  NewNotify = (KEYBOARD_CONSOLE_IN_EX_NOTIFY *) AllocateZeroPool (sizeof (KEYBOARD_CONSOLE_IN_EX_NOTIFY));
+  if (NewNotify == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NewNotify->Signature         = USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE;
+  NewNotify->KeyNotificationFn = KeyNotificationFunction;
+  CopyMem (&NewNotify->KeyData, KeyData, sizeof (EFI_KEY_DATA));
+  InsertTailList (&UsbKeyboardDevice->NotifyList, &NewNotify->NotifyEntry);
+
+
+  *NotifyHandle = NewNotify;
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Remove a registered notification function from a particular keystroke.
+
+  @param  This                      Protocol instance pointer.
+  @param  NotificationHandle        The handle of the notification function being unregistered.
+
+  @retval EFI_SUCCESS              The notification function was unregistered successfully.
+  @retval EFI_INVALID_PARAMETER    The NotificationHandle is invalid
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardUnregisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN VOID                               *NotificationHandle
+  )
+{
+  USB_KB_DEV                        *UsbKeyboardDevice;
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY     *CurrentNotify;
+  LIST_ENTRY                        *Link;
+  LIST_ENTRY                        *NotifyList;
+
+  if (NotificationHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  //
+  // Traverse notify list of USB keyboard and remove the entry of NotificationHandle.
+  //
+  NotifyList = &UsbKeyboardDevice->NotifyList;
+  for (Link = GetFirstNode (NotifyList);
+       !IsNull (NotifyList, Link);
+       Link = GetNextNode (NotifyList, Link)) {
+    CurrentNotify = CR (
+                      Link,
+                      KEYBOARD_CONSOLE_IN_EX_NOTIFY,
+                      NotifyEntry,
+                      USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE
+                      );
+    if (CurrentNotify == NotificationHandle) {
+      //
+      // Remove the notification function from NotifyList and free resources
+      //
+      RemoveEntryList (&CurrentNotify->NotifyEntry);
+
+      FreePool (CurrentNotify);
+      return EFI_SUCCESS;
+    }
+  }
+
+  //
+  // Cannot find the matching entry in database.
+  //
+  return EFI_INVALID_PARAMETER;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.h b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.h
new file mode 100644
index 0000000000..58edb3f65f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.h
@@ -0,0 +1,600 @@
+/** @file
+  Header file for USB Keyboard Driver's Data Structures.
+
+Copyright (c) 2004 - 2012, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+#ifndef _EFI_USB_KB_H_
+#define _EFI_USB_KB_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/SimpleTextIn.h>
+#include <Protocol/SimpleTextInEx.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Guid/HiiKeyBoardLayout.h>
+#include <Guid/UsbKeyBoardLayout.h>
+
+#include <Library/DebugLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiUsbLib.h>
+#include <Library/HiiLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#define KEYBOARD_TIMER_INTERVAL         200000  // 0.02s
+
+#define MAX_KEY_ALLOWED     32
+
+#define HZ                  1000 * 1000 * 10
+#define USBKBD_REPEAT_DELAY ((HZ) / 2)
+#define USBKBD_REPEAT_RATE  ((HZ) / 50)
+
+#define CLASS_HID           3
+#define SUBCLASS_BOOT       1
+#define PROTOCOL_KEYBOARD   1
+
+#define BOOT_PROTOCOL       0
+#define REPORT_PROTOCOL     1
+
+typedef struct {
+  BOOLEAN  Down;
+  UINT8    KeyCode;
+} USB_KEY;
+
+typedef struct {
+  VOID          *Buffer[MAX_KEY_ALLOWED + 1];
+  UINTN         Head;
+  UINTN         Tail;
+  UINTN         ItemSize;
+} USB_SIMPLE_QUEUE;
+
+#define USB_KB_DEV_SIGNATURE  SIGNATURE_32 ('u', 'k', 'b', 'd')
+#define USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE SIGNATURE_32 ('u', 'k', 'b', 'x')
+
+typedef struct _KEYBOARD_CONSOLE_IN_EX_NOTIFY {
+  UINTN                                 Signature;
+  EFI_KEY_DATA                          KeyData;
+  EFI_KEY_NOTIFY_FUNCTION               KeyNotificationFn;
+  LIST_ENTRY                            NotifyEntry;
+} KEYBOARD_CONSOLE_IN_EX_NOTIFY;
+
+#define USB_NS_KEY_SIGNATURE  SIGNATURE_32 ('u', 'n', 's', 'k')
+
+typedef struct {
+  UINTN                         Signature;
+  LIST_ENTRY                    Link;
+
+  //
+  // The number of EFI_NS_KEY_MODIFIER children definitions
+  //
+  UINTN                         KeyCount;
+
+  //
+  // NsKey[0] : Non-spacing key
+  // NsKey[1] ~ NsKey[KeyCount] : Physical keys
+  //
+  EFI_KEY_DESCRIPTOR            *NsKey;
+} USB_NS_KEY;
+
+#define USB_NS_KEY_FORM_FROM_LINK(a)  CR (a, USB_NS_KEY, Link, USB_NS_KEY_SIGNATURE)
+
+///
+/// Structure to describe USB keyboard device
+///
+typedef struct {
+  UINTN                             Signature;
+  EFI_HANDLE                        ControllerHandle;
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath;
+  EFI_EVENT                         DelayedRecoveryEvent;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL    SimpleInput;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL SimpleInputEx;
+  EFI_USB_IO_PROTOCOL               *UsbIo;
+
+  EFI_USB_INTERFACE_DESCRIPTOR      InterfaceDescriptor;
+  EFI_USB_ENDPOINT_DESCRIPTOR       IntEndpointDescriptor;
+
+  USB_SIMPLE_QUEUE                  UsbKeyQueue;
+  USB_SIMPLE_QUEUE                  EfiKeyQueue;
+  BOOLEAN                           CtrlOn;
+  BOOLEAN                           AltOn;
+  BOOLEAN                           ShiftOn;
+  BOOLEAN                           NumLockOn;
+  BOOLEAN                           CapsOn;
+  BOOLEAN                           ScrollOn;
+  UINT8                             LastKeyCodeArray[8];
+  UINT8                             CurKeyCode;
+
+  EFI_EVENT                         TimerEvent;
+
+  UINT8                             RepeatKey;
+  EFI_EVENT                         RepeatTimer;
+
+  EFI_UNICODE_STRING_TABLE          *ControllerNameTable;
+
+  BOOLEAN                           LeftCtrlOn;
+  BOOLEAN                           LeftAltOn;
+  BOOLEAN                           LeftShiftOn;
+  BOOLEAN                           LeftLogoOn;
+  BOOLEAN                           RightCtrlOn;
+  BOOLEAN                           RightAltOn;
+  BOOLEAN                           RightShiftOn;
+  BOOLEAN                           RightLogoOn;
+  BOOLEAN                           MenuKeyOn;
+  BOOLEAN                           SysReqOn;
+  BOOLEAN                           AltGrOn;
+
+  BOOLEAN                         IsSupportPartialKey;
+
+  EFI_KEY_STATE                     KeyState;
+  //
+  // Notification function list
+  //
+  LIST_ENTRY                        NotifyList;
+
+  //
+  // Non-spacing key list
+  //
+  LIST_ENTRY                        NsKeyList;
+  USB_NS_KEY                        *CurrentNsKey;
+  EFI_KEY_DESCRIPTOR                *KeyConvertionTable;
+  EFI_EVENT                         KeyboardLayoutEvent;
+} USB_KB_DEV;
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gUsbKeyboardDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gUsbKeyboardComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUsbKeyboardComponentName2;
+
+#define USB_KB_DEV_FROM_THIS(a) \
+    CR(a, USB_KB_DEV, SimpleInput, USB_KB_DEV_SIGNATURE)
+#define TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS(a) \
+    CR(a, USB_KB_DEV, SimpleInputEx, USB_KB_DEV_SIGNATURE)
+
+//
+// According to Universal Serial Bus HID Usage Tables document ver 1.12,
+// a Boot Keyboard should support the keycode range from 0x0 to 0x65 and 0xE0 to 0xE7.
+// 0xE0 to 0xE7 are for modifier keys, and 0x0 to 0x3 are reserved for typical
+// keyboard status or keyboard errors.
+// So the number of valid non-modifier USB keycodes is 0x62, and the number of
+// valid keycodes is 0x6A.
+//
+#define NUMBER_OF_VALID_NON_MODIFIER_USB_KEYCODE      0x62
+#define NUMBER_OF_VALID_USB_KEYCODE                   0x6A
+//
+// 0x0 to 0x3 are reserved for typical keyboard status or keyboard errors.
+//
+#define USBKBD_VALID_KEYCODE(Key) ((UINT8) (Key) > 3)
+
+typedef struct {
+  UINT8 NumLock : 1;
+  UINT8 CapsLock : 1;
+  UINT8 ScrollLock : 1;
+  UINT8 Resrvd : 5;
+} LED_MAP;
+
+//
+// Functions of Driver Binding Protocol
+//
+/**
+  Check whether USB keyboard driver supports this device.
+
+  @param  This                   The USB keyboard driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Starts the keyboard device with this driver.
+
+  This function produces Simple Text Input Protocol and Simple Text Input Ex Protocol,
+  initializes the keyboard device, and submit Asynchronous Interrupt Transfer to manage
+  this keyboard device.
+
+  @param  This                   The USB keyboard driver binding instance.
+  @param  Controller             Handle of device to bind driver to.
+  @param  RemainingDevicePath    Optional parameter use to pick a specific child
+                                 device to start.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb keyboard driver.
+  @retval EFI_UNSUPPORTED        No interrupt endpoint can be found.
+  @retval Other                  This controller cannot be started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop the USB keyboard device handled by this driver.
+
+  @param  This                   The USB keyboard driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Simple Text In Protocol or Simple Text In Ex Protocol
+                                 is not installed on Controller.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbKeyboardComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is not a valid EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbKeyboardComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+//
+// Functions of Simple Text Input Protocol
+//
+/**
+  Reset the input device and optionaly run diagnostics
+
+  There are 2 types of reset for USB keyboard.
+  For non-exhaustive reset, only keyboard buffer is cleared.
+  For exhaustive reset, in addition to clearance of keyboard buffer, the hardware status
+  is also re-initialized.
+
+  @param  This                 Protocol instance pointer.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReset (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL   *This,
+  IN  BOOLEAN                          ExtendedVerification
+  );
+
+/**
+  Reads the next keystroke from the input device.
+
+  @param  This                 The EFI_SIMPLE_TEXT_INPUT_PROTOCOL instance.
+  @param  Key                  A pointer to a buffer that is filled in with the keystroke
+                               information for the key that was pressed.
+
+  @retval EFI_SUCCESS          The keystroke information was returned.
+  @retval EFI_NOT_READY        There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR     The keydtroke information was not returned due to
+                               hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReadKeyStroke (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL   *This,
+  OUT EFI_INPUT_KEY                    *Key
+  );
+
+//
+// Simple Text Input Ex protocol functions
+//
+/**
+  Resets the input device hardware.
+
+  The Reset() function resets the input device hardware. As part
+  of initialization process, the firmware/device will make a quick
+  but reasonable attempt to verify that the device is functioning.
+  If the ExtendedVerification flag is TRUE the firmware may take
+  an extended amount of time to verify the device is operating on
+  reset. Otherwise the reset operation is to occur as quickly as
+  possible. The hardware verification process is not defined by
+  this specification and is left up to the platform firmware or
+  driver to implement.
+
+  @param This                 A pointer to the EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL instance.
+
+  @param ExtendedVerification Indicates that the driver may perform a more exhaustive
+                              verification operation of the device during reset.
+
+  @retval EFI_SUCCESS         The device was reset.
+  @retval EFI_DEVICE_ERROR    The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardResetEx (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN BOOLEAN                            ExtendedVerification
+  );
+
+/**
+  Reads the next keystroke from the input device.
+
+  @param  This                   Protocol instance pointer.
+  @param  KeyData                A pointer to a buffer that is filled in with the keystroke
+                                 state data for the key that was pressed.
+
+  @retval EFI_SUCCESS            The keystroke information was returned.
+  @retval EFI_NOT_READY          There was no keystroke data available.
+  @retval EFI_DEVICE_ERROR       The keystroke information was not returned due to
+                                 hardware errors.
+  @retval EFI_INVALID_PARAMETER  KeyData is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReadKeyStrokeEx (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *This,
+  OUT EFI_KEY_DATA                      *KeyData
+  );
+
+/**
+  Set certain state for the input device.
+
+  @param  This                    Protocol instance pointer.
+  @param  KeyToggleState          A pointer to the EFI_KEY_TOGGLE_STATE to set the
+                                  state for the input device.
+
+  @retval EFI_SUCCESS             The device state was set appropriately.
+  @retval EFI_DEVICE_ERROR        The device is not functioning correctly and could
+                                  not have the setting adjusted.
+  @retval EFI_UNSUPPORTED         The device does not support the ability to have its state set.
+  @retval EFI_INVALID_PARAMETER   KeyToggleState is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardSetState (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_TOGGLE_STATE               *KeyToggleState
+  );
+
+/**
+  Register a notification function for a particular keystroke for the input device.
+
+  @param  This                        Protocol instance pointer.
+  @param  KeyData                     A pointer to a buffer that is filled in with the keystroke
+                                      information data for the key that was pressed.
+  @param  KeyNotificationFunction     Points to the function to be called when the key
+                                      sequence is typed specified by KeyData.
+  @param  NotifyHandle                Points to the unique handle assigned to the registered notification.
+
+  @retval EFI_SUCCESS                 The notification function was registered successfully.
+  @retval EFI_OUT_OF_RESOURCES        Unable to allocate resources for necesssary data structures.
+  @retval EFI_INVALID_PARAMETER       KeyData or NotifyHandle or KeyNotificationFunction is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardRegisterKeyNotify (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN  EFI_KEY_DATA                       *KeyData,
+  IN  EFI_KEY_NOTIFY_FUNCTION            KeyNotificationFunction,
+  OUT VOID                               **NotifyHandle
+  );
+
+/**
+  Remove a registered notification function from a particular keystroke.
+
+  @param  This                      Protocol instance pointer.
+  @param  NotificationHandle        The handle of the notification function being unregistered.
+
+  @retval EFI_SUCCESS              The notification function was unregistered successfully.
+  @retval EFI_INVALID_PARAMETER    The NotificationHandle is invalid
+  @retval EFI_NOT_FOUND            Cannot find the matching entry in database.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardUnregisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN VOID                               *NotificationHandle
+  );
+
+/**
+  Event notification function registered for EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL.WaitForKeyEx
+  and EFI_SIMPLE_TEXT_INPUT_PROTOCOL.WaitForKey.
+
+  @param  Event        Event to be signaled when a key is pressed.
+  @param  Context      Points to USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardWaitForKey (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  );
+
+/**
+  Free keyboard notify list.
+
+  @param  NotifyList              The keyboard notify list to free.
+
+  @retval EFI_SUCCESS             Free the notify list successfully.
+  @retval EFI_INVALID_PARAMETER   NotifyList is NULL.
+
+**/
+EFI_STATUS
+KbdFreeNotifyList (
+  IN OUT LIST_ENTRY           *NotifyList
+  );
+
+/**
+  Check whether the pressed key matches a registered key or not.
+
+  @param  RegsiteredData    A pointer to keystroke data for the key that was registered.
+  @param  InputData         A pointer to keystroke data for the key that was pressed.
+
+  @retval TRUE              Key pressed matches a registered key.
+  @retval FLASE             Key pressed does not matche a registered key.
+
+**/
+BOOLEAN
+IsKeyRegistered (
+  IN EFI_KEY_DATA  *RegsiteredData,
+  IN EFI_KEY_DATA  *InputData
+  );
+
+/**
+  Timer handler to convert the key from USB.
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+**/
+VOID
+EFIAPI
+USBKeyboardTimerHandler (
+  IN  EFI_EVENT                 Event,
+  IN  VOID                      *Context
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.c b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.c
new file mode 100644
index 0000000000..86dd99d254
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.c
@@ -0,0 +1,1956 @@
+/** @file
+  Helper functions for USB Keyboard Driver.
+
+Copyright (c) 2004 - 2013, 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 "KeyBoard.h"
+
+USB_KEYBOARD_LAYOUT_PACK_BIN  mUsbKeyboardLayoutBin = {
+  sizeof (USB_KEYBOARD_LAYOUT_PACK_BIN),   // Binary size
+
+  //
+  // EFI_HII_PACKAGE_HEADER
+  //
+  {
+    sizeof (USB_KEYBOARD_LAYOUT_PACK_BIN) - sizeof (UINT32),
+    EFI_HII_PACKAGE_KEYBOARD_LAYOUT
+  },
+  1,  // LayoutCount
+  sizeof (USB_KEYBOARD_LAYOUT_PACK_BIN) - sizeof (UINT32) - sizeof (EFI_HII_PACKAGE_HEADER) - sizeof (UINT16), // LayoutLength
+  USB_KEYBOARD_LAYOUT_KEY_GUID,  // KeyGuid
+  sizeof (UINT16) + sizeof (EFI_GUID) + sizeof (UINT32) + sizeof (UINT8) + (USB_KEYBOARD_KEY_COUNT * sizeof (EFI_KEY_DESCRIPTOR)), // LayoutDescriptorStringOffset
+  USB_KEYBOARD_KEY_COUNT, // DescriptorCount
+  {
+    //
+    // EFI_KEY_DESCRIPTOR (total number is USB_KEYBOARD_KEY_COUNT)
+    //
+    {EfiKeyC1,         'a',      'A',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB5,         'b',      'B',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB3,         'c',      'C',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC3,         'd',      'D',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD3,         'e',      'E',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC4,         'f',      'F',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC5,         'g',      'G',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC6,         'h',      'H',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD8,         'i',      'I',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC7,         'j',      'J',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC8,         'k',      'K',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC9,         'l',      'L',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB7,         'm',      'M',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB6,         'n',      'N',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD9,         'o',      'O',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD10,        'p',      'P',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD1,         'q',      'Q',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD4,         'r',      'R',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC2,         's',      'S',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD5,         't',      'T',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD7,         'u',      'U',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB4,         'v',      'V',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD2,         'w',      'W',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB2,         'x',      'X',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD6,         'y',      'Y',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB1,         'z',      'Z',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyE1,         '1',      '!',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE2,         '2',      '@',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE3,         '3',      '#',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE4,         '4',      '$',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE5,         '5',      '%',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE6,         '6',      '^',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE7,         '7',      '&',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE8,         '8',      '*',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE9,         '9',      '(',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE10,        '0',      ')',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyEnter,      0x0d,     0x0d, 0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeyEsc,        0x1b,     0x1b, 0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeyBackSpace,  0x08,     0x08, 0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeyTab,        0x09,     0x09, 0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeySpaceBar,   ' ',      ' ',  0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeyE11,        '-',      '_',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE12,        '=',      '+',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyD11,        '[',      '{',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyD12,        ']',      '}',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyD13,        '\\',     '|',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyC12,        '\\',     '|',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyC10,        ';',      ':',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyC11,        '\'',     '"',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE0,         '`',      '~',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyB8,         ',',      '<',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyB9,         '.',      '>',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyB10,        '/',      '?',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyCapsLock,   0x00,     0x00, 0, 0,  EFI_CAPS_LOCK_MODIFIER,            0},
+    {EfiKeyF1,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_ONE_MODIFIER,     0},
+    {EfiKeyF2,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_TWO_MODIFIER,     0},
+    {EfiKeyF3,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_THREE_MODIFIER,   0},
+    {EfiKeyF4,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_FOUR_MODIFIER,    0},
+    {EfiKeyF5,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_FIVE_MODIFIER,    0},
+    {EfiKeyF6,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_SIX_MODIFIER,     0},
+    {EfiKeyF7,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_SEVEN_MODIFIER,   0},
+    {EfiKeyF8,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_EIGHT_MODIFIER,   0},
+    {EfiKeyF9,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_NINE_MODIFIER,    0},
+    {EfiKeyF10,        0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_TEN_MODIFIER,     0},
+    {EfiKeyF11,        0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_ELEVEN_MODIFIER,  0},
+    {EfiKeyF12,        0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_TWELVE_MODIFIER,  0},
+    {EfiKeyPrint,      0x00,     0x00, 0, 0,  EFI_PRINT_MODIFIER,                0},
+    {EfiKeySLck,       0x00,     0x00, 0, 0,  EFI_SCROLL_LOCK_MODIFIER,          0},
+    {EfiKeyPause,      0x00,     0x00, 0, 0,  EFI_PAUSE_MODIFIER,                0},
+    {EfiKeyIns,        0x00,     0x00, 0, 0,  EFI_INSERT_MODIFIER,               0},
+    {EfiKeyHome,       0x00,     0x00, 0, 0,  EFI_HOME_MODIFIER,                 0},
+    {EfiKeyPgUp,       0x00,     0x00, 0, 0,  EFI_PAGE_UP_MODIFIER,              0},
+    {EfiKeyDel,        0x00,     0x00, 0, 0,  EFI_DELETE_MODIFIER,               0},
+    {EfiKeyEnd,        0x00,     0x00, 0, 0,  EFI_END_MODIFIER,                  0},
+    {EfiKeyPgDn,       0x00,     0x00, 0, 0,  EFI_PAGE_DOWN_MODIFIER,            0},
+    {EfiKeyRightArrow, 0x00,     0x00, 0, 0,  EFI_RIGHT_ARROW_MODIFIER,          0},
+    {EfiKeyLeftArrow,  0x00,     0x00, 0, 0,  EFI_LEFT_ARROW_MODIFIER,           0},
+    {EfiKeyDownArrow,  0x00,     0x00, 0, 0,  EFI_DOWN_ARROW_MODIFIER,           0},
+    {EfiKeyUpArrow,    0x00,     0x00, 0, 0,  EFI_UP_ARROW_MODIFIER,             0},
+    {EfiKeyNLck,       0x00,     0x00, 0, 0,  EFI_NUM_LOCK_MODIFIER,             0},
+    {EfiKeySlash,      '/',      '/',  0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyAsterisk,   '*',      '*',  0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyMinus,      '-',      '-',  0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyPlus,       '+',      '+',  0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyEnter,      0x0d,     0x0d, 0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyOne,        '1',      '1',  0, 0,  EFI_END_MODIFIER,         EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyTwo,        '2',      '2',  0, 0,  EFI_DOWN_ARROW_MODIFIER,  EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyThree,      '3',      '3',  0, 0,  EFI_PAGE_DOWN_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyFour,       '4',      '4',  0, 0,  EFI_LEFT_ARROW_MODIFIER,  EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyFive,       '5',      '5',  0, 0,  EFI_NULL_MODIFIER,        EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeySix,        '6',      '6',  0, 0,  EFI_RIGHT_ARROW_MODIFIER, EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeySeven,      '7',      '7',  0, 0,  EFI_HOME_MODIFIER,        EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyEight,      '8',      '8',  0, 0,  EFI_UP_ARROW_MODIFIER,    EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyNine,       '9',      '9',  0, 0,  EFI_PAGE_UP_MODIFIER,     EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyZero,       '0',      '0',  0, 0,  EFI_INSERT_MODIFIER,      EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyPeriod,     '.',      '.',  0, 0,  EFI_DELETE_MODIFIER,      EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyA4,         0x00,     0x00, 0, 0,  EFI_MENU_MODIFIER,            0},
+    {EfiKeyLCtrl,      0,        0,    0, 0,  EFI_LEFT_CONTROL_MODIFIER,    0},
+    {EfiKeyLShift,     0,        0,    0, 0,  EFI_LEFT_SHIFT_MODIFIER,      0},
+    {EfiKeyLAlt,       0,        0,    0, 0,  EFI_LEFT_ALT_MODIFIER,        0},
+    {EfiKeyA0,         0,        0,    0, 0,  EFI_LEFT_LOGO_MODIFIER,       0},
+    {EfiKeyRCtrl,      0,        0,    0, 0,  EFI_RIGHT_CONTROL_MODIFIER,   0},
+    {EfiKeyRShift,     0,        0,    0, 0,  EFI_RIGHT_SHIFT_MODIFIER,     0},
+    {EfiKeyA2,         0,        0,    0, 0,  EFI_RIGHT_ALT_MODIFIER,       0},
+    {EfiKeyA3,         0,        0,    0, 0,  EFI_RIGHT_LOGO_MODIFIER,      0},
+  },
+  1,                          // DescriptionCount
+  {'e', 'n', '-', 'U', 'S'},  // RFC4646 language code
+  ' ',                        // Space
+  {'E', 'n', 'g', 'l', 'i', 's', 'h', ' ', 'K', 'e', 'y', 'b', 'o', 'a', 'r', 'd', '\0'}, // DescriptionString[]
+};
+
+//
+// EFI_KEY to USB Keycode conversion table
+// EFI_KEY is defined in UEFI spec.
+// USB Keycode is defined in USB HID Firmware spec.
+//
+UINT8 EfiKeyToUsbKeyCodeConvertionTable[] = {
+  0xe0,  //  EfiKeyLCtrl
+  0xe3,  //  EfiKeyA0
+  0xe2,  //  EfiKeyLAlt
+  0x2c,  //  EfiKeySpaceBar
+  0xe6,  //  EfiKeyA2
+  0xe7,  //  EfiKeyA3
+  0x65,  //  EfiKeyA4
+  0xe4,  //  EfiKeyRCtrl
+  0x50,  //  EfiKeyLeftArrow
+  0x51,  //  EfiKeyDownArrow
+  0x4F,  //  EfiKeyRightArrow
+  0x62,  //  EfiKeyZero
+  0x63,  //  EfiKeyPeriod
+  0x28,  //  EfiKeyEnter
+  0xe1,  //  EfiKeyLShift
+  0x64,  //  EfiKeyB0
+  0x1D,  //  EfiKeyB1
+  0x1B,  //  EfiKeyB2
+  0x06,  //  EfiKeyB3
+  0x19,  //  EfiKeyB4
+  0x05,  //  EfiKeyB5
+  0x11,  //  EfiKeyB6
+  0x10,  //  EfiKeyB7
+  0x36,  //  EfiKeyB8
+  0x37,  //  EfiKeyB9
+  0x38,  //  EfiKeyB10
+  0xe5,  //  EfiKeyRShift
+  0x52,  //  EfiKeyUpArrow
+  0x59,  //  EfiKeyOne
+  0x5A,  //  EfiKeyTwo
+  0x5B,  //  EfiKeyThree
+  0x39,  //  EfiKeyCapsLock
+  0x04,  //  EfiKeyC1
+  0x16,  //  EfiKeyC2
+  0x07,  //  EfiKeyC3
+  0x09,  //  EfiKeyC4
+  0x0A,  //  EfiKeyC5
+  0x0B,  //  EfiKeyC6
+  0x0D,  //  EfiKeyC7
+  0x0E,  //  EfiKeyC8
+  0x0F,  //  EfiKeyC9
+  0x33,  //  EfiKeyC10
+  0x34,  //  EfiKeyC11
+  0x32,  //  EfiKeyC12
+  0x5C,  //  EfiKeyFour
+  0x5D,  //  EfiKeyFive
+  0x5E,  //  EfiKeySix
+  0x57,  //  EfiKeyPlus
+  0x2B,  //  EfiKeyTab
+  0x14,  //  EfiKeyD1
+  0x1A,  //  EfiKeyD2
+  0x08,  //  EfiKeyD3
+  0x15,  //  EfiKeyD4
+  0x17,  //  EfiKeyD5
+  0x1C,  //  EfiKeyD6
+  0x18,  //  EfiKeyD7
+  0x0C,  //  EfiKeyD8
+  0x12,  //  EfiKeyD9
+  0x13,  //  EfiKeyD10
+  0x2F,  //  EfiKeyD11
+  0x30,  //  EfiKeyD12
+  0x31,  //  EfiKeyD13
+  0x4C,  //  EfiKeyDel
+  0x4D,  //  EfiKeyEnd
+  0x4E,  //  EfiKeyPgDn
+  0x5F,  //  EfiKeySeven
+  0x60,  //  EfiKeyEight
+  0x61,  //  EfiKeyNine
+  0x35,  //  EfiKeyE0
+  0x1E,  //  EfiKeyE1
+  0x1F,  //  EfiKeyE2
+  0x20,  //  EfiKeyE3
+  0x21,  //  EfiKeyE4
+  0x22,  //  EfiKeyE5
+  0x23,  //  EfiKeyE6
+  0x24,  //  EfiKeyE7
+  0x25,  //  EfiKeyE8
+  0x26,  //  EfiKeyE9
+  0x27,  //  EfiKeyE10
+  0x2D,  //  EfiKeyE11
+  0x2E,  //  EfiKeyE12
+  0x2A,  //  EfiKeyBackSpace
+  0x49,  //  EfiKeyIns
+  0x4A,  //  EfiKeyHome
+  0x4B,  //  EfiKeyPgUp
+  0x53,  //  EfiKeyNLck
+  0x54,  //  EfiKeySlash
+  0x55,  //  EfiKeyAsterisk
+  0x56,  //  EfiKeyMinus
+  0x29,  //  EfiKeyEsc
+  0x3A,  //  EfiKeyF1
+  0x3B,  //  EfiKeyF2
+  0x3C,  //  EfiKeyF3
+  0x3D,  //  EfiKeyF4
+  0x3E,  //  EfiKeyF5
+  0x3F,  //  EfiKeyF6
+  0x40,  //  EfiKeyF7
+  0x41,  //  EfiKeyF8
+  0x42,  //  EfiKeyF9
+  0x43,  //  EfiKeyF10
+  0x44,  //  EfiKeyF11
+  0x45,  //  EfiKeyF12
+  0x46,  //  EfiKeyPrint
+  0x47,  //  EfiKeySLck
+  0x48   //  EfiKeyPause
+};
+
+//
+// Keyboard modifier value to EFI Scan Code convertion table
+// EFI Scan Code and the modifier values are defined in UEFI spec.
+//
+UINT8 ModifierValueToEfiScanCodeConvertionTable[] = {
+  SCAN_NULL,       // EFI_NULL_MODIFIER
+  SCAN_NULL,       // EFI_LEFT_CONTROL_MODIFIER
+  SCAN_NULL,       // EFI_RIGHT_CONTROL_MODIFIER
+  SCAN_NULL,       // EFI_LEFT_ALT_MODIFIER
+  SCAN_NULL,       // EFI_RIGHT_ALT_MODIFIER
+  SCAN_NULL,       // EFI_ALT_GR_MODIFIER
+  SCAN_INSERT,     // EFI_INSERT_MODIFIER
+  SCAN_DELETE,     // EFI_DELETE_MODIFIER
+  SCAN_PAGE_DOWN,  // EFI_PAGE_DOWN_MODIFIER
+  SCAN_PAGE_UP,    // EFI_PAGE_UP_MODIFIER
+  SCAN_HOME,       // EFI_HOME_MODIFIER
+  SCAN_END,        // EFI_END_MODIFIER
+  SCAN_NULL,       // EFI_LEFT_SHIFT_MODIFIER
+  SCAN_NULL,       // EFI_RIGHT_SHIFT_MODIFIER
+  SCAN_NULL,       // EFI_CAPS_LOCK_MODIFIER
+  SCAN_NULL,       // EFI_NUM_LOCK_MODIFIER
+  SCAN_LEFT,       // EFI_LEFT_ARROW_MODIFIER
+  SCAN_RIGHT,      // EFI_RIGHT_ARROW_MODIFIER
+  SCAN_DOWN,       // EFI_DOWN_ARROW_MODIFIER
+  SCAN_UP,         // EFI_UP_ARROW_MODIFIER
+  SCAN_NULL,       // EFI_NS_KEY_MODIFIER
+  SCAN_NULL,       // EFI_NS_KEY_DEPENDENCY_MODIFIER
+  SCAN_F1,         // EFI_FUNCTION_KEY_ONE_MODIFIER
+  SCAN_F2,         // EFI_FUNCTION_KEY_TWO_MODIFIER
+  SCAN_F3,         // EFI_FUNCTION_KEY_THREE_MODIFIER
+  SCAN_F4,         // EFI_FUNCTION_KEY_FOUR_MODIFIER
+  SCAN_F5,         // EFI_FUNCTION_KEY_FIVE_MODIFIER
+  SCAN_F6,         // EFI_FUNCTION_KEY_SIX_MODIFIER
+  SCAN_F7,         // EFI_FUNCTION_KEY_SEVEN_MODIFIER
+  SCAN_F8,         // EFI_FUNCTION_KEY_EIGHT_MODIFIER
+  SCAN_F9,         // EFI_FUNCTION_KEY_NINE_MODIFIER
+  SCAN_F10,        // EFI_FUNCTION_KEY_TEN_MODIFIER
+  SCAN_F11,        // EFI_FUNCTION_KEY_ELEVEN_MODIFIER
+  SCAN_F12,        // EFI_FUNCTION_KEY_TWELVE_MODIFIER
+  //
+  // For Partial Keystroke support
+  //
+  SCAN_NULL,       // EFI_PRINT_MODIFIER
+  SCAN_NULL,       // EFI_SYS_REQUEST_MODIFIER
+  SCAN_NULL,       // EFI_SCROLL_LOCK_MODIFIER
+  SCAN_PAUSE,      // EFI_PAUSE_MODIFIER
+  SCAN_NULL,       // EFI_BREAK_MODIFIER
+  SCAN_NULL,       // EFI_LEFT_LOGO_MODIFIER
+  SCAN_NULL,       // EFI_RIGHT_LOGO_MODIFER
+  SCAN_NULL,       // EFI_MENU_MODIFER
+};
+
+/**
+  Initialize Key Convention Table by using default keyboard layout.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS          The default keyboard layout was installed successfully
+  @retval Others               Failure to install default keyboard layout.
+**/
+EFI_STATUS
+InstallDefaultKeyboardLayout (
+   IN OUT USB_KB_DEV           *UsbKeyboardDevice
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_HII_DATABASE_PROTOCOL    *HiiDatabase;
+  EFI_HII_HANDLE               HiiHandle;
+
+  //
+  // Locate Hii database protocol
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiDatabaseProtocolGuid,
+                  NULL,
+                  (VOID **) &HiiDatabase
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Install Keyboard Layout package to HII database
+  //
+  HiiHandle = HiiAddPackages (
+                &gUsbKeyboardLayoutPackageGuid,
+                UsbKeyboardDevice->ControllerHandle,
+                &mUsbKeyboardLayoutBin,
+                NULL
+                );
+  if (HiiHandle == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Set current keyboard layout
+  //
+  Status = HiiDatabase->SetKeyboardLayout (HiiDatabase, &gUsbKeyboardLayoutKeyGuid);
+
+  return Status;
+}
+
+
+/**
+  Uses USB I/O to check whether the device is a USB keyboard device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB keyboard device.
+  @retval FALSE    Device is a not USB keyboard device.
+
+**/
+BOOLEAN
+IsUSBKeyboard (
+  IN  EFI_USB_IO_PROTOCOL       *UsbIo
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+
+  //
+  // Get the default interface descriptor
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (
+                    UsbIo,
+                    &InterfaceDescriptor
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if (InterfaceDescriptor.InterfaceClass == CLASS_HID &&
+      InterfaceDescriptor.InterfaceSubClass == SUBCLASS_BOOT &&
+      InterfaceDescriptor.InterfaceProtocol == PROTOCOL_KEYBOARD
+      ) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Get current keyboard layout from HII database.
+
+  @return Pointer to HII Keyboard Layout.
+          NULL means failure occurred while trying to get keyboard layout.
+
+**/
+EFI_HII_KEYBOARD_LAYOUT *
+GetCurrentKeyboardLayout (
+  VOID
+  )
+{
+  EFI_STATUS                Status;
+  EFI_HII_DATABASE_PROTOCOL *HiiDatabase;
+  EFI_HII_KEYBOARD_LAYOUT   *KeyboardLayout;
+  UINT16                    Length;
+
+  //
+  // Locate HII Database Protocol
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiDatabaseProtocolGuid,
+                  NULL,
+                  (VOID **) &HiiDatabase
+                  );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  //
+  // Get current keyboard layout from HII database
+  //
+  Length = 0;
+  KeyboardLayout = NULL;
+  Status = HiiDatabase->GetKeyboardLayout (
+                          HiiDatabase,
+                          NULL,
+                          &Length,
+                          KeyboardLayout
+                          );
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    KeyboardLayout = AllocatePool (Length);
+    ASSERT (KeyboardLayout != NULL);
+
+    Status = HiiDatabase->GetKeyboardLayout (
+                            HiiDatabase,
+                            NULL,
+                            &Length,
+                            KeyboardLayout
+                            );
+    if (EFI_ERROR (Status)) {
+      FreePool (KeyboardLayout);
+      KeyboardLayout = NULL;
+    }
+  }
+
+  return KeyboardLayout;
+}
+
+/**
+  Find Key Descriptor in Key Convertion Table given its USB keycode.
+
+  @param  UsbKeyboardDevice   The USB_KB_DEV instance.
+  @param  KeyCode             USB Keycode.
+
+  @return The Key Descriptor in Key Convertion Table.
+          NULL means not found.
+
+**/
+EFI_KEY_DESCRIPTOR *
+GetKeyDescriptor (
+  IN USB_KB_DEV        *UsbKeyboardDevice,
+  IN UINT8             KeyCode
+  )
+{
+  UINT8  Index;
+
+  //
+  // Make sure KeyCode is in the range of [0x4, 0x65] or [0xe0, 0xe7]
+  //
+  if ((!USBKBD_VALID_KEYCODE (KeyCode)) || ((KeyCode > 0x65) && (KeyCode < 0xe0)) || (KeyCode > 0xe7)) {
+    return NULL;
+  }
+
+  //
+  // Calculate the index of Key Descriptor in Key Convertion Table
+  //
+  if (KeyCode <= 0x65) {
+    Index = (UINT8) (KeyCode - 4);
+  } else {
+    Index = (UINT8) (KeyCode - 0xe0 + NUMBER_OF_VALID_NON_MODIFIER_USB_KEYCODE);
+  }
+
+  return &UsbKeyboardDevice->KeyConvertionTable[Index];
+}
+
+/**
+  Find Non-Spacing key for given Key descriptor.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+  @param  KeyDescriptor        Key descriptor.
+
+  @return The Non-Spacing key corresponding to KeyDescriptor
+          NULL means not found.
+
+**/
+USB_NS_KEY *
+FindUsbNsKey (
+  IN USB_KB_DEV          *UsbKeyboardDevice,
+  IN EFI_KEY_DESCRIPTOR  *KeyDescriptor
+  )
+{
+  LIST_ENTRY      *Link;
+  LIST_ENTRY      *NsKeyList;
+  USB_NS_KEY      *UsbNsKey;
+
+  NsKeyList = &UsbKeyboardDevice->NsKeyList;
+  Link = GetFirstNode (NsKeyList);
+  while (!IsNull (NsKeyList, Link)) {
+    UsbNsKey = USB_NS_KEY_FORM_FROM_LINK (Link);
+
+    if (UsbNsKey->NsKey[0].Key == KeyDescriptor->Key) {
+      return UsbNsKey;
+    }
+
+    Link = GetNextNode (NsKeyList, Link);
+  }
+
+  return NULL;
+}
+
+/**
+  Find physical key definition for a given key descriptor.
+
+  For a specified non-spacing key, there are a list of physical
+  keys following it. This function traverses the list of
+  physical keys and tries to find the physical key matching
+  the KeyDescriptor.
+
+  @param  UsbNsKey          The non-spacing key information.
+  @param  KeyDescriptor     The key descriptor.
+
+  @return The physical key definition.
+          If no physical key is found, parameter KeyDescriptor is returned.
+
+**/
+EFI_KEY_DESCRIPTOR *
+FindPhysicalKey (
+  IN USB_NS_KEY          *UsbNsKey,
+  IN EFI_KEY_DESCRIPTOR  *KeyDescriptor
+  )
+{
+  UINTN               Index;
+  EFI_KEY_DESCRIPTOR  *PhysicalKey;
+
+  PhysicalKey = &UsbNsKey->NsKey[1];
+  for (Index = 0; Index < UsbNsKey->KeyCount; Index++) {
+    if (KeyDescriptor->Key == PhysicalKey->Key) {
+      return PhysicalKey;
+    }
+
+    PhysicalKey++;
+  }
+
+  //
+  // No children definition matched, return original key
+  //
+  return KeyDescriptor;
+}
+
+/**
+  The notification function for EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID.
+
+  This function is registered to event of EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID
+  group type, which will be triggered by EFI_HII_DATABASE_PROTOCOL.SetKeyboardLayout().
+  It tries to get curent keyboard layout from HII database.
+
+  @param  Event        Event being signaled.
+  @param  Context      Points to USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+SetKeyboardLayoutEvent (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  USB_KB_DEV                *UsbKeyboardDevice;
+  EFI_HII_KEYBOARD_LAYOUT   *KeyboardLayout;
+  EFI_KEY_DESCRIPTOR        TempKey;
+  EFI_KEY_DESCRIPTOR        *KeyDescriptor;
+  EFI_KEY_DESCRIPTOR        *TableEntry;
+  EFI_KEY_DESCRIPTOR        *NsKey;
+  USB_NS_KEY                *UsbNsKey;
+  UINTN                     Index;
+  UINTN                     Index2;
+  UINTN                     KeyCount;
+  UINT8                     KeyCode;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+  if (UsbKeyboardDevice->Signature != USB_KB_DEV_SIGNATURE) {
+    return;
+  }
+
+  //
+  // Try to get current keyboard layout from HII database
+  //
+  KeyboardLayout = GetCurrentKeyboardLayout ();
+  if (KeyboardLayout == NULL) {
+    return;
+  }
+
+  //
+  // Re-allocate resource for KeyConvertionTable
+  //
+  ReleaseKeyboardLayoutResources (UsbKeyboardDevice);
+  UsbKeyboardDevice->KeyConvertionTable = AllocateZeroPool ((NUMBER_OF_VALID_USB_KEYCODE) * sizeof (EFI_KEY_DESCRIPTOR));
+  ASSERT (UsbKeyboardDevice->KeyConvertionTable != NULL);
+
+  //
+  // Traverse the list of key descriptors following the header of EFI_HII_KEYBOARD_LAYOUT
+  //
+  KeyDescriptor = (EFI_KEY_DESCRIPTOR *) (((UINT8 *) KeyboardLayout) + sizeof (EFI_HII_KEYBOARD_LAYOUT));
+  for (Index = 0; Index < KeyboardLayout->DescriptorCount; Index++) {
+    //
+    // Copy from HII keyboard layout package binary for alignment
+    //
+    CopyMem (&TempKey, KeyDescriptor, sizeof (EFI_KEY_DESCRIPTOR));
+
+    //
+    // Fill the key into KeyConvertionTable, whose index is calculated from USB keycode.
+    //
+    KeyCode = EfiKeyToUsbKeyCodeConvertionTable [(UINT8) (TempKey.Key)];
+    TableEntry = GetKeyDescriptor (UsbKeyboardDevice, KeyCode);
+    if (TableEntry == NULL) {
+      ReleaseKeyboardLayoutResources (UsbKeyboardDevice);
+      FreePool (KeyboardLayout);
+      return;
+    }
+    CopyMem (TableEntry, KeyDescriptor, sizeof (EFI_KEY_DESCRIPTOR));
+
+    //
+    // For non-spacing key, create the list with a non-spacing key followed by physical keys.
+    //
+    if (TempKey.Modifier == EFI_NS_KEY_MODIFIER) {
+      UsbNsKey = AllocateZeroPool (sizeof (USB_NS_KEY));
+      ASSERT (UsbNsKey != NULL);
+
+      //
+      // Search for sequential children physical key definitions
+      //
+      KeyCount = 0;
+      NsKey = KeyDescriptor + 1;
+      for (Index2 = (UINT8) Index + 1; Index2 < KeyboardLayout->DescriptorCount; Index2++) {
+        CopyMem (&TempKey, NsKey, sizeof (EFI_KEY_DESCRIPTOR));
+        if (TempKey.Modifier == EFI_NS_KEY_DEPENDENCY_MODIFIER) {
+          KeyCount++;
+        } else {
+          break;
+        }
+        NsKey++;
+      }
+
+      UsbNsKey->Signature = USB_NS_KEY_SIGNATURE;
+      UsbNsKey->KeyCount = KeyCount;
+      UsbNsKey->NsKey = AllocateCopyPool (
+                          (KeyCount + 1) * sizeof (EFI_KEY_DESCRIPTOR),
+                          KeyDescriptor
+                          );
+      InsertTailList (&UsbKeyboardDevice->NsKeyList, &UsbNsKey->Link);
+
+      //
+      // Skip over the child physical keys
+      //
+      Index += KeyCount;
+      KeyDescriptor += KeyCount;
+    }
+
+    KeyDescriptor++;
+  }
+
+  //
+  // There are two EfiKeyEnter, duplicate its key descriptor
+  //
+  TableEntry = GetKeyDescriptor (UsbKeyboardDevice, 0x58);
+  KeyDescriptor = GetKeyDescriptor (UsbKeyboardDevice, 0x28);
+  CopyMem (TableEntry, KeyDescriptor, sizeof (EFI_KEY_DESCRIPTOR));
+
+  FreePool (KeyboardLayout);
+}
+
+/**
+  Destroy resources for keyboard layout.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+
+**/
+VOID
+ReleaseKeyboardLayoutResources (
+  IN OUT USB_KB_DEV              *UsbKeyboardDevice
+  )
+{
+  USB_NS_KEY      *UsbNsKey;
+  LIST_ENTRY      *Link;
+
+  if (UsbKeyboardDevice->KeyConvertionTable != NULL) {
+    FreePool (UsbKeyboardDevice->KeyConvertionTable);
+  }
+  UsbKeyboardDevice->KeyConvertionTable = NULL;
+
+  while (!IsListEmpty (&UsbKeyboardDevice->NsKeyList)) {
+    Link = GetFirstNode (&UsbKeyboardDevice->NsKeyList);
+    UsbNsKey = USB_NS_KEY_FORM_FROM_LINK (Link);
+    RemoveEntryList (&UsbNsKey->Link);
+
+    FreePool (UsbNsKey->NsKey);
+    FreePool (UsbNsKey);
+  }
+}
+
+/**
+  Initialize USB keyboard layout.
+
+  This function initializes Key Convertion Table for the USB keyboard device.
+  It first tries to retrieve layout from HII database. If failed and default
+  layout is enabled, then it just uses the default layout.
+
+  @param  UsbKeyboardDevice      The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS            Initialization succeeded.
+  @retval EFI_NOT_READY          Keyboard layout cannot be retrieve from HII
+                                 database, and default layout is disabled.
+  @retval Other                  Fail to register event to EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group.
+
+**/
+EFI_STATUS
+InitKeyboardLayout (
+  OUT USB_KB_DEV   *UsbKeyboardDevice
+  )
+{
+  EFI_HII_KEYBOARD_LAYOUT   *KeyboardLayout;
+  EFI_STATUS                Status;
+
+  UsbKeyboardDevice->KeyConvertionTable = AllocateZeroPool ((NUMBER_OF_VALID_USB_KEYCODE) * sizeof (EFI_KEY_DESCRIPTOR));
+  ASSERT (UsbKeyboardDevice->KeyConvertionTable != NULL);
+
+  InitializeListHead (&UsbKeyboardDevice->NsKeyList);
+  UsbKeyboardDevice->CurrentNsKey = NULL;
+  UsbKeyboardDevice->KeyboardLayoutEvent = NULL;
+
+  //
+  // Register event to EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group,
+  // which will be triggered by EFI_HII_DATABASE_PROTOCOL.SetKeyboardLayout().
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  SetKeyboardLayoutEvent,
+                  UsbKeyboardDevice,
+                  &gEfiHiiKeyBoardLayoutGuid,
+                  &UsbKeyboardDevice->KeyboardLayoutEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  KeyboardLayout = GetCurrentKeyboardLayout ();
+  if (KeyboardLayout != NULL) {
+    //
+    // If current keyboard layout is successfully retrieved from HII database,
+    // force to initialize the keyboard layout.
+    //
+    gBS->SignalEvent (UsbKeyboardDevice->KeyboardLayoutEvent);
+  } else {
+    if (FeaturePcdGet (PcdDisableDefaultKeyboardLayoutInUsbKbDriver)) {
+      //
+      // If no keyboard layout can be retrieved from HII database, and default layout
+      // is disabled, then return EFI_NOT_READY.
+      //
+      return EFI_NOT_READY;
+    }
+    //
+    // If no keyboard layout can be retrieved from HII database, and default layout
+    // is enabled, then load the default keyboard layout.
+    //
+    InstallDefaultKeyboardLayout (UsbKeyboardDevice);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialize USB keyboard device and all private data structures.
+
+  @param  UsbKeyboardDevice  The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS        Initialization is successful.
+  @retval EFI_DEVICE_ERROR   Keyboard initialization failed.
+
+**/
+EFI_STATUS
+InitUSBKeyboard (
+  IN OUT USB_KB_DEV   *UsbKeyboardDevice
+  )
+{
+  UINT16              ConfigValue;
+  UINT8               Protocol;
+  EFI_STATUS          Status;
+  UINT32              TransferResult;
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_KEYBOARD_PC_SELF_TEST),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  InitQueue (&UsbKeyboardDevice->UsbKeyQueue, sizeof (USB_KEY));
+  InitQueue (&UsbKeyboardDevice->EfiKeyQueue, sizeof (EFI_KEY_DATA));
+
+  //
+  // Use the config out of the descriptor
+  // Assumed the first config is the correct one and this is not always the case
+  //
+  Status = UsbGetConfiguration (
+             UsbKeyboardDevice->UsbIo,
+             &ConfigValue,
+             &TransferResult
+             );
+  if (EFI_ERROR (Status)) {
+    ConfigValue = 0x01;
+    //
+    // Uses default configuration to configure the USB Keyboard device.
+    //
+    Status = UsbSetConfiguration (
+               UsbKeyboardDevice->UsbIo,
+               ConfigValue,
+               &TransferResult
+               );
+    if (EFI_ERROR (Status)) {
+      //
+      // If configuration could not be set here, it means
+      // the keyboard interface has some errors and could
+      // not be initialized
+      //
+      REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+        EFI_ERROR_CODE | EFI_ERROR_MINOR,
+        (EFI_PERIPHERAL_KEYBOARD | EFI_P_EC_INTERFACE_ERROR),
+        UsbKeyboardDevice->DevicePath
+        );
+
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  UsbGetProtocolRequest (
+    UsbKeyboardDevice->UsbIo,
+    UsbKeyboardDevice->InterfaceDescriptor.InterfaceNumber,
+    &Protocol
+    );
+  //
+  // Set boot protocol for the USB Keyboard.
+  // This driver only supports boot protocol.
+  //
+  if (Protocol != BOOT_PROTOCOL) {
+    UsbSetProtocolRequest (
+      UsbKeyboardDevice->UsbIo,
+      UsbKeyboardDevice->InterfaceDescriptor.InterfaceNumber,
+      BOOT_PROTOCOL
+      );
+  }
+
+  UsbKeyboardDevice->CtrlOn     = FALSE;
+  UsbKeyboardDevice->AltOn      = FALSE;
+  UsbKeyboardDevice->ShiftOn    = FALSE;
+  UsbKeyboardDevice->NumLockOn  = FALSE;
+  UsbKeyboardDevice->CapsOn     = FALSE;
+  UsbKeyboardDevice->ScrollOn   = FALSE;
+
+  UsbKeyboardDevice->LeftCtrlOn   = FALSE;
+  UsbKeyboardDevice->LeftAltOn    = FALSE;
+  UsbKeyboardDevice->LeftShiftOn  = FALSE;
+  UsbKeyboardDevice->LeftLogoOn   = FALSE;
+  UsbKeyboardDevice->RightCtrlOn  = FALSE;
+  UsbKeyboardDevice->RightAltOn   = FALSE;
+  UsbKeyboardDevice->RightShiftOn = FALSE;
+  UsbKeyboardDevice->RightLogoOn  = FALSE;
+  UsbKeyboardDevice->MenuKeyOn    = FALSE;
+  UsbKeyboardDevice->SysReqOn     = FALSE;
+
+  UsbKeyboardDevice->AltGrOn      = FALSE;
+
+  UsbKeyboardDevice->CurrentNsKey = NULL;
+
+  //
+  // Sync the initial state of lights on keyboard.
+  //
+  SetKeyLED (UsbKeyboardDevice);
+
+  ZeroMem (UsbKeyboardDevice->LastKeyCodeArray, sizeof (UINT8) * 8);
+
+  //
+  // Create event for repeat keys' generation.
+  //
+  if (UsbKeyboardDevice->RepeatTimer != NULL) {
+    gBS->CloseEvent (UsbKeyboardDevice->RepeatTimer);
+    UsbKeyboardDevice->RepeatTimer = NULL;
+  }
+
+  gBS->CreateEvent (
+         EVT_TIMER | EVT_NOTIFY_SIGNAL,
+         TPL_CALLBACK,
+         USBKeyboardRepeatHandler,
+         UsbKeyboardDevice,
+         &UsbKeyboardDevice->RepeatTimer
+         );
+
+  //
+  // Create event for delayed recovery, which deals with device error.
+  //
+  if (UsbKeyboardDevice->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbKeyboardDevice->DelayedRecoveryEvent);
+    UsbKeyboardDevice->DelayedRecoveryEvent = NULL;
+  }
+
+  gBS->CreateEvent (
+         EVT_TIMER | EVT_NOTIFY_SIGNAL,
+         TPL_NOTIFY,
+         USBKeyboardRecoveryHandler,
+         UsbKeyboardDevice,
+         &UsbKeyboardDevice->DelayedRecoveryEvent
+         );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Handler function for USB keyboard's asynchronous interrupt transfer.
+
+  This function is the handler function for USB keyboard's asynchronous interrupt transfer
+  to manage the keyboard. It parses the USB keyboard input report, and inserts data to
+  keyboard buffer according to state of modifer keys and normal keys. Timer for repeat key
+  is also set accordingly.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+KeyboardHandler (
+  IN  VOID          *Data,
+  IN  UINTN         DataLength,
+  IN  VOID          *Context,
+  IN  UINT32        Result
+  )
+{
+  USB_KB_DEV          *UsbKeyboardDevice;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+  UINT8               *CurKeyCodeBuffer;
+  UINT8               *OldKeyCodeBuffer;
+  UINT8               CurModifierMap;
+  UINT8               OldModifierMap;
+  UINT8               Mask;
+  UINTN               Index;
+  UINT8               Index2;
+  BOOLEAN             KeyRelease;
+  BOOLEAN             KeyPress;
+  USB_KEY             UsbKey;
+  UINT8               NewRepeatKey;
+  UINT32              UsbStatus;
+  EFI_KEY_DESCRIPTOR  *KeyDescriptor;
+
+  ASSERT (Context != NULL);
+
+  NewRepeatKey      = 0;
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+  UsbIo             = UsbKeyboardDevice->UsbIo;
+
+  //
+  // Analyzes Result and performs corresponding action.
+  //
+  if (Result != EFI_USB_NOERROR) {
+    //
+    // Some errors happen during the process
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_KEYBOARD | EFI_P_EC_INPUT_ERROR),
+      UsbKeyboardDevice->DevicePath
+      );
+
+    //
+    // Stop the repeat key generation if any
+    //
+    UsbKeyboardDevice->RepeatKey = 0;
+
+    gBS->SetTimer (
+          UsbKeyboardDevice->RepeatTimer,
+          TimerCancel,
+          USBKBD_REPEAT_RATE
+          );
+
+    if ((Result & EFI_USB_ERR_STALL) == EFI_USB_ERR_STALL) {
+      UsbClearEndpointHalt (
+        UsbIo,
+        UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress,
+        &UsbStatus
+        );
+    }
+
+    //
+    // Delete & Submit this interrupt again
+    // Handler of DelayedRecoveryEvent triggered by timer will re-submit the interrupt.
+    //
+    UsbIo->UsbAsyncInterruptTransfer (
+             UsbIo,
+             UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress,
+             FALSE,
+             0,
+             0,
+             NULL,
+             NULL
+             );
+    //
+    // EFI_USB_INTERRUPT_DELAY is defined in USB standard for error handling.
+    //
+    gBS->SetTimer (
+           UsbKeyboardDevice->DelayedRecoveryEvent,
+           TimerRelative,
+           EFI_USB_INTERRUPT_DELAY
+           );
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // If no error and no data, just return EFI_SUCCESS.
+  //
+  if (DataLength == 0 || Data == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Following code checks current keyboard input report against old key code buffer.
+  // According to USB HID Firmware Specification, the report consists of 8 bytes.
+  // Byte 0 is map of Modifier keys.
+  // Byte 1 is reserved.
+  // Bytes 2 to 7 are keycodes.
+  //
+  CurKeyCodeBuffer  = (UINT8 *) Data;
+  OldKeyCodeBuffer  = UsbKeyboardDevice->LastKeyCodeArray;
+
+  //
+  // Checks for new key stroke.
+  //
+  for (Index = 0; Index < 8; Index++) {
+    if (OldKeyCodeBuffer[Index] != CurKeyCodeBuffer[Index]) {
+      break;
+    }
+  }
+
+  //
+  // If no new key, return EFI_SUCCESS immediately.
+  //
+  if (Index == 8) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Parse the modifier key, which is the first byte of keyboard input report.
+  //
+  CurModifierMap  = CurKeyCodeBuffer[0];
+  OldModifierMap  = OldKeyCodeBuffer[0];
+
+  //
+  // Handle modifier key's pressing or releasing situation.
+  // According to USB HID Firmware spec, Byte 0 uses folloing map of Modifier keys:
+  // Bit0: Left Control,  Keycode: 0xe0
+  // Bit1: Left Shift,    Keycode: 0xe1
+  // Bit2: Left Alt,      Keycode: 0xe2
+  // Bit3: Left GUI,      Keycode: 0xe3
+  // Bit4: Right Control, Keycode: 0xe4
+  // Bit5: Right Shift,   Keycode: 0xe5
+  // Bit6: Right Alt,     Keycode: 0xe6
+  // Bit7: Right GUI,     Keycode: 0xe7
+  //
+  for (Index = 0; Index < 8; Index++) {
+    Mask = (UINT8) (1 << Index);
+    if ((CurModifierMap & Mask) != (OldModifierMap & Mask)) {
+      //
+      // If current modifier key is up, then CurModifierMap & Mask = 0;
+      // otherwise it is a non-zero value.
+      // Insert the changed modifier key into key buffer.
+      //
+      UsbKey.KeyCode = (UINT8) (0xe0 + Index);
+      UsbKey.Down    = (BOOLEAN) ((CurModifierMap & Mask) != 0);
+      Enqueue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+    }
+  }
+
+  //
+  // Handle normal key's releasing situation
+  // Bytes 2 to 7 are for normal keycodes
+  //
+  KeyRelease = FALSE;
+  for (Index = 2; Index < 8; Index++) {
+
+    if (!USBKBD_VALID_KEYCODE (OldKeyCodeBuffer[Index])) {
+      continue;
+    }
+    //
+    // For any key in old keycode buffer, if it is not in current keycode buffer,
+    // then it is released. Otherwise, it is not released.
+    //
+    KeyRelease = TRUE;
+    for (Index2 = 2; Index2 < 8; Index2++) {
+
+      if (!USBKBD_VALID_KEYCODE (CurKeyCodeBuffer[Index2])) {
+        continue;
+      }
+
+      if (OldKeyCodeBuffer[Index] == CurKeyCodeBuffer[Index2]) {
+        KeyRelease = FALSE;
+        break;
+      }
+    }
+
+    if (KeyRelease) {
+      UsbKey.KeyCode = OldKeyCodeBuffer[Index];
+      UsbKey.Down    = FALSE;
+      Enqueue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+      //
+      // The original repeat key is released.
+      //
+      if (OldKeyCodeBuffer[Index] == UsbKeyboardDevice->RepeatKey) {
+        UsbKeyboardDevice->RepeatKey = 0;
+      }
+    }
+  }
+
+  //
+  // If original repeat key is released, cancel the repeat timer
+  //
+  if (UsbKeyboardDevice->RepeatKey == 0) {
+    gBS->SetTimer (
+           UsbKeyboardDevice->RepeatTimer,
+           TimerCancel,
+           USBKBD_REPEAT_RATE
+           );
+  }
+
+  //
+  // Handle normal key's pressing situation
+  //
+  KeyPress = FALSE;
+  for (Index = 2; Index < 8; Index++) {
+
+    if (!USBKBD_VALID_KEYCODE (CurKeyCodeBuffer[Index])) {
+      continue;
+    }
+    //
+    // For any key in current keycode buffer, if it is not in old keycode buffer,
+    // then it is pressed. Otherwise, it is not pressed.
+    //
+    KeyPress = TRUE;
+    for (Index2 = 2; Index2 < 8; Index2++) {
+
+      if (!USBKBD_VALID_KEYCODE (OldKeyCodeBuffer[Index2])) {
+        continue;
+      }
+
+      if (CurKeyCodeBuffer[Index] == OldKeyCodeBuffer[Index2]) {
+        KeyPress = FALSE;
+        break;
+      }
+    }
+
+    if (KeyPress) {
+      UsbKey.KeyCode = CurKeyCodeBuffer[Index];
+      UsbKey.Down    = TRUE;
+      Enqueue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+
+      //
+      // Handle repeat key
+      //
+      KeyDescriptor = GetKeyDescriptor (UsbKeyboardDevice, CurKeyCodeBuffer[Index]);
+      ASSERT (KeyDescriptor != NULL);
+
+      if (KeyDescriptor->Modifier == EFI_NUM_LOCK_MODIFIER || KeyDescriptor->Modifier == EFI_CAPS_LOCK_MODIFIER) {
+        //
+        // For NumLock or CapsLock pressed, there is no need to handle repeat key for them.
+        //
+        UsbKeyboardDevice->RepeatKey = 0;
+      } else {
+        //
+        // Prepare new repeat key, and clear the original one.
+        //
+        NewRepeatKey = CurKeyCodeBuffer[Index];
+        UsbKeyboardDevice->RepeatKey = 0;
+      }
+    }
+  }
+
+  //
+  // Update LastKeycodeArray buffer in the UsbKeyboardDevice data structure.
+  //
+  for (Index = 0; Index < 8; Index++) {
+    UsbKeyboardDevice->LastKeyCodeArray[Index] = CurKeyCodeBuffer[Index];
+  }
+
+  //
+  // If there is new key pressed, update the RepeatKey value, and set the
+  // timer to repeate delay timer
+  //
+  if (NewRepeatKey != 0) {
+    //
+    // Sets trigger time to "Repeat Delay Time",
+    // to trigger the repeat timer when the key is hold long
+    // enough time.
+    //
+    gBS->SetTimer (
+           UsbKeyboardDevice->RepeatTimer,
+           TimerRelative,
+           USBKBD_REPEAT_DELAY
+           );
+    UsbKeyboardDevice->RepeatKey = NewRepeatKey;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieves a USB keycode after parsing the raw data in keyboard buffer.
+
+  This function parses keyboard buffer. It updates state of modifier key for
+  USB_KB_DEV instancem, and returns keycode for output.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+  @param  KeyCode              Pointer to the USB keycode for output.
+
+  @retval EFI_SUCCESS          Keycode successfully parsed.
+  @retval EFI_NOT_READY        Keyboard buffer is not ready for a valid keycode
+
+**/
+EFI_STATUS
+USBParseKey (
+  IN OUT  USB_KB_DEV  *UsbKeyboardDevice,
+     OUT  UINT8       *KeyCode
+  )
+{
+  USB_KEY             UsbKey;
+  EFI_KEY_DESCRIPTOR  *KeyDescriptor;
+
+  *KeyCode = 0;
+
+  while (!IsQueueEmpty (&UsbKeyboardDevice->UsbKeyQueue)) {
+    //
+    // Pops one raw data off.
+    //
+    Dequeue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+
+    KeyDescriptor = GetKeyDescriptor (UsbKeyboardDevice, UsbKey.KeyCode);
+    ASSERT (KeyDescriptor != NULL);
+
+    if (!UsbKey.Down) {
+      //
+      // Key is released.
+      //
+      switch (KeyDescriptor->Modifier) {
+
+      //
+      // Ctrl release
+      //
+      case EFI_LEFT_CONTROL_MODIFIER:
+        UsbKeyboardDevice->LeftCtrlOn = FALSE;
+        UsbKeyboardDevice->CtrlOn = FALSE;
+        break;
+      case EFI_RIGHT_CONTROL_MODIFIER:
+        UsbKeyboardDevice->RightCtrlOn = FALSE;
+        UsbKeyboardDevice->CtrlOn = FALSE;
+        break;
+
+      //
+      // Shift release
+      //
+      case EFI_LEFT_SHIFT_MODIFIER:
+        UsbKeyboardDevice->LeftShiftOn = FALSE;
+        UsbKeyboardDevice->ShiftOn = FALSE;
+        break;
+      case EFI_RIGHT_SHIFT_MODIFIER:
+        UsbKeyboardDevice->RightShiftOn = FALSE;
+        UsbKeyboardDevice->ShiftOn = FALSE;
+        break;
+
+      //
+      // Alt release
+      //
+      case EFI_LEFT_ALT_MODIFIER:
+        UsbKeyboardDevice->LeftAltOn = FALSE;
+        UsbKeyboardDevice->AltOn = FALSE;
+        break;
+      case EFI_RIGHT_ALT_MODIFIER:
+        UsbKeyboardDevice->RightAltOn = FALSE;
+        UsbKeyboardDevice->AltOn = FALSE;
+        break;
+
+      //
+      // Left Logo release
+      //
+      case EFI_LEFT_LOGO_MODIFIER:
+        UsbKeyboardDevice->LeftLogoOn = FALSE;
+        break;
+
+      //
+      // Right Logo release
+      //
+      case EFI_RIGHT_LOGO_MODIFIER:
+        UsbKeyboardDevice->RightLogoOn = FALSE;
+        break;
+
+      //
+      // Menu key release
+      //
+      case EFI_MENU_MODIFIER:
+        UsbKeyboardDevice->MenuKeyOn = FALSE;
+        break;
+
+      //
+      // SysReq release
+      //
+      case EFI_PRINT_MODIFIER:
+      case EFI_SYS_REQUEST_MODIFIER:
+        UsbKeyboardDevice->SysReqOn = FALSE;
+        break;
+
+      //
+      // AltGr release
+      //
+      case EFI_ALT_GR_MODIFIER:
+        UsbKeyboardDevice->AltGrOn = FALSE;
+        break;
+
+      default:
+        break;
+      }
+
+      continue;
+    }
+
+    //
+    // Analyzes key pressing situation
+    //
+    switch (KeyDescriptor->Modifier) {
+
+    //
+    // Ctrl press
+    //
+    case EFI_LEFT_CONTROL_MODIFIER:
+      UsbKeyboardDevice->LeftCtrlOn = TRUE;
+      UsbKeyboardDevice->CtrlOn = TRUE;
+      break;
+    case EFI_RIGHT_CONTROL_MODIFIER:
+      UsbKeyboardDevice->RightCtrlOn = TRUE;
+      UsbKeyboardDevice->CtrlOn = TRUE;
+      break;
+
+    //
+    // Shift press
+    //
+    case EFI_LEFT_SHIFT_MODIFIER:
+      UsbKeyboardDevice->LeftShiftOn = TRUE;
+      UsbKeyboardDevice->ShiftOn = TRUE;
+      break;
+    case EFI_RIGHT_SHIFT_MODIFIER:
+      UsbKeyboardDevice->RightShiftOn = TRUE;
+      UsbKeyboardDevice->ShiftOn = TRUE;
+      break;
+
+    //
+    // Alt press
+    //
+    case EFI_LEFT_ALT_MODIFIER:
+      UsbKeyboardDevice->LeftAltOn = TRUE;
+      UsbKeyboardDevice->AltOn = TRUE;
+      break;
+    case EFI_RIGHT_ALT_MODIFIER:
+      UsbKeyboardDevice->RightAltOn = TRUE;
+      UsbKeyboardDevice->AltOn = TRUE;
+      break;
+
+    //
+    // Left Logo press
+    //
+    case EFI_LEFT_LOGO_MODIFIER:
+      UsbKeyboardDevice->LeftLogoOn = TRUE;
+      break;
+
+    //
+    // Right Logo press
+    //
+    case EFI_RIGHT_LOGO_MODIFIER:
+      UsbKeyboardDevice->RightLogoOn = TRUE;
+      break;
+
+    //
+    // Menu key press
+    //
+    case EFI_MENU_MODIFIER:
+      UsbKeyboardDevice->MenuKeyOn = TRUE;
+      break;
+
+    //
+    // SysReq press
+    //
+    case EFI_PRINT_MODIFIER:
+    case EFI_SYS_REQUEST_MODIFIER:
+      UsbKeyboardDevice->SysReqOn = TRUE;
+      break;
+
+    //
+    // AltGr press
+    //
+    case EFI_ALT_GR_MODIFIER:
+      UsbKeyboardDevice->AltGrOn = TRUE;
+      break;
+
+    case EFI_NUM_LOCK_MODIFIER:
+      //
+      // Toggle NumLock
+      //
+      UsbKeyboardDevice->NumLockOn = (BOOLEAN) (!(UsbKeyboardDevice->NumLockOn));
+      SetKeyLED (UsbKeyboardDevice);
+      break;
+
+    case EFI_CAPS_LOCK_MODIFIER:
+      //
+      // Toggle CapsLock
+      //
+      UsbKeyboardDevice->CapsOn = (BOOLEAN) (!(UsbKeyboardDevice->CapsOn));
+      SetKeyLED (UsbKeyboardDevice);
+      break;
+
+    case EFI_SCROLL_LOCK_MODIFIER:
+      //
+      // Toggle ScrollLock
+      //
+      UsbKeyboardDevice->ScrollOn = (BOOLEAN) (!(UsbKeyboardDevice->ScrollOn));
+      SetKeyLED (UsbKeyboardDevice);
+      break;
+
+    default:
+      break;
+    }
+
+    //
+    // When encountering Ctrl + Alt + Del, then warm reset.
+    //
+    if (KeyDescriptor->Modifier == EFI_DELETE_MODIFIER) {
+      if ((UsbKeyboardDevice->CtrlOn) && (UsbKeyboardDevice->AltOn)) {
+        gRT->ResetSystem (EfiResetWarm, EFI_SUCCESS, 0, NULL);
+      }
+    }
+
+    *KeyCode = UsbKey.KeyCode;
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_READY;
+}
+
+
+/**
+  Converts USB Keycode ranging from 0x4 to 0x65 to EFI_INPUT_KEY.
+
+  @param  UsbKeyboardDevice     The USB_KB_DEV instance.
+  @param  KeyCode               Indicates the key code that will be interpreted.
+  @param  KeyData               A pointer to a buffer that is filled in with
+                                the keystroke information for the key that
+                                was pressed.
+
+  @retval EFI_SUCCESS           Success.
+  @retval EFI_INVALID_PARAMETER KeyCode is not in the range of 0x4 to 0x65.
+  @retval EFI_INVALID_PARAMETER Translated EFI_INPUT_KEY has zero for both ScanCode and UnicodeChar.
+  @retval EFI_NOT_READY         KeyCode represents a dead key with EFI_NS_KEY_MODIFIER
+  @retval EFI_DEVICE_ERROR      Keyboard layout is invalid.
+
+**/
+EFI_STATUS
+UsbKeyCodeToEfiInputKey (
+  IN  USB_KB_DEV                *UsbKeyboardDevice,
+  IN  UINT8                     KeyCode,
+  OUT EFI_KEY_DATA              *KeyData
+  )
+{
+  EFI_KEY_DESCRIPTOR            *KeyDescriptor;
+  LIST_ENTRY                    *Link;
+  LIST_ENTRY                    *NotifyList;
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY *CurrentNotify;
+
+  //
+  // KeyCode must in the range of  [0x4, 0x65] or [0xe0, 0xe7].
+  //
+  KeyDescriptor = GetKeyDescriptor (UsbKeyboardDevice, KeyCode);
+  ASSERT (KeyDescriptor != NULL);
+
+  if (KeyDescriptor->Modifier == EFI_NS_KEY_MODIFIER) {
+    //
+    // If this is a dead key with EFI_NS_KEY_MODIFIER, then record it and return.
+    //
+    UsbKeyboardDevice->CurrentNsKey = FindUsbNsKey (UsbKeyboardDevice, KeyDescriptor);
+    return EFI_NOT_READY;
+  }
+
+  if (UsbKeyboardDevice->CurrentNsKey != NULL) {
+    //
+    // If this keystroke follows a non-spacing key, then find the descriptor for corresponding
+    // physical key.
+    //
+    KeyDescriptor = FindPhysicalKey (UsbKeyboardDevice->CurrentNsKey, KeyDescriptor);
+    UsbKeyboardDevice->CurrentNsKey = NULL;
+  }
+
+  //
+  // Make sure modifier of Key Descriptor is in the valid range according to UEFI spec.
+  //
+  if (KeyDescriptor->Modifier >= (sizeof (ModifierValueToEfiScanCodeConvertionTable) / sizeof (UINT8))) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  KeyData->Key.ScanCode    = ModifierValueToEfiScanCodeConvertionTable[KeyDescriptor->Modifier];
+  KeyData->Key.UnicodeChar = KeyDescriptor->Unicode;
+
+  if ((KeyDescriptor->AffectedAttribute & EFI_AFFECTED_BY_STANDARD_SHIFT)!= 0) {
+    if (UsbKeyboardDevice->ShiftOn) {
+      KeyData->Key.UnicodeChar = KeyDescriptor->ShiftedUnicode;
+
+      //
+      // Need not return associated shift state if a class of printable characters that
+      // are normally adjusted by shift modifiers. e.g. Shift Key + 'f' key = 'F'
+      //
+      if ((KeyDescriptor->AffectedAttribute & EFI_AFFECTED_BY_CAPS_LOCK) != 0) {
+        UsbKeyboardDevice->LeftShiftOn = FALSE;
+        UsbKeyboardDevice->RightShiftOn = FALSE;
+      }
+
+      if (UsbKeyboardDevice->AltGrOn) {
+        KeyData->Key.UnicodeChar = KeyDescriptor->ShiftedAltGrUnicode;
+      }
+    } else {
+      //
+      // Shift off
+      //
+      KeyData->Key.UnicodeChar = KeyDescriptor->Unicode;
+
+      if (UsbKeyboardDevice->AltGrOn) {
+        KeyData->Key.UnicodeChar = KeyDescriptor->AltGrUnicode;
+      }
+    }
+  }
+
+  if ((KeyDescriptor->AffectedAttribute & EFI_AFFECTED_BY_CAPS_LOCK) != 0) {
+    if (UsbKeyboardDevice->CapsOn) {
+      if (KeyData->Key.UnicodeChar == KeyDescriptor->Unicode) {
+        KeyData->Key.UnicodeChar = KeyDescriptor->ShiftedUnicode;
+      } else if (KeyData->Key.UnicodeChar == KeyDescriptor->ShiftedUnicode) {
+        KeyData->Key.UnicodeChar = KeyDescriptor->Unicode;
+      }
+    }
+  }
+
+  if ((KeyDescriptor->AffectedAttribute & EFI_AFFECTED_BY_NUM_LOCK) != 0) {
+    //
+    // For key affected by NumLock, if NumLock is on and Shift is not pressed, then it means
+    // normal key, instead of original control key. So the ScanCode should be cleaned.
+    // Otherwise, it means control key, so preserve the EFI Scan Code and clear the unicode keycode.
+    //
+    if ((UsbKeyboardDevice->NumLockOn) && (!(UsbKeyboardDevice->ShiftOn))) {
+      KeyData->Key.ScanCode = SCAN_NULL;
+    } else {
+      KeyData->Key.UnicodeChar = CHAR_NULL;
+    }
+  }
+
+  //
+  // Translate Unicode 0x1B (ESC) to EFI Scan Code
+  //
+  if (KeyData->Key.UnicodeChar == 0x1B && KeyData->Key.ScanCode == SCAN_NULL) {
+    KeyData->Key.ScanCode = SCAN_ESC;
+    KeyData->Key.UnicodeChar = CHAR_NULL;
+  }
+
+  //
+  // Not valid for key without both unicode key code and EFI Scan Code.
+  //
+  if (KeyData->Key.UnicodeChar == 0 && KeyData->Key.ScanCode == SCAN_NULL) {
+    if (!UsbKeyboardDevice->IsSupportPartialKey) {
+    return EFI_NOT_READY;
+    }
+  }
+
+  //
+  // Save Shift/Toggle state
+  //
+  KeyData->KeyState.KeyShiftState  = EFI_SHIFT_STATE_VALID;
+  KeyData->KeyState.KeyToggleState = EFI_TOGGLE_STATE_VALID;
+
+  if (UsbKeyboardDevice->LeftCtrlOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_LEFT_CONTROL_PRESSED;
+  }
+  if (UsbKeyboardDevice->RightCtrlOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_RIGHT_CONTROL_PRESSED;
+  }
+  if (UsbKeyboardDevice->LeftAltOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_LEFT_ALT_PRESSED;
+  }
+  if (UsbKeyboardDevice->RightAltOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_RIGHT_ALT_PRESSED;
+  }
+  if (UsbKeyboardDevice->LeftShiftOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_LEFT_SHIFT_PRESSED;
+  }
+  if (UsbKeyboardDevice->RightShiftOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_RIGHT_SHIFT_PRESSED;
+  }
+  if (UsbKeyboardDevice->LeftLogoOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_LEFT_LOGO_PRESSED;
+  }
+  if (UsbKeyboardDevice->RightLogoOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_RIGHT_LOGO_PRESSED;
+  }
+  if (UsbKeyboardDevice->MenuKeyOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_MENU_KEY_PRESSED;
+  }
+  if (UsbKeyboardDevice->SysReqOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_SYS_REQ_PRESSED;
+  }
+
+  if (UsbKeyboardDevice->ScrollOn) {
+    KeyData->KeyState.KeyToggleState |= EFI_SCROLL_LOCK_ACTIVE;
+  }
+  if (UsbKeyboardDevice->NumLockOn) {
+    KeyData->KeyState.KeyToggleState |= EFI_NUM_LOCK_ACTIVE;
+  }
+  if (UsbKeyboardDevice->CapsOn) {
+    KeyData->KeyState.KeyToggleState |= EFI_CAPS_LOCK_ACTIVE;
+  }
+  if (UsbKeyboardDevice->IsSupportPartialKey) {
+    KeyData->KeyState.KeyToggleState |= EFI_KEY_STATE_EXPOSED;
+  }
+  //
+  // Invoke notification functions if the key is registered.
+  //
+  NotifyList = &UsbKeyboardDevice->NotifyList;
+  for (Link = GetFirstNode (NotifyList); !IsNull (NotifyList, Link); Link = GetNextNode (NotifyList, Link)) {
+    CurrentNotify = CR (Link, KEYBOARD_CONSOLE_IN_EX_NOTIFY, NotifyEntry, USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE);
+    if (IsKeyRegistered (&CurrentNotify->KeyData, KeyData)) {
+      CurrentNotify->KeyNotificationFn (KeyData);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Create the queue.
+
+  @param  Queue     Points to the queue.
+  @param  ItemSize  Size of the single item.
+
+**/
+VOID
+InitQueue (
+  IN OUT  USB_SIMPLE_QUEUE   *Queue,
+  IN      UINTN              ItemSize
+  )
+{
+  UINTN                      Index;
+
+  Queue->ItemSize  = ItemSize;
+  Queue->Head      = 0;
+  Queue->Tail      = 0;
+
+  if (Queue->Buffer[0] != NULL) {
+    FreePool (Queue->Buffer[0]);
+  }
+
+  Queue->Buffer[0] = AllocatePool (sizeof (Queue->Buffer) / sizeof (Queue->Buffer[0]) * ItemSize);
+  ASSERT (Queue->Buffer[0] != NULL);
+
+  for (Index = 1; Index < sizeof (Queue->Buffer) / sizeof (Queue->Buffer[0]); Index++) {
+    Queue->Buffer[Index] = ((UINT8 *) Queue->Buffer[Index - 1]) + ItemSize;
+  }
+}
+
+/**
+  Destroy the queue
+
+  @param Queue    Points to the queue.
+**/
+VOID
+DestroyQueue (
+  IN OUT USB_SIMPLE_QUEUE   *Queue
+  )
+{
+  FreePool (Queue->Buffer[0]);
+}
+
+
+/**
+  Check whether the queue is empty.
+
+  @param  Queue     Points to the queue.
+
+  @retval TRUE      Queue is empty.
+  @retval FALSE     Queue is not empty.
+
+**/
+BOOLEAN
+IsQueueEmpty (
+  IN  USB_SIMPLE_QUEUE   *Queue
+  )
+{
+  //
+  // Meet FIFO empty condition
+  //
+  return (BOOLEAN) (Queue->Head == Queue->Tail);
+}
+
+
+/**
+  Check whether the queue is full.
+
+  @param  Queue     Points to the queue.
+
+  @retval TRUE      Queue is full.
+  @retval FALSE     Queue is not full.
+
+**/
+BOOLEAN
+IsQueueFull (
+  IN  USB_SIMPLE_QUEUE   *Queue
+  )
+{
+  return (BOOLEAN) (((Queue->Tail + 1) % (MAX_KEY_ALLOWED + 1)) == Queue->Head);
+}
+
+
+/**
+  Enqueue the item to the queue.
+
+  @param  Queue     Points to the queue.
+  @param  Item      Points to the item to be enqueued.
+  @param  ItemSize  Size of the item.
+**/
+VOID
+Enqueue (
+  IN OUT  USB_SIMPLE_QUEUE *Queue,
+  IN      VOID             *Item,
+  IN      UINTN            ItemSize
+  )
+{
+  ASSERT (ItemSize == Queue->ItemSize);
+  //
+  // If keyboard buffer is full, throw the
+  // first key out of the keyboard buffer.
+  //
+  if (IsQueueFull (Queue)) {
+    Queue->Head = (Queue->Head + 1) % (MAX_KEY_ALLOWED + 1);
+  }
+
+  CopyMem (Queue->Buffer[Queue->Tail], Item, ItemSize);
+
+  //
+  // Adjust the tail pointer of the FIFO keyboard buffer.
+  //
+  Queue->Tail = (Queue->Tail + 1) % (MAX_KEY_ALLOWED + 1);
+}
+
+
+/**
+  Dequeue a item from the queue.
+
+  @param  Queue     Points to the queue.
+  @param  Item      Receives the item.
+  @param  ItemSize  Size of the item.
+
+  @retval EFI_SUCCESS        Item was successfully dequeued.
+  @retval EFI_DEVICE_ERROR   The queue is empty.
+
+**/
+EFI_STATUS
+Dequeue (
+  IN OUT  USB_SIMPLE_QUEUE *Queue,
+     OUT  VOID             *Item,
+  IN      UINTN            ItemSize
+  )
+{
+  ASSERT (Queue->ItemSize == ItemSize);
+
+  if (IsQueueEmpty (Queue)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  CopyMem (Item, Queue->Buffer[Queue->Head], ItemSize);
+
+  //
+  // Adjust the head pointer of the FIFO keyboard buffer.
+  //
+  Queue->Head = (Queue->Head + 1) % (MAX_KEY_ALLOWED + 1);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Sets USB keyboard LED state.
+
+  @param  UsbKeyboardDevice  The USB_KB_DEV instance.
+
+**/
+VOID
+SetKeyLED (
+  IN  USB_KB_DEV    *UsbKeyboardDevice
+  )
+{
+  LED_MAP Led;
+  UINT8   ReportId;
+
+  //
+  // Set each field in Led map.
+  //
+  Led.NumLock    = (UINT8) ((UsbKeyboardDevice->NumLockOn) ? 1 : 0);
+  Led.CapsLock   = (UINT8) ((UsbKeyboardDevice->CapsOn) ? 1 : 0);
+  Led.ScrollLock = (UINT8) ((UsbKeyboardDevice->ScrollOn) ? 1 : 0);
+  Led.Resrvd     = 0;
+
+  ReportId       = 0;
+  //
+  // Call Set_Report Request to lighten the LED.
+  //
+  UsbSetReportRequest (
+    UsbKeyboardDevice->UsbIo,
+    UsbKeyboardDevice->InterfaceDescriptor.InterfaceNumber,
+    ReportId,
+    HID_OUTPUT_REPORT,
+    1,
+    (UINT8 *) &Led
+    );
+}
+
+
+/**
+  Handler for Repeat Key event.
+
+  This function is the handler for Repeat Key event triggered
+  by timer.
+  After a repeatable key is pressed, the event would be triggered
+  with interval of USBKBD_REPEAT_DELAY. Once the event is triggered,
+  following trigger will come with interval of USBKBD_REPEAT_RATE.
+
+  @param  Event              The Repeat Key event.
+  @param  Context            Points to the USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardRepeatHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  )
+{
+  USB_KB_DEV  *UsbKeyboardDevice;
+  USB_KEY     UsbKey;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+
+  //
+  // Do nothing when there is no repeat key.
+  //
+  if (UsbKeyboardDevice->RepeatKey != 0) {
+    //
+    // Inserts the repeat key into keyboard buffer,
+    //
+    UsbKey.KeyCode = UsbKeyboardDevice->RepeatKey;
+    UsbKey.Down    = TRUE;
+    Enqueue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+
+    //
+    // Set repeat rate for next repeat key generation.
+    //
+    gBS->SetTimer (
+           UsbKeyboardDevice->RepeatTimer,
+           TimerRelative,
+           USBKBD_REPEAT_RATE
+           );
+  }
+}
+
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event              The Delayed Recovery event.
+  @param  Context            Points to the USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  )
+{
+
+  USB_KB_DEV          *UsbKeyboardDevice;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+  UINT8               PacketSize;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+
+  UsbIo             = UsbKeyboardDevice->UsbIo;
+
+  PacketSize        = (UINT8) (UsbKeyboardDevice->IntEndpointDescriptor.MaxPacketSize);
+
+  //
+  // Re-submit Asynchronous Interrupt Transfer for recovery.
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress,
+           TRUE,
+           UsbKeyboardDevice->IntEndpointDescriptor.Interval,
+           PacketSize,
+           KeyboardHandler,
+           UsbKeyboardDevice
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.h b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.h
new file mode 100644
index 0000000000..b3eb3e462f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.h
@@ -0,0 +1,320 @@
+/** @file
+  Function prototype for USB Keyboard Driver.
+
+Copyright (c) 2004 - 2013, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_KEYBOARD_H_
+#define _EFI_KEYBOARD_H_
+
+
+#include "EfiKey.h"
+
+#define USB_KEYBOARD_KEY_COUNT            105
+
+#define USB_KEYBOARD_LANGUAGE_STR_LEN     5         // RFC4646 Language Code: "en-US"
+#define USB_KEYBOARD_DESCRIPTION_STR_LEN  (16 + 1)  // Description: "English Keyboard"
+
+#pragma pack (1)
+typedef struct {
+  //
+  // This 4-bytes total array length is required by PreparePackageList()
+  //
+  UINT32                 Length;
+
+  //
+  // Keyboard Layout package definition
+  //
+  EFI_HII_PACKAGE_HEADER PackageHeader;
+  UINT16                 LayoutCount;
+
+  //
+  // EFI_HII_KEYBOARD_LAYOUT
+  //
+  UINT16                 LayoutLength;
+  EFI_GUID               Guid;
+  UINT32                 LayoutDescriptorStringOffset;
+  UINT8                  DescriptorCount;
+  EFI_KEY_DESCRIPTOR     KeyDescriptor[USB_KEYBOARD_KEY_COUNT];
+  UINT16                 DescriptionCount;
+  CHAR16                 Language[USB_KEYBOARD_LANGUAGE_STR_LEN];
+  CHAR16                 Space;
+  CHAR16                 DescriptionString[USB_KEYBOARD_DESCRIPTION_STR_LEN];
+} USB_KEYBOARD_LAYOUT_PACK_BIN;
+#pragma pack()
+/**
+  Uses USB I/O to check whether the device is a USB keyboard device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB keyboard device.
+  @retval FALSE    Device is a not USB keyboard device.
+
+**/
+BOOLEAN
+IsUSBKeyboard (
+  IN  EFI_USB_IO_PROTOCOL       *UsbIo
+  );
+
+/**
+  Initialize USB keyboard device and all private data structures.
+
+  @param  UsbKeyboardDevice  The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS        Initialization is successful.
+  @retval EFI_DEVICE_ERROR   Keyboard initialization failed.
+
+**/
+EFI_STATUS
+InitUSBKeyboard (
+  IN OUT USB_KB_DEV   *UsbKeyboardDevice
+  );
+
+/**
+  Initialize USB keyboard layout.
+
+  This function initializes Key Convertion Table for the USB keyboard device.
+  It first tries to retrieve layout from HII database. If failed and default
+  layout is enabled, then it just uses the default layout.
+
+  @param  UsbKeyboardDevice      The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS            Initialization succeeded.
+  @retval EFI_NOT_READY          Keyboard layout cannot be retrieve from HII
+                                 database, and default layout is disabled.
+  @retval Other                  Fail to register event to EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group.
+
+**/
+EFI_STATUS
+InitKeyboardLayout (
+  OUT USB_KB_DEV   *UsbKeyboardDevice
+  );
+
+/**
+  Destroy resources for keyboard layout.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+
+**/
+VOID
+ReleaseKeyboardLayoutResources (
+  IN OUT USB_KB_DEV              *UsbKeyboardDevice
+  );
+
+/**
+  Handler function for USB keyboard's asynchronous interrupt transfer.
+
+  This function is the handler function for USB keyboard's asynchronous interrupt transfer
+  to manage the keyboard. It parses the USB keyboard input report, and inserts data to
+  keyboard buffer according to state of modifer keys and normal keys. Timer for repeat key
+  is also set accordingly.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+KeyboardHandler (
+  IN  VOID          *Data,
+  IN  UINTN         DataLength,
+  IN  VOID          *Context,
+  IN  UINT32        Result
+  );
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event              The Delayed Recovery event.
+  @param  Context            Points to the USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  );
+
+/**
+  Retrieves a USB keycode after parsing the raw data in keyboard buffer.
+
+  This function parses keyboard buffer. It updates state of modifier key for
+  USB_KB_DEV instancem, and returns keycode for output.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+  @param  KeyCode              Pointer to the USB keycode for output.
+
+  @retval EFI_SUCCESS          Keycode successfully parsed.
+  @retval EFI_NOT_READY        Keyboard buffer is not ready for a valid keycode
+
+**/
+EFI_STATUS
+USBParseKey (
+  IN OUT  USB_KB_DEV  *UsbKeyboardDevice,
+  OUT     UINT8       *KeyCode
+  );
+
+/**
+  Converts USB Keycode ranging from 0x4 to 0x65 to EFI_INPUT_KEY.
+
+  @param  UsbKeyboardDevice     The USB_KB_DEV instance.
+  @param  KeyCode               Indicates the key code that will be interpreted.
+  @param  KeyData               A pointer to a buffer that is filled in with
+                                the keystroke information for the key that
+                                was pressed.
+
+  @retval EFI_SUCCESS           Success.
+  @retval EFI_INVALID_PARAMETER KeyCode is not in the range of 0x4 to 0x65.
+  @retval EFI_INVALID_PARAMETER Translated EFI_INPUT_KEY has zero for both ScanCode and UnicodeChar.
+  @retval EFI_NOT_READY         KeyCode represents a dead key with EFI_NS_KEY_MODIFIER
+  @retval EFI_DEVICE_ERROR      Keyboard layout is invalid.
+
+**/
+EFI_STATUS
+UsbKeyCodeToEfiInputKey (
+  IN  USB_KB_DEV      *UsbKeyboardDevice,
+  IN  UINT8           KeyCode,
+  OUT EFI_KEY_DATA    *KeyData
+  );
+
+
+/**
+  Create the queue.
+
+  @param  Queue     Points to the queue.
+  @param  ItemSize  Size of the single item.
+
+**/
+VOID
+InitQueue (
+  IN OUT  USB_SIMPLE_QUEUE   *Queue,
+  IN      UINTN              ItemSize
+  );
+
+/**
+  Destroy the queue
+
+  @param Queue    Points to the queue.
+**/
+VOID
+DestroyQueue (
+  IN OUT USB_SIMPLE_QUEUE   *Queue
+  );
+
+
+/**
+  Check whether the queue is empty.
+
+  @param  Queue     Points to the queue.
+
+  @retval TRUE      Queue is empty.
+  @retval FALSE     Queue is not empty.
+
+**/
+BOOLEAN
+IsQueueEmpty (
+  IN  USB_SIMPLE_QUEUE   *Queue
+  );
+
+
+/**
+  Check whether the queue is full.
+
+  @param  Queue     Points to the queue.
+
+  @retval TRUE      Queue is full.
+  @retval FALSE     Queue is not full.
+
+**/
+BOOLEAN
+IsQueueFull (
+  IN  USB_SIMPLE_QUEUE   *Queue
+  );
+
+
+/**
+  Enqueue the item to the queue.
+
+  @param  Queue     Points to the queue.
+  @param  Item      Points to the item to be enqueued.
+  @param  ItemSize  Size of the item.
+**/
+VOID
+Enqueue (
+  IN OUT  USB_SIMPLE_QUEUE *Queue,
+  IN      VOID             *Item,
+  IN      UINTN            ItemSize
+  );
+
+
+/**
+  Dequeue a item from the queue.
+
+  @param  Queue     Points to the queue.
+  @param  Item      Receives the item.
+  @param  ItemSize  Size of the item.
+
+  @retval EFI_SUCCESS        Item was successfully dequeued.
+  @retval EFI_DEVICE_ERROR   The queue is empty.
+
+**/
+EFI_STATUS
+Dequeue (
+  IN OUT  USB_SIMPLE_QUEUE *Queue,
+     OUT  VOID             *Item,
+  IN      UINTN            ItemSize
+  );
+
+/**
+  Handler for Repeat Key event.
+
+  This function is the handler for Repeat Key event triggered
+  by timer.
+  After a repeatable key is pressed, the event would be triggered
+  with interval of USBKBD_REPEAT_DELAY. Once the event is triggered,
+  following trigger will come with interval of USBKBD_REPEAT_RATE.
+
+  @param  Event              The Repeat Key event.
+  @param  Context            Points to the USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardRepeatHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  );
+
+/**
+  Sets USB keyboard LED state.
+
+  @param  UsbKeyboardDevice  The USB_KB_DEV instance.
+
+**/
+VOID
+SetKeyLED (
+  IN  USB_KB_DEV    *UsbKeyboardDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.inf
new file mode 100644
index 0000000000..7939a1857d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.inf
@@ -0,0 +1,99 @@
+## @file
+# USB Keyboard Driver that manages USB keyboard and produces Simple Text Input(Ex) Protocol.
+#
+# USB Keyboard Driver consumes USB I/O Protocol and Device Path Protocol, and produces
+# Simple Text Input Protocol and Simple Text Input Ex Protocol on USB keyboard devices.
+# It initializes the keyboard layout according to info retrieved from HII database.
+# If HII cannot provide the info, this module uses its carried default one if PCD allows.
+# It manages the USB keyboard device via Asynchronous Interrupt Transfer of USB I/O Protocol,
+# and parses the data according to USB HID documents.
+# This module refers to following specifications:
+# 1. Universal Serial Bus HID Firmware Specification, ver 1.11
+# 2. Universal Serial Bus HID Usage Tables, ver 1.12
+# 3. UEFI Specification, v2.1
+#
+# Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbKbDxe
+  MODULE_UNI_FILE                = UsbKbDxe.uni
+  FILE_GUID                      = 2D2E62CF-9ECF-43b7-8219-94E7FC713DFE
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = USBKeyboardDriverBindingEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gUsbKeyboardDriverBinding                        
+#  COMPONENT_NAME                =  gUsbKeyboardComponentName
+#  COMPONENT_NAME2               =  gUsbKeyboardComponentName2
+#
+
+[Sources]
+  EfiKey.c
+  EfiKey.h
+  KeyBoard.c
+  ComponentName.c
+  KeyBoard.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiRuntimeServicesTableLib
+  BaseMemoryLib
+  ReportStatusCodeLib
+  DebugLib
+  PcdLib
+  UefiUsbLib
+  HiiLib
+
+[Guids]
+  #
+  # Event registered to EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group,
+  # which will be triggered by EFI_HII_DATABASE_PROTOCOL.SetKeyboardLayout().
+  #
+  gEfiHiiKeyBoardLayoutGuid                     ## SOMETIMES_CONSUMES ## Event
+  gUsbKeyboardLayoutPackageGuid                 ## SOMETIMES_CONSUMES ## HII
+  gUsbKeyboardLayoutKeyGuid                     ## SOMETIMES_PRODUCES ## UNDEFINED
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiSimpleTextInProtocolGuid                  ## BY_START
+  gEfiSimpleTextInputExProtocolGuid             ## BY_START
+  #
+  # If HII Database Protocol exists, then keyboard layout from HII database is used.
+  # Otherwise, USB keyboard module tries to use its carried default layout.
+  #
+  gEfiHiiDatabaseProtocolGuid                   ## SOMETIMES_CONSUMES
+  
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDisableDefaultKeyboardLayoutInUsbKbDriver ## CONSUMES
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER        ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbKbDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.uni
new file mode 100644
index 0000000000..77586520c4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxeExtra.uni
new file mode 100644
index 0000000000..b337726abf
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/ComponentName.c
new file mode 100644
index 0000000000..ad879afe59
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/ComponentName.c
@@ -0,0 +1,162 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for USB Mass Storage Driver.
+
+Copyright (c) 2004 - 2011, 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 "UsbMass.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUsbMassStorageComponentName = {
+  UsbMassStorageGetDriverName,
+  UsbMassStorageGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUsbMassStorageComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbMassStorageGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbMassStorageGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE
+mUsbMassStorageDriverNameTable[] = {
+  {"eng;en", L"Usb Mass Storage Driver"},
+  {NULL,  NULL}
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassStorageGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbMassStorageDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUsbMassStorageComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassStorageGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMass.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMass.h
new file mode 100644
index 0000000000..ebfb254de6
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMass.h
@@ -0,0 +1,193 @@
+/** @file
+  Definition of USB Mass Storage Class and its value, USB Mass Transport Protocol, 
+  and other common definitions.
+
+Copyright (c) 2007 - 2014, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_USBMASS_H_
+#define _EFI_USBMASS_H_
+
+
+#include <Uefi.h>
+#include <IndustryStandard/Scsi.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/DiskInfo.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DevicePathLib.h>
+
+typedef struct _USB_MASS_TRANSPORT USB_MASS_TRANSPORT;
+typedef struct _USB_MASS_DEVICE    USB_MASS_DEVICE;
+
+#include "UsbMassBot.h"
+#include "UsbMassCbi.h"
+#include "UsbMassBoot.h"
+#include "UsbMassDiskInfo.h"
+#include "UsbMassImpl.h"
+
+#define USB_IS_IN_ENDPOINT(EndPointAddr)      (((EndPointAddr) & BIT7) == BIT7)
+#define USB_IS_OUT_ENDPOINT(EndPointAddr)     (((EndPointAddr) & BIT7) == 0)
+#define USB_IS_BULK_ENDPOINT(Attribute)       (((Attribute) & (BIT0 | BIT1)) == USB_ENDPOINT_BULK)
+#define USB_IS_INTERRUPT_ENDPOINT(Attribute)  (((Attribute) & (BIT0 | BIT1)) == USB_ENDPOINT_INTERRUPT)
+#define USB_IS_ERROR(Result, Error)           (((Result) & (Error)) != 0)
+
+#define USB_MASS_1_MILLISECOND  1000
+#define USB_MASS_1_SECOND       (1000 * USB_MASS_1_MILLISECOND)
+
+#define USB_MASS_CMD_SUCCESS    0
+#define USB_MASS_CMD_FAIL       1
+#define USB_MASS_CMD_PERSISTENT 2
+
+/**
+  Initializes USB transport protocol.
+
+  This function initializes the USB mass storage class transport protocol.
+  It will save its context in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB transport initialization fails.
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_INIT_TRANSPORT) (
+  IN  EFI_USB_IO_PROTOCOL     *Usb,
+  OUT VOID                    **Context    OPTIONAL
+  );
+
+/**
+  Execute USB mass storage command through the transport protocol.
+
+  @param  Context               The USB Transport Protocol.
+  @param  Cmd                   The command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  DataDir               The direction of data transfer
+  @param  Data                  The buffer to hold the data
+  @param  DataLen               The length of the buffer
+  @param  Lun                   Should be 0, this field for bot only
+  @param  Timeout               The time to wait
+  @param  CmdStatus             The result of the command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to execute the command
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_EXEC_COMMAND) (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  );
+
+/**
+  Reset the USB mass storage device by Transport protocol.
+
+  @param  Context               The USB Transport Protocol
+  @param  ExtendedVerification  The flag controlling the rule of reset.
+                                Not used here.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device.
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_RESET) (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  Get the max LUN (Logical Unit Number) of USB mass storage device.
+
+  @param  Context          The context of the transport protocol.
+  @param  MaxLun           Return pointer to the max number of LUN. (e.g. MaxLun=1 means LUN0 and
+                           LUN1 in all.)
+
+  @retval EFI_SUCCESS      Max LUN is got successfully.
+  @retval Others           Fail to execute this request.
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_GET_MAX_LUN) (
+  IN  VOID                    *Context,
+  IN  UINT8                   *MaxLun
+  );
+
+/**
+  Clean up the transport protocol's resource.
+
+  @param  Context               The instance of transport protocol.
+
+  @retval EFI_SUCCESS           The resource is cleaned up.
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_CLEAN_UP) (
+  IN  VOID                    *Context
+  );
+
+///
+/// This structure contains information necessary to select the
+/// proper transport protocol. The mass storage class defines
+/// two transport protocols. One is the CBI, and the other is BOT.
+/// CBI is being obseleted. The design is made modular by this
+/// structure so that the CBI protocol can be easily removed when
+/// it is no longer necessary.
+///
+struct _USB_MASS_TRANSPORT {
+  UINT8                   Protocol;
+  USB_MASS_INIT_TRANSPORT Init;        ///< Initialize the mass storage transport protocol
+  USB_MASS_EXEC_COMMAND   ExecCommand; ///< Transport command to the device then get result
+  USB_MASS_RESET          Reset;       ///< Reset the device
+  USB_MASS_GET_MAX_LUN    GetMaxLun;   ///< Get max lun, only for bot
+  USB_MASS_CLEAN_UP       CleanUp;     ///< Clean up the resources.
+};
+
+struct _USB_MASS_DEVICE {
+  UINT32                    Signature;
+  EFI_HANDLE                Controller;
+  EFI_USB_IO_PROTOCOL       *UsbIo;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_BLOCK_IO_PROTOCOL     BlockIo;
+  EFI_BLOCK_IO_MEDIA        BlockIoMedia;
+  BOOLEAN                   OpticalStorage;
+  UINT8                     Lun;          ///< Logical Unit Number
+  UINT8                     Pdt;          ///< Peripheral Device Type
+  USB_MASS_TRANSPORT        *Transport;   ///< USB mass storage transport protocol
+  VOID                      *Context;
+  EFI_DISK_INFO_PROTOCOL    DiskInfo;
+  USB_BOOT_INQUIRY_DATA     InquiryData;
+  BOOLEAN                   Cdb16Byte;
+};
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.c
new file mode 100644
index 0000000000..9f99650703
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.c
@@ -0,0 +1,1106 @@
+/** @file
+  Implementation of the command set of USB Mass Storage Specification
+  for Bootability, Revision 1.0.
+
+Copyright (c) 2007 - 2014, 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 "UsbMass.h"
+
+/**
+  Execute REQUEST SENSE Command to retrieve sense data from device.
+
+  @param  UsbMass                The device whose sense data is requested.
+
+  @retval EFI_SUCCESS            The command is excuted successfully.
+  @retval EFI_DEVICE_ERROR       Failed to request sense.
+  @retval EFI_NO_RESPONSE        The device media doesn't response this request.
+  @retval EFI_INVALID_PARAMETER  The command has some invalid parameters.
+  @retval EFI_WRITE_PROTECTED    The device is write protected.
+  @retval EFI_MEDIA_CHANGED      The device media has been changed.
+
+**/
+EFI_STATUS
+UsbBootRequestSense (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  USB_BOOT_REQUEST_SENSE_CMD  SenseCmd;
+  USB_BOOT_REQUEST_SENSE_DATA SenseData;
+  EFI_BLOCK_IO_MEDIA          *Media;
+  USB_MASS_TRANSPORT          *Transport;
+  EFI_STATUS                  Status;
+  UINT32                      CmdResult;
+
+  Transport = UsbMass->Transport;
+
+  //
+  // Request the sense data from the device
+  //
+  ZeroMem (&SenseCmd, sizeof (USB_BOOT_REQUEST_SENSE_CMD));
+  ZeroMem (&SenseData, sizeof (USB_BOOT_REQUEST_SENSE_DATA));
+
+  SenseCmd.OpCode   = USB_BOOT_REQUEST_SENSE_OPCODE;
+  SenseCmd.Lun      = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+  SenseCmd.AllocLen = (UINT8) sizeof (USB_BOOT_REQUEST_SENSE_DATA);
+
+  Status = Transport->ExecCommand (
+                        UsbMass->Context,
+                        &SenseCmd,
+                        sizeof (USB_BOOT_REQUEST_SENSE_CMD),
+                        EfiUsbDataIn,
+                        &SenseData,
+                        sizeof (USB_BOOT_REQUEST_SENSE_DATA),
+                        UsbMass->Lun,
+                        USB_BOOT_GENERAL_CMD_TIMEOUT,
+                        &CmdResult
+                        );
+  if (EFI_ERROR (Status) || CmdResult != USB_MASS_CMD_SUCCESS) {
+    DEBUG ((EFI_D_ERROR, "UsbBootRequestSense: (%r) CmdResult=0x%x\n", Status, CmdResult));
+    if (!EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+    }
+    return Status;
+  }
+
+  //
+  // If sense data is retrieved successfully, interpret the sense data
+  // and update the media status if necessary.
+  //
+  Media = &UsbMass->BlockIoMedia;
+
+  switch (USB_BOOT_SENSE_KEY (SenseData.SenseKey)) {
+
+  case USB_BOOT_SENSE_NO_SENSE:
+    Status = EFI_NO_RESPONSE;
+    break;
+
+  case USB_BOOT_SENSE_RECOVERED:
+    //
+    // Suppose hardware can handle this case, and recover later by itself
+    //
+    Status = EFI_NOT_READY;
+    break;
+
+  case USB_BOOT_SENSE_NOT_READY:
+    Status = EFI_DEVICE_ERROR;
+    if (SenseData.Asc == USB_BOOT_ASC_NO_MEDIA) {
+      Media->MediaPresent = FALSE;
+      Status = EFI_NO_MEDIA;
+    } else if (SenseData.Asc == USB_BOOT_ASC_NOT_READY) {
+      Status = EFI_NOT_READY;
+    }
+    break;
+
+  case USB_BOOT_SENSE_ILLEGAL_REQUEST:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+
+  case USB_BOOT_SENSE_UNIT_ATTENTION:
+    Status = EFI_DEVICE_ERROR;
+    if (SenseData.Asc == USB_BOOT_ASC_MEDIA_CHANGE) {
+      //
+      // If MediaChange, reset ReadOnly and new MediaId
+      //
+      Status = EFI_MEDIA_CHANGED;
+      Media->ReadOnly = FALSE;
+      Media->MediaId++;
+    }
+    break;
+
+  case USB_BOOT_SENSE_DATA_PROTECT:
+    Status = EFI_WRITE_PROTECTED;
+    Media->ReadOnly = TRUE;
+    break;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    break;
+  }
+
+  DEBUG ((EFI_D_INFO, "UsbBootRequestSense: (%r) with sense key %x/%x/%x\n",
+          Status,
+          USB_BOOT_SENSE_KEY (SenseData.SenseKey),
+          SenseData.Asc,
+          SenseData.Ascq
+          ));
+
+  return Status;
+}
+
+
+/**
+  Execute the USB mass storage bootability commands.
+
+  This function executes the USB mass storage bootability commands.
+  If execution failed, retrieve the error by REQUEST_SENSE, then
+  update the device's status, such as ReadyOnly.
+
+  @param  UsbMass                The device to issue commands to
+  @param  Cmd                    The command to execute
+  @param  CmdLen                 The length of the command
+  @param  DataDir                The direction of data transfer
+  @param  Data                   The buffer to hold the data
+  @param  DataLen                The length of expected data
+  @param  Timeout                The timeout used to transfer
+
+  @retval EFI_SUCCESS            Command is excuted successfully
+  @retval Others                 Command execution failed.
+
+**/
+EFI_STATUS
+UsbBootExecCmd (
+  IN USB_MASS_DEVICE            *UsbMass,
+  IN VOID                       *Cmd,
+  IN UINT8                      CmdLen,
+  IN EFI_USB_DATA_DIRECTION     DataDir,
+  IN VOID                       *Data,
+  IN UINT32                     DataLen,
+  IN UINT32                     Timeout
+  )
+{
+  USB_MASS_TRANSPORT          *Transport;
+  EFI_STATUS                  Status;
+  UINT32                      CmdResult;
+
+  Transport = UsbMass->Transport;
+  Status    = Transport->ExecCommand (
+                           UsbMass->Context,
+                           Cmd,
+                           CmdLen,
+                           DataDir,
+                           Data,
+                           DataLen,
+                           UsbMass->Lun,
+                           Timeout,
+                           &CmdResult
+                           );
+
+  if (Status == EFI_TIMEOUT) {
+    DEBUG ((EFI_D_ERROR, "UsbBootExecCmd: Timeout to Exec 0x%x Cmd\n", *(UINT8 *)Cmd));
+    return EFI_TIMEOUT;
+  }
+
+  //
+  // If ExecCommand() returns no error and CmdResult is success,
+  // then the commnad transfer is successful.
+  //
+  if ((CmdResult == USB_MASS_CMD_SUCCESS) && !EFI_ERROR (Status)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // If command execution failed, then retrieve error info via sense request.
+  //
+  return UsbBootRequestSense (UsbMass);
+}
+
+
+/**
+  Execute the USB mass storage bootability commands with retrial.
+
+  This function executes USB mass storage bootability commands.
+  If the device isn't ready, wait for it. If the device is ready
+  and error occurs, retry the command again until it exceeds the
+  limit of retrial times.
+  
+  @param  UsbMass                The device to issue commands to
+  @param  Cmd                    The command to execute
+  @param  CmdLen                 The length of the command
+  @param  DataDir                The direction of data transfer
+  @param  Data                   The buffer to hold the data
+  @param  DataLen                The length of expected data
+  @param  Timeout                The timeout used to transfer
+
+  @retval EFI_SUCCESS            The command is executed successfully.
+  @retval EFI_MEDIA_CHANGED      The device media has been changed.
+  @retval Others                 Command execution failed after retrial.
+
+**/
+EFI_STATUS
+UsbBootExecCmdWithRetry (
+  IN USB_MASS_DEVICE          *UsbMass,
+  IN VOID                     *Cmd,
+  IN UINT8                    CmdLen,
+  IN EFI_USB_DATA_DIRECTION   DataDir,
+  IN VOID                     *Data,
+  IN UINT32                   DataLen,
+  IN UINT32                   Timeout
+  )
+{
+  EFI_STATUS                  Status;
+  UINTN                       Retry;
+  EFI_EVENT                   TimeoutEvt;
+
+  Retry  = 0;
+  Status = EFI_SUCCESS;
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &TimeoutEvt
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->SetTimer (TimeoutEvt, TimerRelative, EFI_TIMER_PERIOD_SECONDS(60));
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  //
+  // Execute the cmd and retry if it fails.
+  //
+  while (EFI_ERROR (gBS->CheckEvent (TimeoutEvt))) {
+    Status = UsbBootExecCmd (
+               UsbMass,
+               Cmd,
+               CmdLen,
+               DataDir,
+               Data,
+               DataLen,
+               Timeout
+               );
+    if (Status == EFI_SUCCESS || Status == EFI_MEDIA_CHANGED || Status == EFI_NO_MEDIA) {
+      break;
+    }
+    //
+    // If the sense data shows the drive is not ready, we need execute the cmd again.
+    // We limit the upper boundary to 60 seconds.
+    //
+    if (Status == EFI_NOT_READY) {
+      continue;
+    }
+    //
+    // If the status is other error, then just retry 5 times.
+    //
+    if (Retry++ >= USB_BOOT_COMMAND_RETRY) {
+      break;
+    }
+  }
+
+EXIT:
+  if (TimeoutEvt != NULL) {
+    gBS->CloseEvent (TimeoutEvt);
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute TEST UNIT READY command to check if the device is ready.
+
+  @param  UsbMass                The device to test
+
+  @retval EFI_SUCCESS            The device is ready.
+  @retval Others                 Device not ready.
+
+**/
+EFI_STATUS
+UsbBootIsUnitReady (
+  IN USB_MASS_DEVICE            *UsbMass
+  )
+{
+  USB_BOOT_TEST_UNIT_READY_CMD  TestCmd;
+
+  ZeroMem (&TestCmd, sizeof (USB_BOOT_TEST_UNIT_READY_CMD));
+
+  TestCmd.OpCode  = USB_BOOT_TEST_UNIT_READY_OPCODE;
+  TestCmd.Lun     = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+
+  return UsbBootExecCmdWithRetry (
+           UsbMass,
+           &TestCmd,
+           (UINT8) sizeof (USB_BOOT_TEST_UNIT_READY_CMD),
+           EfiUsbNoData,
+           NULL,
+           0,
+           USB_BOOT_GENERAL_CMD_TIMEOUT
+           );
+}
+
+
+/**
+  Execute INQUIRY Command to request information regarding parameters of
+  the device be sent to the host computer.
+
+  @param  UsbMass                The device to inquire.
+
+  @retval EFI_SUCCESS            INQUIRY Command is executed successfully.
+  @retval Others                 INQUIRY Command is not executed successfully.
+
+**/
+EFI_STATUS
+UsbBootInquiry (
+  IN USB_MASS_DEVICE            *UsbMass
+  )
+{
+  USB_BOOT_INQUIRY_CMD        InquiryCmd;
+  EFI_BLOCK_IO_MEDIA          *Media;
+  EFI_STATUS                  Status;
+
+  Media = &(UsbMass->BlockIoMedia);
+
+  ZeroMem (&InquiryCmd, sizeof (USB_BOOT_INQUIRY_CMD));
+  ZeroMem (&UsbMass->InquiryData, sizeof (USB_BOOT_INQUIRY_DATA));
+
+  InquiryCmd.OpCode   = USB_BOOT_INQUIRY_OPCODE;
+  InquiryCmd.Lun      = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+  InquiryCmd.AllocLen = (UINT8) sizeof (USB_BOOT_INQUIRY_DATA);
+
+  Status = UsbBootExecCmdWithRetry (
+             UsbMass,
+             &InquiryCmd,
+             (UINT8) sizeof (USB_BOOT_INQUIRY_CMD),
+             EfiUsbDataIn,
+             &UsbMass->InquiryData,
+             sizeof (USB_BOOT_INQUIRY_DATA),
+             USB_BOOT_GENERAL_CMD_TIMEOUT
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get information from PDT (Peripheral Device Type) field and Removable Medium Bit
+  // from the inquiry data.
+  //
+  UsbMass->Pdt          = (UINT8) (USB_BOOT_PDT (UsbMass->InquiryData.Pdt));
+  Media->RemovableMedia = (BOOLEAN) (USB_BOOT_REMOVABLE (UsbMass->InquiryData.Removable));
+  //
+  // Set block size to the default value of 512 Bytes, in case no media is present at first time.
+  //
+  Media->BlockSize      = 0x0200;
+
+  return Status;
+}
+
+/**
+  Execute READ CAPACITY 16 bytes command to request information regarding
+  the capacity of the installed medium of the device.
+
+  This function executes READ CAPACITY 16 bytes command to get the capacity
+  of the USB mass storage media, including the presence, block size,
+  and last block number.
+
+  @param  UsbMass                The device to retireve disk gemotric.
+
+  @retval EFI_SUCCESS            The disk geometry is successfully retrieved.
+  @retval EFI_NOT_READY          The returned block size is zero.
+  @retval Other                  READ CAPACITY 16 bytes command execution failed.
+ 
+**/
+EFI_STATUS
+UsbBootReadCapacity16 (
+  IN USB_MASS_DEVICE            *UsbMass
+  )
+{
+  UINT8                         CapacityCmd[16];
+  EFI_SCSI_DISK_CAPACITY_DATA16 CapacityData;
+  EFI_BLOCK_IO_MEDIA            *Media;
+  EFI_STATUS                    Status;
+  UINT32                        BlockSize;
+
+  Media   = &UsbMass->BlockIoMedia;
+
+  Media->MediaPresent = FALSE;
+  Media->LastBlock    = 0;
+  Media->BlockSize    = 0;
+
+  ZeroMem (CapacityCmd, sizeof (CapacityCmd));
+  ZeroMem (&CapacityData, sizeof (CapacityData));
+
+  CapacityCmd[0]  = EFI_SCSI_OP_READ_CAPACITY16;
+  CapacityCmd[1]  = 0x10;
+  //
+  // Partial medium indicator, set the bytes 2 ~ 9 of the Cdb as ZERO.
+  //
+  ZeroMem ((CapacityCmd + 2), 8);
+
+  CapacityCmd[13] = sizeof (CapacityData);
+  
+  Status = UsbBootExecCmdWithRetry (
+             UsbMass,
+             CapacityCmd,
+             (UINT8) sizeof (CapacityCmd),
+             EfiUsbDataIn,
+             &CapacityData,
+             sizeof (CapacityData),
+             USB_BOOT_GENERAL_CMD_TIMEOUT
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the information on media presence, block size, and last block number
+  // from READ CAPACITY data.
+  //
+  Media->MediaPresent = TRUE;
+  Media->LastBlock    = SwapBytes64 (ReadUnaligned64 ((CONST UINT64 *) &(CapacityData.LastLba7)));
+
+  BlockSize           = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) &(CapacityData.BlockSize3)));
+  
+  Media->LowestAlignedLba = (CapacityData.LowestAlignLogic2 << 8) |
+                             CapacityData.LowestAlignLogic1;
+  Media->LogicalBlocksPerPhysicalBlock  = (1 << CapacityData.LogicPerPhysical);
+  if (BlockSize == 0) {
+    //
+    //  Get sense data  
+    //
+    return UsbBootRequestSense (UsbMass);
+  } else {
+    Media->BlockSize = BlockSize;
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute READ CAPACITY command to request information regarding
+  the capacity of the installed medium of the device.
+
+  This function executes READ CAPACITY command to get the capacity
+  of the USB mass storage media, including the presence, block size,
+  and last block number.
+
+  @param  UsbMass                The device to retireve disk gemotric.
+
+  @retval EFI_SUCCESS            The disk geometry is successfully retrieved.
+  @retval EFI_NOT_READY          The returned block size is zero.
+  @retval Other                  READ CAPACITY command execution failed.
+ 
+**/
+EFI_STATUS
+UsbBootReadCapacity (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  USB_BOOT_READ_CAPACITY_CMD  CapacityCmd;
+  USB_BOOT_READ_CAPACITY_DATA CapacityData;
+  EFI_BLOCK_IO_MEDIA          *Media;
+  EFI_STATUS                  Status;
+  UINT32                      BlockSize;
+
+  Media   = &UsbMass->BlockIoMedia;
+
+  ZeroMem (&CapacityCmd, sizeof (USB_BOOT_READ_CAPACITY_CMD));
+  ZeroMem (&CapacityData, sizeof (USB_BOOT_READ_CAPACITY_DATA));
+
+  CapacityCmd.OpCode = USB_BOOT_READ_CAPACITY_OPCODE;
+  CapacityCmd.Lun    = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+
+  Status = UsbBootExecCmdWithRetry (
+             UsbMass,
+             &CapacityCmd,
+             (UINT8) sizeof (USB_BOOT_READ_CAPACITY_CMD),
+             EfiUsbDataIn,
+             &CapacityData,
+             sizeof (USB_BOOT_READ_CAPACITY_DATA),
+             USB_BOOT_GENERAL_CMD_TIMEOUT
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the information on media presence, block size, and last block number
+  // from READ CAPACITY data.
+  //
+  Media->MediaPresent = TRUE;
+  Media->LastBlock    = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) CapacityData.LastLba));
+
+  BlockSize           = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) CapacityData.BlockLen));
+  if (BlockSize == 0) {
+    //
+    //  Get sense data  
+    //
+    return UsbBootRequestSense (UsbMass);
+  } else {
+    Media->BlockSize = BlockSize;
+  }
+
+  if (Media->LastBlock == 0xFFFFFFFF) {
+    Status = UsbBootReadCapacity16 (UsbMass);
+    if (!EFI_ERROR (Status)) {
+      UsbMass->Cdb16Byte = TRUE;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Retrieves SCSI mode sense information via MODE SENSE(6) command.
+
+  @param  UsbMass                The device whose sense data is requested.
+
+  @retval EFI_SUCCESS            SCSI mode sense information retrieved successfully.
+  @retval Other                  Command execution failed.
+
+**/
+EFI_STATUS
+UsbScsiModeSense (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  EFI_STATUS                       Status;
+  USB_SCSI_MODE_SENSE6_CMD         ModeSenseCmd;
+  USB_SCSI_MODE_SENSE6_PARA_HEADER ModeParaHeader;
+  EFI_BLOCK_IO_MEDIA               *Media;
+
+  Media   = &UsbMass->BlockIoMedia;
+
+  ZeroMem (&ModeSenseCmd, sizeof (USB_SCSI_MODE_SENSE6_CMD));
+  ZeroMem (&ModeParaHeader, sizeof (USB_SCSI_MODE_SENSE6_PARA_HEADER));
+
+  //
+  // MODE SENSE(6) command is defined in Section 8.2.10 of SCSI-2 Spec
+  //
+  ModeSenseCmd.OpCode         = USB_SCSI_MODE_SENSE6_OPCODE;
+  ModeSenseCmd.Lun            = (UINT8) USB_BOOT_LUN (UsbMass->Lun);
+  ModeSenseCmd.PageCode       = 0x3F;
+  ModeSenseCmd.AllocateLen    = (UINT8) sizeof (USB_SCSI_MODE_SENSE6_PARA_HEADER);
+
+  Status = UsbBootExecCmdWithRetry (
+             UsbMass,
+             &ModeSenseCmd,
+             (UINT8) sizeof (USB_SCSI_MODE_SENSE6_CMD),
+             EfiUsbDataIn,
+             &ModeParaHeader,
+             sizeof (USB_SCSI_MODE_SENSE6_PARA_HEADER),
+             USB_BOOT_GENERAL_CMD_TIMEOUT
+             );
+
+  //
+  // Format of device-specific parameter byte of the mode parameter header is defined in
+  // Section 8.2.10 of SCSI-2 Spec.
+  // BIT7 of this byte is indicates whether the medium is write protected.
+  //
+  if (!EFI_ERROR (Status)) {
+    Media->ReadOnly = (BOOLEAN) ((ModeParaHeader.DevicePara & BIT7) != 0);
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the parameters for the USB mass storage media.
+
+  This function get the parameters for the USB mass storage media,
+  It is used both to initialize the media during the Start() phase
+  of Driver Binding Protocol and to re-initialize it when the media is
+  changed. Althought the RemoveableMedia is unlikely to change,
+  it is also included here.
+
+  @param  UsbMass                The device to retrieve disk gemotric.
+
+  @retval EFI_SUCCESS            The disk gemotric is successfully retrieved.
+  @retval Other                  Failed to get the parameters.
+
+**/
+EFI_STATUS
+UsbBootGetParams (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  EFI_BLOCK_IO_MEDIA          *Media;
+  EFI_STATUS                  Status;
+
+  Media  = &(UsbMass->BlockIoMedia);
+
+  Status = UsbBootInquiry (UsbMass);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBootGetParams: UsbBootInquiry (%r)\n", Status));
+    return Status;
+  }
+
+  //
+  // According to USB Mass Storage Specification for Bootability, only following
+  // 4 Peripheral Device Types are in spec.
+  //
+  if ((UsbMass->Pdt != USB_PDT_DIRECT_ACCESS) &&
+       (UsbMass->Pdt != USB_PDT_CDROM) &&
+       (UsbMass->Pdt != USB_PDT_OPTICAL) &&
+       (UsbMass->Pdt != USB_PDT_SIMPLE_DIRECT)) {
+    DEBUG ((EFI_D_ERROR, "UsbBootGetParams: Found an unsupported peripheral type[%d]\n", UsbMass->Pdt));
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Don't use the Removable bit in inquiry data to test whether the media
+  // is removable because many flash disks wrongly set this bit.
+  //
+  if ((UsbMass->Pdt == USB_PDT_CDROM) || (UsbMass->Pdt == USB_PDT_OPTICAL)) {
+    //
+    // CD-Rom device and Non-CD optical device
+    //
+    UsbMass->OpticalStorage = TRUE;
+    //
+    // Default value 2048 Bytes, in case no media present at first time
+    //
+    Media->BlockSize        = 0x0800;
+  }
+
+  Status = UsbBootDetectMedia (UsbMass);
+
+  return Status;
+}
+
+
+/**
+  Detect whether the removable media is present and whether it has changed.
+
+  @param  UsbMass                The device to check.
+
+  @retval EFI_SUCCESS            The media status is successfully checked.
+  @retval Other                  Failed to detect media.
+
+**/
+EFI_STATUS
+UsbBootDetectMedia (
+  IN  USB_MASS_DEVICE       *UsbMass
+  )
+{
+  EFI_BLOCK_IO_MEDIA        OldMedia;
+  EFI_BLOCK_IO_MEDIA        *Media;
+  UINT8                     CmdSet;
+  EFI_TPL                   OldTpl;
+  EFI_STATUS                Status;
+
+  Media    = &UsbMass->BlockIoMedia;
+
+  CopyMem (&OldMedia, &(UsbMass->BlockIoMedia), sizeof (EFI_BLOCK_IO_MEDIA));
+
+  CmdSet = ((EFI_USB_INTERFACE_DESCRIPTOR *) (UsbMass->Context))->InterfaceSubClass;
+
+  Status = UsbBootIsUnitReady (UsbMass);
+  if (EFI_ERROR (Status) && (Status != EFI_MEDIA_CHANGED)) {
+    goto ON_ERROR;
+  }
+
+  if ((UsbMass->Pdt != USB_PDT_CDROM) && (CmdSet == USB_MASS_STORE_SCSI)) {
+    //
+    // MODE SENSE is required for the device with PDT of 0x00/0x07/0x0E,
+    // according to Section 4 of USB Mass Storage Specification for Bootability.
+    // MODE SENSE(10) is useless here, while MODE SENSE(6) defined in SCSI
+    // could get the information of Write Protected.
+    // Since not all device support this command, skip if fail.
+    //
+    UsbScsiModeSense (UsbMass);
+  }
+
+  Status = UsbBootReadCapacity (UsbMass);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBootDetectMedia: UsbBootReadCapacity (%r)\n", Status));
+    goto ON_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  //
+  // Detect whether it is necessary to reinstall the Block I/O Protocol.
+  //
+  // MediaId may change in RequestSense for MediaChanged
+  // MediaPresent may change in RequestSense for NoMedia
+  // MediaReadOnly may change in RequestSense for WriteProtected or MediaChanged
+  // MediaPresent/BlockSize/LastBlock may change in ReadCapacity
+  //
+  if ((Media->MediaId != OldMedia.MediaId) ||
+      (Media->MediaPresent != OldMedia.MediaPresent) ||
+      (Media->ReadOnly != OldMedia.ReadOnly) ||
+      (Media->BlockSize != OldMedia.BlockSize) ||
+      (Media->LastBlock != OldMedia.LastBlock)) {
+
+    //
+    // This function is called by Block I/O Protocol APIs, which run at TPL_NOTIFY.
+    // Here we temporarily restore TPL to TPL_CALLBACK to invoke ReinstallProtocolInterface().
+    //
+    OldTpl = EfiGetCurrentTpl ();
+    gBS->RestoreTPL (TPL_CALLBACK);
+
+    gBS->ReinstallProtocolInterface (
+           UsbMass->Controller,
+           &gEfiBlockIoProtocolGuid,
+           &UsbMass->BlockIo,
+           &UsbMass->BlockIo
+           );
+
+    ASSERT (EfiGetCurrentTpl () == TPL_CALLBACK);
+    gBS->RaiseTPL (OldTpl);
+
+    //
+    // Update MediaId after reinstalling Block I/O Protocol.
+    //
+    if (Media->MediaPresent != OldMedia.MediaPresent) {
+      if (Media->MediaPresent) {
+        Media->MediaId = 1;
+      } else {
+        Media->MediaId = 0;
+      }
+    }
+
+    if ((Media->ReadOnly != OldMedia.ReadOnly) ||
+        (Media->BlockSize != OldMedia.BlockSize) ||
+        (Media->LastBlock != OldMedia.LastBlock)) {
+      Media->MediaId++;
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Read some blocks from the device.
+
+  @param  UsbMass                The USB mass storage device to read from
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to read
+  @param  Buffer                 The buffer to read to
+
+  @retval EFI_SUCCESS            Data are read into the buffer
+  @retval Others                 Failed to read all the data
+
+**/
+EFI_STATUS
+UsbBootReadBlocks (
+  IN  USB_MASS_DEVICE       *UsbMass,
+  IN  UINT32                Lba,
+  IN  UINTN                 TotalBlock,
+  OUT UINT8                 *Buffer
+  )
+{
+  USB_BOOT_READ10_CMD       ReadCmd;
+  EFI_STATUS                Status;
+  UINT16                    Count;
+  UINT32                    BlockSize;
+  UINT32                    ByteSize;
+  UINT32                    Timeout;
+
+  BlockSize = UsbMass->BlockIoMedia.BlockSize;
+  Status    = EFI_SUCCESS;
+
+  while (TotalBlock > 0) {
+    //
+    // Split the total blocks into smaller pieces to ease the pressure
+    // on the device. We must split the total block because the READ10
+    // command only has 16 bit transfer length (in the unit of block).
+    //
+    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);
+    ByteSize  = (UINT32)Count * BlockSize;
+
+    //
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]
+    //
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;
+
+    //
+    // Fill in the command then execute
+    //
+    ZeroMem (&ReadCmd, sizeof (USB_BOOT_READ10_CMD));
+
+    ReadCmd.OpCode  = USB_BOOT_READ10_OPCODE;
+    ReadCmd.Lun     = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+    WriteUnaligned32 ((UINT32 *) ReadCmd.Lba, SwapBytes32 (Lba));
+    WriteUnaligned16 ((UINT16 *) ReadCmd.TransferLen, SwapBytes16 (Count));
+
+    Status = UsbBootExecCmdWithRetry (
+               UsbMass,
+               &ReadCmd,
+               (UINT8) sizeof (USB_BOOT_READ10_CMD),
+               EfiUsbDataIn,
+               Buffer,
+               ByteSize,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    DEBUG ((EFI_D_BLKIO, "UsbBootReadBlocks: LBA (0x%x), Blk (0x%x)\n", Lba, Count));
+    Lba        += Count;
+    Buffer     += Count * BlockSize;
+    TotalBlock -= Count;
+  }
+
+  return Status;
+}
+
+
+/**
+  Write some blocks to the device.
+
+  @param  UsbMass                The USB mass storage device to write to
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to write
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            Data are written into the buffer
+  @retval Others                 Failed to write all the data
+
+**/
+EFI_STATUS
+UsbBootWriteBlocks (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT32                  Lba,
+  IN  UINTN                   TotalBlock,
+  IN  UINT8                   *Buffer
+  )
+{
+  USB_BOOT_WRITE10_CMD  WriteCmd;
+  EFI_STATUS            Status;
+  UINT16                Count;
+  UINT32                BlockSize;
+  UINT32                ByteSize;
+  UINT32                Timeout;
+
+  BlockSize = UsbMass->BlockIoMedia.BlockSize;
+  Status    = EFI_SUCCESS;
+
+  while (TotalBlock > 0) {
+    //
+    // Split the total blocks into smaller pieces to ease the pressure
+    // on the device. We must split the total block because the WRITE10
+    // command only has 16 bit transfer length (in the unit of block).
+    //
+    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);
+    ByteSize  = (UINT32)Count * BlockSize;
+
+    //
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]
+    //
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;
+
+    //
+    // Fill in the write10 command block
+    //
+    ZeroMem (&WriteCmd, sizeof (USB_BOOT_WRITE10_CMD));
+
+    WriteCmd.OpCode = USB_BOOT_WRITE10_OPCODE;
+    WriteCmd.Lun    = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+    WriteUnaligned32 ((UINT32 *) WriteCmd.Lba, SwapBytes32 (Lba));
+    WriteUnaligned16 ((UINT16 *) WriteCmd.TransferLen, SwapBytes16 (Count));
+
+    Status = UsbBootExecCmdWithRetry (
+               UsbMass,
+               &WriteCmd,
+               (UINT8) sizeof (USB_BOOT_WRITE10_CMD),
+               EfiUsbDataOut,
+               Buffer,
+               ByteSize,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    DEBUG ((EFI_D_BLKIO, "UsbBootWriteBlocks: LBA (0x%x), Blk (0x%x)\n", Lba, Count));
+
+    Lba        += Count;
+    Buffer     += Count * BlockSize;
+    TotalBlock -= Count;
+  }
+
+  return Status;
+}
+
+/**
+  Read some blocks from the device by SCSI 16 byte cmd.
+
+  @param  UsbMass                The USB mass storage device to read from
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to read
+  @param  Buffer                 The buffer to read to
+
+  @retval EFI_SUCCESS            Data are read into the buffer
+  @retval Others                 Failed to read all the data
+
+**/
+EFI_STATUS
+UsbBootReadBlocks16 (
+  IN  USB_MASS_DEVICE       *UsbMass,
+  IN  UINT64                Lba,
+  IN  UINTN                 TotalBlock,
+  OUT UINT8                 *Buffer
+  )
+{
+  UINT8                     ReadCmd[16];
+  EFI_STATUS                Status;
+  UINT16                    Count;
+  UINT32                    BlockSize;
+  UINT32                    ByteSize;
+  UINT32                    Timeout;
+
+  BlockSize = UsbMass->BlockIoMedia.BlockSize;
+  Status    = EFI_SUCCESS;
+
+  while (TotalBlock > 0) {
+    //
+    // Split the total blocks into smaller pieces.
+    //
+    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);
+    ByteSize  = (UINT32)Count * BlockSize;
+
+    //
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]
+    //
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;
+
+    //
+    // Fill in the command then execute
+    //
+    ZeroMem (ReadCmd, sizeof (ReadCmd));
+
+    ReadCmd[0]  = EFI_SCSI_OP_READ16;
+    ReadCmd[1]  = (UINT8) ((USB_BOOT_LUN (UsbMass->Lun) & 0xE0));
+    WriteUnaligned64 ((UINT64 *) &ReadCmd[2], SwapBytes64 (Lba));
+    WriteUnaligned32 ((UINT32 *) &ReadCmd[10], SwapBytes32 (Count));
+
+    Status = UsbBootExecCmdWithRetry (
+               UsbMass,
+               ReadCmd,
+               (UINT8) sizeof (ReadCmd),
+               EfiUsbDataIn,
+               Buffer,
+               ByteSize,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    DEBUG ((EFI_D_BLKIO, "UsbBootReadBlocks16: LBA (0x%lx), Blk (0x%x)\n", Lba, Count));
+    Lba        += Count;
+    Buffer     += Count * BlockSize;
+    TotalBlock -= Count;
+  }
+
+  return Status;
+}
+
+
+/**
+  Write some blocks to the device by SCSI 16 byte cmd.
+
+  @param  UsbMass                The USB mass storage device to write to
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to write
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            Data are written into the buffer
+  @retval Others                 Failed to write all the data
+
+**/
+EFI_STATUS
+UsbBootWriteBlocks16 (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT64                  Lba,
+  IN  UINTN                   TotalBlock,
+  IN  UINT8                   *Buffer
+  )
+{
+  UINT8                 WriteCmd[16];
+  EFI_STATUS            Status;
+  UINT16                Count;
+  UINT32                BlockSize;
+  UINT32                ByteSize;
+  UINT32                Timeout;
+
+  BlockSize = UsbMass->BlockIoMedia.BlockSize;
+  Status    = EFI_SUCCESS;
+
+  while (TotalBlock > 0) {
+    //
+    // Split the total blocks into smaller pieces.
+    //
+    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);
+    ByteSize  = (UINT32)Count * BlockSize;
+
+    //
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]
+    //
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;
+
+    //
+    // Fill in the write16 command block
+    //
+    ZeroMem (WriteCmd, sizeof (WriteCmd));
+
+    WriteCmd[0]  = EFI_SCSI_OP_WRITE16;
+    WriteCmd[1]  = (UINT8) ((USB_BOOT_LUN (UsbMass->Lun) & 0xE0));
+    WriteUnaligned64 ((UINT64 *) &WriteCmd[2], SwapBytes64 (Lba));
+    WriteUnaligned32 ((UINT32 *) &WriteCmd[10], SwapBytes32 (Count));
+
+    Status = UsbBootExecCmdWithRetry (
+               UsbMass,
+               WriteCmd,
+               (UINT8) sizeof (WriteCmd),
+               EfiUsbDataOut,
+               Buffer,
+               ByteSize,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    DEBUG ((EFI_D_BLKIO, "UsbBootWriteBlocks: LBA (0x%lx), Blk (0x%x)\n", Lba, Count));
+    Lba        += Count;
+    Buffer     += Count * BlockSize;
+    TotalBlock -= Count;
+  }
+
+  return Status;
+}
+
+/**
+  Use the USB clear feature control transfer to clear the endpoint stall condition.
+
+  @param  UsbIo                  The USB I/O Protocol instance
+  @param  EndpointAddr           The endpoint to clear stall for
+
+  @retval EFI_SUCCESS            The endpoint stall condition is cleared.
+  @retval Others                 Failed to clear the endpoint stall condition.
+
+**/
+EFI_STATUS
+UsbClearEndpointStall (
+  IN EFI_USB_IO_PROTOCOL    *UsbIo,
+  IN UINT8                  EndpointAddr
+  )
+{
+  EFI_USB_DEVICE_REQUEST    Request;
+  EFI_STATUS                Status;
+  UINT32                    CmdResult;
+  UINT32                    Timeout;
+
+  Request.RequestType = 0x02;
+  Request.Request     = USB_REQ_CLEAR_FEATURE;
+  Request.Value       = USB_FEATURE_ENDPOINT_HALT;
+  Request.Index       = EndpointAddr;
+  Request.Length      = 0;
+  Timeout             = USB_BOOT_GENERAL_CMD_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  Status = UsbIo->UsbControlTransfer (
+                    UsbIo,
+                    &Request,
+                    EfiUsbNoData,
+                    Timeout,
+                    NULL,
+                    0,
+                    &CmdResult
+                    );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.h
new file mode 100644
index 0000000000..c4082558fa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.h
@@ -0,0 +1,371 @@
+/** @file
+  Definition of the command set of USB Mass Storage Specification
+  for Bootability, Revision 1.0.
+
+Copyright (c) 2007 - 2012, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_USB_MASS_BOOT_H_
+#define _EFI_USB_MASS_BOOT_H_
+
+//
+// The opcodes of various USB boot commands:
+// INQUIRY/REQUEST_SENSE are "No Timeout Commands" as specified
+// by Multi-Media Commands (MMC) set.
+// Others are "Group 1 Timeout Commands". That is,
+// they should be retried if driver is ready. 
+//
+#define USB_BOOT_INQUIRY_OPCODE         0x12
+#define USB_BOOT_REQUEST_SENSE_OPCODE   0x03
+#define USB_BOOT_MODE_SENSE10_OPCODE    0x5A
+#define USB_BOOT_READ_CAPACITY_OPCODE   0x25
+#define USB_BOOT_TEST_UNIT_READY_OPCODE 0x00
+#define USB_BOOT_READ10_OPCODE          0x28
+#define USB_BOOT_WRITE10_OPCODE         0x2A
+
+#define USB_SCSI_MODE_SENSE6_OPCODE     0x1A
+
+//
+// The Sense Key part of the sense data. Sense data has three levels:
+// Sense key, Additional Sense Code and Additional Sense Code Qualifier
+//
+#define USB_BOOT_SENSE_NO_SENSE         0x00 ///< No sense key
+#define USB_BOOT_SENSE_RECOVERED        0x01 ///< Last command succeed with recovery actions
+#define USB_BOOT_SENSE_NOT_READY        0x02 ///< Device not ready
+#define USB_BOOT_SNESE_MEDIUM_ERROR     0X03 ///< Failed probably because flaw in the media
+#define USB_BOOT_SENSE_HARDWARE_ERROR   0X04 ///< Non-recoverable hardware failure
+#define USB_BOOT_SENSE_ILLEGAL_REQUEST  0X05 ///< Illegal parameters in the request
+#define USB_BOOT_SENSE_UNIT_ATTENTION   0X06 ///< Removable medium may have been changed
+#define USB_BOOT_SENSE_DATA_PROTECT     0X07 ///< Write protected
+#define USB_BOOT_SENSE_BLANK_CHECK      0X08 ///< Blank/non-blank medium while reading/writing
+#define USB_BOOT_SENSE_VENDOR           0X09 ///< Vendor specific sense key
+#define USB_BOOT_SENSE_ABORTED          0X0B ///< Command aborted by the device
+#define USB_BOOT_SENSE_VOLUME_OVERFLOW  0x0D ///< Partition overflow
+#define USB_BOOT_SENSE_MISCOMPARE       0x0E ///< Source data mis-match while verfying.
+
+#define USB_BOOT_ASC_NOT_READY          0x04
+#define USB_BOOT_ASC_NO_MEDIA           0x3A
+#define USB_BOOT_ASC_MEDIA_CHANGE       0x28
+
+//
+// Supported PDT codes, or Peripheral Device Type
+//
+#define USB_PDT_DIRECT_ACCESS           0x00       ///< Direct access device
+#define USB_PDT_CDROM                   0x05       ///< CDROM
+#define USB_PDT_OPTICAL                 0x07       ///< Non-CD optical disks
+#define USB_PDT_SIMPLE_DIRECT           0x0E       ///< Simplified direct access device
+
+//
+// Other parameters, Max carried size is 512B * 128 = 64KB
+//
+#define USB_BOOT_IO_BLOCKS              128
+
+//
+// Retry mass command times, set by experience
+//
+#define USB_BOOT_COMMAND_RETRY          5
+
+//
+// Wait for unit ready command, set by experience
+//
+#define USB_BOOT_RETRY_UNIT_READY_STALL (500 * USB_MASS_1_MILLISECOND)
+
+//
+// Mass command timeout, refers to specification[USB20-9.2.6.1]
+//
+// USB2.0 Spec define the up-limit timeout 5s for all command. USB floppy, 
+// USB CD-Rom and iPod devices are much slower than USB key when reponse 
+// most of commands, So we set 5s as timeout here.
+// 
+#define USB_BOOT_GENERAL_CMD_TIMEOUT    (5 * USB_MASS_1_SECOND)
+
+//
+// The required commands are INQUIRY, READ CAPACITY, TEST UNIT READY,
+// READ10, WRITE10, and REQUEST SENSE. The BLOCK_IO protocol uses LBA
+// so it isn't necessary to issue MODE SENSE / READ FORMAT CAPACITY
+// command to retrieve the disk gemotrics. 
+//
+#pragma pack(1)
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;            ///< Lun (high 3 bits)
+  UINT8             Reserved0[2];
+  UINT8             AllocLen;
+  UINT8             Reserved1;
+  UINT8             Pad[6];
+} USB_BOOT_INQUIRY_CMD;
+
+typedef struct {
+  UINT8             Pdt;            ///< Peripheral Device Type (low 5 bits)
+  UINT8             Removable;      ///< Removable Media (highest bit)
+  UINT8             Reserved0[2];
+  UINT8             AddLen;         ///< Additional length
+  UINT8             Reserved1[3];
+  UINT8             VendorID[8];
+  UINT8             ProductID[16];
+  UINT8             ProductRevision[4];
+} USB_BOOT_INQUIRY_DATA;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             Reserved0[8];
+  UINT8             Pad[2];
+} USB_BOOT_READ_CAPACITY_CMD;
+
+typedef struct {
+  UINT8             LastLba[4];
+  UINT8             BlockLen[4];
+} USB_BOOT_READ_CAPACITY_DATA;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             Reserved[4];
+  UINT8             Pad[6];
+} USB_BOOT_TEST_UNIT_READY_CMD;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             PageCode;
+  UINT8             Reserved0[4];
+  UINT8             ParaListLenMsb;
+  UINT8             ParaListLenLsb;
+  UINT8             Reserved1;
+  UINT8             Pad[2];
+} USB_BOOT_MODE_SENSE10_CMD;
+
+typedef struct {
+  UINT8             ModeDataLenMsb;
+  UINT8             ModeDataLenLsb;
+  UINT8             Reserved0[4];
+  UINT8             BlkDesLenMsb;
+  UINT8             BlkDesLenLsb;
+} USB_BOOT_MODE_SENSE10_PARA_HEADER;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;            ///< Lun (High 3 bits)
+  UINT8             Lba[4];         ///< Logical block address
+  UINT8             Reserved0;
+  UINT8             TransferLen[2]; ///< Transfer length
+  UINT8             Reserverd1;
+  UINT8             Pad[2];
+} USB_BOOT_READ10_CMD;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             Lba[4];
+  UINT8             Reserved0;
+  UINT8             TransferLen[2];
+  UINT8             Reserverd1;
+  UINT8             Pad[2];
+} USB_BOOT_WRITE10_CMD;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;            ///< Lun (High 3 bits)
+  UINT8             Reserved0[2];
+  UINT8             AllocLen;       ///< Allocation length
+  UINT8             Reserved1;
+  UINT8             Pad[6];
+} USB_BOOT_REQUEST_SENSE_CMD;
+
+typedef struct {
+  UINT8             ErrorCode;
+  UINT8             Reserved0;
+  UINT8             SenseKey;       ///< Sense key (low 4 bits)
+  UINT8             Infor[4];
+  UINT8             AddLen;         ///< Additional Sense length, 10
+  UINT8             Reserved1[4];
+  UINT8             Asc;            ///< Additional Sense Code
+  UINT8             Ascq;           ///< Additional Sense Code Qualifier
+  UINT8             Reserverd2[4];
+} USB_BOOT_REQUEST_SENSE_DATA;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             PageCode;
+  UINT8             Reserved0;
+  UINT8             AllocateLen;
+  UINT8             Control;
+} USB_SCSI_MODE_SENSE6_CMD;
+
+typedef struct {
+  UINT8             ModeDataLen;
+  UINT8             MediumType;
+  UINT8             DevicePara;
+  UINT8             BlkDesLen;
+} USB_SCSI_MODE_SENSE6_PARA_HEADER;
+#pragma pack()
+
+//
+// Convert a LUN number to that in the command
+//
+#define USB_BOOT_LUN(Lun) ((Lun) << 5)
+
+//
+// Get the removable, PDT, and sense key bits from the command data
+//
+#define USB_BOOT_REMOVABLE(RmbByte) (((RmbByte) & BIT7) != 0)
+#define USB_BOOT_PDT(Pdt)           ((Pdt) & 0x1f)
+#define USB_BOOT_SENSE_KEY(Key)     ((Key) & 0x0f)
+
+/**
+  Get the parameters for the USB mass storage media.
+
+  This function get the parameters for the USB mass storage media,
+  It is used both to initialize the media during the Start() phase
+  of Driver Binding Protocol and to re-initialize it when the media is
+  changed. Althought the RemoveableMedia is unlikely to change,
+  it is also included here.
+
+  @param  UsbMass                The device to retrieve disk gemotric.
+
+  @retval EFI_SUCCESS            The disk gemotric is successfully retrieved.
+  @retval Other                  Failed to get the parameters.
+
+**/
+EFI_STATUS
+UsbBootGetParams (
+  IN USB_MASS_DEVICE          *UsbMass
+  );
+
+/**
+  Execute TEST UNIT READY command to check if the device is ready.
+
+  @param  UsbMass                The device to test
+
+  @retval EFI_SUCCESS            The device is ready.
+  @retval Others                 Device not ready.
+
+**/
+EFI_STATUS
+UsbBootIsUnitReady (
+  IN USB_MASS_DEVICE          *UsbMass
+  );
+
+/**
+  Detect whether the removable media is present and whether it has changed.
+
+  @param  UsbMass                The device to check.
+
+  @retval EFI_SUCCESS            The media status is successfully checked.
+  @retval Other                  Failed to detect media.
+
+**/
+EFI_STATUS
+UsbBootDetectMedia (
+  IN  USB_MASS_DEVICE       *UsbMass
+  );
+
+/**
+  Read some blocks from the device.
+
+  @param  UsbMass                The USB mass storage device to read from
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to read
+  @param  Buffer                 The buffer to read to
+
+  @retval EFI_SUCCESS            Data are read into the buffer
+  @retval Others                 Failed to read all the data
+
+**/
+EFI_STATUS
+UsbBootReadBlocks (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT32                  Lba,
+  IN  UINTN                   TotalBlock,
+  OUT UINT8                   *Buffer
+  );
+
+/**
+  Write some blocks to the device.
+
+  @param  UsbMass                The USB mass storage device to write to
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to write
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            Data are written into the buffer
+  @retval Others                 Failed to write all the data
+
+**/
+EFI_STATUS
+UsbBootWriteBlocks (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT32                  Lba,
+  IN  UINTN                   TotalBlock,
+  IN  UINT8                   *Buffer
+  );
+
+/**
+  Read some blocks from the device by SCSI 16 byte cmd.
+
+  @param  UsbMass                The USB mass storage device to read from
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to read
+  @param  Buffer                 The buffer to read to
+
+  @retval EFI_SUCCESS            Data are read into the buffer
+  @retval Others                 Failed to read all the data
+
+**/
+EFI_STATUS
+UsbBootReadBlocks16 (
+  IN  USB_MASS_DEVICE       *UsbMass,
+  IN  UINT64                Lba,
+  IN  UINTN                 TotalBlock,
+  OUT UINT8                 *Buffer
+  );
+
+/**
+  Write some blocks to the device by SCSI 16 byte cmd.
+
+  @param  UsbMass                The USB mass storage device to write to
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to write
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            Data are written into the buffer
+  @retval Others                 Failed to write all the data
+
+**/
+EFI_STATUS
+UsbBootWriteBlocks16 (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT64                  Lba,
+  IN  UINTN                   TotalBlock,
+  IN  UINT8                   *Buffer
+  );
+
+
+/**
+  Use the USB clear feature control transfer to clear the endpoint stall condition.
+
+  @param  UsbIo                  The USB I/O Protocol instance
+  @param  EndpointAddr           The endpoint to clear stall for
+
+  @retval EFI_SUCCESS            The endpoint stall condition is cleared.
+  @retval Others                 Failed to clear the endpoint stall condition.
+
+**/
+EFI_STATUS
+UsbClearEndpointStall (
+  IN EFI_USB_IO_PROTOCOL    *UsbIo,
+  IN UINT8                  EndpointAddr
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.c
new file mode 100644
index 0000000000..dd83540285
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.c
@@ -0,0 +1,599 @@
+/** @file
+  Implementation of the USB mass storage Bulk-Only Transport protocol,
+  according to USB Mass Storage Class Bulk-Only Transport, Revision 1.0.
+
+Copyright (c) 2007 - 2011, 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 "UsbMass.h"
+
+//
+// Definition of USB BOT Transport Protocol
+//
+USB_MASS_TRANSPORT mUsbBotTransport = {
+  USB_MASS_STORE_BOT,
+  UsbBotInit,
+  UsbBotExecCommand,
+  UsbBotResetDevice,
+  UsbBotGetMaxLun,
+  UsbBotCleanUp
+};
+
+/**
+  Initializes USB BOT protocol.
+
+  This function initializes the USB mass storage class BOT protocol.
+  It will save its context which is a USB_BOT_PROTOCOL structure
+  in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB BOT initialization fails.
+
+**/
+EFI_STATUS
+UsbBotInit (
+  IN  EFI_USB_IO_PROTOCOL       *UsbIo,
+  OUT VOID                      **Context OPTIONAL
+  )
+{
+  USB_BOT_PROTOCOL              *UsbBot;
+  EFI_USB_INTERFACE_DESCRIPTOR  *Interface;
+  EFI_USB_ENDPOINT_DESCRIPTOR   EndPoint;
+  EFI_STATUS                    Status;
+  UINT8                         Index;
+
+  //
+  // Allocate the BOT context for USB_BOT_PROTOCOL and two endpoint descriptors.
+  //
+  UsbBot = AllocateZeroPool (sizeof (USB_BOT_PROTOCOL) + 2 * sizeof (EFI_USB_ENDPOINT_DESCRIPTOR));
+  ASSERT (UsbBot != NULL);
+
+  UsbBot->UsbIo = UsbIo;
+
+  //
+  // Get the interface descriptor and validate that it
+  // is a USB Mass Storage BOT interface.
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &UsbBot->Interface);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Interface = &UsbBot->Interface;
+
+  if (Interface->InterfaceProtocol != USB_MASS_STORE_BOT) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  //
+  // Locate and save the first bulk-in and bulk-out endpoint
+  //
+  for (Index = 0; Index < Interface->NumEndpoints; Index++) {
+    Status = UsbIo->UsbGetEndpointDescriptor (UsbIo, Index, &EndPoint);
+
+    if (EFI_ERROR (Status) || !USB_IS_BULK_ENDPOINT (EndPoint.Attributes)) {
+      continue;
+    }
+
+    if (USB_IS_IN_ENDPOINT (EndPoint.EndpointAddress) &&
+       (UsbBot->BulkInEndpoint == NULL)) {
+
+      UsbBot->BulkInEndpoint  = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbBot + 1);
+      CopyMem(UsbBot->BulkInEndpoint, &EndPoint, sizeof (EndPoint));
+    }
+
+    if (USB_IS_OUT_ENDPOINT (EndPoint.EndpointAddress) &&
+       (UsbBot->BulkOutEndpoint == NULL)) {
+
+      UsbBot->BulkOutEndpoint   = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbBot + 1) + 1;
+      CopyMem (UsbBot->BulkOutEndpoint, &EndPoint, sizeof(EndPoint));
+    }
+  }
+
+  //
+  // If bulk-in or bulk-out endpoint is not found, report error.
+  //
+  if ((UsbBot->BulkInEndpoint == NULL) || (UsbBot->BulkOutEndpoint == NULL)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  //
+  // The USB BOT protocol uses CBWTag to match the CBW and CSW.
+  //
+  UsbBot->CbwTag = 0x01;
+
+  if (Context != NULL) {
+    *Context = UsbBot;
+  } else {
+    FreePool (UsbBot);
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  FreePool (UsbBot);
+  return Status;
+}
+
+/**
+  Send the command to the device using Bulk-Out endpoint.
+
+  This function sends the command to the device using Bulk-Out endpoint.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Command phase.
+
+  @param  UsbBot                The USB BOT device
+  @param  Cmd                   The command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  DataDir               The direction of the data
+  @param  TransLen              The expected length of the data
+  @param  Lun                   The number of logic unit
+
+  @retval EFI_SUCCESS           The command is sent to the device.
+  @retval EFI_NOT_READY         The device return NAK to the transfer
+  @retval Others                Failed to send the command to device
+
+**/
+EFI_STATUS
+UsbBotSendCommand (
+  IN USB_BOT_PROTOCOL         *UsbBot,
+  IN UINT8                    *Cmd,
+  IN UINT8                    CmdLen,
+  IN EFI_USB_DATA_DIRECTION   DataDir,
+  IN UINT32                   TransLen,
+  IN UINT8                    Lun
+  )
+{
+  USB_BOT_CBW               Cbw;
+  EFI_STATUS                Status;
+  UINT32                    Result;
+  UINTN                     DataLen;
+  UINTN                     Timeout;
+
+  ASSERT ((CmdLen > 0) && (CmdLen <= USB_BOT_MAX_CMDLEN));
+
+  //
+  // Fill in the Command Block Wrapper.
+  //
+  Cbw.Signature = USB_BOT_CBW_SIGNATURE;
+  Cbw.Tag       = UsbBot->CbwTag;
+  Cbw.DataLen   = TransLen;
+  Cbw.Flag      = (UINT8) ((DataDir == EfiUsbDataIn) ? BIT7 : 0);
+  Cbw.Lun       = Lun;
+  Cbw.CmdLen    = CmdLen;
+
+  ZeroMem (Cbw.CmdBlock, USB_BOT_MAX_CMDLEN);
+  CopyMem (Cbw.CmdBlock, Cmd, CmdLen);
+
+  Result  = 0;
+  DataLen = sizeof (USB_BOT_CBW);
+  Timeout = USB_BOT_SEND_CBW_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  //
+  // Use USB I/O Protocol to send the Command Block Wrapper to the device.
+  //
+  Status = UsbBot->UsbIo->UsbBulkTransfer (
+                            UsbBot->UsbIo,
+                            UsbBot->BulkOutEndpoint->EndpointAddress,
+                            &Cbw,
+                            &DataLen,
+                            Timeout,
+                            &Result
+                            );
+  if (EFI_ERROR (Status)) {
+    if (USB_IS_ERROR (Result, EFI_USB_ERR_STALL) && DataDir == EfiUsbDataOut) {
+      //
+      // Respond to Bulk-Out endpoint stall with a Reset Recovery,
+      // according to section 5.3.1 of USB Mass Storage Class Bulk-Only Transport Spec, v1.0.
+      //
+      UsbBotResetDevice (UsbBot, FALSE);
+    } else if (USB_IS_ERROR (Result, EFI_USB_ERR_NAK)) {
+      Status = EFI_NOT_READY;
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Transfer the data between the device and host.
+
+  This function transfers the data between the device and host.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Data phase.
+
+  @param  UsbBot                The USB BOT device
+  @param  DataDir               The direction of the data
+  @param  Data                  The buffer to hold data
+  @param  TransLen              The expected length of the data
+  @param  Timeout               The time to wait the command to complete
+
+  @retval EFI_SUCCESS           The data is transferred
+  @retval EFI_SUCCESS           No data to transfer
+  @retval EFI_NOT_READY         The device return NAK to the transfer
+  @retval Others                Failed to transfer data
+
+**/
+EFI_STATUS
+UsbBotDataTransfer (
+  IN USB_BOT_PROTOCOL         *UsbBot,
+  IN EFI_USB_DATA_DIRECTION   DataDir,
+  IN OUT UINT8                *Data,
+  IN OUT UINTN                *TransLen,
+  IN UINT32                   Timeout
+  )
+{
+  EFI_USB_ENDPOINT_DESCRIPTOR *Endpoint;
+  EFI_STATUS                  Status;
+  UINT32                      Result;
+
+  //
+  // If no data to transfer, just return EFI_SUCCESS.
+  //
+  if ((DataDir == EfiUsbNoData) || (*TransLen == 0)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Select the endpoint then issue the transfer
+  //
+  if (DataDir == EfiUsbDataIn) {
+    Endpoint = UsbBot->BulkInEndpoint;
+  } else {
+    Endpoint = UsbBot->BulkOutEndpoint;
+  }
+
+  Result  = 0;
+  Timeout = Timeout / USB_MASS_1_MILLISECOND;
+
+  Status = UsbBot->UsbIo->UsbBulkTransfer (
+                            UsbBot->UsbIo,
+                            Endpoint->EndpointAddress,
+                            Data,
+                            TransLen,
+                            Timeout,
+                            &Result
+                            );
+  if (EFI_ERROR (Status)) {
+    if (USB_IS_ERROR (Result, EFI_USB_ERR_STALL)) {
+      DEBUG ((EFI_D_INFO, "UsbBotDataTransfer: (%r)\n", Status));      
+      DEBUG ((EFI_D_INFO, "UsbBotDataTransfer: DataIn Stall\n"));
+      UsbClearEndpointStall (UsbBot->UsbIo, Endpoint->EndpointAddress);
+    } else if (USB_IS_ERROR (Result, EFI_USB_ERR_NAK)) {
+      Status = EFI_NOT_READY;
+    } else {
+      DEBUG ((EFI_D_ERROR, "UsbBotDataTransfer: (%r)\n", Status));
+    }
+    if(Status == EFI_TIMEOUT){
+      UsbBotResetDevice(UsbBot, FALSE);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the command execution status from device.
+
+  This function gets the command execution status from device.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Status phase.
+
+  This function returns the transfer status of the BOT's CSW status,
+  and returns the high level command execution result in Result. So
+  even if EFI_SUCCESS is returned, the command may still have failed.
+
+  @param  UsbBot         The USB BOT device.
+  @param  TransLen       The expected length of the data.
+  @param  CmdStatus      The result of the command execution.
+
+  @retval EFI_SUCCESS    Command execute result is retrieved and in the Result.
+  @retval Other          Error occurred when trying to get status.
+
+**/
+EFI_STATUS
+UsbBotGetStatus (
+  IN  USB_BOT_PROTOCOL      *UsbBot,
+  IN  UINT32                TransLen,
+  OUT UINT8                 *CmdStatus
+  )
+{
+  USB_BOT_CSW               Csw;
+  UINTN                     Len;
+  UINT8                     Endpoint;
+  EFI_STATUS                Status;
+  UINT32                    Result;
+  EFI_USB_IO_PROTOCOL       *UsbIo;
+  UINT32                    Index;
+  UINTN                     Timeout;
+
+  *CmdStatus = USB_BOT_COMMAND_ERROR;
+  Status     = EFI_DEVICE_ERROR;
+  Endpoint   = UsbBot->BulkInEndpoint->EndpointAddress;
+  UsbIo      = UsbBot->UsbIo;
+  Timeout    = USB_BOT_RECV_CSW_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  for (Index = 0; Index < USB_BOT_RECV_CSW_RETRY; Index++) {
+    //
+    // Attemp to the read Command Status Wrapper from bulk in endpoint
+    //
+    ZeroMem (&Csw, sizeof (USB_BOT_CSW));
+    Result = 0;
+    Len    = sizeof (USB_BOT_CSW);
+    Status = UsbIo->UsbBulkTransfer (
+                      UsbIo,
+                      Endpoint,
+                      &Csw,
+                      &Len,
+                      Timeout,
+                      &Result
+                      );
+    if (EFI_ERROR(Status)) {
+      if (USB_IS_ERROR (Result, EFI_USB_ERR_STALL)) {
+        UsbClearEndpointStall (UsbIo, Endpoint);
+      }
+      continue;
+    }
+
+    if (Csw.Signature != USB_BOT_CSW_SIGNATURE) {
+      //
+      // CSW is invalid, so perform reset recovery
+      //
+      Status = UsbBotResetDevice (UsbBot, FALSE);
+    } else if (Csw.CmdStatus == USB_BOT_COMMAND_ERROR) {
+      //
+      // Respond phase error also needs reset recovery
+      //
+      Status = UsbBotResetDevice (UsbBot, FALSE);
+    } else {
+      *CmdStatus = Csw.CmdStatus;
+      break;
+    }
+  }
+  //
+  //The tag is increased even if there is an error.
+  //
+  UsbBot->CbwTag++;
+
+  return Status;
+}
+
+
+/**
+  Call the USB Mass Storage Class BOT protocol to issue
+  the command/data/status circle to execute the commands.
+
+  @param  Context               The context of the BOT protocol, that is,
+                                USB_BOT_PROTOCOL
+  @param  Cmd                   The high level command
+  @param  CmdLen                The command length
+  @param  DataDir               The direction of the data transfer
+  @param  Data                  The buffer to hold data
+  @param  DataLen               The length of the data
+  @param  Lun                   The number of logic unit
+  @param  Timeout               The time to wait command
+  @param  CmdStatus             The result of high level command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to excute command
+
+**/
+EFI_STATUS
+UsbBotExecCommand (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  )
+{
+  USB_BOT_PROTOCOL          *UsbBot;
+  EFI_STATUS                Status;
+  UINTN                     TransLen;
+  UINT8                     Result;
+
+  *CmdStatus  = USB_MASS_CMD_FAIL;
+  UsbBot      = (USB_BOT_PROTOCOL *) Context;
+
+  //
+  // Send the command to the device. Return immediately if device
+  // rejects the command.
+  //
+  Status = UsbBotSendCommand (UsbBot, Cmd, CmdLen, DataDir, DataLen, Lun);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBotExecCommand: UsbBotSendCommand (%r)\n", Status));
+    return Status;
+  }
+
+  //
+  // Transfer the data. Don't return immediately even data transfer
+  // failed. The host should attempt to receive the CSW no matter
+  // whether it succeeds or fails.
+  //
+  TransLen = (UINTN) DataLen;
+  UsbBotDataTransfer (UsbBot, DataDir, Data, &TransLen, Timeout);
+
+  //
+  // Get the status, if that succeeds, interpret the result
+  //
+  Status = UsbBotGetStatus (UsbBot, DataLen, &Result);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBotExecCommand: UsbBotGetStatus (%r)\n", Status));
+    return Status;
+  }
+
+  if (Result == 0) {
+    *CmdStatus = USB_MASS_CMD_SUCCESS;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset the USB mass storage device by BOT protocol.
+
+  @param  Context               The context of the BOT protocol, that is,
+                                USB_BOT_PROTOCOL.
+  @param  ExtendedVerification  If FALSE, just issue Bulk-Only Mass Storage Reset request.
+                                If TRUE, additionally reset parent hub port.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device..
+
+**/
+EFI_STATUS
+UsbBotResetDevice (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  USB_BOT_PROTOCOL        *UsbBot;
+  EFI_USB_DEVICE_REQUEST  Request;
+  EFI_STATUS              Status;
+  UINT32                  Result;
+  UINT32                  Timeout;
+
+  UsbBot = (USB_BOT_PROTOCOL *) Context;
+
+  if (ExtendedVerification) {
+    //
+    // If we need to do strictly reset, reset its parent hub port
+    //
+    Status = UsbBot->UsbIo->UsbPortReset (UsbBot->UsbIo);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Issue a class specific Bulk-Only Mass Storage Reset request,
+  // according to section 3.1 of USB Mass Storage Class Bulk-Only Transport Spec, v1.0.
+  //
+  Request.RequestType = 0x21;
+  Request.Request     = USB_BOT_RESET_REQUEST;
+  Request.Value       = 0;
+  Request.Index       = UsbBot->Interface.InterfaceNumber;
+  Request.Length      = 0;
+  Timeout             = USB_BOT_RESET_DEVICE_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  Status = UsbBot->UsbIo->UsbControlTransfer (
+                            UsbBot->UsbIo,
+                            &Request,
+                            EfiUsbNoData,
+                            Timeout,
+                            NULL,
+                            0,
+                            &Result
+                            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // The device shall NAK the host's request until the reset is
+  // complete. We can use this to sync the device and host. For
+  // now just stall 100ms to wait for the device.
+  //
+  gBS->Stall (USB_BOT_RESET_DEVICE_STALL);
+
+  //
+  // Clear the Bulk-In and Bulk-Out stall condition.
+  //
+  UsbClearEndpointStall (UsbBot->UsbIo, UsbBot->BulkInEndpoint->EndpointAddress);
+  UsbClearEndpointStall (UsbBot->UsbIo, UsbBot->BulkOutEndpoint->EndpointAddress);
+
+  return Status;
+}
+
+
+/**
+  Get the max LUN (Logical Unit Number) of USB mass storage device.
+
+  @param  Context          The context of the BOT protocol, that is, USB_BOT_PROTOCOL
+  @param  MaxLun           Return pointer to the max number of LUN. (e.g. MaxLun=1 means LUN0 and
+                           LUN1 in all.)
+
+  @retval EFI_SUCCESS      Max LUN is got successfully.
+  @retval Others           Fail to execute this request.
+
+**/
+EFI_STATUS
+UsbBotGetMaxLun (
+  IN  VOID                    *Context,
+  OUT UINT8                   *MaxLun
+  )
+{
+  USB_BOT_PROTOCOL        *UsbBot;
+  EFI_USB_DEVICE_REQUEST  Request;
+  EFI_STATUS              Status;
+  UINT32                  Result;
+  UINT32                  Timeout;
+
+  ASSERT (Context);
+  
+  UsbBot = (USB_BOT_PROTOCOL *) Context;
+
+  //
+  // Issue a class specific Bulk-Only Mass Storage get max lun reqest.
+  // according to section 3.2 of USB Mass Storage Class Bulk-Only Transport Spec, v1.0.
+  //
+  Request.RequestType = 0xA1;
+  Request.Request     = USB_BOT_GETLUN_REQUEST;
+  Request.Value       = 0;
+  Request.Index       = UsbBot->Interface.InterfaceNumber;
+  Request.Length      = 1;
+  Timeout             = USB_BOT_RESET_DEVICE_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  Status = UsbBot->UsbIo->UsbControlTransfer (
+                            UsbBot->UsbIo,
+                            &Request,
+                            EfiUsbDataIn,
+                            Timeout,
+                            (VOID *) MaxLun,
+                            1,
+                            &Result
+                            );
+
+  return Status;
+}
+
+/**
+  Clean up the resource used by this BOT protocol.
+
+  @param  Context         The context of the BOT protocol, that is, USB_BOT_PROTOCOL.
+
+  @retval EFI_SUCCESS     The resource is cleaned up.
+
+**/
+EFI_STATUS
+UsbBotCleanUp (
+  IN  VOID                    *Context
+  )
+{
+  FreePool (Context);
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.h
new file mode 100644
index 0000000000..bdbddf559b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.h
@@ -0,0 +1,193 @@
+/** @file
+  Definition for the USB mass storage Bulk-Only Transport protocol,
+  based on the "Universal Serial Bus Mass Storage Class Bulk-Only
+  Transport" Revision 1.0, September 31, 1999.
+
+Copyright (c) 2007 - 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_USBMASS_BOT_H_
+#define _EFI_USBMASS_BOT_H_
+
+extern USB_MASS_TRANSPORT mUsbBotTransport;
+
+//
+// Usb Bulk-Only class specfic request
+//
+#define USB_BOT_RESET_REQUEST    0xFF       ///< Bulk-Only Mass Storage Reset
+#define USB_BOT_GETLUN_REQUEST   0xFE       ///< Get Max Lun
+#define USB_BOT_CBW_SIGNATURE    0x43425355 ///< dCBWSignature, tag the packet as CBW
+#define USB_BOT_CSW_SIGNATURE    0x53425355 ///< dCSWSignature, tag the packet as CSW
+#define USB_BOT_MAX_LUN          0x0F       ///< Lun number is from 0 to 15
+#define USB_BOT_MAX_CMDLEN       16         ///< Maxium number of command from command set
+
+//
+// Usb BOT command block status values
+//
+#define USB_BOT_COMMAND_OK       0x00 ///< Command passed, good status
+#define USB_BOT_COMMAND_FAILED   0x01 ///< Command failed
+#define USB_BOT_COMMAND_ERROR    0x02 ///< Phase error, need to reset the device
+
+//
+// Usb Bot retry to get CSW, refers to specification[BOT10-5.3, it says 2 times]
+//
+#define USB_BOT_RECV_CSW_RETRY   3
+
+//
+// Usb Bot wait device reset complete, set by experience
+//  
+#define USB_BOT_RESET_DEVICE_STALL  (100 * USB_MASS_1_MILLISECOND)
+
+//
+// Usb Bot transport timeout, set by experience
+//
+#define USB_BOT_SEND_CBW_TIMEOUT     (3 * USB_MASS_1_SECOND)
+#define USB_BOT_RECV_CSW_TIMEOUT     (3 * USB_MASS_1_SECOND)
+#define USB_BOT_RESET_DEVICE_TIMEOUT (3 * USB_MASS_1_SECOND)
+
+#pragma pack(1)
+///
+/// The CBW (Command Block Wrapper) structures used by the USB BOT protocol.
+///
+typedef struct {
+  UINT32              Signature;
+  UINT32              Tag;
+  UINT32              DataLen;  ///< Length of data between CBW and CSW
+  UINT8               Flag;     ///< Bit 7, 0 ~ Data-Out, 1 ~ Data-In
+  UINT8               Lun;      ///< Lun number. Bits 0~3 are used
+  UINT8               CmdLen;   ///< Length of the command. Bits 0~4 are used
+  UINT8               CmdBlock[USB_BOT_MAX_CMDLEN];
+} USB_BOT_CBW;
+
+///
+/// The and CSW (Command Status Wrapper) structures used by the USB BOT protocol.
+///
+typedef struct {
+  UINT32              Signature;
+  UINT32              Tag;
+  UINT32              DataResidue;
+  UINT8               CmdStatus;
+} USB_BOT_CSW;
+#pragma pack()
+
+typedef struct {
+  //
+  // Put Interface at the first field to make it easy to distinguish BOT/CBI Protocol instance
+  //
+  EFI_USB_INTERFACE_DESCRIPTOR  Interface;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *BulkInEndpoint;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *BulkOutEndpoint;
+  UINT32                        CbwTag;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+} USB_BOT_PROTOCOL;
+
+/**
+  Initializes USB BOT protocol.
+
+  This function initializes the USB mass storage class BOT protocol.
+  It will save its context which is a USB_BOT_PROTOCOL structure
+  in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB BOT initialization fails.
+
+**/
+EFI_STATUS
+UsbBotInit (
+  IN  EFI_USB_IO_PROTOCOL       *UsbIo,
+  OUT VOID                      **Context OPTIONAL
+  );
+
+/**
+  Call the USB Mass Storage Class BOT protocol to issue
+  the command/data/status circle to execute the commands.
+
+  @param  Context               The context of the BOT protocol, that is,
+                                USB_BOT_PROTOCOL
+  @param  Cmd                   The high level command
+  @param  CmdLen                The command length
+  @param  DataDir               The direction of the data transfer
+  @param  Data                  The buffer to hold data
+  @param  DataLen               The length of the data
+  @param  Lun                   The number of logic unit
+  @param  Timeout               The time to wait command
+  @param  CmdStatus             The result of high level command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to excute command
+
+**/
+EFI_STATUS
+UsbBotExecCommand (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  );
+
+/**
+  Reset the USB mass storage device by BOT protocol.
+
+  @param  Context               The context of the BOT protocol, that is,
+                                USB_BOT_PROTOCOL.
+  @param  ExtendedVerification  If FALSE, just issue Bulk-Only Mass Storage Reset request.
+                                If TRUE, additionally reset parent hub port.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device..
+
+**/
+EFI_STATUS
+UsbBotResetDevice (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  Get the max LUN (Logical Unit Number) of USB mass storage device.
+
+  @param  Context          The context of the BOT protocol, that is, USB_BOT_PROTOCOL
+  @param  MaxLun           Return pointer to the max number of LUN. (e.g. MaxLun=1 means LUN0 and
+                           LUN1 in all.)
+
+  @retval EFI_SUCCESS      Max LUN is got successfully.
+  @retval Others           Fail to execute this request.
+
+**/
+EFI_STATUS
+UsbBotGetMaxLun (
+  IN  VOID                    *Context,
+  OUT UINT8                   *MaxLun
+  );
+
+/**
+  Clean up the resource used by this BOT protocol.
+
+  @param  Context         The context of the BOT protocol, that is, USB_BOT_PROTOCOL.
+
+  @retval EFI_SUCCESS     The resource is cleaned up.
+
+**/
+EFI_STATUS
+UsbBotCleanUp (
+  IN  VOID                    *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.c
new file mode 100644
index 0000000000..3f6201d053
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.c
@@ -0,0 +1,612 @@
+/** @file
+  Implementation of the USB mass storage Control/Bulk/Interrupt transport,
+  according to USB Mass Storage Class Control/Bulk/Interrupt (CBI) Transport, Revision 1.1.
+  Notice: it is being obsoleted by the standard body in favor of the BOT
+  (Bulk-Only Transport).
+
+Copyright (c) 2007 - 2011, 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 "UsbMass.h"
+
+//
+// Definition of USB CBI0 Transport Protocol
+//
+USB_MASS_TRANSPORT mUsbCbi0Transport = {
+  USB_MASS_STORE_CBI0,
+  UsbCbiInit,
+  UsbCbiExecCommand,
+  UsbCbiResetDevice,
+  NULL,
+  UsbCbiCleanUp
+};
+
+//
+// Definition of USB CBI1 Transport Protocol
+//
+USB_MASS_TRANSPORT mUsbCbi1Transport = {
+  USB_MASS_STORE_CBI1,
+  UsbCbiInit,
+  UsbCbiExecCommand,
+  UsbCbiResetDevice,
+  NULL,
+  UsbCbiCleanUp
+};
+
+/**
+  Initializes USB CBI protocol.
+
+  This function initializes the USB mass storage class CBI protocol.
+  It will save its context which is a USB_CBI_PROTOCOL structure
+  in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB CBI initialization fails.
+
+**/
+EFI_STATUS
+UsbCbiInit (
+  IN  EFI_USB_IO_PROTOCOL   *UsbIo,
+  OUT VOID                  **Context       OPTIONAL
+  )
+{
+  USB_CBI_PROTOCOL              *UsbCbi;
+  EFI_USB_INTERFACE_DESCRIPTOR  *Interface;
+  EFI_USB_ENDPOINT_DESCRIPTOR   EndPoint;
+  EFI_STATUS                    Status;
+  UINT8                         Index;
+
+  //
+  // Allocate the CBI context for USB_CBI_PROTOCOL and 3 endpoint descriptors.
+  //
+  UsbCbi = AllocateZeroPool (
+             sizeof (USB_CBI_PROTOCOL) + 3 * sizeof (EFI_USB_ENDPOINT_DESCRIPTOR)
+             );
+  ASSERT (UsbCbi != NULL);
+
+  UsbCbi->UsbIo = UsbIo;
+
+  //
+  // Get the interface descriptor and validate that it
+  // is a USB Mass Storage CBI interface.
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &UsbCbi->Interface);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Interface = &UsbCbi->Interface;
+  if ((Interface->InterfaceProtocol != USB_MASS_STORE_CBI0)
+      && (Interface->InterfaceProtocol != USB_MASS_STORE_CBI1)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  //
+  // Locate and save the bulk-in, bulk-out, and interrupt endpoint
+  //
+  for (Index = 0; Index < Interface->NumEndpoints; Index++) {
+    Status = UsbIo->UsbGetEndpointDescriptor (UsbIo, Index, &EndPoint);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if (USB_IS_BULK_ENDPOINT (EndPoint.Attributes)) {
+      //
+      // Use the first Bulk-In and Bulk-Out endpoints
+      //
+      if (USB_IS_IN_ENDPOINT (EndPoint.EndpointAddress) &&
+         (UsbCbi->BulkInEndpoint == NULL)) {
+
+        UsbCbi->BulkInEndpoint  = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbCbi + 1);
+        CopyMem(UsbCbi->BulkInEndpoint, &EndPoint, sizeof (EndPoint));;
+      }
+
+      if (USB_IS_OUT_ENDPOINT (EndPoint.EndpointAddress) &&
+         (UsbCbi->BulkOutEndpoint == NULL)) {
+
+        UsbCbi->BulkOutEndpoint   = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbCbi + 1) + 1;
+        CopyMem(UsbCbi->BulkOutEndpoint, &EndPoint, sizeof (EndPoint));
+      }
+    } else if (USB_IS_INTERRUPT_ENDPOINT (EndPoint.Attributes)) {
+      //
+      // Use the first interrupt endpoint if it is CBI0
+      //
+      if ((Interface->InterfaceProtocol == USB_MASS_STORE_CBI0) &&
+          (UsbCbi->InterruptEndpoint == NULL)) {
+
+        UsbCbi->InterruptEndpoint   = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbCbi + 1) + 2;
+        CopyMem(UsbCbi->InterruptEndpoint, &EndPoint, sizeof (EndPoint));
+      }
+    }
+  }
+
+  if ((UsbCbi->BulkInEndpoint == NULL) || (UsbCbi->BulkOutEndpoint == NULL)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+  if ((Interface->InterfaceProtocol == USB_MASS_STORE_CBI0) && (UsbCbi->InterruptEndpoint == NULL)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  if (Context != NULL) {
+    *Context = UsbCbi;
+  } else {
+    FreePool (UsbCbi);
+  }
+ 
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  FreePool (UsbCbi);
+  return Status;
+}
+
+/**
+  Send the command to the device using class specific control transfer.
+
+  This function sends command to the device using class specific control transfer.
+  The CBI contains three phases: Command, Data, and Status. This is Command phase.
+
+  @param  UsbCbi                The USB CBI protocol
+  @param  Cmd                   The high level command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  Timeout               The time to wait the command to finish
+
+  @retval EFI_SUCCESS           The command is sent to the device.
+  @retval Others                The command failed to transfer to device
+
+**/
+EFI_STATUS
+UsbCbiSendCommand (
+  IN USB_CBI_PROTOCOL       *UsbCbi,
+  IN UINT8                  *Cmd,
+  IN UINT8                  CmdLen,
+  IN UINT32                 Timeout
+  )
+{
+  EFI_USB_DEVICE_REQUEST  Request;
+  EFI_STATUS              Status;
+  UINT32                  TransStatus;
+  UINTN                   DataLen;
+  INTN                    Retry;
+
+  //
+  // Fill in the device request, CBI use the "Accept Device-Specific
+  // Cmd" (ADSC) class specific request to send commands.
+  //
+  Request.RequestType = 0x21;
+  Request.Request     = 0;
+  Request.Value       = 0;
+  Request.Index       = UsbCbi->Interface.InterfaceNumber;
+  Request.Length      = CmdLen;
+
+  Status              = EFI_SUCCESS;
+  Timeout             = Timeout / USB_MASS_1_MILLISECOND;
+
+  for (Retry = 0; Retry < USB_CBI_MAX_RETRY; Retry++) {
+    //
+    // Use USB I/O Protocol to send the command to the device
+    //
+    TransStatus = 0;
+    DataLen     = CmdLen;
+
+    Status = UsbCbi->UsbIo->UsbControlTransfer (
+                              UsbCbi->UsbIo,
+                              &Request,
+                              EfiUsbDataOut,
+                              Timeout,
+                              Cmd,
+                              DataLen,
+                              &TransStatus
+                              );
+    //
+    // The device can fail the command by STALL the control endpoint.
+    // It can delay the command by NAK the data or status stage, this
+    // is a "class-specific exemption to the USB specification". Retry
+    // if the command is NAKed.
+    //
+    if (EFI_ERROR (Status) && (TransStatus == EFI_USB_ERR_NAK)) {
+      continue;
+    }
+
+    break;
+  }
+
+  return Status;
+}
+
+
+/**
+  Transfer data between the device and host.
+
+  This function transfers data between the device and host.
+  The CBI contains three phases: Command, Data, and Status. This is Data phase.
+
+  @param  UsbCbi                The USB CBI device
+  @param  DataDir               The direction of the data transfer
+  @param  Data                  The buffer to hold the data for input or output.
+  @param  TransLen              On input, the expected transfer length.
+                                On output, the length of data actually transferred.
+  @param  Timeout               The time to wait for the command to execute
+
+  @retval EFI_SUCCESS           The data transferred successfully.
+  @retval EFI_SUCCESS           No data to transfer
+  @retval Others                Failed to transfer all the data
+
+**/
+EFI_STATUS
+UsbCbiDataTransfer (
+  IN USB_CBI_PROTOCOL         *UsbCbi,
+  IN EFI_USB_DATA_DIRECTION   DataDir,
+  IN OUT UINT8                *Data,
+  IN OUT UINTN                *TransLen,
+  IN UINT32                   Timeout
+  )
+{
+  EFI_USB_ENDPOINT_DESCRIPTOR *Endpoint;
+  EFI_STATUS                  Status;
+  UINT32                      TransStatus;
+  UINTN                       Remain;
+  UINTN                       Increment;
+  UINT8                       *Next;
+  UINTN                       Retry;
+
+  //
+  // If no data to transfer, just return EFI_SUCCESS.
+  //
+  if ((DataDir == EfiUsbNoData) || (*TransLen == 0)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Select the endpoint then issue the transfer
+  //
+  if (DataDir == EfiUsbDataIn) {
+    Endpoint = UsbCbi->BulkInEndpoint;
+  } else {
+    Endpoint = UsbCbi->BulkOutEndpoint;
+  }
+
+  Next    = Data;
+  Remain  = *TransLen;
+  Retry   = 0;
+  Status  = EFI_SUCCESS;
+  Timeout = Timeout / USB_MASS_1_MILLISECOND;
+
+  //
+  // Transfer the data with a loop. The length of data transferred once is restricted.
+  //
+  while (Remain > 0) {
+    TransStatus = 0;
+
+    if (Remain > (UINTN) USB_CBI_MAX_PACKET_NUM * Endpoint->MaxPacketSize) {
+      Increment = USB_CBI_MAX_PACKET_NUM * Endpoint->MaxPacketSize;
+    } else {
+      Increment = Remain;
+    }
+
+    Status = UsbCbi->UsbIo->UsbBulkTransfer (
+                              UsbCbi->UsbIo,
+                              Endpoint->EndpointAddress,
+                              Next,
+                              &Increment,
+                              Timeout,
+                              &TransStatus
+                              );
+    if (EFI_ERROR (Status)) {
+      if (TransStatus == EFI_USB_ERR_NAK) {
+        //
+        // The device can NAK the host if either the data/buffer isn't
+        // aviable or the command is in-progress.
+        // If data are partially transferred, we just ignore NAK and continue.
+        // If all data have been transferred and status is NAK, then we retry for several times.
+        // If retry exceeds the USB_CBI_MAX_RETRY, then return error status.
+        //
+        if (Increment == 0) {
+          if (++Retry > USB_CBI_MAX_RETRY) {
+            goto ON_EXIT;
+          }
+        } else {
+          Next   += Increment;
+          Remain -= Increment;
+          Retry   = 0;
+        }
+
+        continue;
+      }
+
+      //
+      // The device can fail the command by STALL the bulk endpoint.
+      // Clear the stall if that is the case.
+      //
+      if (TransStatus == EFI_USB_ERR_STALL) {
+        UsbClearEndpointStall (UsbCbi->UsbIo, Endpoint->EndpointAddress);
+      }
+
+      goto ON_EXIT;
+    }
+
+    Next += Increment;
+    Remain -= Increment;
+  }
+
+ON_EXIT:
+  *TransLen -= Remain;
+  return Status;
+}
+
+
+/**
+  Gets the result of high level command execution from interrupt endpoint.
+
+  This function returns the USB transfer status, and put the high level
+  command execution result in Result.
+  The CBI contains three phases: Command, Data, and Status. This is Status phase.
+
+  @param  UsbCbi                The USB CBI protocol
+  @param  Timeout               The time to wait for the command to execute
+  @param  Result                The result of the command execution.
+
+  @retval EFI_SUCCESS           The high level command execution result is
+                                retrieved in Result.
+  @retval Others                Failed to retrieve the result.
+
+**/
+EFI_STATUS
+UsbCbiGetStatus (
+  IN  USB_CBI_PROTOCOL        *UsbCbi,
+  IN  UINT32                  Timeout,
+  OUT USB_CBI_STATUS          *Result
+  )
+{
+  UINTN                     Len;
+  UINT8                     Endpoint;
+  EFI_STATUS                Status;
+  UINT32                    TransStatus;
+  INTN                      Retry;
+
+  Endpoint  = UsbCbi->InterruptEndpoint->EndpointAddress;
+  Status    = EFI_SUCCESS;
+  Timeout   = Timeout / USB_MASS_1_MILLISECOND;
+
+  //
+  // Attemp to the read the result from interrupt endpoint
+  //
+  for (Retry = 0; Retry < USB_CBI_MAX_RETRY; Retry++) {
+    TransStatus = 0;
+    Len         = sizeof (USB_CBI_STATUS);
+
+    Status = UsbCbi->UsbIo->UsbSyncInterruptTransfer (
+                              UsbCbi->UsbIo,
+                              Endpoint,
+                              Result,
+                              &Len,
+                              Timeout,
+                              &TransStatus
+                              );
+    //
+    // The CBI can NAK the interrupt endpoint if the command is in-progress.
+    //
+    if (EFI_ERROR (Status) && (TransStatus == EFI_USB_ERR_NAK)) {
+      continue;
+    }
+
+    break;
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute USB mass storage command through the CBI0/CBI1 transport protocol.
+
+  @param  Context               The USB CBI Protocol.
+  @param  Cmd                   The command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  DataDir               The direction of data transfer
+  @param  Data                  The buffer to hold the data
+  @param  DataLen               The length of the buffer
+  @param  Lun                   Should be 0, this field for bot only
+  @param  Timeout               The time to wait
+  @param  CmdStatus             The result of the command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to execute the command
+
+**/
+EFI_STATUS
+UsbCbiExecCommand (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  )
+{
+  USB_CBI_PROTOCOL          *UsbCbi;
+  USB_CBI_STATUS            Result;
+  EFI_STATUS                Status;
+  UINTN                     TransLen;
+
+  *CmdStatus  = USB_MASS_CMD_SUCCESS;
+  UsbCbi      = (USB_CBI_PROTOCOL *) Context;
+
+  //
+  // Send the command to the device. Return immediately if device
+  // rejects the command.
+  //
+  Status = UsbCbiSendCommand (UsbCbi, Cmd, CmdLen, Timeout);
+  if (EFI_ERROR (Status)) {
+    gBS->Stall(10 * USB_MASS_1_MILLISECOND);
+    DEBUG ((EFI_D_ERROR, "UsbCbiExecCommand: UsbCbiSendCommand (%r)\n",Status));
+    return Status;
+  }
+
+  //
+  // Transfer the data. Return this status if no interrupt endpoint
+  // is used to report the transfer status.
+  //
+  TransLen = (UINTN) DataLen;
+
+  Status   = UsbCbiDataTransfer (UsbCbi, DataDir, Data, &TransLen, Timeout);
+  if (UsbCbi->InterruptEndpoint == NULL) {
+    DEBUG ((EFI_D_ERROR, "UsbCbiExecCommand: UsbCbiDataTransfer (%r)\n",Status));
+    return Status;
+  }
+
+  //
+  // Get the status. If it succeeds, interpret the result.
+  //
+  Status = UsbCbiGetStatus (UsbCbi, Timeout, &Result);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbCbiExecCommand: UsbCbiGetStatus (%r)\n",Status));
+    return Status;
+  }
+
+  if (UsbCbi->Interface.InterfaceSubClass == USB_MASS_STORE_UFI) {
+    //
+    // For UFI device, ASC and ASCQ are returned.
+    //
+    // Do not set the USB_MASS_CMD_FAIL for a request sense command
+    // as a bad result type doesn't mean a cmd failure
+    //
+    if (Result.Type != 0 && *(UINT8*)Cmd != 0x03) {
+      *CmdStatus = USB_MASS_CMD_FAIL;
+    }
+  } else {
+    //
+    // Check page 27, CBI spec 1.1 for vaious reture status.
+    //
+    switch (Result.Value & 0x03) {
+    case 0x00:
+      //
+      // Pass
+      //
+      *CmdStatus = USB_MASS_CMD_SUCCESS;
+      break;
+
+    case 0x02:
+      //
+      // Phase Error, response with reset.
+      // No break here to fall through to "Fail".
+      //
+      UsbCbiResetDevice (UsbCbi, FALSE);
+
+    case 0x01:
+      //
+      // Fail
+      //
+      *CmdStatus = USB_MASS_CMD_FAIL;
+      break;
+
+    case 0x03:
+      //
+      // Persistent Fail. Need to send REQUEST SENSE.
+      //
+      *CmdStatus = USB_MASS_CMD_PERSISTENT;
+      break;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset the USB mass storage device by CBI protocol.
+
+  This function resets the USB mass storage device by CBI protocol.
+  The reset is defined as a non-data command. Don't use UsbCbiExecCommand
+  to send the command to device because that may introduce recursive loop.
+
+  @param  Context               The USB CBI protocol
+  @param  ExtendedVerification  The flag controlling the rule of reset.
+                                Not used here.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device.
+
+**/
+EFI_STATUS
+UsbCbiResetDevice (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                  ExtendedVerification
+  )
+{
+  UINT8                     ResetCmd[USB_CBI_RESET_CMD_LEN];
+  USB_CBI_PROTOCOL          *UsbCbi;
+  USB_CBI_STATUS            Result;
+  EFI_STATUS                Status;
+  UINT32                    Timeout;
+
+  UsbCbi = (USB_CBI_PROTOCOL *) Context;
+
+  //
+  // Fill in the reset command.
+  //
+  SetMem (ResetCmd, USB_CBI_RESET_CMD_LEN, 0xFF);
+
+  ResetCmd[0] = 0x1D;
+  ResetCmd[1] = 0x04;
+  Timeout     = USB_CBI_RESET_DEVICE_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  //
+  // Send the command to the device. Don't use UsbCbiExecCommand here.
+  //
+  Status = UsbCbiSendCommand (UsbCbi, ResetCmd, USB_CBI_RESET_CMD_LEN, Timeout);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Just retrieve the status and ignore that. Then stall
+  // 50ms to wait for it to complete.
+  //
+  UsbCbiGetStatus (UsbCbi, Timeout, &Result);
+  gBS->Stall (USB_CBI_RESET_DEVICE_STALL);
+
+  //
+  // Clear the Bulk-In and Bulk-Out stall condition and init data toggle.
+  //
+  UsbClearEndpointStall (UsbCbi->UsbIo, UsbCbi->BulkInEndpoint->EndpointAddress);
+  UsbClearEndpointStall (UsbCbi->UsbIo, UsbCbi->BulkOutEndpoint->EndpointAddress);
+
+  return Status;
+}
+
+
+/**
+  Clean up the CBI protocol's resource.
+
+  @param  Context               The instance of CBI protocol.
+
+  @retval EFI_SUCCESS           The resource is cleaned up.
+
+**/
+EFI_STATUS
+UsbCbiCleanUp (
+  IN  VOID                   *Context
+  )
+{
+  FreePool (Context);
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.h
new file mode 100644
index 0000000000..b414a46fde
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.h
@@ -0,0 +1,140 @@
+/** @file
+  Defination for the USB mass storage Control/Bulk/Interrupt (CBI) transport,
+  according to USB Mass Storage Class Control/Bulk/Interrupt (CBI) Transport, Revision 1.1.
+
+Copyright (c) 2007 - 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_USBMASS_CBI_H_
+#define _EFI_USBMASS_CBI_H_
+
+extern USB_MASS_TRANSPORT mUsbCbi0Transport;
+extern USB_MASS_TRANSPORT mUsbCbi1Transport;
+
+#define USB_CBI_MAX_PACKET_NUM        16
+#define USB_CBI_RESET_CMD_LEN         12
+//
+// USB CBI retry C/B/I transport times, set by experience
+//
+#define USB_CBI_MAX_RETRY             3
+//
+// Time to wait for USB CBI reset to complete, set by experience
+//  
+#define USB_CBI_RESET_DEVICE_STALL    (50 * USB_MASS_1_MILLISECOND)
+//
+// USB CBI transport timeout, set by experience
+//
+#define USB_CBI_RESET_DEVICE_TIMEOUT  (1 * USB_MASS_1_SECOND)
+
+typedef struct {
+  //
+  // Put Interface at the first field to make it easy to distinguish BOT/CBI Protocol instance
+  //
+  EFI_USB_INTERFACE_DESCRIPTOR  Interface;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *BulkInEndpoint;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *BulkOutEndpoint;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *InterruptEndpoint;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+} USB_CBI_PROTOCOL;
+
+#pragma pack(1)
+typedef struct {
+  UINT8               Type;
+  UINT8               Value;
+} USB_CBI_STATUS;
+#pragma pack()
+
+/**
+  Initializes USB CBI protocol.
+
+  This function initializes the USB mass storage class CBI protocol.
+  It will save its context which is a USB_CBI_PROTOCOL structure
+  in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB CBI initialization fails.
+
+**/
+EFI_STATUS
+UsbCbiInit (
+  IN  EFI_USB_IO_PROTOCOL   *UsbIo,
+  OUT VOID                  **Context       OPTIONAL
+  );
+
+/**
+  Execute USB mass storage command through the CBI0/CBI1 transport protocol.
+
+  @param  Context               The USB CBI Protocol.
+  @param  Cmd                   The command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  DataDir               The direction of data transfer
+  @param  Data                  The buffer to hold the data
+  @param  DataLen               The length of the buffer
+  @param  Lun                   Should be 0, this field for bot only
+  @param  Timeout               The time to wait
+  @param  CmdStatus             The result of the command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to execute the command
+
+**/
+EFI_STATUS
+UsbCbiExecCommand (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  );
+
+/**
+  Reset the USB mass storage device by CBI protocol.
+
+  This function resets the USB mass storage device by CBI protocol.
+  The reset is defined as a non-data command. Don't use UsbCbiExecCommand
+  to send the command to device because that may introduce recursive loop.
+
+  @param  Context               The USB CBI protocol
+  @param  ExtendedVerification  The flag controlling the rule of reset.
+                                Not used here.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device.
+
+**/
+EFI_STATUS
+UsbCbiResetDevice (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                  ExtendedVerification
+  );
+
+/**
+  Clean up the CBI protocol's resource.
+
+  @param  Context               The instance of CBI protocol.
+
+  @retval EFI_SUCCESS           The resource is cleaned up.
+
+**/
+EFI_STATUS
+UsbCbiCleanUp (
+  IN  VOID                   *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.c
new file mode 100644
index 0000000000..1fa8543e2b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.c
@@ -0,0 +1,162 @@
+/** @file
+  This file is used to implement the EFI_DISK_INFO_PROTOCOL interface.
+
+Copyright (c) 2011, 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 "UsbMass.h"
+
+EFI_DISK_INFO_PROTOCOL gUsbDiskInfoProtocolTemplate = {
+  EFI_DISK_INFO_USB_INTERFACE_GUID,
+  UsbDiskInfoInquiry,
+  UsbDiskInfoIdentify,
+  UsbDiskInfoSenseData,
+  UsbDiskInfoWhichIde
+};
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with USB interface GUID. 
+
+  @param[in]  UsbMass  The pointer of USB_MASS_DEVICE.
+
+**/
+VOID
+InitializeDiskInfo (
+  IN  USB_MASS_DEVICE   *UsbMass
+  )
+{
+  CopyMem (&UsbMass->DiskInfo, &gUsbDiskInfoProtocolTemplate, sizeof (gUsbDiskInfoProtocolTemplate));
+}
+
+
+/**
+  Provides inquiry information for the controller type.
+  
+  This function is used to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device 
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  )
+{
+  EFI_STATUS        Status;
+  USB_MASS_DEVICE   *UsbMass;
+
+  UsbMass  = USB_MASS_DEVICE_FROM_DISK_INFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*InquiryDataSize >= sizeof (UsbMass->InquiryData)) {
+    Status = EFI_SUCCESS;
+    CopyMem (InquiryData, &UsbMass->InquiryData, sizeof (UsbMass->InquiryData));
+  }
+  *InquiryDataSize = sizeof (UsbMass->InquiryData);
+  return Status;
+}
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL 
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device 
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Provides sense data information for the controller type.
+  
+  This function is used to get sense data. 
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function is used to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance. 
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.h
new file mode 100644
index 0000000000..e0a32ca8ee
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.h
@@ -0,0 +1,129 @@
+/** @file
+  Header file for EFI_DISK_INFO_PROTOCOL interface.
+
+Copyright (c) 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_USBMASS_DISKINFO_H_
+#define _EFI_USBMASS_DISKINFO_H_
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with USB interface GUID. 
+
+  @param  UsbMass  The pointer of USB_MASS_DEVICE.
+
+**/
+VOID
+InitializeDiskInfo (
+  IN  USB_MASS_DEVICE   *UsbMass
+  );
+
+
+/**
+  Provides inquiry information for the controller type.
+  
+  This function is used to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device 
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  );
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL 
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device 
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  );
+
+/**
+  Provides sense data information for the controller type.
+  
+  This function is used to get sense data. 
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  );
+
+
+/**
+  This function is used to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance. 
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.c
new file mode 100644
index 0000000000..9d1bb25fb3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.c
@@ -0,0 +1,1106 @@
+/** @file
+  USB Mass Storage Driver that manages USB Mass Storage Device and produces Block I/O Protocol.
+
+Copyright (c) 2007 - 2015, 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 "UsbMass.h"
+
+#define USB_MASS_TRANSPORT_COUNT    3
+//
+// Array of USB transport interfaces. 
+//
+USB_MASS_TRANSPORT *mUsbMassTransport[USB_MASS_TRANSPORT_COUNT] = {
+  &mUsbCbi0Transport,
+  &mUsbCbi1Transport,
+  &mUsbBotTransport,
+};
+
+EFI_DRIVER_BINDING_PROTOCOL gUSBMassDriverBinding = {
+  USBMassDriverBindingSupported,
+  USBMassDriverBindingStart,
+  USBMassDriverBindingStop,
+  0x11,
+  NULL,
+  NULL
+};
+
+/**
+  Reset the block device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.Reset(). 
+  It resets the block device hardware.
+  ExtendedVerification is ignored in this implementation.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  ExtendedVerification   Indicates that the driver may perform a more exhaustive
+                                 verification operation of the device during reset.
+
+  @retval EFI_SUCCESS            The block device was reset.
+  @retval EFI_DEVICE_ERROR       The block device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassReset (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN BOOLEAN                  ExtendedVerification
+  )
+{
+  USB_MASS_DEVICE *UsbMass;
+  EFI_TPL         OldTpl;
+  EFI_STATUS      Status;
+
+  //
+  // Raise TPL to TPL_NOTIFY to serialize all its operations
+  // to protect shared data structures.
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+
+  UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (This);
+  Status  = UsbMass->Transport->Reset (UsbMass->Context, ExtendedVerification);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Reads the requested number of blocks from the device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.ReadBlocks(). 
+  It reads the requested number of blocks from the device.
+  All the blocks are read, or an error is returned.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  MediaId                The media ID that the read request is for.
+  @param  Lba                    The starting logical block address to read from on the device.
+  @param  BufferSize             The size of the Buffer in bytes.
+                                 This must be a multiple of the intrinsic block size of the device.
+  @param  Buffer                 A pointer to the destination buffer for the data. The caller is
+                                 responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS            The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to perform the read operation.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+  @retval EFI_MEDIA_CHANGED      The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE    The BufferSize parameter is not a multiple of the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER  The read request contains LBAs that are not valid,
+                                 or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassReadBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN UINT32                   MediaId,
+  IN EFI_LBA                  Lba,
+  IN UINTN                    BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  USB_MASS_DEVICE     *UsbMass;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  EFI_TPL             OldTpl;
+  UINTN               TotalBlock;
+
+  //
+  // Raise TPL to TPL_NOTIFY to serialize all its operations
+  // to protect shared data structures.
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+  UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (This);
+  Media   = &UsbMass->BlockIoMedia;
+
+  //
+  // If it is a removable media, such as CD-Rom or Usb-Floppy,
+  // need to detect the media before each read/write. While some of
+  // Usb-Flash is marked as removable media.
+  //
+  if (Media->RemovableMedia) {
+    Status = UsbBootDetectMedia (UsbMass);
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto ON_EXIT;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto ON_EXIT;
+  }
+
+  if (BufferSize == 0) {
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  //
+  // BufferSize must be a multiple of the intrinsic block size of the device.
+  //
+  if ((BufferSize % Media->BlockSize) != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto ON_EXIT;
+  }
+
+  TotalBlock = BufferSize / Media->BlockSize;
+
+  //
+  // Make sure the range to read is valid.
+  //
+  if (Lba + TotalBlock - 1 > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  if (UsbMass->Cdb16Byte) {
+    Status = UsbBootReadBlocks16 (UsbMass, Lba, TotalBlock, Buffer);
+  } else {
+    Status = UsbBootReadBlocks (UsbMass, (UINT32) Lba, TotalBlock, Buffer);
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbMassReadBlocks: UsbBootReadBlocks (%r) -> Reset\n", Status));
+    UsbMassReset (This, TRUE);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Writes a specified number of blocks to the device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.WriteBlocks(). 
+  It writes a specified number of blocks to the device.
+  All blocks are written, or an error is returned.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  MediaId                The media ID that the write request is for.
+  @param  Lba                    The starting logical block address to be written.
+  @param  BufferSize             The size of the Buffer in bytes.
+                                 This must be a multiple of the intrinsic block size of the device.
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            The data were written correctly to the device.
+  @retval EFI_WRITE_PROTECTED    The device cannot be written to.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+  @retval EFI_MEDIA_CHANGED      The MediaId is not for the current media.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to perform the write operation.
+  @retval EFI_BAD_BUFFER_SIZE    The BufferSize parameter is not a multiple of the intrinsic
+                                 block size of the device.
+  @retval EFI_INVALID_PARAMETER  The write request contains LBAs that are not valid,
+                                 or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassWriteBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN UINT32                   MediaId,
+  IN EFI_LBA                  Lba,
+  IN UINTN                    BufferSize,
+  IN VOID                     *Buffer
+  )
+{
+  USB_MASS_DEVICE     *UsbMass;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  EFI_TPL             OldTpl;
+  UINTN               TotalBlock;
+
+  //
+  // Raise TPL to TPL_NOTIFY to serialize all its operations
+  // to protect shared data structures.
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+  UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (This);
+  Media   = &UsbMass->BlockIoMedia;
+
+  //
+  // If it is a removable media, such as CD-Rom or Usb-Floppy,
+  // need to detect the media before each read/write. Some of
+  // USB Flash is marked as removable media.
+  //
+  if (Media->RemovableMedia) {
+    Status = UsbBootDetectMedia (UsbMass);
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto ON_EXIT;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto ON_EXIT;
+  }
+
+  if (BufferSize == 0) {
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  //
+  // BufferSize must be a multiple of the intrinsic block size of the device.
+  //
+  if ((BufferSize % Media->BlockSize) != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto ON_EXIT;
+  }
+
+  TotalBlock = BufferSize / Media->BlockSize;
+
+  //
+  // Make sure the range to write is valid.
+  //
+  if (Lba + TotalBlock - 1 > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  //
+  // Try to write the data even the device is marked as ReadOnly,
+  // and clear the status should the write succeed.
+  //
+  if (UsbMass->Cdb16Byte) {
+    Status = UsbBootWriteBlocks16 (UsbMass, Lba, TotalBlock, Buffer);
+  } else {
+    Status = UsbBootWriteBlocks (UsbMass, (UINT32) Lba, TotalBlock, Buffer);
+  }  
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbMassWriteBlocks: UsbBootWriteBlocks (%r) -> Reset\n", Status));
+    UsbMassReset (This, TRUE);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Flushes all modified data to a physical block device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.FlushBlocks().
+  USB mass storage device doesn't support write cache,
+  so return EFI_SUCCESS directly.
+
+  @param  This                   Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS            All outstanding data were written correctly to the device.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to write data.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassFlushBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL  *This
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the media parameter data for EFI_BLOCK_IO_MEDIA of Block I/O Protocol.
+
+  @param  UsbMass                The USB mass storage device
+
+  @retval EFI_SUCCESS            The media parameters are updated successfully.
+  @retval Others                 Failed to get the media parameters.
+
+**/
+EFI_STATUS
+UsbMassInitMedia (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  EFI_BLOCK_IO_MEDIA          *Media;
+  EFI_STATUS                  Status;
+
+  Media = &UsbMass->BlockIoMedia;
+
+  //
+  // Fields of EFI_BLOCK_IO_MEDIA are defined in UEFI 2.0 spec,
+  // section for Block I/O Protocol.
+  //
+  Media->MediaPresent     = FALSE;
+  Media->LogicalPartition = FALSE;
+  Media->ReadOnly         = FALSE;
+  Media->WriteCaching     = FALSE;
+  Media->IoAlign          = 0;
+  Media->MediaId          = 1;
+
+  Status = UsbBootGetParams (UsbMass);
+  return Status;
+}
+
+/**
+  Initilize the USB Mass Storage transport.
+
+  This function tries to find the matching USB Mass Storage transport
+  protocol for USB device. If found, initializes the matching transport.
+
+  @param  This            The USB mass driver's driver binding.
+  @param  Controller      The device to test.
+  @param  Transport       The pointer to pointer to USB_MASS_TRANSPORT.
+  @param  Context         The parameter for USB_MASS_DEVICE.Context.
+  @param  MaxLun          Get the MaxLun if is BOT dev.
+
+  @retval EFI_SUCCESS     The initialization is successful.
+  @retval EFI_UNSUPPORTED No matching transport protocol is found.
+  @retval Others          Failed to initialize dev.
+
+**/
+EFI_STATUS
+UsbMassInitTransport (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Controller,
+  OUT USB_MASS_TRANSPORT           **Transport,
+  OUT VOID                         **Context,
+  OUT UINT8                        *MaxLun
+  )
+{
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  EFI_USB_INTERFACE_DESCRIPTOR  Interface;
+  UINT8                         Index;
+  EFI_STATUS                    Status;
+ 
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &Interface);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+  
+  Status = EFI_UNSUPPORTED;
+
+  //
+  // Traverse the USB_MASS_TRANSPORT arrary and try to find the
+  // matching transport protocol.
+  // If not found, return EFI_UNSUPPORTED.
+  // If found, execute USB_MASS_TRANSPORT.Init() to initialize the transport context.
+  //
+  for (Index = 0; Index < USB_MASS_TRANSPORT_COUNT; Index++) {
+    *Transport = mUsbMassTransport[Index];
+
+    if (Interface.InterfaceProtocol == (*Transport)->Protocol) {
+      Status  = (*Transport)->Init (UsbIo, Context);
+      break;
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // For BOT device, try to get its max LUN. 
+  // If max LUN is 0, then it is a non-lun device.
+  // Otherwise, it is a multi-lun device.
+  //
+  if ((*Transport)->Protocol == USB_MASS_STORE_BOT) {
+    (*Transport)->GetMaxLun (*Context, MaxLun);
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  return Status;  
+}
+
+/**
+  Initialize data for device that supports multiple LUNSs.
+
+  @param  This                 The Driver Binding Protocol instance.
+  @param  Controller           The device to initialize.
+  @param  Transport            Pointer to USB_MASS_TRANSPORT.
+  @param  Context              Parameter for USB_MASS_DEVICE.Context.
+  @param  DevicePath           The remaining device path.
+  @param  MaxLun               The max LUN number.
+
+  @retval EFI_SUCCESS          At least one LUN is initialized successfully.
+  @retval EFI_NOT_FOUND        Fail to initialize any of multiple LUNs.
+
+**/
+EFI_STATUS
+UsbMassInitMultiLun (
+  IN EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN EFI_HANDLE                    Controller,
+  IN USB_MASS_TRANSPORT            *Transport,
+  IN VOID                          *Context,
+  IN EFI_DEVICE_PATH_PROTOCOL      *DevicePath,
+  IN UINT8                         MaxLun
+  )
+{
+  USB_MASS_DEVICE                  *UsbMass;
+  EFI_USB_IO_PROTOCOL              *UsbIo;
+  DEVICE_LOGICAL_UNIT_DEVICE_PATH  LunNode;
+  UINT8                            Index;
+  EFI_STATUS                       Status;
+  EFI_STATUS                       ReturnStatus;
+
+  ASSERT (MaxLun > 0);
+  ReturnStatus = EFI_NOT_FOUND;
+
+  for (Index = 0; Index <= MaxLun; Index++) { 
+
+    DEBUG ((EFI_D_INFO, "UsbMassInitMultiLun: Start to initialize No.%d logic unit\n", Index));
+    
+    UsbIo   = NULL;
+    UsbMass = AllocateZeroPool (sizeof (USB_MASS_DEVICE));
+    ASSERT (UsbMass != NULL);
+      
+    UsbMass->Signature            = USB_MASS_SIGNATURE;
+    UsbMass->UsbIo                = UsbIo;
+    UsbMass->BlockIo.Media        = &UsbMass->BlockIoMedia;
+    UsbMass->BlockIo.Reset        = UsbMassReset;
+    UsbMass->BlockIo.ReadBlocks   = UsbMassReadBlocks;
+    UsbMass->BlockIo.WriteBlocks  = UsbMassWriteBlocks;
+    UsbMass->BlockIo.FlushBlocks  = UsbMassFlushBlocks;
+    UsbMass->OpticalStorage       = FALSE;
+    UsbMass->Transport            = Transport;
+    UsbMass->Context              = Context;
+    UsbMass->Lun                  = Index;
+    
+    //
+    // Initialize the media parameter data for EFI_BLOCK_IO_MEDIA of Block I/O Protocol.
+    //
+    Status = UsbMassInitMedia (UsbMass);
+    if ((EFI_ERROR (Status)) && (Status != EFI_NO_MEDIA)) {
+      DEBUG ((EFI_D_ERROR, "UsbMassInitMultiLun: UsbMassInitMedia (%r)\n", Status));
+      FreePool (UsbMass);
+      continue;
+    }
+
+    //
+    // Create a device path node for device logic unit, and append it.
+    //
+    LunNode.Header.Type    = MESSAGING_DEVICE_PATH;
+    LunNode.Header.SubType = MSG_DEVICE_LOGICAL_UNIT_DP;
+    LunNode.Lun            = UsbMass->Lun;
+  
+    SetDevicePathNodeLength (&LunNode.Header, sizeof (LunNode));
+  
+    UsbMass->DevicePath = AppendDevicePathNode (DevicePath, &LunNode.Header);
+  
+    if (UsbMass->DevicePath == NULL) {
+      DEBUG ((EFI_D_ERROR, "UsbMassInitMultiLun: failed to create device logic unit device path\n"));
+      Status = EFI_OUT_OF_RESOURCES;
+      FreePool (UsbMass);
+      continue;
+    }
+
+    InitializeDiskInfo (UsbMass);
+
+    //
+    // Create a new handle for each LUN, and install Block I/O Protocol and Device Path Protocol.
+    //
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &UsbMass->Controller,
+                    &gEfiDevicePathProtocolGuid,
+                    UsbMass->DevicePath,
+                    &gEfiBlockIoProtocolGuid,
+                    &UsbMass->BlockIo,
+                    &gEfiDiskInfoProtocolGuid,
+                    &UsbMass->DiskInfo,
+                    NULL
+                    );
+    
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "UsbMassInitMultiLun: InstallMultipleProtocolInterfaces (%r)\n", Status));
+      FreePool (UsbMass->DevicePath);
+      FreePool (UsbMass);
+      continue;
+    }
+
+    //
+    // Open USB I/O Protocol by child to setup a parent-child relationship.
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiUsbIoProtocolGuid,
+                    (VOID **) &UsbIo,
+                    This->DriverBindingHandle,
+                    UsbMass->Controller,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "UsbMassInitMultiLun: OpenUsbIoProtocol By Child (%r)\n", Status));
+      gBS->UninstallMultipleProtocolInterfaces (
+             &UsbMass->Controller,
+             &gEfiDevicePathProtocolGuid,
+             UsbMass->DevicePath,
+             &gEfiBlockIoProtocolGuid,
+             &UsbMass->BlockIo,
+             &gEfiDiskInfoProtocolGuid,
+             &UsbMass->DiskInfo,
+             NULL
+             );
+      FreePool (UsbMass->DevicePath);
+      FreePool (UsbMass);
+      continue;
+    }
+    ReturnStatus = EFI_SUCCESS;
+    DEBUG ((EFI_D_INFO, "UsbMassInitMultiLun: Success to initialize No.%d logic unit\n", Index));
+  }
+  
+  return ReturnStatus;
+}
+
+/**
+  Initialize data for device that does not support multiple LUNSs.
+
+  @param  This            The Driver Binding Protocol instance.
+  @param  Controller      The device to initialize.
+  @param  Transport       Pointer to USB_MASS_TRANSPORT.
+  @param  Context         Parameter for USB_MASS_DEVICE.Context.
+
+  @retval EFI_SUCCESS     Initialization succeeds.
+  @retval Other           Initialization fails.
+
+**/
+EFI_STATUS
+UsbMassInitNonLun (
+  IN EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN EFI_HANDLE                    Controller,
+  IN USB_MASS_TRANSPORT            *Transport,
+  IN VOID                          *Context
+  )
+{
+  USB_MASS_DEVICE             *UsbMass;
+  EFI_USB_IO_PROTOCOL         *UsbIo;
+  EFI_STATUS                  Status;
+
+  UsbIo   = NULL;
+  UsbMass = AllocateZeroPool (sizeof (USB_MASS_DEVICE));
+  ASSERT (UsbMass != NULL);
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbMassInitNonLun: OpenUsbIoProtocol By Driver (%r)\n", Status));
+    goto ON_ERROR;
+  }
+  
+  UsbMass->Signature            = USB_MASS_SIGNATURE;
+  UsbMass->Controller           = Controller;
+  UsbMass->UsbIo                = UsbIo;
+  UsbMass->BlockIo.Media        = &UsbMass->BlockIoMedia;
+  UsbMass->BlockIo.Reset        = UsbMassReset;
+  UsbMass->BlockIo.ReadBlocks   = UsbMassReadBlocks;
+  UsbMass->BlockIo.WriteBlocks  = UsbMassWriteBlocks;
+  UsbMass->BlockIo.FlushBlocks  = UsbMassFlushBlocks;
+  UsbMass->OpticalStorage       = FALSE;
+  UsbMass->Transport            = Transport;
+  UsbMass->Context              = Context;
+  
+  //
+  // Initialize the media parameter data for EFI_BLOCK_IO_MEDIA of Block I/O Protocol.
+  //
+  Status = UsbMassInitMedia (UsbMass);
+  if ((EFI_ERROR (Status)) && (Status != EFI_NO_MEDIA)) {
+    DEBUG ((EFI_D_ERROR, "UsbMassInitNonLun: UsbMassInitMedia (%r)\n", Status));
+    goto ON_ERROR;
+  }
+    
+  InitializeDiskInfo (UsbMass);
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiBlockIoProtocolGuid,
+                  &UsbMass->BlockIo,
+                  &gEfiDiskInfoProtocolGuid,
+                  &UsbMass->DiskInfo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  if (UsbMass != NULL) {
+    FreePool (UsbMass);
+  }
+  if (UsbIo != NULL) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiUsbIoProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+  return Status;  
+}
+
+
+/**
+  Check whether the controller is a supported USB mass storage.
+
+  @param  This                   The USB mass storage driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  EFI_USB_INTERFACE_DESCRIPTOR  Interface;
+  USB_MASS_TRANSPORT            *Transport;
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the interface descriptor to check the USB class and find a transport
+  // protocol handler.
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &Interface);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = EFI_UNSUPPORTED;
+
+  if (Interface.InterfaceClass != USB_MASS_STORE_CLASS) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Traverse the USB_MASS_TRANSPORT arrary and try to find the
+  // matching transport method.
+  // If not found, return EFI_UNSUPPORTED.
+  // If found, execute USB_MASS_TRANSPORT.Init() to initialize the transport context.
+  //
+  for (Index = 0; Index < USB_MASS_TRANSPORT_COUNT; Index++) {
+    Transport = mUsbMassTransport[Index];
+    if (Interface.InterfaceProtocol == Transport->Protocol) {
+      Status = Transport->Init (UsbIo, NULL);
+      break;
+    }
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+/**
+  Starts the USB mass storage device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mass storage device,
+  installs Block I/O Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mass storage device.
+
+  @param  This                  The USB mass storage driver binding protocol.
+  @param  Controller            The USB mass storage device to start on
+  @param  RemainingDevicePath   The remaining device path.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  USB_MASS_TRANSPORT            *Transport;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+  VOID                          *Context;
+  UINT8                         MaxLun;
+  EFI_STATUS                    Status;
+  EFI_USB_IO_PROTOCOL           *UsbIo; 
+  EFI_TPL                       OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Transport = NULL;
+  Context   = NULL;
+  MaxLun    = 0;
+
+  Status = UsbMassInitTransport (This, Controller, &Transport, &Context, &MaxLun);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: UsbMassInitTransport (%r)\n", Status));
+    goto Exit;
+  }
+  if (MaxLun == 0) {
+    //
+    // Initialize data for device that does not support multiple LUNSs.
+    //
+    Status = UsbMassInitNonLun (This, Controller, Transport, Context);
+    if (EFI_ERROR (Status)) { 
+      DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: UsbMassInitNonLun (%r)\n", Status));
+    }
+  } else {
+    //
+    // Open device path to prepare for appending Device Logic Unit node.
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiDevicePathProtocolGuid,
+                    (VOID **) &DevicePath,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+  
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: OpenDevicePathProtocol By Driver (%r)\n", Status));
+      goto Exit;
+    }
+
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiUsbIoProtocolGuid,
+                    (VOID **) &UsbIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+  
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: OpenUsbIoProtocol By Driver (%r)\n", Status));
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiDevicePathProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      goto Exit;
+    }
+
+    //
+    // Initialize data for device that supports multiple LUNs.
+    // EFI_SUCCESS is returned if at least 1 LUN is initialized successfully.
+    //
+    Status = UsbMassInitMultiLun (This, Controller, Transport, Context, DevicePath, MaxLun);
+    if (EFI_ERROR (Status)) {
+      gBS->CloseProtocol (
+              Controller,
+              &gEfiDevicePathProtocolGuid,
+              This->DriverBindingHandle,
+              Controller
+              );
+      gBS->CloseProtocol (
+              Controller,
+              &gEfiUsbIoProtocolGuid,
+              This->DriverBindingHandle,
+              Controller
+              );
+      DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: UsbMassInitMultiLun (%r) with Maxlun=%d\n", Status, MaxLun));
+    }
+  }
+Exit:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Stop controlling the device.
+
+  @param  This                   The USB mass storage driver binding
+  @param  Controller             The device controller controlled by the driver.
+  @param  NumberOfChildren       The number of children of this device
+  @param  ChildHandleBuffer      The buffer of children handle.
+
+  @retval EFI_SUCCESS            The driver stopped from controlling the device.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+  @retval EFI_UNSUPPORTED        Block I/O Protocol is not installed on Controller.
+  @retval Others                 Failed to stop the driver
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN  EFI_HANDLE                  Controller,
+  IN  UINTN                       NumberOfChildren,
+  IN  EFI_HANDLE                  *ChildHandleBuffer
+  )
+{
+  EFI_STATUS            Status;
+  USB_MASS_DEVICE       *UsbMass;
+  EFI_USB_IO_PROTOCOL   *UsbIo;
+  EFI_BLOCK_IO_PROTOCOL *BlockIo;
+  UINTN                 Index;
+  BOOLEAN               AllChildrenStopped;
+
+  //
+  // This is a bus driver stop function since multi-lun is supported.
+  // There are three kinds of device handles that might be passed:
+  // 1st is a handle with USB I/O & Block I/O installed (non-multi-lun)
+  // 2nd is a handle with Device Path & USB I/O installed (multi-lun root)
+  // 3rd is a handle with Device Path & USB I/O & Block I/O installed (multi-lun).
+  //
+  if (NumberOfChildren == 0) {
+    //
+    // A handle without any children, might be 1st and 2nd type.
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlockIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+  
+    if (EFI_ERROR(Status)) {
+      //
+      // This is a 2nd type handle(multi-lun root), it needs to close devicepath
+      // and usbio protocol.
+      //
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiDevicePathProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiUsbIoProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+      DEBUG ((EFI_D_INFO, "Success to stop multi-lun root handle\n"));
+      return EFI_SUCCESS;
+    }
+    
+    //
+    // This is a 1st type handle(non-multi-lun), which only needs to uninstall
+    // Block I/O Protocol, close USB I/O Protocol and free mass device.
+    //
+    UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (BlockIo);
+  
+    //
+    // Uninstall Block I/O protocol from the device handle,
+    // then call the transport protocol to stop itself.
+    //
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    Controller,
+                    &gEfiBlockIoProtocolGuid,
+                    &UsbMass->BlockIo,
+                    &gEfiDiskInfoProtocolGuid,
+                    &UsbMass->DiskInfo,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  
+    UsbMass->Transport->CleanUp (UsbMass->Context);
+    FreePool (UsbMass);
+    
+    DEBUG ((EFI_D_INFO, "Success to stop non-multi-lun root handle\n"));
+    return EFI_SUCCESS;
+  } 
+
+  //
+  // This is a 3rd type handle(multi-lun), which needs uninstall
+  // Block I/O Protocol and Device Path Protocol, close USB I/O Protocol and 
+  // free mass device for all children.
+  //
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = gBS->OpenProtocol (
+                    ChildHandleBuffer[Index],
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlockIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+      DEBUG ((EFI_D_ERROR, "Fail to stop No.%d multi-lun child handle when opening blockio\n", (UINT32)Index));
+      continue;
+    }
+
+    UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (BlockIo);
+
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiUsbIoProtocolGuid,
+           This->DriverBindingHandle,
+           ChildHandleBuffer[Index]
+           );
+  
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    ChildHandleBuffer[Index],
+                    &gEfiDevicePathProtocolGuid,
+                    UsbMass->DevicePath,
+                    &gEfiBlockIoProtocolGuid,
+                    &UsbMass->BlockIo,
+                    &gEfiDiskInfoProtocolGuid,
+                    &UsbMass->DiskInfo,
+                    NULL
+                    );
+    
+    if (EFI_ERROR (Status)) {
+      //
+      // Fail to uninstall Block I/O Protocol and Device Path Protocol, so re-open USB I/O Protocol by child.
+      //
+      AllChildrenStopped = FALSE;
+      DEBUG ((EFI_D_ERROR, "Fail to stop No.%d multi-lun child handle when uninstalling blockio and devicepath\n", (UINT32)Index));
+      
+      gBS->OpenProtocol (
+             Controller,
+             &gEfiUsbIoProtocolGuid,
+             (VOID **) &UsbIo,
+             This->DriverBindingHandle,
+             ChildHandleBuffer[Index],
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+    } else {
+      //
+      // Succeed to stop this multi-lun handle, so go on with next child.
+      //
+      if (((Index + 1) == NumberOfChildren) && AllChildrenStopped) {
+        UsbMass->Transport->CleanUp (UsbMass->Context);
+      }
+      FreePool (UsbMass);
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+  
+  DEBUG ((EFI_D_INFO, "Success to stop all %d multi-lun children handles\n", (UINT32) NumberOfChildren));
+  return EFI_SUCCESS;
+}
+
+/**
+  Entrypoint of USB Mass Storage Driver.
+
+  This function is the entrypoint of USB Mass Storage Driver. It installs Driver Binding
+  Protocol together with Component Name Protocols.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassStorageEntryPoint (
+  IN EFI_HANDLE               ImageHandle,
+  IN EFI_SYSTEM_TABLE         *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Install driver binding protocol
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUSBMassDriverBinding,
+             ImageHandle,
+             &gUsbMassStorageComponentName,
+             &gUsbMassStorageComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.h
new file mode 100644
index 0000000000..0f013c1ad7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.h
@@ -0,0 +1,333 @@
+/** @file
+  Definitions of functions for Driver Binding Protocol and Block I/O Protocol,
+  and other internal definitions.
+
+Copyright (c) 2007 - 2011, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_USBMASS_IMPL_H_
+#define _EFI_USBMASS_IMPL_H_
+
+#define  USB_MASS_SIGNATURE    SIGNATURE_32 ('U', 's', 'b', 'M')
+
+#define USB_MASS_DEVICE_FROM_BLOCK_IO(a) \
+        CR (a, USB_MASS_DEVICE, BlockIo, USB_MASS_SIGNATURE)
+
+#define USB_MASS_DEVICE_FROM_DISK_INFO(a) \
+        CR (a, USB_MASS_DEVICE, DiskInfo, USB_MASS_SIGNATURE)
+
+
+extern EFI_COMPONENT_NAME_PROTOCOL   gUsbMassStorageComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUsbMassStorageComponentName2;
+
+//
+// Functions for Driver Binding Protocol
+//
+
+/**
+  Check whether the controller is a supported USB mass storage.
+
+  @param  This                   The USB mass storage driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts the USB mass storage device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mass storage device,
+  installs Block I/O Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mass storage device.
+
+  @param  This                  The USB mass storage driver binding protocol.
+  @param  Controller            The USB mass storage device to start on
+  @param  RemainingDevicePath   The remaining device path.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stop controlling the device.
+
+  @param  This                   The USB mass storage driver binding
+  @param  Controller             The device controller controlled by the driver.
+  @param  NumberOfChildren       The number of children of this device
+  @param  ChildHandleBuffer      The buffer of children handle.
+
+  @retval EFI_SUCCESS            The driver stopped from controlling the device.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+  @retval EFI_UNSUPPORTED        Block I/O Protocol is not installed on Controller.
+  @retval Others                 Failed to stop the driver
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN  EFI_HANDLE                  Controller,
+  IN  UINTN                       NumberOfChildren,
+  IN  EFI_HANDLE                  *ChildHandleBuffer
+  );
+
+//
+// Functions for Block I/O Protocol
+//
+
+/**
+  Reset the block device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.Reset(). 
+  It resets the block device hardware.
+  ExtendedVerification is ignored in this implementation.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  ExtendedVerification   Indicates that the driver may perform a more exhaustive
+                                 verification operation of the device during reset.
+
+  @retval EFI_SUCCESS            The block device was reset.
+  @retval EFI_DEVICE_ERROR       The block device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassReset (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN BOOLEAN                  ExtendedVerification
+  );
+
+/**
+  Reads the requested number of blocks from the device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.ReadBlocks(). 
+  It reads the requested number of blocks from the device.
+  All the blocks are read, or an error is returned.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  MediaId                The media ID that the read request is for.
+  @param  Lba                    The starting logical block address to read from on the device.
+  @param  BufferSize             The size of the Buffer in bytes.
+                                 This must be a multiple of the intrinsic block size of the device.
+  @param  Buffer                 A pointer to the destination buffer for the data. The caller is
+                                 responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS            The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to perform the read operation.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+  @retval EFI_MEDIA_CHANGED      The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE    The BufferSize parameter is not a multiple of the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER  The read request contains LBAs that are not valid,
+                                 or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassReadBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN UINT32                   MediaId,
+  IN EFI_LBA                  Lba,
+  IN UINTN                    BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+/**
+  Writes a specified number of blocks to the device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.WriteBlocks(). 
+  It writes a specified number of blocks to the device.
+  All blocks are written, or an error is returned.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  MediaId                The media ID that the write request is for.
+  @param  Lba                    The starting logical block address to be written.
+  @param  BufferSize             The size of the Buffer in bytes.
+                                 This must be a multiple of the intrinsic block size of the device.
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            The data were written correctly to the device.
+  @retval EFI_WRITE_PROTECTED    The device cannot be written to.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+  @retval EFI_MEDIA_CHANGED      The MediaId is not for the current media.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to perform the write operation.
+  @retval EFI_BAD_BUFFER_SIZE    The BufferSize parameter is not a multiple of the intrinsic
+                                 block size of the device.
+  @retval EFI_INVALID_PARAMETER  The write request contains LBAs that are not valid,
+                                 or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassWriteBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN UINT32                   MediaId,
+  IN EFI_LBA                  Lba,
+  IN UINTN                    BufferSize,
+  IN VOID                     *Buffer
+  );
+
+/**
+  Flushes all modified data to a physical block device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.FlushBlocks().
+  USB mass storage device doesn't support write cache,
+  so return EFI_SUCCESS directly.
+
+  @param  This                   Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS            All outstanding data were written correctly to the device.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to write data.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassFlushBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL  *This
+  );
+
+//
+// EFI Component Name Functions
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassStorageGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassStorageGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.inf
new file mode 100644
index 0000000000..26d15c7679
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.inf
@@ -0,0 +1,87 @@
+## @file
+# USB Mass Storage Driver that manages USB mass storage devices and produces Block I/O Protocol.
+#
+# The USB mass storage class is specified in two layers: the bottom layer
+# is the transportation protocol. The top layer is the command set.
+# The transportation layer provides the transportation of the command, data and result.
+# The command set defines the command, data and result.
+# The Bulk-Only-Transport and Control/Bulk/Interrupt transport are two transportation protocol.
+# USB mass storage class adopts various industrial standard as its command set.
+# This module refers to following specifications:
+# 1. USB Mass Storage Specification for Bootability, Revision 1.0
+# 2. USB Mass Storage Class Control/Bulk/Interrupt (CBI) Transport, Revision 1.1
+# 3. USB Mass Storage Class Bulk-Only Transport, Revision 1.0.
+# 4. UEFI Specification, v2.1
+#
+# Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbMassStorageDxe
+  MODULE_UNI_FILE                = UsbMassStorageDxe.uni
+  FILE_GUID                      = 9FB4B4A7-42C0-4bcd-8540-9BCC6711F83E
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = USBMassStorageEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUSBMassDriverBinding                        
+#  COMPONENT_NAME                =  gUsbMassStorageComponentName
+#  COMPONENT_NAME2               =  gUsbMassStorageComponentName2
+#
+
+[Sources]
+  UsbMassBoot.h
+  UsbMassImpl.h
+  UsbMassBot.h
+  UsbMassBot.c
+  ComponentName.c
+  UsbMassImpl.c
+  UsbMassBoot.c
+  UsbMassCbi.h
+  UsbMass.h
+  UsbMassCbi.c
+  UsbMassDiskInfo.h
+  UsbMassDiskInfo.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiBlockIoProtocolGuid                       ## BY_START
+  gEfiDiskInfoProtocolGuid                      ## BY_START
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER        ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbMassStorageDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.uni
new file mode 100644
index 0000000000..77cf18cd97
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxeExtra.uni
new file mode 100644
index 0000000000..7603d47e39
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/ComponentName.c
new file mode 100644
index 0000000000..4b79a0f07e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/ComponentName.c
@@ -0,0 +1,224 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for USB Mouse Absolute Pointer Driver.
+
+Copyright (c) 2004 - 2011, 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 "UsbMouseAbsolutePointer.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUsbMouseAbsolutePointerComponentName = {
+  UsbMouseAbsolutePointerComponentNameGetDriverName,
+  UsbMouseAbsolutePointerComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUsbMouseAbsolutePointerComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbMouseAbsolutePointerComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbMouseAbsolutePointerComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUsbMouseAbsolutePointerDriverNameTable[] = {
+  { "eng;en", L"Usb Mouse Absolute Pointer Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbMouseAbsolutePointerDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUsbMouseAbsolutePointerComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                       Status;
+  USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouseAbsolutePointerDev;
+  EFI_ABSOLUTE_POINTER_PROTOCOL    *AbsolutePointerProtocol;
+  EFI_USB_IO_PROTOCOL              *UsbIoProtocol;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIoProtocol,
+                  gUsbMouseAbsolutePointerDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiUsbIoProtocolGuid,
+           gUsbMouseAbsolutePointerDriverBinding.DriverBindingHandle,
+           ControllerHandle
+           );
+
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Status != EFI_ALREADY_STARTED) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                 &gEfiAbsolutePointerProtocolGuid,
+                  (VOID **) &AbsolutePointerProtocol,
+                  gUsbMouseAbsolutePointerDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UsbMouseAbsolutePointerDev = USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL (AbsolutePointerProtocol);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           UsbMouseAbsolutePointerDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUsbMouseAbsolutePointerComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/MouseHid.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/MouseHid.c
new file mode 100644
index 0000000000..92ce18f425
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/MouseHid.c
@@ -0,0 +1,281 @@
+/** @file
+  Helper functions to parse HID report descriptor and items.
+
+Copyright (c) 2004 - 2010, 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 "UsbMouseAbsolutePointer.h"
+
+
+/**
+  Get next HID item from report descriptor.
+
+  This function retrieves next HID item from report descriptor, according to
+  the start position.
+  According to USB HID Specification, An item is piece of information
+  about the device. All items have a one-byte prefix that contains
+  the item tag, item type, and item size.
+  There are two basic types of items: short items and long items.
+  If the item is a short item, its optional data size may be 0, 1, 2, or 4 bytes.
+  Only short item is supported here.
+
+  @param  StartPos          Start position of the HID item to get.
+  @param  EndPos            End position of the range to get the the next HID item.
+  @param  HidItem           Buffer for the HID Item to return.
+
+  @return Pointer to end of the HID item returned.
+          NULL if no HID item retrieved.
+
+**/
+UINT8 *
+GetNextHidItem (
+  IN  UINT8    *StartPos,
+  IN  UINT8    *EndPos,
+  OUT HID_ITEM *HidItem
+  )
+{
+  UINT8 Temp;
+
+  if (EndPos <= StartPos) {
+    return NULL;
+  }
+
+  Temp = *StartPos;
+  StartPos++;
+
+  //
+  // Bit format of prefix byte:
+  // Bits 0-1: Size
+  // Bits 2-3: Type
+  // Bits 4-7: Tag
+  //
+  HidItem->Type = BitFieldRead8 (Temp, 2, 3);
+  HidItem->Tag  = BitFieldRead8 (Temp, 4, 7);
+
+  if (HidItem->Tag == HID_ITEM_TAG_LONG) {
+    //
+    // Long Items are not supported, although we try to parse it.
+    //
+    HidItem->Format = HID_ITEM_FORMAT_LONG;
+
+    if ((EndPos - StartPos) >= 2) {
+      HidItem->Size = *StartPos++;
+      HidItem->Tag  = *StartPos++;
+
+      if ((EndPos - StartPos) >= HidItem->Size) {
+        HidItem->Data.LongData = StartPos;
+        StartPos += HidItem->Size;
+        return StartPos;
+      }
+    }
+  } else {
+    HidItem->Format = HID_ITEM_FORMAT_SHORT;
+    HidItem->Size   = BitFieldRead8 (Temp, 0, 1);
+
+    switch (HidItem->Size) {
+    case 0:
+      //
+      // No data
+      //
+      return StartPos;
+
+    case 1:
+      //
+      // 1-byte data
+      //
+      if ((EndPos - StartPos) >= 1) {
+        HidItem->Data.Uint8 = *StartPos++;
+        return StartPos;
+      }
+
+    case 2:
+      //
+      // 2-byte data
+      //
+      if ((EndPos - StartPos) >= 2) {
+        CopyMem (&HidItem->Data.Uint16, StartPos, sizeof (UINT16));
+        StartPos += 2;
+        return StartPos;
+      }
+
+    case 3:
+      //
+      // 4-byte data, adjust size
+      //
+      HidItem->Size = 4;
+      if ((EndPos - StartPos) >= 4) {
+        CopyMem (&HidItem->Data.Uint32, StartPos, sizeof (UINT32));
+        StartPos += 4;
+        return StartPos;
+      }
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Get data from HID item.
+
+  This function retrieves data from HID item.
+  It only supports short items, which has 4 types of data:
+  0, 1, 2, or 4 bytes.
+
+  @param  HidItem       Pointer to the HID item.
+
+  @return The data of HID item.
+
+**/
+UINT32
+GetItemData (
+  IN  HID_ITEM *HidItem
+  )
+{
+  //
+  // Get data from HID item.
+  //
+  switch (HidItem->Size) {
+  case 1:
+    return HidItem->Data.Uint8;
+  case 2:
+    return HidItem->Data.Uint16;
+  case 4:
+    return HidItem->Data.Uint32;
+  }
+  return 0;
+}
+
+/**
+  Parse HID item from report descriptor.
+
+  There are three item types: Main, Global, and Local.
+  This function parses these types of HID items according
+  to tag info.
+
+  @param  UsbMouse          The instance of USB_MOUSE_ABSOLUTE_POINTER_DEV
+  @param  HidItem           The HID item to parse
+
+**/
+VOID
+ParseHidItem (
+  IN  USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouse,
+  IN  HID_ITEM                         *HidItem
+  )
+{
+  UINT8  Data;
+
+  switch (HidItem->Type) {
+
+  case HID_ITEM_TYPE_MAIN:
+    //
+    // we don't care any main items, just skip
+    //
+    return ;
+
+  case HID_ITEM_TYPE_GLOBAL:
+    //
+    // For global items, we only care Usage Page tag for Button Page here
+    //
+    if (HidItem->Tag == HID_GLOBAL_ITEM_TAG_USAGE_PAGE) {
+      Data = (UINT8) GetItemData (HidItem);
+      if (Data == 0x09) {
+        //
+        // Button Page
+        //
+        UsbMouse->PrivateData.ButtonDetected = TRUE;
+      }
+  }
+    return;
+
+  case HID_ITEM_TYPE_LOCAL:
+    if (HidItem->Size == 0) {
+    //
+      // No expected data for local item
+    //
+    return ;
+    }
+
+    Data = (UINT8) GetItemData (HidItem);
+
+    switch (HidItem->Tag) {
+    case HID_LOCAL_ITEM_TAG_USAGE_MINIMUM:
+      if (UsbMouse->PrivateData.ButtonDetected) {
+        UsbMouse->PrivateData.ButtonMinIndex = Data;
+    }
+    return ;
+
+    case HID_LOCAL_ITEM_TAG_USAGE_MAXIMUM:
+    {
+      if (UsbMouse->PrivateData.ButtonDetected) {
+        UsbMouse->PrivateData.ButtonMaxIndex = Data;
+      }
+      return ;
+    }
+
+    default:
+        return ;
+      }
+  }
+}
+
+
+/**
+  Parse Mouse Report Descriptor.
+
+  According to USB HID Specification, report descriptors are
+  composed of pieces of information. Each piece of information
+  is called an Item. This function retrieves each item from
+  the report descriptor and updates USB_MOUSE_ABSOLUTE_POINTER_DEV.
+
+  @param  UsbMouseAbsolutePointer  The instance of USB_MOUSE_ABSOLUTE_POINTER_DEV
+  @param  ReportDescriptor         Report descriptor to parse
+  @param  ReportSize               Report descriptor size
+
+  @retval EFI_SUCCESS              Report descriptor successfully parsed.
+  @retval EFI_UNSUPPORTED          Report descriptor contains long item.
+
+**/
+EFI_STATUS
+ParseMouseReportDescriptor (
+  OUT USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouseAbsolutePointer,
+  IN  UINT8                            *ReportDescriptor,
+  IN  UINTN                            ReportSize
+  )
+{
+  UINT8     *DescriptorEnd;
+  UINT8     *Ptr;
+  HID_ITEM  HidItem;
+
+  DescriptorEnd = ReportDescriptor + ReportSize;
+
+  Ptr = GetNextHidItem (ReportDescriptor, DescriptorEnd, &HidItem);
+  while (Ptr != NULL) {
+    if (HidItem.Format != HID_ITEM_FORMAT_SHORT) {
+      //
+      // Long Item is not supported at current HID revision
+      //
+      return EFI_UNSUPPORTED;
+    }
+
+    ParseHidItem (UsbMouseAbsolutePointer, &HidItem);
+
+    Ptr = GetNextHidItem (Ptr, DescriptorEnd, &HidItem);
+  }
+
+  UsbMouseAbsolutePointer->NumberOfButtons = (UINT8) (UsbMouseAbsolutePointer->PrivateData.ButtonMaxIndex - UsbMouseAbsolutePointer->PrivateData.ButtonMinIndex + 1);
+  UsbMouseAbsolutePointer->XLogicMax = 1023;
+  UsbMouseAbsolutePointer->YLogicMax = 1023;
+  UsbMouseAbsolutePointer->XLogicMin = -1023;
+  UsbMouseAbsolutePointer->YLogicMin = -1023;
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.c
new file mode 100644
index 0000000000..42e696fcc9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.c
@@ -0,0 +1,995 @@
+/** @file
+  USB Mouse Driver that manages USB mouse and produces Absolute Pointer Protocol.
+
+Copyright (c) 2004 - 2012, 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 "UsbMouseAbsolutePointer.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gUsbMouseAbsolutePointerDriverBinding = {
+  USBMouseAbsolutePointerDriverBindingSupported,
+  USBMouseAbsolutePointerDriverBindingStart,
+  USBMouseAbsolutePointerDriverBindingStop,
+  0x1,
+  NULL,
+  NULL
+};
+
+/**
+  Entrypoint of USB Mouse Absolute Pointer Driver.
+
+  This function is the entrypoint of USB Mouse Driver. It installs Driver Binding
+  Protocols together with Component Name Protocols.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUsbMouseAbsolutePointerDriverBinding,
+             ImageHandle,
+             &gUsbMouseAbsolutePointerComponentName,
+             &gUsbMouseAbsolutePointerComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Check whether USB Mouse Absolute Pointer Driver supports this device.
+
+  @param  This                   The driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS          Status;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // Use the USB I/O Protocol interface to check whether Controller is
+  // a mouse device that can be managed by this driver.
+  //
+  Status = EFI_SUCCESS;
+  if (!IsUsbMouse (UsbIo)) {
+    Status = EFI_UNSUPPORTED;
+  }
+  
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiUsbIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+        
+  return Status;
+}
+
+
+/**
+  Starts the mouse device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mouse device,
+  installs Absolute Pointer Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mouse device.
+
+  @param  This                  The driver binding instance.
+  @param  Controller            Handle of device to bind driver to.
+  @param  RemainingDevicePath   Optional parameter use to pick a specific child
+                                device to start.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_USB_IO_PROTOCOL            *UsbIo;
+  USB_MOUSE_ABSOLUTE_POINTER_DEV *UsbMouseAbsolutePointerDevice;
+  UINT8                          EndpointNumber;
+  EFI_USB_ENDPOINT_DESCRIPTOR    EndpointDescriptor;
+  UINT8                          Index;
+  UINT8                          EndpointAddr;
+  UINT8                          PollingInterval;
+  UINT8                          PacketSize;
+  BOOLEAN                        Found;
+  EFI_TPL                        OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  //
+  // Open USB I/O Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER                  
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit1;
+  }
+  
+  UsbMouseAbsolutePointerDevice = AllocateZeroPool (sizeof (USB_MOUSE_ABSOLUTE_POINTER_DEV));
+  ASSERT (UsbMouseAbsolutePointerDevice != NULL);
+
+  UsbMouseAbsolutePointerDevice->UsbIo     = UsbIo;
+  UsbMouseAbsolutePointerDevice->Signature = USB_MOUSE_ABSOLUTE_POINTER_DEV_SIGNATURE;
+
+  //
+  // Get the Device Path Protocol on Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &UsbMouseAbsolutePointerDevice->DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Report Status Code here since USB mouse will be detected next.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_PRESENCE_DETECT),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  //
+  // Get interface & endpoint descriptor
+  //
+  UsbIo->UsbGetInterfaceDescriptor (
+           UsbIo,
+           &UsbMouseAbsolutePointerDevice->InterfaceDescriptor
+           );
+
+  EndpointNumber = UsbMouseAbsolutePointerDevice->InterfaceDescriptor.NumEndpoints;
+
+  //
+  // Traverse endpoints to find interrupt endpoint
+  //
+  Found = FALSE;
+  for (Index = 0; Index < EndpointNumber; Index++) {
+    UsbIo->UsbGetEndpointDescriptor (
+             UsbIo,
+             Index,
+             &EndpointDescriptor
+             );
+
+    if ((EndpointDescriptor.Attributes & (BIT0 | BIT1)) == USB_ENDPOINT_INTERRUPT) {
+      //
+      // We only care interrupt endpoint here
+      //
+      CopyMem (&UsbMouseAbsolutePointerDevice->IntEndpointDescriptor, &EndpointDescriptor, sizeof(EndpointDescriptor));
+      Found = TRUE;
+      break;
+    }
+  }
+
+  if (!Found) {
+    //
+    // Report Status Code to indicate that there is no USB mouse
+    //
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_NOT_DETECTED)
+      );
+    //
+    // No interrupt endpoint found, then return unsupported.
+    //
+    Status = EFI_UNSUPPORTED;
+    goto ErrorExit;
+  }
+
+  //
+  // Report Status Code here since USB mouse has be detected.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_DETECTED),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  Status = InitializeUsbMouseDevice (UsbMouseAbsolutePointerDevice);
+  if (EFI_ERROR (Status)) {
+    //
+    // Fail to initialize USB mouse device.
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_INTERFACE_ERROR),
+      UsbMouseAbsolutePointerDevice->DevicePath
+      );
+
+    goto ErrorExit;
+  }
+
+  //
+  // Initialize and install EFI Absolute Pointer Protocol.
+  //
+  UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol.GetState = GetMouseAbsolutePointerState;
+  UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol.Reset	  = UsbMouseAbsolutePointerReset;
+  UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol.Mode	  = &UsbMouseAbsolutePointerDevice->Mode;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  UsbMouseAbsolutePointerWaitForInput,
+                  UsbMouseAbsolutePointerDevice,
+                  &((UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol).WaitForInput)
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiAbsolutePointerProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // The next step would be submitting Asynchronous Interrupt Transfer on this mouse device.
+  // After that we will be able to get key data from it. Thus this is deemed as
+  // the enable action of the mouse, so report status code accordingly.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_ENABLE),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  //
+  // Submit Asynchronous Interrupt Transfer to manage this device.
+  //
+  EndpointAddr    = UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.EndpointAddress;
+  PollingInterval = UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.Interval;
+  PacketSize      = (UINT8) (UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.MaxPacketSize);
+
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    EndpointAddr,
+                    TRUE,
+                    PollingInterval,
+                    PacketSize,
+                    OnMouseInterruptComplete,
+                    UsbMouseAbsolutePointerDevice
+                    );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // If submit error, uninstall that interface
+    //
+    gBS->UninstallProtocolInterface (
+           Controller,
+           &gEfiAbsolutePointerProtocolGuid,
+           &UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol
+           );
+    goto ErrorExit;
+  }
+
+  UsbMouseAbsolutePointerDevice->ControllerNameTable = NULL;
+  AddUnicodeString2 (
+    "eng",
+    gUsbMouseAbsolutePointerComponentName.SupportedLanguages,
+    &UsbMouseAbsolutePointerDevice->ControllerNameTable,
+    L"Generic Usb Mouse Absolute Pointer",
+      TRUE
+      );
+  AddUnicodeString2 (
+    "en",
+    gUsbMouseAbsolutePointerComponentName2.SupportedLanguages,
+    &UsbMouseAbsolutePointerDevice->ControllerNameTable,
+    L"Generic Usb Mouse Absolute Pointer",
+    FALSE
+    );
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+
+//
+// Error handler
+//
+ErrorExit:
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    if (UsbMouseAbsolutePointerDevice != NULL) {
+      if ((UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol).WaitForInput != NULL) {
+        gBS->CloseEvent ((UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol).WaitForInput);
+      }
+
+      FreePool (UsbMouseAbsolutePointerDevice);
+      UsbMouseAbsolutePointerDevice = NULL;
+    }
+  }
+
+ErrorExit1:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Stop the USB mouse device handled by this driver.
+
+  @param  This                   The driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Absolute Pointer Protocol is not installed on Controller.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                      Status;
+  USB_MOUSE_ABSOLUTE_POINTER_DEV  *UsbMouseAbsolutePointerDevice;
+  EFI_ABSOLUTE_POINTER_PROTOCOL   *AbsolutePointerProtocol;
+  EFI_USB_IO_PROTOCOL             *UsbIo;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiAbsolutePointerProtocolGuid,
+                  (VOID **) &AbsolutePointerProtocol,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  UsbMouseAbsolutePointerDevice = USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL (AbsolutePointerProtocol);
+
+  UsbIo = UsbMouseAbsolutePointerDevice->UsbIo;
+
+  //
+  // The key data input from this device will be disabled.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_DISABLE),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  //
+  // Delete the Asynchronous Interrupt Transfer from this device
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.EndpointAddress,
+           FALSE,
+           UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.Interval,
+           0,
+           NULL,
+           NULL
+           );
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiAbsolutePointerProtocolGuid,
+                  &UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  //
+  // Free all resources.
+  //
+  gBS->CloseEvent (UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol.WaitForInput);
+  
+  if (UsbMouseAbsolutePointerDevice->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbMouseAbsolutePointerDevice->DelayedRecoveryEvent);
+    UsbMouseAbsolutePointerDevice->DelayedRecoveryEvent = NULL;
+  }
+
+  if (UsbMouseAbsolutePointerDevice->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (UsbMouseAbsolutePointerDevice->ControllerNameTable);
+  }
+
+  FreePool (UsbMouseAbsolutePointerDevice);
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  Uses USB I/O to check whether the device is a USB mouse device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB mouse device.
+  @retval FALSE    Device is a not USB mouse device.
+
+**/
+BOOLEAN
+IsUsbMouse (
+  IN  EFI_USB_IO_PROTOCOL     *UsbIo
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+
+  //
+  // Get the default interface descriptor
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (
+                    UsbIo,
+                    &InterfaceDescriptor
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if ((InterfaceDescriptor.InterfaceClass == CLASS_HID) &&
+      (InterfaceDescriptor.InterfaceSubClass == SUBCLASS_BOOT) &&
+      (InterfaceDescriptor.InterfaceProtocol == PROTOCOL_MOUSE)
+      ) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Initialize the USB mouse device.
+
+  This function retrieves and parses HID report descriptor, and
+  initializes state of USB_MOUSE_ABSOLUTE_POINTER_DEV. Then it sets indefinite idle
+  rate for the device. Finally it creates event for delayed recovery,
+  which deals with device error.
+
+  @param  UsbMouseAbsolutePointerDev   Device instance to be initialized.
+
+  @retval EFI_SUCCESS                  USB mouse device successfully initialized.
+  @retval EFI_UNSUPPORTED              HID descriptor type is not report descriptor.
+  @retval Other                        USB mouse device was not initialized successfully.
+
+**/
+EFI_STATUS
+InitializeUsbMouseDevice (
+  IN  USB_MOUSE_ABSOLUTE_POINTER_DEV           *UsbMouseAbsolutePointerDev
+  )
+{
+  EFI_USB_IO_PROTOCOL       *UsbIo;
+  UINT8                     Protocol;
+  EFI_STATUS                Status;
+  EFI_USB_HID_DESCRIPTOR    *MouseHidDesc;
+  UINT8                     *ReportDesc;
+  EFI_USB_CONFIG_DESCRIPTOR ConfigDesc;
+  VOID                      *Buf;
+  UINT32                    TransferResult;
+  UINT16                    Total;
+  USB_DESC_HEAD             *Head;
+  BOOLEAN                   Start;
+
+  UsbIo = UsbMouseAbsolutePointerDev->UsbIo;
+
+  //
+  // Get the current configuration descriptor. Note that it doesn't include other descriptors.
+  //
+  Status = UsbIo->UsbGetConfigDescriptor (
+                    UsbIo,
+                    &ConfigDesc
+                    );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // By issuing Get_Descriptor(Configuration) request with total length, we get the Configuration descriptor,
+  // all Interface descriptors, all Endpoint descriptors, and the HID descriptor for each interface.
+  //
+  Buf = AllocateZeroPool (ConfigDesc.TotalLength);
+  if (Buf == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = UsbGetDescriptor (
+             UsbIo,
+             (UINT16)((USB_DESC_TYPE_CONFIG << 8) | (ConfigDesc.ConfigurationValue - 1)),
+             0,
+             ConfigDesc.TotalLength,
+             Buf,
+             &TransferResult
+             );
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    return Status;
+  }
+
+  Total = 0;
+  Start = FALSE;
+  Head  = (USB_DESC_HEAD *)Buf;  
+  MouseHidDesc = NULL;
+
+  //
+  // Get HID descriptor from the receipt of Get_Descriptor(Configuration) request.
+  // This algorithm is based on the fact that the HID descriptor shall be interleaved
+  // between the interface and endpoint descriptors for HID interfaces.
+  //
+  while (Total < ConfigDesc.TotalLength) {
+    if (Head->Type == USB_DESC_TYPE_INTERFACE) {
+      if ((((USB_INTERFACE_DESCRIPTOR *)Head)->InterfaceNumber == UsbMouseAbsolutePointerDev->InterfaceDescriptor.InterfaceNumber) &&
+        (((USB_INTERFACE_DESCRIPTOR *)Head)->AlternateSetting == UsbMouseAbsolutePointerDev->InterfaceDescriptor.AlternateSetting)) {
+        Start = TRUE;
+      }
+    }
+    if (Start && (Head->Type == USB_DESC_TYPE_ENDPOINT)) {
+      break;
+    }
+    if (Start && (Head->Type == USB_DESC_TYPE_HID)) {
+      MouseHidDesc = (EFI_USB_HID_DESCRIPTOR *)Head;
+      break;
+    }
+    Total = Total + (UINT16)Head->Len;
+    Head  = (USB_DESC_HEAD*)((UINT8 *)Buf + Total);
+  }
+
+  if (MouseHidDesc == NULL) {
+    FreePool (Buf);
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get report descriptor
+  //
+  if (MouseHidDesc->HidClassDesc[0].DescriptorType != USB_DESC_TYPE_REPORT) {
+    FreePool (Buf);
+    return EFI_UNSUPPORTED;
+  }
+
+  ReportDesc = AllocateZeroPool (MouseHidDesc->HidClassDesc[0].DescriptorLength);
+  ASSERT (ReportDesc != NULL);
+
+  Status = UsbGetReportDescriptor (
+             UsbIo,
+             UsbMouseAbsolutePointerDev->InterfaceDescriptor.InterfaceNumber,
+             MouseHidDesc->HidClassDesc[0].DescriptorLength,
+             ReportDesc
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    FreePool (ReportDesc);
+    return Status;
+  }
+
+  //
+  // Parse report descriptor
+  //
+  Status = ParseMouseReportDescriptor (
+             UsbMouseAbsolutePointerDev,
+             ReportDesc,
+             MouseHidDesc->HidClassDesc[0].DescriptorLength
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    FreePool (ReportDesc);
+    return Status;
+  }
+
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMaxX = 1024;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMaxY = 1024;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMaxZ = 0;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMinX = 0;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMinY = 0;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMinZ = 0;
+  UsbMouseAbsolutePointerDev->Mode.Attributes   = 0x3;
+  
+  //
+  // Set boot protocol for the USB mouse.
+  // This driver only supports boot protocol.
+  //
+  UsbGetProtocolRequest (
+    UsbIo,
+    UsbMouseAbsolutePointerDev->InterfaceDescriptor.InterfaceNumber,
+    &Protocol
+    );
+  if (Protocol != BOOT_PROTOCOL) {
+    Status = UsbSetProtocolRequest (
+               UsbIo,
+               UsbMouseAbsolutePointerDev->InterfaceDescriptor.InterfaceNumber,
+               BOOT_PROTOCOL
+               );
+
+    if (EFI_ERROR (Status)) {
+      FreePool (Buf);
+      FreePool (ReportDesc);
+      return Status;
+    }
+  }
+
+  FreePool (Buf);
+  FreePool (ReportDesc);
+
+  //
+  // Create event for delayed recovery, which deals with device error.
+  //
+  if (UsbMouseAbsolutePointerDev->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbMouseAbsolutePointerDev->DelayedRecoveryEvent);
+    UsbMouseAbsolutePointerDev->DelayedRecoveryEvent = 0;
+  }
+
+  gBS->CreateEvent (
+         EVT_TIMER | EVT_NOTIFY_SIGNAL,
+         TPL_NOTIFY,
+         USBMouseRecoveryHandler,
+         UsbMouseAbsolutePointerDev,
+         &UsbMouseAbsolutePointerDev->DelayedRecoveryEvent
+         );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Handler function for USB mouse's asynchronous interrupt transfer.
+
+  This function is the handler function for USB mouse's asynchronous interrupt transfer
+  to manage the mouse. It parses data returned from asynchronous interrupt transfer, and
+  get button and movement state.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+OnMouseInterruptComplete (
+  IN  VOID        *Data,
+  IN  UINTN       DataLength,
+  IN  VOID        *Context,
+  IN  UINT32      Result
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouseAbsolutePointerDevice;
+  EFI_USB_IO_PROTOCOL              *UsbIo;
+  UINT8                            EndpointAddr;
+  UINT32                           UsbResult;
+
+  UsbMouseAbsolutePointerDevice  = (USB_MOUSE_ABSOLUTE_POINTER_DEV *) Context;
+  UsbIo                          = UsbMouseAbsolutePointerDevice->UsbIo;
+
+  if (Result != EFI_USB_NOERROR) {
+    //
+    // Some errors happen during the process
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_INPUT_ERROR),
+      UsbMouseAbsolutePointerDevice->DevicePath
+      );
+
+    if ((Result & EFI_USB_ERR_STALL) == EFI_USB_ERR_STALL) {
+      EndpointAddr = UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.EndpointAddress;
+
+      UsbClearEndpointHalt (
+        UsbIo,
+        EndpointAddr,
+        &UsbResult
+        );
+    }
+
+    //
+    // Delete & Submit this interrupt again
+    // Handler of DelayedRecoveryEvent triggered by timer will re-submit the interrupt. 
+    //
+    UsbIo->UsbAsyncInterruptTransfer (
+             UsbIo,
+             UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.EndpointAddress,
+             FALSE,
+             0,
+             0,
+             NULL,
+             NULL
+             );
+    //
+    // EFI_USB_INTERRUPT_DELAY is defined in USB standard for error handling.
+    //
+    gBS->SetTimer (
+           UsbMouseAbsolutePointerDevice->DelayedRecoveryEvent,
+           TimerRelative,
+           EFI_USB_INTERRUPT_DELAY
+           );
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // If no error and no data, just return EFI_SUCCESS.
+  //
+  if (DataLength == 0 || Data == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  UsbMouseAbsolutePointerDevice->StateChanged = TRUE;
+
+  //
+  // Check mouse Data
+  // USB HID Specification specifies following data format:
+  // Byte    Bits    Description
+  // 0       0       Button 1
+  //         1       Button 2
+  //         2       Button 3
+  //         4 to 7  Device-specific
+  // 1       0 to 7  X displacement
+  // 2       0 to 7  Y displacement
+  // 3 to n  0 to 7  Device specific (optional)
+  //
+  UsbMouseAbsolutePointerDevice->State.CurrentX += *((INT8 *) Data + 1);
+  UsbMouseAbsolutePointerDevice->State.CurrentY += *((INT8 *) Data + 2);
+  
+  if (DataLength > 3) {
+    UsbMouseAbsolutePointerDevice->State.CurrentZ += *((INT8 *) Data + 3);
+  }
+  UsbMouseAbsolutePointerDevice->State.ActiveButtons = *(UINT8 *) Data & (BIT0 | BIT1);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the current state of a pointer device.
+
+  @param  This                  A pointer to the EFI_ABSOLUTE_POINTER_PROTOCOL instance.                                   
+  @param  MouseState            A pointer to the state information on the pointer device.
+
+  @retval EFI_SUCCESS           The state of the pointer device was returned in State.
+  @retval EFI_NOT_READY         The state of the pointer device has not changed since the last call to
+                                GetState().                                                           
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to retrieve the pointer device's
+                                current state.                                                           
+  @retval EFI_INVALID_PARAMETER State is NULL.                                                           
+
+**/
+EFI_STATUS
+EFIAPI
+GetMouseAbsolutePointerState (
+  IN   EFI_ABSOLUTE_POINTER_PROTOCOL  *This,
+  OUT  EFI_ABSOLUTE_POINTER_STATE     *State
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV *MouseAbsolutePointerDev;
+
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MouseAbsolutePointerDev = USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL (This);
+
+  if (!MouseAbsolutePointerDev->StateChanged) {
+    return EFI_NOT_READY;
+  }
+
+  //
+  // Retrieve mouse state from USB_MOUSE_ABSOLUTE_POINTER_DEV,
+  // which was filled by OnMouseInterruptComplete()
+  //
+  CopyMem (
+    State,
+    &MouseAbsolutePointerDev->State,
+    sizeof (EFI_ABSOLUTE_POINTER_STATE)
+    );
+
+  //
+  // Clear previous move state
+  //
+  MouseAbsolutePointerDev->State.CurrentX      = 0;
+  MouseAbsolutePointerDev->State.CurrentY      = 0;
+  MouseAbsolutePointerDev->State.CurrentZ      = 0;
+  MouseAbsolutePointerDev->State.ActiveButtons = 0;
+
+  MouseAbsolutePointerDev->StateChanged = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Resets the pointer device hardware.
+
+  @param  This                  A pointer to the EFI_ABSOLUTE_POINTER_PROTOCOL instance.
+  @param  ExtendedVerification  Indicates that the driver may perform a more exhaustive
+                                verification operation of the device during reset.
+
+  @retval EFI_SUCCESS           The device was reset.
+  @retval EFI_DEVICE_ERROR      The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerReset (
+  IN EFI_ABSOLUTE_POINTER_PROTOCOL  *This,
+  IN BOOLEAN                        ExtendedVerification
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV       *UsbMouseAbsolutePointerDevice;
+
+  UsbMouseAbsolutePointerDevice  = USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL (This);
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_RESET),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  //
+  // Clear mouse state.
+  //
+  ZeroMem (
+    &UsbMouseAbsolutePointerDevice->State,
+    sizeof (EFI_ABSOLUTE_POINTER_STATE)
+    );
+  UsbMouseAbsolutePointerDevice->StateChanged = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Event notification function for EFI_ABSOLUTE_POINTER_PROTOCOL.WaitForInput event.
+
+  @param  Event        Event to be signaled when there's input from mouse.
+  @param  Context      Points to USB_MOUSE_ABSOLUTE_POINTER_DEV instance.
+
+**/
+VOID
+EFIAPI
+UsbMouseAbsolutePointerWaitForInput (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV *UsbMouseAbsolutePointerDev;
+
+  UsbMouseAbsolutePointerDev = (USB_MOUSE_ABSOLUTE_POINTER_DEV *) Context;
+
+  //
+  // If there's input from mouse, signal the event.
+  //
+  if (UsbMouseAbsolutePointerDev->StateChanged) {
+    gBS->SignalEvent (Event);
+  }
+}
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event                 The Delayed Recovery event.
+  @param  Context               Points to the USB_MOUSE_ABSOLUTE_POINTER_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBMouseRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV       *UsbMouseAbsolutePointerDev;
+  EFI_USB_IO_PROTOCOL                  *UsbIo;
+
+  UsbMouseAbsolutePointerDev = (USB_MOUSE_ABSOLUTE_POINTER_DEV *) Context;
+
+  UsbIo       = UsbMouseAbsolutePointerDev->UsbIo;
+
+  //
+  // Re-submit Asynchronous Interrupt Transfer for recovery.
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbMouseAbsolutePointerDev->IntEndpointDescriptor.EndpointAddress,
+           TRUE,
+           UsbMouseAbsolutePointerDev->IntEndpointDescriptor.Interval,
+           UsbMouseAbsolutePointerDev->IntEndpointDescriptor.MaxPacketSize,
+           OnMouseInterruptComplete,
+           UsbMouseAbsolutePointerDev
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.h b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.h
new file mode 100644
index 0000000000..08b8d56dd8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.h
@@ -0,0 +1,471 @@
+/** @file
+  Helper routine and corresponding data struct used by USB Mouse Absolute Pointer Driver.
+
+Copyright (c) 2004 - 2012, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _USB_MOUSE_ABSOLUTE_POINTER_H_
+#define _USB_MOUSE_ABSOLUTE_POINTER_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/AbsolutePointer.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiUsbLib.h>
+#include <Library/DebugLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#define CLASS_HID               3
+#define SUBCLASS_BOOT           1
+#define PROTOCOL_MOUSE          2
+
+#define BOOT_PROTOCOL           0
+#define REPORT_PROTOCOL         1
+
+#define USB_MOUSE_ABSOLUTE_POINTER_DEV_SIGNATURE SIGNATURE_32 ('u', 'm', 's', 't')
+
+//
+// A common header for usb standard descriptor.
+// Each stand descriptor has a length and type.
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Len;
+  UINT8                   Type;
+} USB_DESC_HEAD;
+#pragma pack()
+
+///
+/// Button range and status
+///
+typedef struct {
+  BOOLEAN ButtonDetected;
+  UINT8   ButtonMinIndex;
+  UINT8   ButtonMaxIndex;
+  UINT8   Reserved;
+} USB_MOUSE_BUTTON_DATA;
+
+///
+/// Device instance of USB mouse.
+///
+typedef struct {
+  UINTN                         Signature;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+  EFI_EVENT                     DelayedRecoveryEvent;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+  EFI_USB_ENDPOINT_DESCRIPTOR   IntEndpointDescriptor;
+  UINT8                         NumberOfButtons;
+  INT32                         XLogicMax;
+  INT32                         XLogicMin;
+  INT32                         YLogicMax;
+  INT32                         YLogicMin;
+  EFI_ABSOLUTE_POINTER_PROTOCOL AbsolutePointerProtocol;
+  EFI_ABSOLUTE_POINTER_STATE    State;
+  EFI_ABSOLUTE_POINTER_MODE     Mode;
+  BOOLEAN                       StateChanged;
+  USB_MOUSE_BUTTON_DATA         PrivateData;
+  EFI_UNICODE_STRING_TABLE      *ControllerNameTable;
+} USB_MOUSE_ABSOLUTE_POINTER_DEV;
+
+///
+/// General HID Item structure
+///
+
+typedef union {
+  UINT8   Uint8;
+  UINT16  Uint16;
+  UINT32  Uint32;
+  INT8    Int8;
+  INT16   Int16;
+  INT32   Int32;
+  UINT8   *LongData;
+} HID_DATA;
+
+typedef struct {
+  UINT16    Format;
+  UINT8     Size;
+  UINT8     Type;
+  UINT8     Tag;
+  HID_DATA  Data;
+} HID_ITEM;
+
+#define USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL(a) \
+    CR(a, USB_MOUSE_ABSOLUTE_POINTER_DEV, AbsolutePointerProtocol, USB_MOUSE_ABSOLUTE_POINTER_DEV_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gUsbMouseAbsolutePointerDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gUsbMouseAbsolutePointerComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUsbMouseAbsolutePointerComponentName2;
+
+//
+// Functions of Driver Binding Protocol
+//
+
+/**
+  Check whether USB Mouse Absolute Pointer Driver supports this device.
+
+  @param  This                   The driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Starts the mouse device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mouse device,
+  installs Absolute Pointer Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mouse device.
+
+  @param  This                  The driver binding instance.
+  @param  Controller            Handle of device to bind driver to.
+  @param  RemainingDevicePath   Optional parameter use to pick a specific child
+                                device to start.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop the USB mouse device handled by this driver.
+
+  @param  This                   The driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Absolute Pointer Protocol is not installed on Controller.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+//
+// Functions of EFI_ABSOLUTE_POINTER_PROTOCOL
+//
+
+/**
+  Retrieves the current state of a pointer device.
+
+  @param  This                  A pointer to the EFI_ABSOLUTE_POINTER_PROTOCOL instance.                                   
+  @param  MouseState            A pointer to the state information on the pointer device.
+
+  @retval EFI_SUCCESS           The state of the pointer device was returned in State.
+  @retval EFI_NOT_READY         The state of the pointer device has not changed since the last call to
+                                GetState().                                                           
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to retrieve the pointer device's
+                                current state.                                                           
+  @retval EFI_INVALID_PARAMETER State is NULL.                                                           
+
+**/
+EFI_STATUS
+EFIAPI
+GetMouseAbsolutePointerState (
+  IN   EFI_ABSOLUTE_POINTER_PROTOCOL  *This,
+  OUT  EFI_ABSOLUTE_POINTER_STATE     *State
+  );
+
+/**
+  Resets the pointer device hardware.
+
+  @param  This                  A pointer to the EFI_ABSOLUTE_POINTER_PROTOCOL instance.
+  @param  ExtendedVerification  Indicates that the driver may perform a more exhaustive
+                                verification operation of the device during reset.
+
+  @retval EFI_SUCCESS           The device was reset.
+  @retval EFI_DEVICE_ERROR      The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerReset (
+  IN EFI_ABSOLUTE_POINTER_PROTOCOL  *This,
+  IN BOOLEAN                        ExtendedVerification
+  );
+
+/**
+  Event notification function for EFI_ABSOLUTE_POINTER_PROTOCOL.WaitForInput event.
+
+  @param  Event        Event to be signaled when there's input from mouse.
+  @param  Context      Points to USB_MOUSE_ABSOLUTE_POINTER_DEV instance.
+
+**/
+VOID
+EFIAPI
+UsbMouseAbsolutePointerWaitForInput (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  );
+
+//
+// Internal worker functions
+//
+
+/**
+  Uses USB I/O to check whether the device is a USB mouse device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB mouse device.
+  @retval FALSE    Device is a not USB mouse device.
+
+**/
+BOOLEAN
+IsUsbMouse (
+  IN  EFI_USB_IO_PROTOCOL     *UsbIo
+  );
+
+/**
+  Initialize the USB mouse device.
+
+  This function retrieves and parses HID report descriptor, and
+  initializes state of USB_MOUSE_ABSOLUTE_POINTER_DEV. Then it sets indefinite idle
+  rate for the device. Finally it creates event for delayed recovery,
+  which deals with device error.
+
+  @param  UsbMouseAbsolutePointerDev   Device instance to be initialized.
+
+  @retval EFI_SUCCESS                  USB mouse device successfully initialized.
+  @retval EFI_UNSUPPORTED              HID descriptor type is not report descriptor.
+  @retval Other                        USB mouse device was not initialized successfully.
+
+**/
+EFI_STATUS
+InitializeUsbMouseDevice (
+  IN  USB_MOUSE_ABSOLUTE_POINTER_DEV           *UsbMouseAbsolutePointerDev
+  );
+
+/**
+  Handler function for USB mouse's asynchronous interrupt transfer.
+
+  This function is the handler function for USB mouse's asynchronous interrupt transfer
+  to manage the mouse. It parses data returned from asynchronous interrupt transfer, and
+  get button and movement state.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+OnMouseInterruptComplete (
+  IN  VOID        *Data,
+  IN  UINTN       DataLength,
+  IN  VOID        *Context,
+  IN  UINT32      Result
+  );
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event                 The Delayed Recovery event.
+  @param  Context               Points to the USB_MOUSE_ABSOLUTE_POINTER_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBMouseRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  );
+
+/**
+  Parse Mouse Report Descriptor.
+
+  According to USB HID Specification, report descriptors are
+  composed of pieces of information. Each piece of information
+  is called an Item. This function retrieves each item from
+  the report descriptor and updates USB_MOUSE_ABSOLUTE_POINTER_DEV.
+
+  @param  UsbMouseAbsolutePointer  The instance of USB_MOUSE_ABSOLUTE_POINTER_DEV
+  @param  ReportDescriptor         Report descriptor to parse
+  @param  ReportSize               Report descriptor size
+
+  @retval EFI_SUCCESS              Report descriptor successfully parsed.
+  @retval EFI_UNSUPPORTED          Report descriptor contains long item.
+
+**/
+EFI_STATUS
+ParseMouseReportDescriptor (
+  OUT USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouseAbsolutePointer,
+  IN  UINT8                            *ReportDescriptor,
+  IN  UINTN                            ReportSize
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.inf
new file mode 100644
index 0000000000..af10382d52
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.inf
@@ -0,0 +1,72 @@
+## @file
+# USB Mouse Driver that manages USB mouse and produces Absolute Pointer Protocol.
+#
+# USB Mouse Driver consumes USB I/O Protocol and Device Path Protocol, and produces
+# Absolute Pointer Protocol on USB mouse devices.
+# It manages the USB mouse device via Asynchronous Interrupt Transfer of USB I/O Protocol,
+# and parses the data according to USB HID Specification.
+# This module refers to following specifications:
+# 1. Universal Serial Bus HID Firmware Specification, ver 1.11
+# 2. UEFI Specification, v2.1
+#
+# Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbMouseAbsolutePointerDxe
+  MODULE_UNI_FILE                = UsbMouseAbsolutePointerDxe.uni
+  FILE_GUID                      = 4EA43463-747C-46eb-97FB-B0E5C5F05306
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = USBMouseAbsolutePointerDriverBindingEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUsbMouseAbsolutePointerDriverBinding                        
+#  COMPONENT_NAME                =  gUsbMouseAbsolutePointerComponentName
+#  COMPONENT_NAME2               =  gUsbMouseAbsolutePointerComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  MouseHid.c
+  UsbMouseAbsolutePointer.c
+  UsbMouseAbsolutePointer.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  ReportStatusCodeLib
+  UefiUsbLib
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiAbsolutePointerProtocolGuid               ## BY_START
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER        ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbMouseAbsolutePointerDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.uni
new file mode 100644
index 0000000000..628d6933c0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxeExtra.uni
new file mode 100644
index 0000000000..763dcaed03
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/ComponentName.c
new file mode 100644
index 0000000000..a7df52221c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/ComponentName.c
@@ -0,0 +1,224 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for USB Mouse driver.
+
+Copyright (c) 2004 - 2011, 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 "UsbMouse.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUsbMouseComponentName = {
+  UsbMouseComponentNameGetDriverName,
+  UsbMouseComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUsbMouseComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbMouseComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbMouseComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUsbMouseDriverNameTable[] = {
+  { "eng;en", L"Usb Mouse Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbMouseDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUsbMouseComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                  Status;
+  USB_MOUSE_DEV               *UsbMouseDev;
+  EFI_SIMPLE_POINTER_PROTOCOL *SimplePointerProtocol;
+  EFI_USB_IO_PROTOCOL         *UsbIoProtocol;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIoProtocol,
+                  gUsbMouseDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiUsbIoProtocolGuid,
+           gUsbMouseDriverBinding.DriverBindingHandle,
+           ControllerHandle
+           );
+
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Status != EFI_ALREADY_STARTED) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                 &gEfiSimplePointerProtocolGuid,
+                  (VOID **) &SimplePointerProtocol,
+                  gUsbMouseDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UsbMouseDev = USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL (SimplePointerProtocol);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           UsbMouseDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUsbMouseComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/MouseHid.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/MouseHid.c
new file mode 100644
index 0000000000..ec6a420c14
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/MouseHid.c
@@ -0,0 +1,281 @@
+/** @file
+  Helper functions to parse HID report descriptor and items.
+
+Copyright (c) 2004 - 2010, 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 "UsbMouse.h"
+
+
+/**
+  Get next HID item from report descriptor.
+
+  This function retrieves next HID item from report descriptor, according to
+  the start position.
+  According to USB HID Specification, An item is piece of information
+  about the device. All items have a one-byte prefix that contains
+  the item tag, item type, and item size.
+  There are two basic types of items: short items and long items.
+  If the item is a short item, its optional data size may be 0, 1, 2, or 4 bytes.
+  Only short item is supported here.
+
+  @param  StartPos          Start position of the HID item to get.
+  @param  EndPos            End position of the range to get the the next HID item.
+  @param  HidItem           Buffer for the HID Item to return.
+
+  @return Pointer to end of the HID item returned.
+          NULL if no HID item retrieved.
+
+**/
+UINT8 *
+GetNextHidItem (
+  IN  UINT8    *StartPos,
+  IN  UINT8    *EndPos,
+  OUT HID_ITEM *HidItem
+  )
+{
+  UINT8 Temp;
+
+  if (EndPos <= StartPos) {
+    return NULL;
+  }
+
+  Temp = *StartPos;
+  StartPos++;
+
+  //
+  // Bit format of prefix byte:
+  // Bits 0-1: Size
+  // Bits 2-3: Type
+  // Bits 4-7: Tag
+  //
+  HidItem->Type = BitFieldRead8 (Temp, 2, 3);
+  HidItem->Tag  = BitFieldRead8 (Temp, 4, 7);
+
+  if (HidItem->Tag == HID_ITEM_TAG_LONG) {
+    //
+    // Long Items are not supported, although we try to parse it.
+    //
+    HidItem->Format = HID_ITEM_FORMAT_LONG;
+
+    if ((EndPos - StartPos) >= 2) {
+      HidItem->Size = *StartPos++;
+      HidItem->Tag  = *StartPos++;
+
+      if ((EndPos - StartPos) >= HidItem->Size) {
+        HidItem->Data.LongData = StartPos;
+        StartPos += HidItem->Size;
+        return StartPos;
+      }
+    }
+  } else {
+    HidItem->Format = HID_ITEM_FORMAT_SHORT;
+    HidItem->Size   = BitFieldRead8 (Temp, 0, 1);
+
+    switch (HidItem->Size) {
+    case 0:
+      //
+      // No data
+      //
+      return StartPos;
+
+    case 1:
+      //
+      // 1-byte data
+      //
+      if ((EndPos - StartPos) >= 1) {
+        HidItem->Data.Uint8 = *StartPos++;
+        return StartPos;
+      }
+
+    case 2:
+      //
+      // 2-byte data
+      //
+      if ((EndPos - StartPos) >= 2) {
+        CopyMem (&HidItem->Data.Uint16, StartPos, sizeof (UINT16));
+        StartPos += 2;
+        return StartPos;
+      }
+
+    case 3:
+      //
+      // 4-byte data, adjust size
+      //
+      HidItem->Size = 4;
+      if ((EndPos - StartPos) >= 4) {
+        CopyMem (&HidItem->Data.Uint32, StartPos, sizeof (UINT32));
+        StartPos += 4;
+        return StartPos;
+      }
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Get data from HID item.
+
+  This function retrieves data from HID item.
+  It only supports short items, which has 4 types of data:
+  0, 1, 2, or 4 bytes.
+
+  @param  HidItem       Pointer to the HID item.
+
+  @return The data of HID item.
+
+**/
+UINT32
+GetItemData (
+  IN  HID_ITEM *HidItem
+  )
+{
+  //
+  // Get data from HID item.
+  //
+  switch (HidItem->Size) {
+  case 1:
+    return HidItem->Data.Uint8;
+  case 2:
+    return HidItem->Data.Uint16;
+  case 4:
+    return HidItem->Data.Uint32;
+  }
+  return 0;
+}
+
+/**
+  Parse HID item from report descriptor.
+
+  There are three item types: Main, Global, and Local.
+  This function parses these types of HID items according
+  to tag info.
+  
+  @param  UsbMouse          The instance of USB_MOUSE_DEV
+  @param  HidItem           The HID item to parse
+
+**/
+VOID
+ParseHidItem (
+  IN  USB_MOUSE_DEV   *UsbMouse,
+  IN  HID_ITEM        *HidItem
+  )
+{
+  UINT8  Data;
+
+  switch (HidItem->Type) {
+
+  case HID_ITEM_TYPE_MAIN:
+    //
+    // we don't care any main items, just skip
+    //
+    return;
+
+  case HID_ITEM_TYPE_GLOBAL:
+    //
+    // For global items, we only care Usage Page tag for Button Page here
+    //
+    if (HidItem->Tag == HID_GLOBAL_ITEM_TAG_USAGE_PAGE) {
+      Data = (UINT8) GetItemData (HidItem);
+      if (Data == 0x09) {
+        //
+        // Button Page
+        //
+        UsbMouse->PrivateData.ButtonDetected = TRUE;
+      }
+    }
+    return;
+
+  case HID_ITEM_TYPE_LOCAL:
+    if (HidItem->Size == 0) {
+      //
+      // No expected data for local item
+      //
+      return ;
+    }
+
+    Data = (UINT8) GetItemData (HidItem);
+
+    switch (HidItem->Tag) {
+    case HID_LOCAL_ITEM_TAG_USAGE_MINIMUM:
+      if (UsbMouse->PrivateData.ButtonDetected) {
+        UsbMouse->PrivateData.ButtonMinIndex = Data;
+      }
+      return ;
+
+    case HID_LOCAL_ITEM_TAG_USAGE_MAXIMUM:
+    {
+      if (UsbMouse->PrivateData.ButtonDetected) {
+        UsbMouse->PrivateData.ButtonMaxIndex = Data;
+      }
+      return ;
+    }
+
+    default:
+      return;
+    }
+  }
+}
+
+
+/**
+  Parse Mouse Report Descriptor.
+
+  According to USB HID Specification, report descriptors are
+  composed of pieces of information. Each piece of information
+  is called an Item. This function retrieves each item from
+  the report descriptor and updates USB_MOUSE_DEV.
+
+  @param  UsbMouse          The instance of USB_MOUSE_DEV
+  @param  ReportDescriptor  Report descriptor to parse
+  @param  ReportSize        Report descriptor size
+
+  @retval EFI_SUCCESS       Report descriptor successfully parsed.
+  @retval EFI_UNSUPPORTED   Report descriptor contains long item.
+
+**/
+EFI_STATUS
+ParseMouseReportDescriptor (
+  OUT USB_MOUSE_DEV   *UsbMouse,
+  IN  UINT8           *ReportDescriptor,
+  IN  UINTN           ReportSize
+  )
+{
+  UINT8     *DescriptorEnd;
+  UINT8     *Ptr;
+  HID_ITEM  HidItem;
+
+  DescriptorEnd = ReportDescriptor + ReportSize;
+
+  Ptr = GetNextHidItem (ReportDescriptor, DescriptorEnd, &HidItem);
+  while (Ptr != NULL) {
+    if (HidItem.Format != HID_ITEM_FORMAT_SHORT) {
+      //
+      // Long Item is not supported at current HID revision
+      //
+      return EFI_UNSUPPORTED;
+    }
+
+    ParseHidItem (UsbMouse, &HidItem);
+
+    Ptr = GetNextHidItem (Ptr, DescriptorEnd, &HidItem);
+  }
+
+  UsbMouse->NumberOfButtons = (UINT8) (UsbMouse->PrivateData.ButtonMaxIndex - UsbMouse->PrivateData.ButtonMinIndex + 1);
+  UsbMouse->XLogicMax = 127;
+  UsbMouse->YLogicMax = 127;
+  UsbMouse->XLogicMin = -127;
+  UsbMouse->YLogicMin = -127;
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.c
new file mode 100644
index 0000000000..0ad5616d96
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.c
@@ -0,0 +1,1000 @@
+/** @file
+  USB Mouse Driver that manages USB mouse and produces Simple Pointer Protocol.
+
+Copyright (c) 2004 - 2012, 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 "UsbMouse.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gUsbMouseDriverBinding = {
+  USBMouseDriverBindingSupported,
+  USBMouseDriverBindingStart,
+  USBMouseDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  Entrypoint of USB Mouse Driver.
+
+  This function is the entrypoint of USB Mouse Driver. It installs Driver Binding
+  Protocols together with Component Name Protocols.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUsbMouseDriverBinding,
+             ImageHandle,
+             &gUsbMouseComponentName,
+             &gUsbMouseComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Check whether USB mouse driver supports this device.
+
+  @param  This                   The USB mouse driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS          Status;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Use the USB I/O Protocol interface to check whether Controller is
+  // a mouse device that can be managed by this driver.
+  //
+  Status = EFI_SUCCESS;
+  if (!IsUsbMouse (UsbIo)) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiUsbIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return Status;
+}
+
+
+/**
+  Starts the mouse device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mouse device,
+  installs Simple Pointer Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mouse device.
+
+  @param  This                  The USB mouse driver binding instance.
+  @param  Controller            Handle of device to bind driver to.
+  @param  RemainingDevicePath   Optional parameter use to pick a specific child
+                                device to start.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_USB_IO_PROTOCOL         *UsbIo;
+  USB_MOUSE_DEV               *UsbMouseDevice;
+  UINT8                       EndpointNumber;
+  EFI_USB_ENDPOINT_DESCRIPTOR EndpointDescriptor;
+  UINT8                       Index;
+  UINT8                       EndpointAddr;
+  UINT8                       PollingInterval;
+  UINT8                       PacketSize;
+  BOOLEAN                     Found;
+  EFI_TPL                     OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  //
+  // Open USB I/O Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit1;
+  }
+
+  UsbMouseDevice = AllocateZeroPool (sizeof (USB_MOUSE_DEV));
+  ASSERT (UsbMouseDevice != NULL);
+
+  UsbMouseDevice->UsbIo     = UsbIo;
+  UsbMouseDevice->Signature = USB_MOUSE_DEV_SIGNATURE;
+
+  //
+  // Get the Device Path Protocol on Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &UsbMouseDevice->DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Report Status Code here since USB mouse will be detected next.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_PRESENCE_DETECT),
+    UsbMouseDevice->DevicePath
+    );
+
+  //
+  // Get interface & endpoint descriptor
+  //
+  UsbIo->UsbGetInterfaceDescriptor (
+           UsbIo,
+           &UsbMouseDevice->InterfaceDescriptor
+           );
+
+  EndpointNumber = UsbMouseDevice->InterfaceDescriptor.NumEndpoints;
+
+  //
+  // Traverse endpoints to find interrupt endpoint
+  //
+  Found = FALSE;
+  for (Index = 0; Index < EndpointNumber; Index++) {
+    UsbIo->UsbGetEndpointDescriptor (
+             UsbIo,
+             Index,
+             &EndpointDescriptor
+             );
+
+    if ((EndpointDescriptor.Attributes & (BIT0 | BIT1)) == USB_ENDPOINT_INTERRUPT) {
+      //
+      // We only care interrupt endpoint here
+      //
+      CopyMem(&UsbMouseDevice->IntEndpointDescriptor, &EndpointDescriptor, sizeof(EndpointDescriptor));
+      Found = TRUE;
+      break;
+    }
+  }
+
+  if (!Found) {
+    //
+    // Report Status Code to indicate that there is no USB mouse
+    //
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_NOT_DETECTED)
+      );
+    //
+    // No interrupt endpoint found, then return unsupported.
+    //
+    Status = EFI_UNSUPPORTED;
+    goto ErrorExit;
+  }
+
+  //
+  // Report Status Code here since USB mouse has be detected.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_DETECTED),
+    UsbMouseDevice->DevicePath
+    );
+
+  Status = InitializeUsbMouseDevice (UsbMouseDevice);
+  if (EFI_ERROR (Status)) {
+    //
+    // Fail to initialize USB mouse device.
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_INTERFACE_ERROR),
+      UsbMouseDevice->DevicePath
+      );
+
+    goto ErrorExit;
+  }
+
+  //
+  // Initialize and install EFI Simple Pointer Protocol.
+  //
+  UsbMouseDevice->SimplePointerProtocol.GetState  = GetMouseState;
+  UsbMouseDevice->SimplePointerProtocol.Reset     = UsbMouseReset;
+  UsbMouseDevice->SimplePointerProtocol.Mode      = &UsbMouseDevice->Mode;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  UsbMouseWaitForInput,
+                  UsbMouseDevice,
+                  &((UsbMouseDevice->SimplePointerProtocol).WaitForInput)
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiSimplePointerProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &UsbMouseDevice->SimplePointerProtocol
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // The next step would be submitting Asynchronous Interrupt Transfer on this mouse device.
+  // After that we will be able to get key data from it. Thus this is deemed as
+  // the enable action of the mouse, so report status code accordingly.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_ENABLE),
+    UsbMouseDevice->DevicePath
+    );
+
+  //
+  // Submit Asynchronous Interrupt Transfer to manage this device.
+  //
+  EndpointAddr    = UsbMouseDevice->IntEndpointDescriptor.EndpointAddress;
+  PollingInterval = UsbMouseDevice->IntEndpointDescriptor.Interval;
+  PacketSize      = (UINT8) (UsbMouseDevice->IntEndpointDescriptor.MaxPacketSize);
+
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    EndpointAddr,
+                    TRUE,
+                    PollingInterval,
+                    PacketSize,
+                    OnMouseInterruptComplete,
+                    UsbMouseDevice
+                    );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // If submit error, uninstall that interface
+    //
+    gBS->UninstallProtocolInterface (
+           Controller,
+           &gEfiSimplePointerProtocolGuid,
+           &UsbMouseDevice->SimplePointerProtocol
+           );
+    goto ErrorExit;
+  }
+  
+  UsbMouseDevice->ControllerNameTable = NULL;
+  AddUnicodeString2 (
+    "eng",
+    gUsbMouseComponentName.SupportedLanguages,
+    &UsbMouseDevice->ControllerNameTable,
+    L"Generic Usb Mouse",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gUsbMouseComponentName2.SupportedLanguages,
+    &UsbMouseDevice->ControllerNameTable,
+    L"Generic Usb Mouse",
+    FALSE
+    );
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+
+//
+// Error handler
+//
+ErrorExit:
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    if (UsbMouseDevice != NULL) {
+      if ((UsbMouseDevice->SimplePointerProtocol).WaitForInput != NULL) {
+        gBS->CloseEvent ((UsbMouseDevice->SimplePointerProtocol).WaitForInput);
+      }
+
+      FreePool (UsbMouseDevice);
+      UsbMouseDevice = NULL;
+    }
+  }
+
+ErrorExit1:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Stop the USB mouse device handled by this driver.
+
+  @param  This                   The USB mouse driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Simple Pointer Protocol is not installed on Controller.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  USB_MOUSE_DEV               *UsbMouseDevice;
+  EFI_SIMPLE_POINTER_PROTOCOL *SimplePointerProtocol;
+  EFI_USB_IO_PROTOCOL         *UsbIo;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSimplePointerProtocolGuid,
+                  (VOID **) &SimplePointerProtocol,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  UsbMouseDevice = USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL (SimplePointerProtocol);
+
+  UsbIo = UsbMouseDevice->UsbIo;
+
+  //
+  // The key data input from this device will be disabled.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_DISABLE),
+    UsbMouseDevice->DevicePath
+    );
+
+  //
+  // Delete the Asynchronous Interrupt Transfer from this device
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbMouseDevice->IntEndpointDescriptor.EndpointAddress,
+           FALSE,
+           UsbMouseDevice->IntEndpointDescriptor.Interval,
+           0,
+           NULL,
+           NULL
+           );
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiSimplePointerProtocolGuid,
+                  &UsbMouseDevice->SimplePointerProtocol
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  //
+  // Free all resources.
+  //
+  gBS->CloseEvent (UsbMouseDevice->SimplePointerProtocol.WaitForInput);
+
+  if (UsbMouseDevice->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbMouseDevice->DelayedRecoveryEvent);
+    UsbMouseDevice->DelayedRecoveryEvent = NULL;
+  }
+
+  if (UsbMouseDevice->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (UsbMouseDevice->ControllerNameTable);
+  }
+
+  FreePool (UsbMouseDevice);
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  Uses USB I/O to check whether the device is a USB mouse device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB mouse device.
+  @retval FALSE    Device is a not USB mouse device.
+
+**/
+BOOLEAN
+IsUsbMouse (
+  IN  EFI_USB_IO_PROTOCOL     *UsbIo
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+
+  //
+  // Get the default interface descriptor
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (
+                    UsbIo,
+                    &InterfaceDescriptor
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if ((InterfaceDescriptor.InterfaceClass == CLASS_HID) &&
+      (InterfaceDescriptor.InterfaceSubClass == SUBCLASS_BOOT) &&
+      (InterfaceDescriptor.InterfaceProtocol == PROTOCOL_MOUSE)
+      ) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Initialize the USB mouse device.
+
+  This function retrieves and parses HID report descriptor, and
+  initializes state of USB_MOUSE_DEV. Then it sets indefinite idle
+  rate for the device. Finally it creates event for delayed recovery,
+  which deals with device error.
+
+  @param  UsbMouseDev           Device instance to be initialized.
+
+  @retval EFI_SUCCESS           USB mouse device successfully initialized..
+  @retval EFI_UNSUPPORTED       HID descriptor type is not report descriptor.
+  @retval Other                 USB mouse device was not initialized successfully.
+
+**/
+EFI_STATUS
+InitializeUsbMouseDevice (
+  IN OUT USB_MOUSE_DEV           *UsbMouseDev
+  )
+{
+  EFI_USB_IO_PROTOCOL       *UsbIo;
+  UINT8                     Protocol;
+  EFI_STATUS                Status;
+  EFI_USB_HID_DESCRIPTOR    *MouseHidDesc;
+  UINT8                     *ReportDesc;
+  EFI_USB_CONFIG_DESCRIPTOR ConfigDesc;
+  VOID                      *Buf;
+  UINT32                    TransferResult;
+  UINT16                    Total;
+  USB_DESC_HEAD             *Head;
+  BOOLEAN                   Start;
+
+  UsbIo = UsbMouseDev->UsbIo;
+
+  //
+  // Get the current configuration descriptor. Note that it doesn't include other descriptors.
+  //
+  Status = UsbIo->UsbGetConfigDescriptor (
+                    UsbIo,
+                    &ConfigDesc
+                    );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // By issuing Get_Descriptor(Configuration) request with total length, we get the Configuration descriptor,
+  // all Interface descriptors, all Endpoint descriptors, and the HID descriptor for each interface.
+  //
+  Buf = AllocateZeroPool (ConfigDesc.TotalLength);
+  if (Buf == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = UsbGetDescriptor (
+             UsbIo,
+             (UINT16)((USB_DESC_TYPE_CONFIG << 8) | (ConfigDesc.ConfigurationValue - 1)),
+             0,
+             ConfigDesc.TotalLength,
+             Buf,
+             &TransferResult
+             );
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    return Status;
+  }
+
+  Total = 0;
+  Start = FALSE;
+  Head  = (USB_DESC_HEAD *)Buf;  
+  MouseHidDesc = NULL;
+
+  //
+  // Get HID descriptor from the receipt of Get_Descriptor(Configuration) request.
+  // This algorithm is based on the fact that the HID descriptor shall be interleaved
+  // between the interface and endpoint descriptors for HID interfaces.
+  //
+  while (Total < ConfigDesc.TotalLength) {
+    if (Head->Type == USB_DESC_TYPE_INTERFACE) {
+      if ((((USB_INTERFACE_DESCRIPTOR *)Head)->InterfaceNumber == UsbMouseDev->InterfaceDescriptor.InterfaceNumber) &&
+        (((USB_INTERFACE_DESCRIPTOR *)Head)->AlternateSetting == UsbMouseDev->InterfaceDescriptor.AlternateSetting)) {
+        Start = TRUE;
+      }
+    }
+    if (Start && (Head->Type == USB_DESC_TYPE_ENDPOINT)) {
+      break;
+    }
+    if (Start && (Head->Type == USB_DESC_TYPE_HID)) {
+      MouseHidDesc = (EFI_USB_HID_DESCRIPTOR *)Head;
+      break;
+    }
+    Total = Total + (UINT16)Head->Len;
+    Head  = (USB_DESC_HEAD*)((UINT8 *)Buf + Total);
+  }
+
+  if (MouseHidDesc == NULL) {
+    FreePool (Buf);
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get report descriptor
+  //
+  if (MouseHidDesc->HidClassDesc[0].DescriptorType != USB_DESC_TYPE_REPORT) {
+    FreePool (Buf);
+    return EFI_UNSUPPORTED;
+  }
+
+  ReportDesc = AllocateZeroPool (MouseHidDesc->HidClassDesc[0].DescriptorLength);
+  ASSERT (ReportDesc != NULL);
+
+  Status = UsbGetReportDescriptor (
+             UsbIo,
+             UsbMouseDev->InterfaceDescriptor.InterfaceNumber,
+             MouseHidDesc->HidClassDesc[0].DescriptorLength,
+             ReportDesc
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    FreePool (ReportDesc);
+    return Status;
+  }
+
+  //
+  // Parse report descriptor
+  //
+  Status = ParseMouseReportDescriptor (
+             UsbMouseDev,
+             ReportDesc,
+             MouseHidDesc->HidClassDesc[0].DescriptorLength
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    FreePool (ReportDesc);
+    return Status;
+  }
+
+  //
+  // Check the presence of left and right buttons,
+  // and initialize fields of EFI_SIMPLE_POINTER_MODE.
+  //
+  if (UsbMouseDev->NumberOfButtons >= 1) {
+    UsbMouseDev->Mode.LeftButton = TRUE;
+  }
+  if (UsbMouseDev->NumberOfButtons > 1) {
+    UsbMouseDev->Mode.RightButton = TRUE;
+  }
+  UsbMouseDev->Mode.ResolutionX = 8;
+  UsbMouseDev->Mode.ResolutionY = 8;
+  UsbMouseDev->Mode.ResolutionZ = 0;
+
+  //
+  // Set boot protocol for the USB mouse.
+  // This driver only supports boot protocol.
+  //
+  UsbGetProtocolRequest (
+    UsbIo,
+    UsbMouseDev->InterfaceDescriptor.InterfaceNumber,
+    &Protocol
+    );
+  if (Protocol != BOOT_PROTOCOL) {
+    Status = UsbSetProtocolRequest (
+               UsbIo,
+               UsbMouseDev->InterfaceDescriptor.InterfaceNumber,
+               BOOT_PROTOCOL
+               );
+
+    if (EFI_ERROR (Status)) {
+      FreePool (Buf);
+      FreePool (ReportDesc);
+      return Status;
+    }
+  }
+
+  FreePool (Buf);
+  FreePool (ReportDesc);
+
+  //
+  // Create event for delayed recovery, which deals with device error.
+  //
+  if (UsbMouseDev->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbMouseDev->DelayedRecoveryEvent);
+    UsbMouseDev->DelayedRecoveryEvent = 0;
+  }
+
+  gBS->CreateEvent (
+         EVT_TIMER | EVT_NOTIFY_SIGNAL,
+         TPL_NOTIFY,
+         USBMouseRecoveryHandler,
+         UsbMouseDev,
+         &UsbMouseDev->DelayedRecoveryEvent
+         );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Handler function for USB mouse's asynchronous interrupt transfer.
+
+  This function is the handler function for USB mouse's asynchronous interrupt transfer
+  to manage the mouse. It parses data returned from asynchronous interrupt transfer, and
+  get button and movement state.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+OnMouseInterruptComplete (
+  IN  VOID        *Data,
+  IN  UINTN       DataLength,
+  IN  VOID        *Context,
+  IN  UINT32      Result
+  )
+{
+  USB_MOUSE_DEV       *UsbMouseDevice;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+  UINT8               EndpointAddr;
+  UINT32              UsbResult;
+
+  UsbMouseDevice  = (USB_MOUSE_DEV *) Context;
+  UsbIo           = UsbMouseDevice->UsbIo;
+
+  if (Result != EFI_USB_NOERROR) {
+    //
+    // Some errors happen during the process
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_INPUT_ERROR),
+      UsbMouseDevice->DevicePath
+      );
+
+    if ((Result & EFI_USB_ERR_STALL) == EFI_USB_ERR_STALL) {
+      EndpointAddr = UsbMouseDevice->IntEndpointDescriptor.EndpointAddress;
+
+      UsbClearEndpointHalt (
+        UsbIo,
+        EndpointAddr,
+        &UsbResult
+        );
+    }
+
+    //
+    // Delete & Submit this interrupt again
+    // Handler of DelayedRecoveryEvent triggered by timer will re-submit the interrupt. 
+    //
+    UsbIo->UsbAsyncInterruptTransfer (
+             UsbIo,
+             UsbMouseDevice->IntEndpointDescriptor.EndpointAddress,
+             FALSE,
+             0,
+             0,
+             NULL,
+             NULL
+             );
+    //
+    // EFI_USB_INTERRUPT_DELAY is defined in USB standard for error handling.
+    //
+    gBS->SetTimer (
+           UsbMouseDevice->DelayedRecoveryEvent,
+           TimerRelative,
+           EFI_USB_INTERRUPT_DELAY
+           );
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // If no error and no data, just return EFI_SUCCESS.
+  //
+  if (DataLength == 0 || Data == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  UsbMouseDevice->StateChanged = TRUE;
+
+  //
+  // Check mouse Data
+  // USB HID Specification specifies following data format:
+  // Byte    Bits    Description
+  // 0       0       Button 1
+  //         1       Button 2
+  //         2       Button 3
+  //         4 to 7  Device-specific
+  // 1       0 to 7  X displacement
+  // 2       0 to 7  Y displacement
+  // 3 to n  0 to 7  Device specific (optional)
+  //
+  UsbMouseDevice->State.LeftButton  = (BOOLEAN) ((*(UINT8 *) Data & BIT0) != 0);
+  UsbMouseDevice->State.RightButton = (BOOLEAN) ((*(UINT8 *) Data & BIT1) != 0);
+  UsbMouseDevice->State.RelativeMovementX += *((INT8 *) Data + 1);
+  UsbMouseDevice->State.RelativeMovementY += *((INT8 *) Data + 2);
+
+  if (DataLength > 3) {
+    UsbMouseDevice->State.RelativeMovementZ += *((INT8 *) Data + 3);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the current state of a pointer device.
+    
+  @param  This                  A pointer to the EFI_SIMPLE_POINTER_PROTOCOL instance.                                   
+  @param  MouseState            A pointer to the state information on the pointer device.
+                                
+  @retval EFI_SUCCESS           The state of the pointer device was returned in State.
+  @retval EFI_NOT_READY         The state of the pointer device has not changed since the last call to
+                                GetState().                                                           
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to retrieve the pointer device's
+                                current state.                                                           
+  @retval EFI_INVALID_PARAMETER MouseState is NULL.                                                           
+
+**/
+EFI_STATUS
+EFIAPI
+GetMouseState (
+  IN   EFI_SIMPLE_POINTER_PROTOCOL  *This,
+  OUT  EFI_SIMPLE_POINTER_STATE     *MouseState
+  )
+{
+  USB_MOUSE_DEV *MouseDev;
+
+  if (MouseState == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MouseDev = USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL (This);
+
+  if (!MouseDev->StateChanged) {
+    return EFI_NOT_READY;
+  }
+
+  //
+  // Retrieve mouse state from USB_MOUSE_DEV, which was filled by OnMouseInterruptComplete()
+  //
+  CopyMem (
+    MouseState,
+    &MouseDev->State,
+    sizeof (EFI_SIMPLE_POINTER_STATE)
+    );
+
+  //
+  // Clear previous move state
+  //
+  MouseDev->State.RelativeMovementX = 0;
+  MouseDev->State.RelativeMovementY = 0;
+  MouseDev->State.RelativeMovementZ = 0;
+
+  MouseDev->StateChanged            = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+
+/**                                                                 
+  Resets the pointer device hardware.
+  
+  @param  This                  A pointer to the EFI_SIMPLE_POINTER_PROTOCOL instance.
+  @param  ExtendedVerification  Indicates that the driver may perform a more exhaustive
+                                verification operation of the device during reset.
+                                
+  @retval EFI_SUCCESS           The device was reset.
+  @retval EFI_DEVICE_ERROR      The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseReset (
+  IN EFI_SIMPLE_POINTER_PROTOCOL    *This,
+  IN BOOLEAN                        ExtendedVerification
+  )
+{
+  USB_MOUSE_DEV       *UsbMouseDevice;
+
+  UsbMouseDevice  = USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL (This);
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_RESET),
+    UsbMouseDevice->DevicePath
+    );
+
+  //
+  // Clear mouse state.
+  //
+  ZeroMem (
+    &UsbMouseDevice->State,
+    sizeof (EFI_SIMPLE_POINTER_STATE)
+    );
+  UsbMouseDevice->StateChanged = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Event notification function for EFI_SIMPLE_POINTER_PROTOCOL.WaitForInput event.
+
+  @param  Event        Event to be signaled when there's input from mouse.
+  @param  Context      Points to USB_MOUSE_DEV instance.
+ 
+**/
+VOID
+EFIAPI
+UsbMouseWaitForInput (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  )
+{
+  USB_MOUSE_DEV *UsbMouseDev;
+
+  UsbMouseDev = (USB_MOUSE_DEV *) Context;
+
+  //
+  // If there's input from mouse, signal the event.
+  //
+  if (UsbMouseDev->StateChanged) {
+    gBS->SignalEvent (Event);
+  }
+}
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event              The Delayed Recovery event.
+  @param  Context            Points to the USB_MOUSE_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBMouseRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  )
+{
+  USB_MOUSE_DEV       *UsbMouseDev;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+
+  UsbMouseDev = (USB_MOUSE_DEV *) Context;
+
+  UsbIo       = UsbMouseDev->UsbIo;
+
+  //
+  // Re-submit Asynchronous Interrupt Transfer for recovery.
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbMouseDev->IntEndpointDescriptor.EndpointAddress,
+           TRUE,
+           UsbMouseDev->IntEndpointDescriptor.Interval,
+           UsbMouseDev->IntEndpointDescriptor.MaxPacketSize,
+           OnMouseInterruptComplete,
+           UsbMouseDev
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.h b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.h
new file mode 100644
index 0000000000..0dab9de11d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.h
@@ -0,0 +1,471 @@
+/** @file
+  Helper routine and corresponding data struct used by USB Mouse Driver.
+
+Copyright (c) 2004 - 2012, Intel Corporation. All rights reserved.<BR>
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_USB_MOUSE_H_
+#define _EFI_USB_MOUSE_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/SimplePointer.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiUsbLib.h>
+#include <Library/DebugLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#define CLASS_HID               3
+#define SUBCLASS_BOOT           1
+#define PROTOCOL_MOUSE          2
+
+#define BOOT_PROTOCOL           0
+#define REPORT_PROTOCOL         1
+
+#define USB_MOUSE_DEV_SIGNATURE SIGNATURE_32 ('u', 'm', 'o', 'u')
+
+//
+// A common header for usb standard descriptor.
+// Each stand descriptor has a length and type.
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Len;
+  UINT8                   Type;
+} USB_DESC_HEAD;
+#pragma pack()
+
+///
+/// Button range and status
+///
+typedef struct {
+  BOOLEAN ButtonDetected;
+  UINT8   ButtonMinIndex;
+  UINT8   ButtonMaxIndex;
+  UINT8   Reserved;
+} USB_MOUSE_BUTTON_DATA;
+
+///
+/// Device instance of USB mouse.
+///
+typedef struct {
+  UINTN                         Signature;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+  EFI_EVENT                     DelayedRecoveryEvent;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+  EFI_USB_ENDPOINT_DESCRIPTOR   IntEndpointDescriptor;
+  UINT8                         NumberOfButtons;
+  INT32                         XLogicMax;
+  INT32                         XLogicMin;
+  INT32                         YLogicMax;
+  INT32                         YLogicMin;
+  EFI_SIMPLE_POINTER_PROTOCOL   SimplePointerProtocol;
+  EFI_SIMPLE_POINTER_STATE      State;
+  EFI_SIMPLE_POINTER_MODE       Mode;
+  BOOLEAN                       StateChanged;
+  USB_MOUSE_BUTTON_DATA         PrivateData;
+  EFI_UNICODE_STRING_TABLE      *ControllerNameTable;
+} USB_MOUSE_DEV;
+
+///
+/// General HID Item structure
+///
+
+typedef union {
+  UINT8   Uint8;
+  UINT16  Uint16;
+  UINT32  Uint32;
+  INT8    Int8;
+  INT16   Int16;
+  INT32   Int32;
+  UINT8   *LongData;
+} HID_DATA;
+
+typedef struct {
+  UINT16    Format;
+  UINT8     Size;
+  UINT8     Type;
+  UINT8     Tag;
+  HID_DATA  Data;
+} HID_ITEM;
+
+#define USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL(a) \
+    CR(a, USB_MOUSE_DEV, SimplePointerProtocol, USB_MOUSE_DEV_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gUsbMouseDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gUsbMouseComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUsbMouseComponentName2;
+
+//
+// Functions of Driver Binding Protocol
+//
+
+/**
+  Check whether USB mouse driver supports this device.
+
+  @param  This                   The USB mouse driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Starts the mouse device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mouse device,
+  installs Simple Pointer Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mouse device.
+
+  @param  This                  The USB mouse driver binding instance.
+  @param  Controller            Handle of device to bind driver to.
+  @param  RemainingDevicePath   Optional parameter use to pick a specific child
+                                device to start.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop the USB mouse device handled by this driver.
+
+  @param  This                   The USB mouse driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Simple Pointer Protocol is not installed on Controller.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+//
+// Functions of EFI_SIMPLE_POINTER_PROTOCOL
+//
+
+/**
+  Retrieves the current state of a pointer device.
+    
+  @param  This                  A pointer to the EFI_SIMPLE_POINTER_PROTOCOL instance.                                   
+  @param  MouseState            A pointer to the state information on the pointer device.
+                                
+  @retval EFI_SUCCESS           The state of the pointer device was returned in State.
+  @retval EFI_NOT_READY         The state of the pointer device has not changed since the last call to
+                                GetState().                                                           
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to retrieve the pointer device's
+                                current state.                                                           
+  @retval EFI_INVALID_PARAMETER MouseState is NULL.                                                           
+
+**/
+EFI_STATUS
+EFIAPI
+GetMouseState (
+  IN   EFI_SIMPLE_POINTER_PROTOCOL  *This,
+  OUT  EFI_SIMPLE_POINTER_STATE     *MouseState
+  );
+
+/**                                                                 
+  Resets the pointer device hardware.
+  
+  @param  This                  A pointer to the EFI_SIMPLE_POINTER_PROTOCOL instance.
+  @param  ExtendedVerification  Indicates that the driver may perform a more exhaustive
+                                verification operation of the device during reset.
+                                
+  @retval EFI_SUCCESS           The device was reset.
+  @retval EFI_DEVICE_ERROR      The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseReset (
+  IN EFI_SIMPLE_POINTER_PROTOCOL    *This,
+  IN BOOLEAN                        ExtendedVerification
+  );
+
+/**
+  Event notification function for SIMPLE_POINTER.WaitForInput event.
+
+  @param  Event        Event to be signaled when there's input from mouse.
+  @param  Context      Points to USB_MOUSE_DEV instance.
+ 
+**/
+VOID
+EFIAPI
+UsbMouseWaitForInput (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  );
+
+//
+// Internal worker functions
+//
+
+/**
+  Uses USB I/O to check whether the device is a USB mouse device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB mouse device.
+  @retval FALSE    Device is a not USB mouse device.
+
+**/
+BOOLEAN
+IsUsbMouse (
+  IN  EFI_USB_IO_PROTOCOL     *UsbIo
+  );
+
+/**
+  Initialize the USB mouse device.
+
+  This function retrieves and parses HID report descriptor, and
+  initializes state of USB_MOUSE_DEV. Then it sets indefinite idle
+  rate for the device. Finally it creates event for delayed recovery,
+  which deals with device error.
+
+  @param  UsbMouseDev           Device instance to be initialized.
+
+  @retval EFI_SUCCESS           USB mouse device successfully initialized..
+  @retval EFI_UNSUPPORTED       HID descriptor type is not report descriptor.
+  @retval Other                 USB mouse device was not initialized successfully.
+
+**/
+EFI_STATUS
+InitializeUsbMouseDevice (
+  IN OUT USB_MOUSE_DEV           *UsbMouseDev
+  );
+
+/**
+  Handler function for USB mouse's asynchronous interrupt transfer.
+
+  This function is the handler function for USB mouse's asynchronous interrupt transfer
+  to manage the mouse. It parses data returned from asynchronous interrupt transfer, and
+  get button and movement state.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+OnMouseInterruptComplete (
+  IN  VOID        *Data,
+  IN  UINTN       DataLength,
+  IN  VOID        *Context,
+  IN  UINT32      Result
+  );
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event              The Delayed Recovery event.
+  @param  Context            Points to the USB_MOUSE_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBMouseRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  );
+
+/**
+  Parse Mouse Report Descriptor.
+
+  According to USB HID Specification, report descriptors are
+  composed of pieces of information. Each piece of information
+  is called an Item. This function retrieves each item from
+  the report descriptor and updates USB_MOUSE_DEV.
+
+  @param  UsbMouse          The instance of USB_MOUSE_DEV
+  @param  ReportDescriptor  Report descriptor to parse
+  @param  ReportSize        Report descriptor size
+
+  @retval EFI_SUCCESS       Report descriptor successfully parsed.
+  @retval EFI_UNSUPPORTED   Report descriptor contains long item.
+
+**/
+EFI_STATUS
+ParseMouseReportDescriptor (
+  OUT USB_MOUSE_DEV   *UsbMouse,
+  IN  UINT8           *ReportDescriptor,
+  IN  UINTN           ReportSize
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.inf
new file mode 100644
index 0000000000..13d8859f98
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.inf
@@ -0,0 +1,72 @@
+## @file
+# USB Mouse Driver that manages USB mouse and produces Simple Pointer Protocol.
+#
+# USB Mouse Driver consumes USB I/O Protocol and Device Path Protocol, and produces
+# Simple Pointer Protocol on USB mouse devices.
+# It manages the USB mouse device via Asynchronous Interrupt Transfer of USB I/O Protocol,
+# and parses the data according to USB HID Specification.
+# This module refers to following specifications:
+# 1. Universal Serial Bus HID Firmware Specification, ver 1.11
+# 2. UEFI Specification, v2.1
+#
+# Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbMouseDxe
+  MODULE_UNI_FILE                = UsbMouseDxe.uni
+  FILE_GUID                      = 2D2E62AA-9ECF-43b7-8219-94E7FC713DFE
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = USBMouseDriverBindingEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUsbMouseDriverBinding                        
+#  COMPONENT_NAME                =  gUsbMouseComponentName
+#  COMPONENT_NAME2               =  gUsbMouseComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  MouseHid.c
+  UsbMouse.c
+  UsbMouse.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  ReportStatusCodeLib
+  UefiUsbLib
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiSimplePointerProtocolGuid                 ## BY_START
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER        ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbMouseDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.uni
new file mode 100644
index 0000000000..6da63cbd48
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxeExtra.uni
new file mode 100644
index 0000000000..6aab5aaa10
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxeExtra.uni