]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.c
MdeModulePkg: Clean up source files
[mirror_edk2.git] / MdeModulePkg / Bus / Usb / UsbBusDxe / UsbUtility.c
index 28d73daffef09cb07054f0653c2e06424b7ee9d6..e9b5fefb9ad796291c91acad4e8d6e92a3c79a35 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
+    Wrapper function for usb host controller interface.\r
+\r
+Copyright (c) 2007 - 2018, 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,32 +11,53 @@ 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
-    UsbUtility.c\r
-\r
-  Abstract:\r
-\r
-    Wrapper function for usb host controller interface\r
-\r
-  Revision History\r
-\r
-\r
 **/\r
 \r
 \r
 #include "UsbBus.h"\r
 \r
+//\r
+// if RemainingDevicePath== NULL, then all Usb child devices in this bus are wanted.\r
+// Use a shor form Usb class Device Path, which could match any usb device, in WantedUsbIoDPList to indicate all Usb devices\r
+// are wanted Usb devices\r
+//\r
+USB_CLASS_FORMAT_DEVICE_PATH mAllUsbClassDevicePath = {\r
+  {\r
+    {\r
+      MESSAGING_DEVICE_PATH,\r
+      MSG_USB_CLASS_DP,\r
+      {\r
+        (UINT8) (sizeof (USB_CLASS_DEVICE_PATH)),\r
+        (UINT8) ((sizeof (USB_CLASS_DEVICE_PATH)) >> 8)\r
+      }\r
+    },\r
+    0xffff, // VendorId\r
+    0xffff, // ProductId\r
+    0xff,   // DeviceClass\r
+    0xff,   // DeviceSubClass\r
+    0xff    // DeviceProtocol\r
+  },\r
+\r
+  {\r
+    END_DEVICE_PATH_TYPE,\r
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,\r
+    {\r
+      END_DEVICE_PATH_LENGTH,\r
+      0\r
+    }\r
+  }\r
+};\r
+\r
 \r
 /**\r
-  Get the capability of the host controller\r
+  Get the capability of the host controller.\r
 \r
-  @param  UsbBus           The usb driver\r
-  @param  MaxSpeed         The maximum speed this host controller supports\r
-  @param  NumOfPort        The number of the root hub port\r
-  @param  Is64BitCapable   Whether this controller support 64 bit addressing\r
+  @param  UsbBus           The usb driver.\r
+  @param  MaxSpeed         The maximum speed this host controller supports.\r
+  @param  NumOfPort        The number of the root hub port.\r
+  @param  Is64BitCapable   Whether this controller support 64 bit addressing.\r
 \r
-  @retval EFI_SUCCESS      The host controller capability is returned\r
+  @retval EFI_SUCCESS      The host controller capability is returned.\r
   @retval Others           Failed to retrieve the host controller capability.\r
 \r
 **/\r
