]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.c
MdeModulePkg: Replace BSD License with BSD+Patent License
[mirror_edk2.git] / MdeModulePkg / Bus / Pci / EhciDxe / EhciReg.c
index 290cb3847a5d7825bb3a9f295260b187922adf0a..ca63736f20f4b0726b5b4ecc82f1fcea3f7e5c7b 100644 (file)
@@ -1,43 +1,65 @@
 /** @file\r
 \r
-Copyright (c) 2007, Intel Corporation\r
-All rights reserved. This program and the accompanying materials\r
-are licensed and made available under the terms and conditions of the BSD License\r
-which accompanies this distribution.  The full text of the license may be found at\r
-http://opensource.org/licenses/bsd-license.php\r
+  The EHCI register operation routines.\r
 \r
-THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
-WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
+Copyright (c) 2007 - 2017, Intel Corporation. All rights reserved.<BR>\r
+SPDX-License-Identifier: BSD-2-Clause-Patent\r
 \r
-Module Name:\r
+**/\r
 \r
-    EhciReg.c\r
 \r
-Abstract:\r
+#include "Ehci.h"\r
 \r
-    The EHCI register operation routines.\r
 \r
+/**\r
+  Read EHCI capability register.\r
 \r
-Revision History\r
+  @param  Ehc          The EHCI device.\r
+  @param  Offset       Capability register address.\r
+\r
+  @return The register content read.\r
+  @retval If err, return 0xffff.\r
 \r
 **/\r
+UINT32\r
+EhcReadCapRegister (\r
+  IN  USB2_HC_DEV         *Ehc,\r
+  IN  UINT32              Offset\r
+  )\r
+{\r
+  UINT32                  Data;\r
+  EFI_STATUS              Status;\r
 \r
+  Status = Ehc->PciIo->Mem.Read (\r
+                             Ehc->PciIo,\r
+                             EfiPciIoWidthUint32,\r
+                             EHC_BAR_INDEX,\r
+                             (UINT64) Offset,\r
+                             1,\r
+                             &Data\r
+                             );\r
 \r
-#include "Ehci.h"\r
+  if (EFI_ERROR (Status)) {\r
+    DEBUG ((EFI_D_ERROR, "EhcReadCapRegister: Pci Io read error - %r at %d\n", Status, Offset));\r
+    Data = 0xFFFF;\r
+  }\r
 \r
+  return Data;\r
+}\r
 \r
 /**\r
-  Read  EHCI capability register\r
+  Read EHCI debug port register.\r
 \r
-  @param  Ehc          The Ehc device\r
-  @param  Offset       Capability register address\r
+  @param  Ehc          The EHCI device.\r
+  @param  Offset       Debug port register offset.\r
 \r
-  @return The register content read\r
+  @return The register content read.\r
+  @retval If err, return 0xffff.\r
 \r
 **/\r
 UINT32\r
