]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.c
MdeModulePkg/EhciDxe: factor out EhcIsDebugPortInUse()
[mirror_edk2.git] / MdeModulePkg / Bus / Pci / EhciDxe / EhciReg.c
index aa166055569fe4fad33c5a8b0190362d7e1c3ba5..11c36132fd4f77a57b6f4cc4926c7e931a04bdc9 100644 (file)
@@ -1,7 +1,9 @@
 /** @file\r
 \r
-Copyright (c) 2007, Intel Corporation\r
-All rights reserved. This program and the accompanying materials\r
+  The EHCI register operation routines.\r
+\r
+Copyright (c) 2007 - 2017, Intel Corporation. All rights reserved.<BR>\r
+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
@@ -9,35 +11,61 @@ http://opensource.org/licenses/bsd-license.php
 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
 \r
-Module Name:\r
+**/\r
+\r
 \r
-    EhciReg.c\r
+#include "Ehci.h"\r
 \r
-Abstract:\r
 \r
-    The EHCI register operation routines.\r
+/**\r
+  Read EHCI capability register.\r
 \r
+  @param  Ehc          The EHCI device.\r
+  @param  Offset       Capability register address.\r
 \r
-Revision History\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 +75,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 +91,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 +168,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 +183,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 +205,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 +240,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 +264,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 +287,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 +301,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 +316,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 +329,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 +349,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 +385,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 +403,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,44 +426,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
-STATIC\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
@@ -412,40 +455,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
-STATIC\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
@@ -458,12 +478,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
@@ -476,13 +496,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
@@ -511,13 +531,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
@@ -535,13 +555,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
@@ -560,17 +580,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
@@ -579,8 +599,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
@@ -595,38 +620,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