@@ -68,13 +91,16 @@ UsbHcGetCapability (
 \r
 \r
 /**\r
-  Reset the host controller\r
+  Reset the host controller.\r
 \r
-  @param  UsbBus           The usb bus driver\r
-  @param  Attributes       The reset type, only global reset is used by this driver\r
-\r
-  @return GC_TODO: add return values\r
+  @param  UsbBus                The usb bus driver.\r
+  @param  Attributes            The reset type, only global reset is used by this driver.\r
 \r
+  @retval EFI_SUCCESS           The reset operation succeeded.\r
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.\r
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is\r
+                                not currently supported by the host controller.\r
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.\r
 **/\r
 EFI_STATUS\r
 UsbHcReset (\r
@@ -95,13 +121,13 @@ UsbHcReset (
 \r
 \r
 /**\r
-  Get the current operation state of the host controller\r
+  Get the current operation state of the host controller.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  State            The host controller operation state\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  State            The host controller operation state.\r
 \r
-  @retval EFI_SUCCESS      The operation state is returned in State\r
-  @retval Others           Failed to get the host controller state\r
+  @retval EFI_SUCCESS      The operation state is returned in State.\r
+  @retval Others           Failed to get the host controller state.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -123,13 +149,13 @@ UsbHcGetState (
 \r
 \r
 /**\r
-  Set the host controller operation state\r
+  Set the host controller operation state.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  State            The state to set\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  State            The state to set.\r
 \r
-  @retval EFI_SUCCESS      The host controller is now working at State\r
-  @retval Others           Failed to set operation state\r
+  @retval EFI_SUCCESS      The host controller is now working at State.\r
+  @retval Others           Failed to set operation state.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -151,14 +177,14 @@ UsbHcSetState (
 \r
 \r
 /**\r
-  Get the root hub port state\r
+  Get the root hub port state.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  PortIndex        The index of port\r
-  @param  PortStatus       The variable to save port state\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  PortIndex        The index of port.\r
+  @param  PortStatus       The variable to save port state.\r
 \r
-  @retval EFI_SUCCESS      The root port state is returned in\r
-  @retval Others           Failed to get the root hub port state\r
+  @retval EFI_SUCCESS      The root port state is returned in.\r
+  @retval Others           Failed to get the root hub port state.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -181,14 +207,14 @@ UsbHcGetRootHubPortStatus (
 \r
 \r
 /**\r
-  Set the root hub port feature\r
+  Set the root hub port feature.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  PortIndex        The port index\r
-  @param  Feature          The port feature to set\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  PortIndex        The port index.\r
+  @param  Feature          The port feature to set.\r
 \r
-  @retval EFI_SUCCESS      The port feature is set\r
-  @retval Others           Failed to set port feature\r
+  @retval EFI_SUCCESS      The port feature is set.\r
+  @retval Others           Failed to set port feature.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -212,14 +238,14 @@ UsbHcSetRootHubPortFeature (
 \r
 \r
 /**\r
-  Clear the root hub port feature\r
+  Clear the root hub port feature.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  PortIndex        The port index\r
-  @param  Feature          The port feature to clear\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  PortIndex        The port index.\r
+  @param  Feature          The port feature to clear.\r
 \r
-  @retval EFI_SUCCESS      The port feature is clear\r
-  @retval Others           Failed to clear port feature\r
+  @retval EFI_SUCCESS      The port feature is clear.\r
+  @retval Others           Failed to clear port feature.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -242,22 +268,22 @@ UsbHcClearRootHubPortFeature (
 \r
 \r
 /**\r
-  Execute a control transfer to the device\r
-\r
-  @param  UsbBus           The USB bus driver\r
-  @param  DevAddr          The device address\r
-  @param  DevSpeed         The device speed\r
-  @param  MaxPacket        Maximum packet size of endpoint 0\r
-  @param  Request          The control transfer request\r
-  @param  Direction        The direction of data stage\r
-  @param  Data             The buffer holding data\r
-  @param  DataLength       The length of the data\r
-  @param  TimeOut          Timeout (in ms) to wait until timeout\r
-  @param  Translator       The transaction translator for low/full speed device\r
-  @param  UsbResult        The result of transfer\r
-\r
-  @retval EFI_SUCCESS      The control transfer finished without error\r
-  @retval Others           The control transfer failed, reason returned in UsbReslt\r
+  Execute a control transfer to the device.\r
+\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  DevAddr          The device address.\r
+  @param  DevSpeed         The device speed.\r
+  @param  MaxPacket        Maximum packet size of endpoint 0.\r
+  @param  Request          The control transfer request.\r
+  @param  Direction        The direction of data stage.\r
+  @param  Data             The buffer holding data.\r
+  @param  DataLength       The length of the data.\r
+  @param  TimeOut          Timeout (in ms) to wait until timeout.\r
+  @param  Translator       The transaction translator for low/full speed device.\r
+  @param  UsbResult        The result of transfer.\r
+\r
+  @retval EFI_SUCCESS      The control transfer finished without error.\r
+  @retval Others           The control transfer failed, reason returned in UsbReslt.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -314,25 +340,25 @@ UsbHcControlTransfer (
 \r
 \r
 /**\r
-  Execute a bulk transfer to the device's endpoint\r
+  Execute a bulk transfer to the device's endpoint.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  DevAddr          The target device address\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  DevAddr          The target device address.\r
   @param  EpAddr           The target endpoint address, with direction encoded in\r
-                           bit 7\r
-  @param  DevSpeed         The device's speed\r
-  @param  MaxPacket        The endpoint's max packet size\r
-  @param  BufferNum        The number of data buffer\r
-  @param  Data             Array of pointers to data buffer\r
-  @param  DataLength       The length of data buffer\r
+                           bit 7.\r
+  @param  DevSpeed         The device's speed.\r
+  @param  MaxPacket        The endpoint's max packet size.\r
+  @param  BufferNum        The number of data buffer.\r
+  @param  Data             Array of pointers to data buffer.\r
+  @param  DataLength       The length of data buffer.\r
   @param  DataToggle       On input, the initial data toggle to use, also  return\r
                            the next toggle on output.\r
-  @param  TimeOut          The time to wait until timeout\r
-  @param  Translator       The transaction translator for low/full speed device\r
-  @param  UsbResult        The result of USB execution\r
+  @param  TimeOut          The time to wait until timeout.\r
+  @param  Translator       The transaction translator for low/full speed device.\r
+  @param  UsbResult        The result of USB execution.\r
 \r
-  @retval EFI_SUCCESS      The bulk transfer is finished without error\r
-  @retval Others           Failed to execute bulk transfer, result in UsbResult\r
+  @retval EFI_SUCCESS      The bulk transfer is finished without error.\r
+  @retval Others           Failed to execute bulk transfer, result in UsbResult.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -387,25 +413,25 @@ UsbHcBulkTransfer (
 \r
 \r
 /**\r
-  Queue or cancel an asynchronous interrupt transfer\r
+  Queue or cancel an asynchronous interrupt transfer.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  DevAddr          The target device address\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  DevAddr          The target device address.\r
   @param  EpAddr           The target endpoint address, with direction encoded in\r
-                           bit 7\r
-  @param  DevSpeed         The device's speed\r
-  @param  MaxPacket        The endpoint's max packet size\r
+                           bit 7.\r
+  @param  DevSpeed         The device's speed.\r
+  @param  MaxPacket        The endpoint's max packet size.\r
   @param  IsNewTransfer    Whether this is a new request. If not, cancel the old\r
-                           request\r
-  @param  DataToggle       Data toggle to use on input, next toggle on output\r
-  @param  PollingInterval  The interval to poll the interrupt transfer (in ms)\r
-  @param  DataLength       The length of periodical data receive\r
-  @param  Translator       The transaction translator for low/full speed device\r
-  @param  Callback         Function to call when data is received\r
-  @param  Context          The context to the callback\r
+                           request.\r
+  @param  DataToggle       Data toggle to use on input, next toggle on output.\r
+  @param  PollingInterval  The interval to poll the interrupt transfer (in ms).\r
+  @param  DataLength       The length of periodical data receive.\r
+  @param  Translator       The transaction translator for low/full speed device.\r
+  @param  Callback         Function to call when data is received.\r
+  @param  Context          The context to the callback.\r
 \r
-  @retval EFI_SUCCESS      The asynchronous transfer is queued\r
-  @retval Others           Failed to queue the transfer\r
+  @retval EFI_SUCCESS      The asynchronous transfer is queued.\r
+  @retval Others           Failed to queue the transfer.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -465,24 +491,24 @@ UsbHcAsyncInterruptTransfer (
 \r
 \r
 /**\r
-  Execute a synchronous interrupt transfer to the target endpoint\r
+  Execute a synchronous interrupt transfer to the target endpoint.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  DevAddr          The target device address\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  DevAddr          The target device address.\r
   @param  EpAddr           The target endpoint address, with direction encoded in\r
-                           bit 7\r
-  @param  DevSpeed         The device's speed\r
-  @param  MaxPacket        The endpoint's max packet size\r
-  @param  Data             Pointer to data buffer\r
-  @param  DataLength       The length of data buffer\r
+                           bit 7.\r
+  @param  DevSpeed         The device's speed.\r
+  @param  MaxPacket        The endpoint's max packet size.\r
+  @param  Data             Pointer to data buffer.\r
+  @param  DataLength       The length of data buffer.\r
   @param  DataToggle       On input, the initial data toggle to use, also  return\r
                            the next toggle on output.\r
-  @param  TimeOut          The time to wait until timeout\r
-  @param  Translator       The transaction translator for low/full speed device\r
-  @param  UsbResult        The result of USB execution\r
+  @param  TimeOut          The time to wait until timeout.\r
+  @param  Translator       The transaction translator for low/full speed device.\r
+  @param  UsbResult        The result of USB execution.\r
 \r
-  @retval EFI_SUCCESS      The synchronous interrupt transfer is OK\r
-  @retval Others           Failed to execute the synchronous interrupt transfer\r
+  @retval EFI_SUCCESS      The synchronous interrupt transfer is OK.\r
+  @retval Others           Failed to execute the synchronous interrupt transfer.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -538,21 +564,21 @@ UsbHcSyncInterruptTransfer (
 \r
 \r
 /**\r
-  Execute a synchronous Isochronous USB transfer\r
+  Execute a synchronous Isochronous USB transfer.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  DevAddr          The target device address\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  DevAddr          The target device address.\r
   @param  EpAddr           The target endpoint address, with direction encoded in\r
-                           bit 7\r
-  @param  DevSpeed         The device's speed\r
-  @param  MaxPacket        The endpoint's max packet size\r
-  @param  BufferNum        The number of data buffer\r
-  @param  Data             Array of pointers to data buffer\r
-  @param  DataLength       The length of data buffer\r
-  @param  Translator       The transaction translator for low/full speed device\r
-  @param  UsbResult        The result of USB execution\r
+                           bit 7.\r
+  @param  DevSpeed         The device's speed.\r
+  @param  MaxPacket        The endpoint's max packet size.\r
+  @param  BufferNum        The number of data buffer.\r
+  @param  Data             Array of pointers to data buffer.\r
+  @param  DataLength       The length of data buffer.\r
+  @param  Translator       The transaction translator for low/full speed device.\r
+  @param  UsbResult        The result of USB execution.\r
 \r
-  @retval EFI_UNSUPPORTED  The isochronous transfer isn't supported now\r
+  @retval EFI_UNSUPPORTED  The isochronous transfer isn't supported now.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -574,22 +600,22 @@ UsbHcIsochronousTransfer (
 \r
 \r
 /**\r
-  Queue an asynchronous isochronous transfer\r
+  Queue an asynchronous isochronous transfer.\r
 \r
-  @param  UsbBus           The USB bus driver\r
-  @param  DevAddr          The target device address\r
+  @param  UsbBus           The USB bus driver.\r
+  @param  DevAddr          The target device address.\r
   @param  EpAddr           The target endpoint address, with direction encoded in\r
-                           bit 7\r
-  @param  DevSpeed         The device's speed\r
-  @param  MaxPacket        The endpoint's max packet size\r
-  @param  BufferNum        The number of data buffer\r
-  @param  Data             Array of pointers to data buffer\r
-  @param  DataLength       The length of data buffer\r
-  @param  Translator       The transaction translator for low/full speed device\r
-  @param  Callback         The function to call when data is transferred\r
-  @param  Context          The context to the callback function\r
-\r
-  @retval EFI_UNSUPPORTED  The asynchronous isochronous transfer isn't supported\r
+                           bit 7.\r
+  @param  DevSpeed         The device's speed.\r
+  @param  MaxPacket        The endpoint's max packet size.\r
+  @param  BufferNum        The number of data buffer.\r
+  @param  Data             Array of pointers to data buffer.\r
+  @param  DataLength       The length of data buffer.\r
+  @param  Translator       The transaction translator for low/full speed device.\r
+  @param  Callback         The function to call when data is transferred.\r
+  @param  Context          The context to the callback function.\r
+\r
+  @retval EFI_UNSUPPORTED  The asynchronous isochronous transfer isn't supported.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -612,12 +638,12 @@ UsbHcAsyncIsochronousTransfer (
 \r
 \r
 /**\r
-  Open the USB host controller protocol BY_CHILD\r
+  Open the USB host controller protocol BY_CHILD.\r
 \r
-  @param  Bus              The USB bus driver\r
-  @param  Child            The child handle\r
+  @param  Bus              The USB bus driver.\r
+  @param  Child            The child handle.\r
 \r
-  @return The open protocol return\r
+  @return The open protocol return.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -656,12 +682,10 @@ UsbOpenHostProtoByChild (
 \r
 \r
 /**\r
-  Close the USB host controller protocol BY_CHILD\r
-\r
-  @param  Bus              The USB bus driver\r
-  @param  Child            The child handle\r
+  Close the USB host controller protocol BY_CHILD.\r
 \r
-  @return None\r
+  @param  Bus              The USB bus driver.\r
+  @param  Child            The child handle.\r
 \r
 **/\r
 VOID\r
@@ -689,13 +713,12 @@ UsbCloseHostProtoByChild (
 }\r
 \r
 \r
-\r
 /**\r
   return the current TPL, copied from the EDKII glue lib.\r
 \r
-  VOID\r
+  @param  VOID.\r
 \r
-  @return Current TPL\r
+  @return Current TPL.\r
 \r
 **/\r
 EFI_TPL\r
@@ -711,59 +734,646 @@ UsbGetCurrentTpl (
   return Tpl;\r
 }\r
 \r
+/**\r
+  Create a new device path which only contain the first Usb part of the DevicePath.\r
 \r
-#ifdef EFI_DEBUG\r
-VOID\r
-UsbDebug (\r
-  IN  CHAR8               *Format,\r
-  ...\r
+  @param DevicePath  A full device path which contain the usb nodes.\r
+\r
+  @return            A new device path which only contain the Usb part of the DevicePath.\r
+\r
+**/\r
+EFI_DEVICE_PATH_PROTOCOL *\r
+EFIAPI\r
+GetUsbDPFromFullDP (\r
+  IN EFI_DEVICE_PATH_PROTOCOL     *DevicePath\r
   )\r
-/*++\r
+{\r
+  EFI_DEVICE_PATH_PROTOCOL    *UsbDevicePathPtr;\r
+  EFI_DEVICE_PATH_PROTOCOL    *UsbDevicePathBeginPtr;\r
+  EFI_DEVICE_PATH_PROTOCOL    *UsbDevicePathEndPtr;\r
+  UINTN                       Size;\r
+\r
+  //\r
+  // Get the Usb part first Begin node in full device path\r
+  //\r
+  UsbDevicePathBeginPtr = DevicePath;\r
+  while ( (!IsDevicePathEnd (UsbDevicePathBeginPtr))&&\r
+         ((UsbDevicePathBeginPtr->Type != MESSAGING_DEVICE_PATH) ||\r
+         (UsbDevicePathBeginPtr->SubType != MSG_USB_DP &&\r
+          UsbDevicePathBeginPtr->SubType != MSG_USB_CLASS_DP\r
+          && UsbDevicePathBeginPtr->SubType != MSG_USB_WWID_DP\r
+          ))) {\r
+\r
+    UsbDevicePathBeginPtr = NextDevicePathNode(UsbDevicePathBeginPtr);\r
+  }\r
 \r
-Routine Description:\r
+  //\r
+  // Get the Usb part first End node in full device path\r
+  //\r
+  UsbDevicePathEndPtr = UsbDevicePathBeginPtr;\r
+  while ((!IsDevicePathEnd (UsbDevicePathEndPtr))&&\r
+         (UsbDevicePathEndPtr->Type == MESSAGING_DEVICE_PATH) &&\r
+         (UsbDevicePathEndPtr->SubType == MSG_USB_DP ||\r
+          UsbDevicePathEndPtr->SubType == MSG_USB_CLASS_DP\r
+          || UsbDevicePathEndPtr->SubType == MSG_USB_WWID_DP\r
+          )) {\r
+\r
+    UsbDevicePathEndPtr = NextDevicePathNode(UsbDevicePathEndPtr);\r
+  }\r
+\r
+  Size  = GetDevicePathSize (UsbDevicePathBeginPtr);\r
+  Size -= GetDevicePathSize (UsbDevicePathEndPtr);\r
+  if (Size ==0){\r
+    //\r
+    // The passed in DevicePath does not contain the usb nodes\r
+    //\r
+    return NULL;\r
+  }\r
+\r
+  //\r
+  // Create a new device path which only contain the above Usb part\r
+  //\r
+  UsbDevicePathPtr = AllocateZeroPool (Size + sizeof (EFI_DEVICE_PATH_PROTOCOL));\r
+  ASSERT (UsbDevicePathPtr != NULL);\r
+  CopyMem (UsbDevicePathPtr, UsbDevicePathBeginPtr, Size);\r
+  //\r
+  // Append end device path node\r
+  //\r
+  UsbDevicePathEndPtr = (EFI_DEVICE_PATH_PROTOCOL *) ((UINTN) UsbDevicePathPtr + Size);\r
+  SetDevicePathEndNode (UsbDevicePathEndPtr);\r
+  return UsbDevicePathPtr;\r
+}\r
+\r
+/**\r
+  Check whether a usb device path is in a DEVICE_PATH_LIST_ITEM list.\r
 \r
-  USB's debug output function.\r
+  @param UsbDP       a usb device path of DEVICE_PATH_LIST_ITEM.\r
+  @param UsbIoDPList a DEVICE_PATH_LIST_ITEM list.\r
+\r
+  @retval TRUE       there is a DEVICE_PATH_LIST_ITEM in UsbIoDPList which contains the passed in UsbDP.\r
+  @retval FALSE      there is no DEVICE_PATH_LIST_ITEM in UsbIoDPList which contains the passed in UsbDP.\r
+\r
+**/\r
+BOOLEAN\r
+EFIAPI\r
+SearchUsbDPInList (\r
+  IN EFI_DEVICE_PATH_PROTOCOL     *UsbDP,\r
+  IN LIST_ENTRY                   *UsbIoDPList\r
+  )\r
+{\r
+  LIST_ENTRY                  *ListIndex;\r
+  DEVICE_PATH_LIST_ITEM       *ListItem;\r
+  BOOLEAN                     Found;\r
+  UINTN                       UsbDpDevicePathSize;\r
+\r
+  //\r
+  // Check that UsbDP and UsbIoDPList are valid\r
+  //\r
+  if ((UsbIoDPList == NULL) || (UsbDP == NULL)) {\r
+    return FALSE;\r
+  }\r
+\r
+  Found = FALSE;\r
+  ListIndex = UsbIoDPList->ForwardLink;\r
+  while (ListIndex != UsbIoDPList){\r
+    ListItem = CR(ListIndex, DEVICE_PATH_LIST_ITEM, Link, DEVICE_PATH_LIST_ITEM_SIGNATURE);\r
+    //\r
+    // Compare DEVICE_PATH_LIST_ITEM.DevicePath[]\r
+    //\r
+    ASSERT (ListItem->DevicePath != NULL);\r
+\r
+    UsbDpDevicePathSize  = GetDevicePathSize (UsbDP);\r
+    if (UsbDpDevicePathSize == GetDevicePathSize (ListItem->DevicePath)) {\r
+      if ((CompareMem (UsbDP, ListItem->DevicePath, UsbDpDevicePathSize)) == 0) {\r
+        Found = TRUE;\r
+        break;\r
+      }\r
+    }\r
+    ListIndex =  ListIndex->ForwardLink;\r
+  }\r
 \r
-Arguments:\r
+  return Found;\r
+}\r
 \r
-  Format  - The format parameters to the print\r
-  ...     - The variable length parameters after format\r
+/**\r
+  Add a usb device path into the DEVICE_PATH_LIST_ITEM list.\r
 \r
-Returns:\r
+  @param UsbDP                   a usb device path of DEVICE_PATH_LIST_ITEM.\r
+  @param UsbIoDPList             a DEVICE_PATH_LIST_ITEM list.\r
 \r
-  None\r
+  @retval EFI_INVALID_PARAMETER  If parameters are invalid, return this value.\r
+  @retval EFI_SUCCESS            If Add operation is successful, return this value.\r
 \r
---*/\r
+**/\r
+EFI_STATUS\r
+EFIAPI\r
+AddUsbDPToList (\r
+  IN EFI_DEVICE_PATH_PROTOCOL     *UsbDP,\r
+  IN LIST_ENTRY                   *UsbIoDPList\r
+  )\r
 {\r
-  VA_LIST                 Marker;\r
+  DEVICE_PATH_LIST_ITEM       *ListItem;\r
+\r
+  //\r
+  // Check that UsbDP and UsbIoDPList are valid\r
+  //\r
+  if ((UsbIoDPList == NULL) || (UsbDP == NULL)) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
+  if (SearchUsbDPInList (UsbDP, UsbIoDPList)){\r
+    return EFI_SUCCESS;\r
+  }\r
 \r
-  VA_START (Marker, Format);\r
-  DebugVPrint (DEBUG_INFO, Format, Marker);\r
-  VA_END (Marker);\r
+  //\r
+  // Prepare the usbio device path DEVICE_PATH_LIST_ITEM structure.\r
+  //\r
+  ListItem = AllocateZeroPool (sizeof (DEVICE_PATH_LIST_ITEM));\r
+  ASSERT (ListItem != NULL);\r
+  ListItem->Signature = DEVICE_PATH_LIST_ITEM_SIGNATURE;\r
+  ListItem->DevicePath = DuplicateDevicePath (UsbDP);\r
+\r
+  InsertTailList (UsbIoDPList, &ListItem->Link);\r
+\r
+  return EFI_SUCCESS;\r
 }\r
 \r
+/**\r
+  Check whether usb device, whose interface is UsbIf, matches the usb class which indicated by\r
+  UsbClassDevicePathPtr whose is a short form usb class device path.\r
+\r
+  @param UsbClassDevicePathPtr    a short form usb class device path.\r
+  @param UsbIf                    a usb device interface.\r
+\r
+  @retval TRUE                    the usb device match the usb class.\r
+  @retval FALSE                   the usb device does not match the usb class.\r
+\r
+**/\r
+BOOLEAN\r
+EFIAPI\r
+MatchUsbClass (\r
+  IN USB_CLASS_DEVICE_PATH      *UsbClassDevicePathPtr,\r
+  IN USB_INTERFACE              *UsbIf\r
+  )\r
+{\r
+  USB_INTERFACE_DESC            *IfDesc;\r
+  EFI_USB_INTERFACE_DESCRIPTOR  *ActIfDesc;\r
+  EFI_USB_DEVICE_DESCRIPTOR     *DevDesc;\r
 \r
 \r
+  if ((UsbClassDevicePathPtr->Header.Type != MESSAGING_DEVICE_PATH) ||\r
+      (UsbClassDevicePathPtr->Header.SubType != MSG_USB_CLASS_DP)){\r
+    ASSERT (0);\r
+    return FALSE;\r
+  }\r
+\r
+  IfDesc       = UsbIf->IfDesc;\r
+  ASSERT (IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);\r
+  ActIfDesc    = &(IfDesc->Settings[IfDesc->ActiveIndex]->Desc);\r
+  DevDesc      = &(UsbIf->Device->DevDesc->Desc);\r
+\r
+  //\r
+  // If connect class policy, determine whether to create device handle by the five fields\r
+  // in class device path node.\r
+  //\r
+  // In addtion, hub interface is always matched for this policy.\r
+  //\r
+  if ((ActIfDesc->InterfaceClass == USB_HUB_CLASS_CODE) &&\r
+      (ActIfDesc->InterfaceSubClass == USB_HUB_SUBCLASS_CODE)) {\r
+    return TRUE;\r
+  }\r
+\r
+  //\r
+  // If vendor id or product id is 0xffff, they will be ignored.\r
+  //\r
+  if ((UsbClassDevicePathPtr->VendorId == 0xffff || UsbClassDevicePathPtr->VendorId == DevDesc->IdVendor) &&\r
+      (UsbClassDevicePathPtr->ProductId == 0xffff || UsbClassDevicePathPtr->ProductId == DevDesc->IdProduct)) {\r
+\r
+    //\r
+    // If Class in Device Descriptor is set to 0, the counterparts in interface should be checked.\r
+    //\r
+    if (DevDesc->DeviceClass == 0) {\r
+      if ((UsbClassDevicePathPtr->DeviceClass == ActIfDesc->InterfaceClass ||\r
+                                          UsbClassDevicePathPtr->DeviceClass == 0xff) &&\r
+          (UsbClassDevicePathPtr->DeviceSubClass == ActIfDesc->InterfaceSubClass ||\r
+                                       UsbClassDevicePathPtr->DeviceSubClass == 0xff) &&\r
+          (UsbClassDevicePathPtr->DeviceProtocol == ActIfDesc->InterfaceProtocol ||\r
+                                       UsbClassDevicePathPtr->DeviceProtocol == 0xff)) {\r
+        return TRUE;\r
+      }\r
+\r
+    } else if ((UsbClassDevicePathPtr->DeviceClass == DevDesc->DeviceClass ||\r
+                                         UsbClassDevicePathPtr->DeviceClass == 0xff) &&\r
+               (UsbClassDevicePathPtr->DeviceSubClass == DevDesc->DeviceSubClass ||\r
+                                      UsbClassDevicePathPtr->DeviceSubClass == 0xff) &&\r
+               (UsbClassDevicePathPtr->DeviceProtocol == DevDesc->DeviceProtocol ||\r
+                                      UsbClassDevicePathPtr->DeviceProtocol == 0xff)) {\r
+\r
+      return TRUE;\r
+    }\r
+  }\r
+\r
+  return FALSE;\r
+}\r
+\r
 /**\r
-  USB's error output function.\r
+  Check whether usb device, whose interface is UsbIf, matches the usb WWID requirement which indicated by\r
+  UsbWWIDDevicePathPtr whose is a short form usb WWID device path.\r
 \r
-  @param  Format           The format parameters to the print\r
-  @param  ...              The variable length parameters after format\r
+  @param UsbWWIDDevicePathPtr    a short form usb WWID device path.\r
+  @param UsbIf                   a usb device interface.\r
 \r
-  @return None\r
+  @retval TRUE                   the usb device match the usb WWID requirement.\r
+  @retval FALSE                  the usb device does not match the usb WWID requirement.\r
 \r
 **/\r
-VOID\r
-UsbError (\r
-  IN  CHAR8               *Format,\r
-  ...\r
+BOOLEAN\r
+MatchUsbWwid (\r
+  IN USB_WWID_DEVICE_PATH       *UsbWWIDDevicePathPtr,\r
+  IN USB_INTERFACE              *UsbIf\r
   )\r
 {\r
-  VA_LIST                 Marker;\r
+  USB_INTERFACE_DESC            *IfDesc;\r
+  EFI_USB_INTERFACE_DESCRIPTOR  *ActIfDesc;\r
+  EFI_USB_DEVICE_DESCRIPTOR     *DevDesc;\r
+  EFI_USB_STRING_DESCRIPTOR     *StrDesc;\r
+  UINT16                        Index;\r
+  CHAR16                        *CompareStr;\r
+  UINTN                         CompareLen;\r
+  UINTN                         Length;\r
+\r
+  if ((UsbWWIDDevicePathPtr->Header.Type != MESSAGING_DEVICE_PATH) ||\r
+     (UsbWWIDDevicePathPtr->Header.SubType != MSG_USB_WWID_DP )){\r
+    ASSERT (0);\r
+    return FALSE;\r
+  }\r
+\r
+  IfDesc       = UsbIf->IfDesc;\r
+  ASSERT (IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);\r
+  ActIfDesc    = &(IfDesc->Settings[IfDesc->ActiveIndex]->Desc);\r
+  DevDesc      = &(UsbIf->Device->DevDesc->Desc);\r
+\r
+  //\r
+  // In addition, Hub interface is always matched for this policy.\r
+  //\r
+  if ((ActIfDesc->InterfaceClass == USB_HUB_CLASS_CODE) &&\r
+      (ActIfDesc->InterfaceSubClass == USB_HUB_SUBCLASS_CODE)) {\r
+    return TRUE;\r
+  }\r
+\r
+  //\r
+  // Check Vendor Id, Product Id and Interface Number.\r
+  //\r
+  if ((DevDesc->IdVendor != UsbWWIDDevicePathPtr->VendorId) ||\r
+      (DevDesc->IdProduct != UsbWWIDDevicePathPtr->ProductId) ||\r
+      (ActIfDesc->InterfaceNumber != UsbWWIDDevicePathPtr->InterfaceNumber)) {\r
+    return FALSE;\r
+  }\r
+\r
+  //\r
+  // Check SerialNumber.\r
+  //\r
+  if (DevDesc->StrSerialNumber == 0) {\r
+    return FALSE;\r
+  }\r
+\r
+  //\r
+  // Serial number in USB WWID device path is the last 64-or-less UTF-16 characters.\r
+  //\r
+  CompareStr = (CHAR16 *) (UINTN) (UsbWWIDDevicePathPtr + 1);\r
+  CompareLen = (DevicePathNodeLength (UsbWWIDDevicePathPtr) - sizeof (USB_WWID_DEVICE_PATH)) / sizeof (CHAR16);\r
+  if (CompareStr[CompareLen - 1] == L'\0') {\r
+    CompareLen--;\r
+  }\r
+\r
+  //\r
+  // Compare serial number in each supported language.\r
+  //\r
+  for (Index = 0; Index < UsbIf->Device->TotalLangId; Index++) {\r
+    StrDesc = UsbGetOneString (UsbIf->Device, DevDesc->StrSerialNumber, UsbIf->Device->LangId[Index]);\r
+    if (StrDesc == NULL) {\r
+      continue;\r
+    }\r
+\r
+    Length = (StrDesc->Length - 2) / sizeof (CHAR16);\r
+    if ((Length >= CompareLen) &&\r
+        (CompareMem (StrDesc->String + Length - CompareLen, CompareStr, CompareLen * sizeof (CHAR16)) == 0)) {\r
+      return TRUE;\r
+    }\r
+  }\r
+\r
+  return FALSE;\r
+}\r
+\r
+/**\r
+  Free a DEVICE_PATH_LIST_ITEM list.\r
+\r
+  @param  UsbIoDPList            a DEVICE_PATH_LIST_ITEM list pointer.\r
+\r
+  @retval EFI_INVALID_PARAMETER  If parameters are invalid, return this value.\r
+  @retval EFI_SUCCESS            If free operation is successful, return this value.\r
+\r
+**/\r
+EFI_STATUS\r
+EFIAPI\r
+UsbBusFreeUsbDPList (\r
+  IN LIST_ENTRY          *UsbIoDPList\r
+  )\r
+{\r
+  LIST_ENTRY                  *ListIndex;\r
+  DEVICE_PATH_LIST_ITEM       *ListItem;\r
+\r
+  //\r
+  // Check that ControllerHandle is a valid handle\r
+  //\r
+  if (UsbIoDPList == NULL) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
+  ListIndex = UsbIoDPList->ForwardLink;\r
+  while (ListIndex != UsbIoDPList){\r
+    ListItem = CR(ListIndex, DEVICE_PATH_LIST_ITEM, Link, DEVICE_PATH_LIST_ITEM_SIGNATURE);\r
+    //\r
+    // Free DEVICE_PATH_LIST_ITEM.DevicePath[]\r
+    //\r
+    if (ListItem->DevicePath != NULL){\r
+      FreePool(ListItem->DevicePath);\r
+    }\r
+    //\r
+    // Free DEVICE_PATH_LIST_ITEM itself\r
+    //\r
+    ListIndex =  ListIndex->ForwardLink;\r
+    RemoveEntryList (&ListItem->Link);\r
+    FreePool (ListItem);\r
+  }\r
+\r
+  InitializeListHead (UsbIoDPList);\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  Store a wanted usb child device info (its Usb part of device path) which is indicated by\r
+  RemainingDevicePath in a Usb bus which  is indicated by UsbBusId.\r
+\r
+  @param  UsbBusId               Point to EFI_USB_BUS_PROTOCOL interface.\r
+  @param  RemainingDevicePath    The remaining device patch.\r
+\r
+  @retval EFI_SUCCESS            Add operation is successful.\r
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.\r
+\r
+**/\r
+EFI_STATUS\r
+EFIAPI\r
+UsbBusAddWantedUsbIoDP (\r
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId,\r
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath\r
+  )\r
+{\r
+  USB_BUS                       *Bus;\r
+  EFI_STATUS                    Status;\r
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePathPtr;\r
+\r
+  //\r
+  // Check whether remaining device path is valid\r
+  //\r
+  if (RemainingDevicePath != NULL && !IsDevicePathEnd (RemainingDevicePath)) {\r
+    if ((RemainingDevicePath->Type    != MESSAGING_DEVICE_PATH) ||\r
+        (RemainingDevicePath->SubType != MSG_USB_DP &&\r
+         RemainingDevicePath->SubType != MSG_USB_CLASS_DP\r
+         && RemainingDevicePath->SubType != MSG_USB_WWID_DP\r
+         )) {\r
+      return EFI_INVALID_PARAMETER;\r
+    }\r
+  }\r
+\r
+  if (UsbBusId == NULL){\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
+  Bus = USB_BUS_FROM_THIS (UsbBusId);\r
+\r
+  if (RemainingDevicePath == NULL) {\r
+    //\r
+    // RemainingDevicePath == NULL means all Usb devices in this bus are wanted.\r
+    // Here use a Usb class Device Path in WantedUsbIoDPList to indicate all Usb devices\r
+    // are wanted Usb devices\r
+    //\r
+    Status = UsbBusFreeUsbDPList (&Bus->WantedUsbIoDPList);\r
+    ASSERT (!EFI_ERROR (Status));\r
+    DevicePathPtr = DuplicateDevicePath ((EFI_DEVICE_PATH_PROTOCOL *) &mAllUsbClassDevicePath);\r
+  } else if (!IsDevicePathEnd (RemainingDevicePath)) {\r
+    //\r
+    // If RemainingDevicePath isn't the End of Device Path Node,\r
+    // Create new Usb device path according to the usb part in remaining device path\r
+    //\r
+    DevicePathPtr = GetUsbDPFromFullDP (RemainingDevicePath);\r
+  } else {\r
+    //\r
+    // If RemainingDevicePath is the End of Device Path Node,\r
+    // skip enumerate any device and return EFI_SUCESSS\r
+    //\r
+    return EFI_SUCCESS;\r
+  }\r
+\r
+  ASSERT (DevicePathPtr != NULL);\r
+  Status = AddUsbDPToList (DevicePathPtr, &Bus->WantedUsbIoDPList);\r
+  ASSERT (!EFI_ERROR (Status));\r
+  FreePool (DevicePathPtr);\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  Check whether a usb child device is the wanted device in a bus.\r
+\r
+  @param  Bus     The Usb bus's private data pointer.\r
+  @param  UsbIf   The usb child device inferface.\r
+\r
+  @retval True    If a usb child device is the wanted device in a bus.\r
+  @retval False   If a usb child device is *NOT* the wanted device in a bus.\r
+\r
+**/\r
+BOOLEAN\r
+EFIAPI\r
+UsbBusIsWantedUsbIO (\r
+  IN USB_BUS                 *Bus,\r
+  IN USB_INTERFACE           *UsbIf\r
+  )\r
+{\r
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePathPtr;\r
+  LIST_ENTRY                    *WantedUsbIoDPListPtr;\r
+  LIST_ENTRY                    *WantedListIndex;\r
+  DEVICE_PATH_LIST_ITEM         *WantedListItem;\r
+  BOOLEAN                       DoConvert;\r
+  UINTN                         FirstDevicePathSize;\r
+\r
+  //\r
+  // Check whether passed in parameters are valid\r
+  //\r
+  if ((UsbIf == NULL) || (Bus == NULL)) {\r
+    return FALSE;\r
+  }\r
+  //\r
+  // Check whether UsbIf is Hub\r
+  //\r
+  if (UsbIf->IsHub) {\r
+    return TRUE;\r
+  }\r
+\r
+  //\r
+  // Check whether all Usb devices in this bus are wanted\r
+  //\r
+  if (SearchUsbDPInList ((EFI_DEVICE_PATH_PROTOCOL *)&mAllUsbClassDevicePath, &Bus->WantedUsbIoDPList)){\r
+    return TRUE;\r
+  }\r
+\r
+  //\r
+  // Check whether the Usb device match any item in WantedUsbIoDPList\r
+  //\r
+  WantedUsbIoDPListPtr = &Bus->WantedUsbIoDPList;\r
+  //\r
+  // Create new Usb device path according to the usb part in UsbIo full device path\r
+  //\r
+  DevicePathPtr = GetUsbDPFromFullDP (UsbIf->DevicePath);\r
+  ASSERT (DevicePathPtr != NULL);\r
+\r
+  DoConvert = FALSE;\r
+  WantedListIndex = WantedUsbIoDPListPtr->ForwardLink;\r
+  while (WantedListIndex != WantedUsbIoDPListPtr){\r
+    WantedListItem = CR(WantedListIndex, DEVICE_PATH_LIST_ITEM, Link, DEVICE_PATH_LIST_ITEM_SIGNATURE);\r
+    ASSERT (WantedListItem->DevicePath->Type == MESSAGING_DEVICE_PATH);\r
+    switch (WantedListItem->DevicePath->SubType) {\r
+    case MSG_USB_DP:\r
+      FirstDevicePathSize = GetDevicePathSize (WantedListItem->DevicePath);\r
+      if (FirstDevicePathSize == GetDevicePathSize (DevicePathPtr)) {\r
+        if (CompareMem (\r
+              WantedListItem->DevicePath,\r
+              DevicePathPtr,\r
+              GetDevicePathSize (DevicePathPtr)) == 0\r
+            ) {\r
+          DoConvert = TRUE;\r
+        }\r
+      }\r
+      break;\r
+    case MSG_USB_CLASS_DP:\r
+      if (MatchUsbClass((USB_CLASS_DEVICE_PATH *)WantedListItem->DevicePath, UsbIf)) {\r
+        DoConvert = TRUE;\r
+      }\r
+      break;\r
+   case MSG_USB_WWID_DP:\r
+      if (MatchUsbWwid((USB_WWID_DEVICE_PATH *)WantedListItem->DevicePath, UsbIf)) {\r
+        DoConvert = TRUE;\r
+      }\r
+      break;\r
+    default:\r
+      ASSERT (0);\r
+      break;\r
+    }\r
+\r
+    if (DoConvert) {\r
+      break;\r
+    }\r
+\r
+    WantedListIndex =  WantedListIndex->ForwardLink;\r
+  }\r
+  gBS->FreePool (DevicePathPtr);\r
+\r
+  //\r
+  // Check whether the new Usb device path is wanted\r
+  //\r
+  if (DoConvert){\r
+    return TRUE;\r
+  } else {\r
+    return FALSE;\r
+  }\r
+}\r
+\r
+/**\r
+  Recursively connnect every wanted usb child device to ensure they all fully connected.\r
+  Check all the child Usb IO handles in this bus, recursively connecte if it is wanted usb child device.\r
+\r
+  @param  UsbBusId                  Point to EFI_USB_BUS_PROTOCOL interface.\r
+\r
+  @retval EFI_SUCCESS               Connect is done successfully.\r
+  @retval EFI_INVALID_PARAMETER     The parameter is invalid.\r
+\r
+**/\r
+EFI_STATUS\r
+EFIAPI\r
+UsbBusRecursivelyConnectWantedUsbIo (\r
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId\r
+  )\r
+{\r
+  USB_BUS                       *Bus;\r
+  EFI_STATUS                    Status;\r
+  UINTN                         Index;\r
+  EFI_USB_IO_PROTOCOL           *UsbIo;\r
+  USB_INTERFACE                 *UsbIf;\r
+  UINTN                         UsbIoHandleCount;\r
+  EFI_HANDLE                    *UsbIoBuffer;\r
+  EFI_DEVICE_PATH_PROTOCOL      *UsbIoDevicePath;\r
+\r
+  if (UsbBusId == NULL){\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
+  Bus = USB_BUS_FROM_THIS (UsbBusId);\r
+\r
+  //\r
+  // Get all Usb IO handles in system\r
+  //\r
+  UsbIoHandleCount = 0;\r
+  Status = gBS->LocateHandleBuffer (ByProtocol, &gEfiUsbIoProtocolGuid, NULL, &UsbIoHandleCount, &UsbIoBuffer);\r
+  if (Status == EFI_NOT_FOUND || UsbIoHandleCount == 0) {\r
+    return EFI_SUCCESS;\r
+  }\r
+  ASSERT (!EFI_ERROR (Status));\r
+\r
+  for (Index = 0; Index < UsbIoHandleCount; Index++) {\r
+    //\r
+    // Check whether the USB IO handle is a child of this bus\r
+    // Note: The usb child handle maybe invalid because of hot plugged out during the loop\r
+    //\r
+    UsbIoDevicePath = NULL;\r
+    Status = gBS->HandleProtocol (UsbIoBuffer[Index], &gEfiDevicePathProtocolGuid, (VOID *) &UsbIoDevicePath);\r
+    if (EFI_ERROR (Status) || UsbIoDevicePath == NULL) {\r
+      continue;\r
+    }\r
+    if (CompareMem (\r
+            UsbIoDevicePath,\r
+            Bus->DevicePath,\r
+            (GetDevicePathSize (Bus->DevicePath) - sizeof (EFI_DEVICE_PATH_PROTOCOL))\r
+            ) != 0) {\r
+      continue;\r
+    }\r
+\r
+    //\r
+    // Get the child Usb IO interface\r
+    //\r
+    Status = gBS->HandleProtocol(\r
+                     UsbIoBuffer[Index],\r
+                     &gEfiUsbIoProtocolGuid,\r
+                     (VOID **) &UsbIo\r
+                     );\r
+    if (EFI_ERROR (Status)) {\r
+      continue;\r
+    }\r
+    UsbIf   = USB_INTERFACE_FROM_USBIO (UsbIo);\r
+\r
+    if (UsbBusIsWantedUsbIO (Bus, UsbIf)) {\r
+      if (!UsbIf->IsManaged) {\r
+        //\r
+        // Recursively connect the wanted Usb Io handle\r
+        //\r
+        DEBUG ((EFI_D_INFO, "UsbBusRecursivelyConnectWantedUsbIo: TPL before connect is %d\n", (UINT32)UsbGetCurrentTpl ()));\r
+        Status            = gBS->ConnectController (UsbIf->Handle, NULL, NULL, TRUE);\r
+        UsbIf->IsManaged  = (BOOLEAN)!EFI_ERROR (Status);\r
+        DEBUG ((EFI_D_INFO, "UsbBusRecursivelyConnectWantedUsbIo: TPL after connect is %d\n", (UINT32)UsbGetCurrentTpl()));\r
+      }\r
+    }\r
+  }\r
+\r
+  FreePool (UsbIoBuffer);\r
 \r
-  VA_START (Marker, Format);\r
-  DebugVPrint (DEBUG_ERROR, Format, Marker);\r
-  VA_END (Marker);\r
+  return EFI_SUCCESS;\r
 }\r
 \r
-#endif\r