-EhcReadCapRegister (\r
-  IN  USB2_HC_DEV         *Ehc,\r
+EhcReadDbgRegister (\r
+  IN  CONST USB2_HC_DEV   *Ehc,\r
   IN  UINT32              Offset\r
   )\r
 {\r
@@ -47,14 +69,14 @@ EhcReadCapRegister (
   Status = Ehc->PciIo->Mem.Read (\r
                              Ehc->PciIo,\r
                              EfiPciIoWidthUint32,\r
-                             EHC_BAR_INDEX,\r
-                             (UINT64) Offset,\r
+                             Ehc->DebugPortBarNum,\r
+                             Ehc->DebugPortOffset + Offset,\r
                              1,\r
                              &Data\r
                              );\r
 \r
   if (EFI_ERROR (Status)) {\r
-    EHC_ERROR (("EhcReadCapRegister: Pci Io read error - %r at %d\n", Status, Offset));\r
+    DEBUG ((EFI_D_ERROR, "EhcReadDbgRegister: Pci Io read error - %r at %d\n", Status, Offset));\r
     Data = 0xFFFF;\r
   }\r
 \r
@@ -63,12 +85,66 @@ EhcReadCapRegister (
 \r
 \r
 /**\r
-  Read  Ehc Operation register\r
+  Check whether the host controller has an in-use debug port.\r
+\r
+  @param[in] Ehc         The Enhanced Host Controller to query.\r
+\r
+  @param[in] PortNumber  If PortNumber is not NULL, then query whether\r
+                         PortNumber is an in-use debug port on Ehc. (PortNumber\r
+                         is taken in UEFI notation, i.e., zero-based.)\r
+                         Otherwise, query whether Ehc has any in-use debug\r
+                         port.\r
+\r
+  @retval TRUE   PortNumber is an in-use debug port on Ehc (if PortNumber is\r
+                 not NULL), or some port on Ehc is an in-use debug port\r
+                 (otherwise).\r
+\r
+  @retval FALSE  PortNumber is not an in-use debug port on Ehc (if PortNumber\r
+                 is not NULL), or no port on Ehc is an in-use debug port\r
+                 (otherwise).\r
+**/\r
+BOOLEAN\r
+EhcIsDebugPortInUse (\r
+  IN CONST USB2_HC_DEV *Ehc,\r
+  IN CONST UINT8       *PortNumber OPTIONAL\r
+  )\r
+{\r
+  UINT32 State;\r
+\r
+  if (Ehc->DebugPortNum == 0) {\r
+    //\r
+    // The host controller has no debug port.\r
+    //\r
+    return FALSE;\r
+  }\r
+\r
+  //\r
+  // The Debug Port Number field in HCSPARAMS is one-based.\r
+  //\r
+  if (PortNumber != NULL && *PortNumber != Ehc->DebugPortNum - 1) {\r
+    //\r
+    // The caller specified a port, but it's not the debug port of the host\r
+    // controller.\r
+    //\r
+    return FALSE;\r
+  }\r
+\r
+  //\r
+  // Deduce usage from the Control Register.\r
+  //\r
+  State = EhcReadDbgRegister(Ehc, 0);\r
+  return (State & USB_DEBUG_PORT_IN_USE_MASK) == USB_DEBUG_PORT_IN_USE_MASK;\r
+}\r
+\r
+\r
+/**\r
+  Read EHCI Operation register.\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Offset       The operation register offset\r
+  @param  Ehc          The EHCI device.\r
+  @param  Offset       The operation register offset.\r
 \r
-  @return The register content read\r
+  @return The register content read.\r
+  @retval If err, return 0xffff.\r
 \r
 **/\r
 UINT32\r
@@ -86,13 +162,13 @@ EhcReadOpReg (
                              Ehc->PciIo,\r
                              EfiPciIoWidthUint32,\r
                              EHC_BAR_INDEX,\r
-                             (UINT64) (Ehc->CapLen + Offset),\r
+                             Ehc->CapLen + Offset,\r
                              1,\r
                              &Data\r
                              );\r
 \r
   if (EFI_ERROR (Status)) {\r
-    EHC_ERROR (("EhcReadOpReg: Pci Io Read error - %r at %d\n", Status, Offset));\r
+    DEBUG ((EFI_D_ERROR, "EhcReadOpReg: Pci Io Read error - %r at %d\n", Status, Offset));\r
     Data = 0xFFFF;\r
   }\r
 \r
@@ -101,13 +177,11 @@ EhcReadOpReg (
 \r
 \r
 /**\r
-  Write  the data to the EHCI operation register\r
-\r
-  @param  Ehc          The EHCI device\r
-  @param  Offset       EHCI operation register offset\r
-  @param  Data         The data to write\r
+  Write  the data to the EHCI operation register.\r
 \r
-  @return None\r
+  @param  Ehc          The EHCI device.\r
+  @param  Offset       EHCI operation register offset.\r
+  @param  Data         The data to write.\r
 \r
 **/\r
 VOID\r
@@ -125,28 +199,25 @@ EhcWriteOpReg (
                              Ehc->PciIo,\r
                              EfiPciIoWidthUint32,\r
                              EHC_BAR_INDEX,\r
-                             (UINT64) (Ehc->CapLen + Offset),\r
+                             Ehc->CapLen + Offset,\r
                              1,\r
                              &Data\r
                              );\r
 \r
   if (EFI_ERROR (Status)) {\r
-    EHC_ERROR (("EhcWriteOpReg: Pci Io Write error: %r at %d\n", Status, Offset));\r
+    DEBUG ((EFI_D_ERROR, "EhcWriteOpReg: Pci Io Write error: %r at %d\n", Status, Offset));\r
   }\r
 }\r
 \r
 \r
 /**\r
-  Set one bit of the operational register while keeping other bits\r
+  Set one bit of the operational register while keeping other bits.\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Offset       The offset of the operational register\r
-  @param  Bit          The bit mask of the register to set\r
-\r
-  @return None\r
+  @param  Ehc          The EHCI device.\r
+  @param  Offset       The offset of the operational register.\r
+  @param  Bit          The bit mask of the register to set.\r
 \r
 **/\r
-STATIC\r
 VOID\r
 EhcSetOpRegBit (\r
   IN USB2_HC_DEV          *Ehc,\r
@@ -163,16 +234,13 @@ EhcSetOpRegBit (
 \r
 \r
 /**\r
-  Clear one bit of the operational register while keeping other bits\r
+  Clear one bit of the operational register while keeping other bits.\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Offset       The offset of the operational register\r
-  @param  Bit          The bit mask of the register to clear\r
-\r
-  @return None\r
+  @param  Ehc          The EHCI device.\r
+  @param  Offset       The offset of the operational register.\r
+  @param  Bit          The bit mask of the register to clear.\r
 \r
 **/\r
-STATIC\r
 VOID\r
 EhcClearOpRegBit (\r
   IN USB2_HC_DEV          *Ehc,\r
@@ -190,19 +258,18 @@ EhcClearOpRegBit (
 \r
 /**\r
   Wait the operation register's bit as specified by Bit\r
-  to become set (or clear)\r
+  to become set (or clear).\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Offset       The offset of the operation register\r
-  @param  Bit          The bit of the register to wait for\r
-  @param  WaitToSet    Wait the bit to set or clear\r
-  @param  Timeout      The time to wait before abort (in millisecond)\r
+  @param  Ehc          The EHCI device.\r
+  @param  Offset       The offset of the operation register.\r
+  @param  Bit          The bit of the register to wait for.\r
+  @param  WaitToSet    Wait the bit to set or clear.\r
+  @param  Timeout      The time to wait before abort (in millisecond).\r
 \r
-  @retval EFI_SUCCESS  The bit successfully changed by host controller\r
-  @retval EFI_TIMEOUT  The time out occurred\r
+  @retval EFI_SUCCESS  The bit successfully changed by host controller.\r
+  @retval EFI_TIMEOUT  The time out occurred.\r
 \r
 **/\r
-STATIC\r
 EFI_STATUS\r
 EhcWaitOpRegBit (\r
   IN USB2_HC_DEV          *Ehc,\r
@@ -214,12 +281,12 @@ EhcWaitOpRegBit (
 {\r
   UINT32                  Index;\r
 \r
-  for (Index = 0; Index < Timeout / EHC_SYNC_POLL_TIME + 1; Index++) {\r
+  for (Index = 0; Index < Timeout / EHC_SYNC_POLL_INTERVAL + 1; Index++) {\r
     if (EHC_REG_BIT_IS_SET (Ehc, Offset, Bit) == WaitToSet) {\r
       return EFI_SUCCESS;\r
     }\r
 \r
-    gBS->Stall (EHC_SYNC_POLL_TIME);\r
+    gBS->Stall (EHC_SYNC_POLL_INTERVAL);\r
   }\r
 \r
   return EFI_TIMEOUT;\r
@@ -228,12 +295,10 @@ EhcWaitOpRegBit (
 \r
 /**\r
   Add support for UEFI Over Legacy (UoL) feature, stop\r
-  the legacy USB SMI support\r
+  the legacy USB SMI support.\r
 \r
   @param  Ehc          The EHCI device.\r
 \r
-  @return None\r
-\r
 **/\r
 VOID\r
 EhcClearLegacySupport (\r
@@ -245,7 +310,7 @@ EhcClearLegacySupport (
   UINT32                    Value;\r
   UINT32                    TimeOut;\r
 \r
-  EHC_DEBUG (("EhcClearLegacySupport: called to clear legacy support\n"));\r
+  DEBUG ((EFI_D_INFO, "EhcClearLegacySupport: called to clear legacy support\n"));\r
 \r
   PciIo     = Ehc->PciIo;\r
   ExtendCap = (Ehc->HcCapParams >> 8) & 0xFF;\r
@@ -258,7 +323,7 @@ EhcClearLegacySupport (
   PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);\r
 \r
   TimeOut = 40;\r
-  while (TimeOut--) {\r
+  while (TimeOut-- != 0) {\r
     gBS->Stall (500);\r
 \r
     PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);\r
@@ -278,11 +343,11 @@ EhcClearLegacySupport (
   Set door bell and wait it to be ACKed by host controller.\r
   This function is used to synchronize with the hardware.\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Timeout      The time to wait before abort (in millisecond, ms)\r
+  @param  Ehc          The EHCI device.\r
+  @param  Timeout      The time to wait before abort (in millisecond, ms).\r
 \r
-  @return EFI_SUCCESS : Synchronized with the hardware\r
-  @return EFI_TIMEOUT : Time out happened while waiting door bell to set\r
+  @retval EFI_SUCCESS  Synchronized with the hardware.\r
+  @retval EFI_TIMEOUT  Time out happened while waiting door bell to set.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -314,11 +379,9 @@ EhcSetAndWaitDoorBell (
 \r
 /**\r
   Clear all the interrutp status bits, these bits\r
-  are Write-Clean\r
+  are Write-Clean.\r
 \r
-  @param  Ehc          The EHCI device\r
-\r
-  @return None\r
+  @param  Ehc          The EHCI device.\r
 \r
 **/\r
 VOID\r
@@ -334,14 +397,13 @@ EhcAckAllInterrupt (
   Enable the periodic schedule then wait EHC to\r
   actually enable it.\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Timeout      The time to wait before abort (in millisecond, ms)\r
+  @param  Ehc          The EHCI device.\r
+  @param  Timeout      The time to wait before abort (in millisecond, ms).\r
 \r
-  @return EFI_SUCCESS : The periodical schedule is enabled\r
-  @return EFI_TIMEOUT : Time out happened while enabling periodic schedule\r
+  @retval EFI_SUCCESS  The periodical schedule is enabled.\r
+  @retval EFI_TIMEOUT  Time out happened while enabling periodic schedule.\r
 \r
 **/\r
-STATIC\r
 EFI_STATUS\r
 EhcEnablePeriodSchd (\r
   IN USB2_HC_DEV          *Ehc,\r
@@ -358,43 +420,19 @@ EhcEnablePeriodSchd (
 \r
 \r
 \r
-/**\r
-  Disable periodic schedule\r
-\r
-  @param  Ehc          The EHCI device\r
-  @param  Timeout      Time to wait before abort (in millisecond, ms)\r
-\r
-  @return EFI_SUCCESS      : Periodic schedule is disabled.\r
-  @return EFI_DEVICE_ERROR : Fail to disable periodic schedule\r
-\r
-**/\r
-EFI_STATUS\r
-EhcDisablePeriodSchd (\r
-  IN USB2_HC_DEV          *Ehc,\r
-  IN UINT32               Timeout\r
-  )\r
-{\r
-  EFI_STATUS              Status;\r
-\r
-  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_PERIOD);\r
-\r
-  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_PERIOD_ENABLED, FALSE, Timeout);\r
-  return Status;\r
-}\r
 \r
 \r
 \r
 /**\r
-  Enable asynchrounous schedule\r
+  Enable asynchrounous schedule.\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Timeout      Time to wait before abort\r
+  @param  Ehc          The EHCI device.\r
+  @param  Timeout      Time to wait before abort.\r
 \r
-  @return EFI_SUCCESS : The EHCI asynchronous schedule is enabled\r
-  @return Others      : Failed to enable the asynchronous scheudle\r
+  @retval EFI_SUCCESS  The EHCI asynchronous schedule is enabled.\r
+  @return Others       Failed to enable the asynchronous scheudle.\r
 \r
 **/\r
-STATIC\r
 EFI_STATUS\r
 EhcEnableAsyncSchd (\r
   IN USB2_HC_DEV          *Ehc,\r
@@ -411,39 +449,17 @@ EhcEnableAsyncSchd (
 \r
 \r
 \r
-/**\r
-  Disable asynchrounous schedule\r
-\r
-  @param  Ehc          The EHCI device\r
-  @param  Timeout      Time to wait before abort (in millisecond, ms)\r
-\r
-  @return EFI_SUCCESS : The asynchronous schedule is disabled\r
-  @return Others      : Failed to disable the asynchronous schedule\r
-\r
-**/\r
-EFI_STATUS\r
-EhcDisableAsyncSchd (\r
-  IN USB2_HC_DEV          *Ehc,\r
-  IN UINT32               Timeout\r
-  )\r
-{\r
-  EFI_STATUS  Status;\r
-\r
-  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_ASYNC);\r
 \r
-  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_ASYNC_ENABLED, FALSE, Timeout);\r
-  return Status;\r
-}\r
 \r
 \r
 \r
 /**\r
-  Whether Ehc is halted\r
+  Whether Ehc is halted.\r
 \r
-  @param  Ehc          The EHCI device\r
+  @param  Ehc          The EHCI device.\r
 \r
-  @return TRUE  : The controller is halted\r
-  @return FALSE : It isn't halted\r
+  @retval TRUE         The controller is halted.\r
+  @retval FALSE        It isn't halted.\r
 \r
 **/\r
 BOOLEAN\r
@@ -456,12 +472,12 @@ EhcIsHalt (
 \r
 \r
 /**\r
-  Whether system error occurred\r
+  Whether system error occurred.\r
 \r
-  @param  Ehc          The EHCI device\r
+  @param  Ehc          The EHCI device.\r
 \r
-  @return TRUE  : System error happened\r
-  @return FALSE : No system error\r
+  @return TRUE         System error happened.\r
+  @return FALSE        No system error.\r
 \r
 **/\r
 BOOLEAN\r
@@ -474,13 +490,13 @@ EhcIsSysError (
 \r
 \r
 /**\r
-  Reset the host controller\r
+  Reset the host controller.\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Timeout      Time to wait before abort (in millisecond, ms)\r
+  @param  Ehc          The EHCI device.\r
+  @param  Timeout      Time to wait before abort (in millisecond, ms).\r
 \r
-  @return EFI_SUCCESS : The host controller is reset\r
-  @return Others      : Failed to reset the host\r
+  @retval EFI_SUCCESS  The host controller is reset.\r
+  @return Others       Failed to reset the host.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -509,13 +525,13 @@ EhcResetHC (
 \r
 \r
 /**\r
-  Halt the host controller\r
+  Halt the host controller.\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Timeout      Time to wait before abort\r
+  @param  Ehc          The EHCI device.\r
+  @param  Timeout      Time to wait before abort.\r
 \r
-  @return EFI_SUCCESS : The EHCI is halt\r
-  @return EFI_TIMEOUT : Failed to halt the controller before Timeout\r
+  @retval EFI_SUCCESS  The EHCI is halt.\r
+  @retval EFI_TIMEOUT  Failed to halt the controller before Timeout.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -533,13 +549,13 @@ EhcHaltHC (
 \r
 \r
 /**\r
-  Set the EHCI to run\r
+  Set the EHCI to run.\r
 \r
-  @param  Ehc          The EHCI device\r
-  @param  Timeout      Time to wait before abort\r
+  @param  Ehc          The EHCI device.\r
+  @param  Timeout      Time to wait before abort.\r
 \r
-  @return EFI_SUCCESS : The EHCI is running\r
-  @return Others      : Failed to set the EHCI to run\r
+  @retval EFI_SUCCESS  The EHCI is running.\r
+  @return Others       Failed to set the EHCI to run.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -558,17 +574,17 @@ EhcRunHC (
 \r
 /**\r
   Initialize the HC hardware.\r
-  EHCI spec lists the five things to do to initialize the hardware\r
+  EHCI spec lists the five things to do to initialize the hardware:\r
   1. Program CTRLDSSEGMENT\r
   2. Set USBINTR to enable interrupts\r
   3. Set periodic list base\r
   4. Set USBCMD, interrupt threshold, frame list size etc\r
   5. Write 1 to CONFIGFLAG to route all ports to EHCI\r
 \r
-  @param  Ehc          The EHCI device\r
+  @param  Ehc          The EHCI device.\r
 \r
-  @return EFI_SUCCESS : The EHCI has come out of halt state\r
-  @return EFI_TIMEOUT : Time out happened\r
+  @return EFI_SUCCESS  The EHCI has come out of halt state.\r
+  @return EFI_TIMEOUT  Time out happened.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -577,8 +593,13 @@ EhcInitHC (
   )\r
 {\r
   EFI_STATUS              Status;\r
+  UINT32                  Index;\r
+  UINT32                  RegVal;\r
 \r
-  ASSERT (EhcIsHalt (Ehc));\r
+  // This ASSERT crashes the BeagleBoard. There is some issue in the USB stack.\r
+  // This ASSERT needs to be removed so the BeagleBoard will boot. When we fix\r
+  // the USB stack we can put this ASSERT back in\r
+  // ASSERT (EhcIsHalt (Ehc));\r
 \r
   //\r
   // Allocate the periodic frame and associated memeory\r
@@ -593,38 +614,54 @@ EhcInitHC (
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
-  //\r
-  // 1. Program the CTRLDSSEGMENT register with the high 32 bit addr\r
-  //\r
-  EhcWriteOpReg (Ehc, EHC_CTRLDSSEG_OFFSET, Ehc->High32bitAddr);\r
 \r
   //\r
-  // 2. Clear USBINTR to disable all the interrupt. UEFI works by polling\r
+  // 1. Clear USBINTR to disable all the interrupt. UEFI works by polling\r
   //\r
   EhcWriteOpReg (Ehc, EHC_USBINTR_OFFSET, 0);\r
 \r
   //\r
-  // 3. Program periodic frame list, already done in EhcInitSched\r
-  // 4. Start the Host Controller\r
+  // 2. Start the Host Controller\r
   //\r
   EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);\r
 \r
   //\r
-  // 5. Set all ports routing to EHC\r
+  // 3. Power up all ports if EHCI has Port Power Control (PPC) support\r
+  //\r
+  if (Ehc->HcStructParams & HCSP_PPC) {\r
+    for (Index = 0; Index < (UINT8) (Ehc->HcStructParams & HCSP_NPORTS); Index++) {\r
+      //\r
+      // Do not clear port status bits on initialization.  Otherwise devices will\r
+      // not enumerate properly at startup.\r
+      //\r
+      RegVal  = EhcReadOpReg(Ehc, (UINT32)(EHC_PORT_STAT_OFFSET + (4 * Index)));\r
+      RegVal &= ~PORTSC_CHANGE_MASK;\r
+      RegVal |= PORTSC_POWER;\r
+      EhcWriteOpReg (Ehc, (UINT32) (EHC_PORT_STAT_OFFSET + (4 * Index)), RegVal);\r
+    }\r
+  }\r
+\r
+  //\r
+  // Wait roothub port power stable\r
+  //\r
+  gBS->Stall (EHC_ROOT_PORT_RECOVERY_STALL);\r
+\r
+  //\r
+  // 4. Set all ports routing to EHC\r
   //\r
   EhcSetOpRegBit (Ehc, EHC_CONFIG_FLAG_OFFSET, CONFIGFLAG_ROUTE_EHC);\r
 \r
-  Status = EhcEnablePeriodSchd (Ehc, EHC_GENERIC_TIME);\r
+  Status = EhcEnablePeriodSchd (Ehc, EHC_GENERIC_TIMEOUT);\r
 \r
   if (EFI_ERROR (Status)) {\r
-    EHC_ERROR (("EhcInitHC: failed to enable period schedule\n"));\r
+    DEBUG ((EFI_D_ERROR, "EhcInitHC: failed to enable period schedule\n"));\r
     return Status;\r
   }\r
 \r
-  Status = EhcEnableAsyncSchd (Ehc, EHC_GENERIC_TIME);\r
+  Status = EhcEnableAsyncSchd (Ehc, EHC_GENERIC_TIMEOUT);\r
 \r
   if (EFI_ERROR (Status)) {\r
-    EHC_ERROR (("EhcInitHC: failed to enable async schedule\n"));\r
+    DEBUG ((EFI_D_ERROR, "EhcInitHC: failed to enable async schedule\n"));\r
     return Status;\r
   }\r
 \r