MdeModulePkg/UsbBusPei: Fix out-of-bound read access to descriptors

[mirror_edk2.git] / MdeModulePkg / Bus / Usb / UsbBusPei / UsbPeim.c

diff --git a/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c b/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c
index b63173d18184d677ece1163bff627d607450d5e9..86734f2f7362b4bc0c9f25705e8c9d65086bb2c8 100644 (file)
--- a/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c
+++ b/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c
@@ -1,8 +1,8 @@
 /** @file\r
 The module to produce Usb Bus PPI.\r
 \r
 /** @file\r
 The module to produce Usb Bus PPI.\r
 \r

-Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR>\r
-  \r
+Copyright (c) 2006 - 2018, Intel Corporation. All rights reserved.<BR>\r
+\r

 This program and the accompanying materials\r
 are licensed and made available under the terms and conditions\r
 of the BSD License which accompanies this distribution.  The\r
 This program and the accompanying materials\r
 are licensed and made available under the terms and conditions\r
 of the BSD License which accompanies this distribution.  The\r


@@ -37,7 +37,7 @@ EFI_PEI_PPI_DESCRIPTOR mUsbIoPpiList = {
 \r
 /**\r
   The enumeration routine to detect device change.\r
 \r
 /**\r
   The enumeration routine to detect device change.\r

-  \r
+\r

   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  UsbHcPpi               The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.\r
   @param  Usb2HcPpi              The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.\r
   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  UsbHcPpi               The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.\r
   @param  Usb2HcPpi              The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.\r


@@ -56,7 +56,7 @@ PeiUsbEnumeration (
 \r
 /**\r
   Configure new detected usb device.\r
 \r
 /**\r
   Configure new detected usb device.\r

-  \r
+\r

   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
   @param  Port                   The port to be configured.\r
   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
   @param  Port                   The port to be configured.\r


@@ -77,7 +77,7 @@ PeiConfigureUsbDevice (
 \r
 /**\r
   Get all configurations from a detected usb device.\r
 \r
 /**\r
   Get all configurations from a detected usb device.\r

-  \r
+\r

   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
 \r
   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
 \r


@@ -94,7 +94,7 @@ PeiUsbGetAllConfiguration (
 \r
 /**\r
   Get the start position of next wanted descriptor.\r
 \r
 /**\r
   Get the start position of next wanted descriptor.\r

-  \r
+\r

   @param  Buffer            Buffer containing data to parse.\r
   @param  Length            Buffer length.\r
   @param  DescType          Descriptor type.\r
   @param  Buffer            Buffer containing data to parse.\r
   @param  Length            Buffer length.\r
   @param  DescType          Descriptor type.\r


@@ -116,7 +116,7 @@ GetExpectedDescriptor (
 \r
 /**\r
   The entrypoint of the module, it will enumerate all HCs.\r
 \r
 /**\r
   The entrypoint of the module, it will enumerate all HCs.\r

-  \r
+\r

   @param  FileHandle             Handle of the file being invoked.\r
   @param  PeiServices            Describes the list of possible PEI Services.\r
 \r
   @param  FileHandle             Handle of the file being invoked.\r
   @param  PeiServices            Describes the list of possible PEI Services.\r
 \r


@@ -142,7 +142,7 @@ PeimInitializeUsb (
   }\r
 \r
   //\r
   }\r
 \r
   //\r

-  // gPeiUsbHostControllerPpiGuid and gPeiUsb2HostControllerPpiGuid should not \r
+  // gPeiUsbHostControllerPpiGuid and gPeiUsb2HostControllerPpiGuid should not\r

   // be produced at the same time\r
   //\r
   Index = 0;\r
   // be produced at the same time\r
   //\r
   Index = 0;\r


@@ -176,18 +176,18 @@ PeimInitializeUsb (
                  Index,\r
                  NULL,\r
                  (VOID **) &Usb2HcPpi\r
                  Index,\r
                  NULL,\r
                  (VOID **) &Usb2HcPpi\r

-                 );    \r
+                 );\r

       if (EFI_ERROR (Status)) {\r
         //\r
         // No more host controller, break out\r
         //\r
         break;\r
       if (EFI_ERROR (Status)) {\r
         //\r
         // No more host controller, break out\r
         //\r
         break;\r

-      }   \r
-      PeiUsbEnumeration ((EFI_PEI_SERVICES **) PeiServices, NULL, Usb2HcPpi);   \r
+      }\r
+      PeiUsbEnumeration ((EFI_PEI_SERVICES **) PeiServices, NULL, Usb2HcPpi);\r

       Index++;\r
     }\r
   }\r
       Index++;\r
     }\r
   }\r

-  \r
+\r

   if (Index == 0) {\r
     return EFI_UNSUPPORTED;\r
   }\r
   if (Index == 0) {\r
     return EFI_UNSUPPORTED;\r
   }\r


@@ -198,7 +198,7 @@ PeimInitializeUsb (
 /**\r
   The Hub Enumeration just scans the hub ports one time. It also\r
   doesn't support hot-plug.\r
 /**\r
   The Hub Enumeration just scans the hub ports one time. It also\r
   doesn't support hot-plug.\r

-  \r
+\r

   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
   @param  CurrentAddress         The DeviceAddress of usb device.\r
   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
   @param  CurrentAddress         The DeviceAddress of usb device.\r


@@ -222,10 +222,14 @@ PeiHubEnumeration (
   UINTN                 MemPages;\r
   EFI_PHYSICAL_ADDRESS  AllocateAddress;\r
   PEI_USB_DEVICE        *NewPeiUsbDevice;\r
   UINTN                 MemPages;\r
   EFI_PHYSICAL_ADDRESS  AllocateAddress;\r
   PEI_USB_DEVICE        *NewPeiUsbDevice;\r

+  UINTN                 InterfaceIndex;\r
+  UINTN                 EndpointIndex;\r

 \r
 \r
   UsbIoPpi    = &PeiUsbDevice->UsbIoPpi;\r
 \r
 \r
 \r
   UsbIoPpi    = &PeiUsbDevice->UsbIoPpi;\r
 \r

+  DEBUG ((EFI_D_INFO, "PeiHubEnumeration: DownStreamPortNo: %x\n", PeiUsbDevice->DownStreamPortNo));\r
+\r

   for (Index = 0; Index < PeiUsbDevice->DownStreamPortNo; Index++) {\r
 \r
     Status = PeiHubGetPortStatus (\r
   for (Index = 0; Index < PeiUsbDevice->DownStreamPortNo; Index++) {\r
 \r
     Status = PeiHubGetPortStatus (\r


@@ -239,25 +243,14 @@ PeiHubEnumeration (
       continue;\r
     }\r
 \r
       continue;\r
     }\r
 \r

-    if (IsPortConnectChange (PortStatus.PortChangeStatus)) {\r
-      PeiHubClearPortFeature (\r
-        PeiServices,\r
-        UsbIoPpi,\r
-        (UINT8) (Index + 1),\r
-        EfiUsbPortConnectChange\r
-        );\r
-\r
-      MicroSecondDelay (100 * 1000);\r
-\r
+    DEBUG ((EFI_D_INFO, "USB Status --- Port: %x ConnectChange[%04x] Status[%04x]\n", Index, PortStatus.PortChangeStatus, PortStatus.PortStatus));\r
+    //\r
+    // Only handle connection/enable/overcurrent/reset change.\r
+    //\r
+    if ((PortStatus.PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {\r
+      continue;\r
+    } else {\r

       if (IsPortConnect (PortStatus.PortStatus)) {\r
       if (IsPortConnect (PortStatus.PortStatus)) {\r

-\r
-        PeiHubGetPortStatus (\r
-          PeiServices,\r
-          UsbIoPpi,\r
-          (UINT8) (Index + 1),\r
-          (UINT32 *) &PortStatus\r
-          );\r
-\r

         //\r
         // Begin to deal with the new device\r
         //\r
         //\r
         // Begin to deal with the new device\r
         //\r


@@ -292,19 +285,44 @@ PeiHubEnumeration (
         NewPeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;\r
         NewPeiUsbDevice->UsbHcPpi         = PeiUsbDevice->UsbHcPpi;\r
         NewPeiUsbDevice->Usb2HcPpi        = PeiUsbDevice->Usb2HcPpi;\r
         NewPeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;\r
         NewPeiUsbDevice->UsbHcPpi         = PeiUsbDevice->UsbHcPpi;\r
         NewPeiUsbDevice->Usb2HcPpi        = PeiUsbDevice->Usb2HcPpi;\r

+        NewPeiUsbDevice->Tier             = (UINT8) (PeiUsbDevice->Tier + 1);\r

         NewPeiUsbDevice->IsHub            = 0x0;\r
         NewPeiUsbDevice->DownStreamPortNo = 0x0;\r
 \r
         NewPeiUsbDevice->IsHub            = 0x0;\r
         NewPeiUsbDevice->DownStreamPortNo = 0x0;\r
 \r

-        PeiResetHubPort (PeiServices, UsbIoPpi, (UINT8)(Index + 1));\r
+        if (((PortStatus.PortChangeStatus & USB_PORT_STAT_C_RESET) == 0) ||\r
+             ((PortStatus.PortStatus & (USB_PORT_STAT_CONNECTION | USB_PORT_STAT_ENABLE)) == 0)) {\r
+          //\r
+          // If the port already has reset change flag and is connected and enabled, skip the port reset logic.\r
+          //\r
+          PeiResetHubPort (PeiServices, UsbIoPpi, (UINT8)(Index + 1));\r
+\r
+          PeiHubGetPortStatus (\r
+             PeiServices,\r
+             UsbIoPpi,\r
+             (UINT8) (Index + 1),\r
+             (UINT32 *) &PortStatus\r
+             );\r
+        } else {\r
+          PeiHubClearPortFeature (\r
+            PeiServices,\r
+            UsbIoPpi,\r
+            (UINT8) (Index + 1),\r
+            EfiUsbPortResetChange\r
+            );\r
+        }\r

 \r
 \r

-        PeiHubGetPortStatus (\r
-           PeiServices,\r
-           UsbIoPpi,\r
-           (UINT8) (Index + 1),\r
-           (UINT32 *) &PortStatus\r
-           );\r
+        NewPeiUsbDevice->DeviceSpeed = (UINT8) PeiUsbGetDeviceSpeed (PortStatus.PortStatus);\r
+        DEBUG ((EFI_D_INFO, "Device Speed =%d\n", PeiUsbDevice->DeviceSpeed));\r

 \r
 \r

-        NewPeiUsbDevice->DeviceSpeed = (UINT8)IsPortLowSpeedDeviceAttached (PortStatus.PortStatus);\r
+        if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_SUPER_SPEED)){\r
+          NewPeiUsbDevice->MaxPacketSize0 = 512;\r
+        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_HIGH_SPEED)) {\r
+          NewPeiUsbDevice->MaxPacketSize0 = 64;\r
+        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_LOW_SPEED)) {\r
+          NewPeiUsbDevice->MaxPacketSize0 = 8;\r
+        } else {\r
+          NewPeiUsbDevice->MaxPacketSize0 = 8;\r
+        }\r

 \r
         if(NewPeiUsbDevice->DeviceSpeed != EFI_USB_SPEED_HIGH) {\r
           if (PeiUsbDevice->DeviceSpeed == EFI_USB_SPEED_HIGH) {\r
 \r
         if(NewPeiUsbDevice->DeviceSpeed != EFI_USB_SPEED_HIGH) {\r
           if (PeiUsbDevice->DeviceSpeed == EFI_USB_SPEED_HIGH) {\r


@@ -328,6 +346,7 @@ PeiHubEnumeration (
         if (EFI_ERROR (Status)) {\r
           continue;\r
         }\r
         if (EFI_ERROR (Status)) {\r
           continue;\r
         }\r

+        DEBUG ((EFI_D_INFO, "PeiHubEnumeration: PeiConfigureUsbDevice Success\n"));\r

 \r
         Status = PeiServicesInstallPpi (&NewPeiUsbDevice->UsbIoPpiList);\r
 \r
 \r
         Status = PeiServicesInstallPpi (&NewPeiUsbDevice->UsbIoPpiList);\r
 \r


@@ -341,8 +360,43 @@ PeiHubEnumeration (
 \r
           PeiHubEnumeration (PeiServices, NewPeiUsbDevice, CurrentAddress);\r
         }\r
 \r
           PeiHubEnumeration (PeiServices, NewPeiUsbDevice, CurrentAddress);\r
         }\r

-      }\r

 \r
 \r

+        for (InterfaceIndex = 1; InterfaceIndex < NewPeiUsbDevice->ConfigDesc->NumInterfaces; InterfaceIndex++) {\r
+          //\r
+          // Begin to deal with the new device\r
+          //\r
+          MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;\r
+          Status = PeiServicesAllocatePages (\r
+                     EfiBootServicesCode,\r
+                     MemPages,\r
+                     &AllocateAddress\r
+                     );\r
+          if (EFI_ERROR (Status)) {\r
+            return EFI_OUT_OF_RESOURCES;\r
+          }\r
+          CopyMem ((VOID *)(UINTN)AllocateAddress, NewPeiUsbDevice, sizeof (PEI_USB_DEVICE));\r
+          NewPeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);\r
+          NewPeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;\r
+          NewPeiUsbDevice->UsbIoPpiList.Ppi = &NewPeiUsbDevice->UsbIoPpi;\r
+          NewPeiUsbDevice->InterfaceDesc = NewPeiUsbDevice->InterfaceDescList[InterfaceIndex];\r
+          for (EndpointIndex = 0; EndpointIndex < NewPeiUsbDevice->InterfaceDesc->NumEndpoints; EndpointIndex++) {\r
+            NewPeiUsbDevice->EndpointDesc[EndpointIndex] = NewPeiUsbDevice->EndpointDescList[InterfaceIndex][EndpointIndex];\r
+          }\r
+\r
+          Status = PeiServicesInstallPpi (&NewPeiUsbDevice->UsbIoPpiList);\r
+\r
+          if (NewPeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {\r
+            NewPeiUsbDevice->IsHub  = 0x1;\r
+\r
+            Status = PeiDoHubConfig (PeiServices, NewPeiUsbDevice);\r
+            if (EFI_ERROR (Status)) {\r
+              return Status;\r
+            }\r
+\r
+            PeiHubEnumeration (PeiServices, NewPeiUsbDevice, CurrentAddress);\r
+          }\r
+        }\r
+      }\r

     }\r
   }\r
 \r
     }\r
   }\r
 \r


@@ -352,7 +406,7 @@ PeiHubEnumeration (
 \r
 /**\r
   The enumeration routine to detect device change.\r
 \r
 /**\r
   The enumeration routine to detect device change.\r

-  \r
+\r

   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  UsbHcPpi               The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.\r
   @param  Usb2HcPpi              The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.\r
   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  UsbHcPpi               The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.\r
   @param  Usb2HcPpi              The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.\r


@@ -377,7 +431,8 @@ PeiUsbEnumeration (
   UINTN                 MemPages;\r
   EFI_PHYSICAL_ADDRESS  AllocateAddress;\r
   UINT8                 CurrentAddress;\r
   UINTN                 MemPages;\r
   EFI_PHYSICAL_ADDRESS  AllocateAddress;\r
   UINT8                 CurrentAddress;\r

-\r
+  UINTN                 InterfaceIndex;\r
+  UINTN                 EndpointIndex;\r

 \r
   CurrentAddress = 0;\r
   if (Usb2HcPpi != NULL) {\r
 \r
   CurrentAddress = 0;\r
   if (Usb2HcPpi != NULL) {\r


@@ -385,7 +440,7 @@ PeiUsbEnumeration (
                 PeiServices,\r
                 Usb2HcPpi,\r
                 (UINT8 *) &NumOfRootPort\r
                 PeiServices,\r
                 Usb2HcPpi,\r
                 (UINT8 *) &NumOfRootPort\r

-                );    \r
+                );\r

   } else if (UsbHcPpi != NULL) {\r
     UsbHcPpi->GetRootHubPortNumber (\r
                 PeiServices,\r
   } else if (UsbHcPpi != NULL) {\r
     UsbHcPpi->GetRootHubPortNumber (\r
                 PeiServices,\r


@@ -397,6 +452,8 @@ PeiUsbEnumeration (
     return EFI_INVALID_PARAMETER;\r
   }\r
 \r
     return EFI_INVALID_PARAMETER;\r
   }\r
 \r

+  DEBUG ((EFI_D_INFO, "PeiUsbEnumeration: NumOfRootPort: %x\n", NumOfRootPort));\r
+\r

   for (Index = 0; Index < NumOfRootPort; Index++) {\r
     //\r
     // First get root port status to detect changes happen\r
   for (Index = 0; Index < NumOfRootPort; Index++) {\r
     //\r
     // First get root port status to detect changes happen\r


@@ -407,7 +464,7 @@ PeiUsbEnumeration (
                   Usb2HcPpi,\r
                   (UINT8) Index,\r
                   &PortStatus\r
                   Usb2HcPpi,\r
                   (UINT8) Index,\r
                   &PortStatus\r

-                  );      \r
+                  );\r

     } else {\r
       UsbHcPpi->GetRootHubPortStatus (\r
                   PeiServices,\r
     } else {\r
       UsbHcPpi->GetRootHubPortStatus (\r
                   PeiServices,\r


@@ -416,48 +473,14 @@ PeiUsbEnumeration (
                   &PortStatus\r
                   );\r
     }\r
                   &PortStatus\r
                   );\r
     }\r

-    DEBUG ((EFI_D_INFO, "USB Status --- ConnectChange[%04x] Status[%04x]\n", PortStatus.PortChangeStatus, PortStatus.PortStatus));\r
-    if (IsPortConnectChange (PortStatus.PortChangeStatus)) {\r
-      //\r
-      // Changes happen, first clear this change status\r
-      //\r
-      if (Usb2HcPpi != NULL) {\r
-        Usb2HcPpi->ClearRootHubPortFeature (\r
-                    PeiServices,\r
-                    Usb2HcPpi,\r
-                    (UINT8) Index,\r
-                    EfiUsbPortConnectChange\r
-                    );        \r
-      } else {\r
-        UsbHcPpi->ClearRootHubPortFeature (\r
-                    PeiServices,\r
-                    UsbHcPpi,\r
-                    (UINT8) Index,\r
-                    EfiUsbPortConnectChange\r
-                    );\r
-      }\r
-      MicroSecondDelay (100 * 1000);\r
-\r
+    DEBUG ((EFI_D_INFO, "USB Status --- Port: %x ConnectChange[%04x] Status[%04x]\n", Index, PortStatus.PortChangeStatus, PortStatus.PortStatus));\r
+    //\r
+    // Only handle connection/enable/overcurrent/reset change.\r
+    //\r
+    if ((PortStatus.PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {\r
+      continue;\r
+    } else {\r

       if (IsPortConnect (PortStatus.PortStatus)) {\r
       if (IsPortConnect (PortStatus.PortStatus)) {\r

-        if (Usb2HcPpi != NULL) {\r
-          Usb2HcPpi->GetRootHubPortStatus (\r
-                      PeiServices,\r
-                      Usb2HcPpi,\r
-                      (UINT8) Index,\r
-                      &PortStatus\r
-                      );\r
-        } else {\r
-          UsbHcPpi->GetRootHubPortStatus (\r
-                      PeiServices,\r
-                      UsbHcPpi,\r
-                      (UINT8) Index,\r
-                      &PortStatus\r
-                      );\r
-        }\r
-\r
-        //\r
-        // Connect change happen\r
-        //\r

         MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;\r
         Status = PeiServicesAllocatePages (\r
                    EfiBootServicesCode,\r
         MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;\r
         Status = PeiServicesAllocatePages (\r
                    EfiBootServicesCode,\r


@@ -492,33 +515,65 @@ PeiUsbEnumeration (
         PeiUsbDevice->IsHub             = 0x0;\r
         PeiUsbDevice->DownStreamPortNo  = 0x0;\r
 \r
         PeiUsbDevice->IsHub             = 0x0;\r
         PeiUsbDevice->DownStreamPortNo  = 0x0;\r
 \r

-        ResetRootPort (\r
-          PeiServices,\r
-          PeiUsbDevice->UsbHcPpi,\r
-          PeiUsbDevice->Usb2HcPpi,\r
-          Index,\r
-          0\r
-          );\r
+        if (((PortStatus.PortChangeStatus & USB_PORT_STAT_C_RESET) == 0) ||\r
+             ((PortStatus.PortStatus & (USB_PORT_STAT_CONNECTION | USB_PORT_STAT_ENABLE)) == 0)) {\r
+          //\r
+          // If the port already has reset change flag and is connected and enabled, skip the port reset logic.\r
+          //\r
+          ResetRootPort (\r
+            PeiServices,\r
+            PeiUsbDevice->UsbHcPpi,\r
+            PeiUsbDevice->Usb2HcPpi,\r
+            Index,\r
+            0\r
+            );\r

 \r
 \r

-        if (Usb2HcPpi != NULL) {\r
-          Usb2HcPpi->GetRootHubPortStatus (\r
-                      PeiServices,\r
-                      Usb2HcPpi,\r
-                      (UINT8) Index,\r
-                      &PortStatus\r
-                      );\r
+          if (Usb2HcPpi != NULL) {\r
+            Usb2HcPpi->GetRootHubPortStatus (\r
+                         PeiServices,\r
+                         Usb2HcPpi,\r
+                         (UINT8) Index,\r
+                         &PortStatus\r
+                         );\r
+          } else {\r
+            UsbHcPpi->GetRootHubPortStatus (\r
+                        PeiServices,\r
+                        UsbHcPpi,\r
+                        (UINT8) Index,\r
+                        &PortStatus\r
+                        );\r
+          }\r

         } else {\r
         } else {\r

-          UsbHcPpi->GetRootHubPortStatus (\r
-                      PeiServices,\r
-                      UsbHcPpi,\r
-                      (UINT8) Index,\r
-                      &PortStatus\r
-                      );\r
+          if (Usb2HcPpi != NULL) {\r
+            Usb2HcPpi->ClearRootHubPortFeature (\r
+                        PeiServices,\r
+                        Usb2HcPpi,\r
+                        (UINT8) Index,\r
+                        EfiUsbPortResetChange\r
+                        );\r
+          } else {\r
+            UsbHcPpi->ClearRootHubPortFeature (\r
+                        PeiServices,\r
+                        UsbHcPpi,\r
+                        (UINT8) Index,\r
+                        EfiUsbPortResetChange\r
+                        );\r
+          }\r

         }\r
 \r
         }\r
 \r

-        PeiUsbDevice->DeviceSpeed = (UINT8)IsPortLowSpeedDeviceAttached (PortStatus.PortStatus);\r
+        PeiUsbDevice->DeviceSpeed = (UINT8) PeiUsbGetDeviceSpeed (PortStatus.PortStatus);\r

         DEBUG ((EFI_D_INFO, "Device Speed =%d\n", PeiUsbDevice->DeviceSpeed));\r
 \r
         DEBUG ((EFI_D_INFO, "Device Speed =%d\n", PeiUsbDevice->DeviceSpeed));\r
 \r

+        if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_SUPER_SPEED)){\r
+          PeiUsbDevice->MaxPacketSize0 = 512;\r
+        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_HIGH_SPEED)) {\r
+          PeiUsbDevice->MaxPacketSize0 = 64;\r
+        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_LOW_SPEED)) {\r
+          PeiUsbDevice->MaxPacketSize0 = 8;\r
+        } else {\r
+          PeiUsbDevice->MaxPacketSize0 = 8;\r
+        }\r
+\r

         //\r
         // Configure that Usb Device\r
         //\r
         //\r
         // Configure that Usb Device\r
         //\r


@@ -532,7 +587,7 @@ PeiUsbEnumeration (
         if (EFI_ERROR (Status)) {\r
           continue;\r
         }\r
         if (EFI_ERROR (Status)) {\r
           continue;\r
         }\r

-        DEBUG ((EFI_D_INFO, "PeiConfigureUsbDevice Success\n"));\r
+        DEBUG ((EFI_D_INFO, "PeiUsbEnumeration: PeiConfigureUsbDevice Success\n"));\r

 \r
         Status = PeiServicesInstallPpi (&PeiUsbDevice->UsbIoPpiList);\r
 \r
 \r
         Status = PeiServicesInstallPpi (&PeiUsbDevice->UsbIoPpiList);\r
 \r


@@ -546,6 +601,42 @@ PeiUsbEnumeration (
 \r
           PeiHubEnumeration (PeiServices, PeiUsbDevice, &CurrentAddress);\r
         }\r
 \r
           PeiHubEnumeration (PeiServices, PeiUsbDevice, &CurrentAddress);\r
         }\r

+\r
+        for (InterfaceIndex = 1; InterfaceIndex < PeiUsbDevice->ConfigDesc->NumInterfaces; InterfaceIndex++) {\r
+          //\r
+          // Begin to deal with the new device\r
+          //\r
+          MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;\r
+          Status = PeiServicesAllocatePages (\r
+                     EfiBootServicesCode,\r
+                     MemPages,\r
+                     &AllocateAddress\r
+                     );\r
+          if (EFI_ERROR (Status)) {\r
+            return EFI_OUT_OF_RESOURCES;\r
+          }\r
+          CopyMem ((VOID *)(UINTN)AllocateAddress, PeiUsbDevice, sizeof (PEI_USB_DEVICE));\r
+          PeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);\r
+          PeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;\r
+          PeiUsbDevice->UsbIoPpiList.Ppi = &PeiUsbDevice->UsbIoPpi;\r
+          PeiUsbDevice->InterfaceDesc = PeiUsbDevice->InterfaceDescList[InterfaceIndex];\r
+          for (EndpointIndex = 0; EndpointIndex < PeiUsbDevice->InterfaceDesc->NumEndpoints; EndpointIndex++) {\r
+            PeiUsbDevice->EndpointDesc[EndpointIndex] = PeiUsbDevice->EndpointDescList[InterfaceIndex][EndpointIndex];\r
+          }\r
+\r
+          Status = PeiServicesInstallPpi (&PeiUsbDevice->UsbIoPpiList);\r
+\r
+          if (PeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {\r
+            PeiUsbDevice->IsHub = 0x1;\r
+\r
+            Status = PeiDoHubConfig (PeiServices, PeiUsbDevice);\r
+            if (EFI_ERROR (Status)) {\r
+              return Status;\r
+            }\r
+\r
+            PeiHubEnumeration (PeiServices, PeiUsbDevice, &CurrentAddress);\r
+          }\r
+        }\r

       } else {\r
         //\r
         // Disconnect change happen, currently we don't support\r
       } else {\r
         //\r
         // Disconnect change happen, currently we don't support\r


@@ -559,7 +650,7 @@ PeiUsbEnumeration (
 \r
 /**\r
   Configure new detected usb device.\r
 \r
 /**\r
   Configure new detected usb device.\r

-  \r
+\r

   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
   @param  Port                   The port to be configured.\r
   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
   @param  Port                   The port to be configured.\r


@@ -591,9 +682,6 @@ PeiConfigureUsbDevice (
   //\r
 \r
   for (Retry = 0; Retry < 3; Retry ++) {\r
   //\r
 \r
   for (Retry = 0; Retry < 3; Retry ++) {\r

-\r
-    PeiUsbDevice->MaxPacketSize0 = 8;\r
-\r

     Status = PeiUsbGetDescriptor (\r
                PeiServices,\r
                UsbIoPpi,\r
     Status = PeiUsbGetDescriptor (\r
                PeiServices,\r
                UsbIoPpi,\r


@@ -604,17 +692,21 @@ PeiConfigureUsbDevice (
                );\r
 \r
     if (!EFI_ERROR (Status)) {\r
                );\r
 \r
     if (!EFI_ERROR (Status)) {\r

-      DEBUG ((EFI_D_INFO, "PeiUsbGet Device Descriptor the %d time Sucess\n", Retry));\r
+      DEBUG ((EFI_D_INFO, "PeiUsbGet Device Descriptor the %d time Success\n", Retry));\r

       break;\r
     }\r
   }\r
 \r
   if (Retry == 3) {\r
       break;\r
     }\r
   }\r
 \r
   if (Retry == 3) {\r

-    DEBUG ((EFI_D_ERROR, "PeiUsbGet Device Descriptor fail\n", Retry));\r
+    DEBUG ((EFI_D_ERROR, "PeiUsbGet Device Descriptor fail: %x %r\n", Retry, Status));\r

     return Status;\r
   }\r
 \r
     return Status;\r
   }\r
 \r

-  PeiUsbDevice->MaxPacketSize0 = DeviceDescriptor.MaxPacketSize0;\r
+  if ((DeviceDescriptor.BcdUSB >= 0x0300) && (DeviceDescriptor.MaxPacketSize0 == 9)) {\r
+    PeiUsbDevice->MaxPacketSize0 = 1 << 9;\r
+  } else {\r
+    PeiUsbDevice->MaxPacketSize0 = DeviceDescriptor.MaxPacketSize0;\r
+  }\r

 \r
   (*DeviceAddress) ++;\r
 \r
 \r
   (*DeviceAddress) ++;\r
 \r


@@ -625,9 +717,10 @@ PeiConfigureUsbDevice (
             );\r
 \r
   if (EFI_ERROR (Status)) {\r
             );\r
 \r
   if (EFI_ERROR (Status)) {\r

-    DEBUG ((EFI_D_ERROR, "PeiUsbSetDeviceAddress Failed\n"));\r
+    DEBUG ((EFI_D_ERROR, "PeiUsbSetDeviceAddress Failed: %r\n", Status));\r

     return Status;\r
   }\r
     return Status;\r
   }\r

+  MicroSecondDelay (USB_SET_DEVICE_ADDRESS_STALL);\r

 \r
   PeiUsbDevice->DeviceAddress = *DeviceAddress;\r
 \r
 \r
   PeiUsbDevice->DeviceAddress = *DeviceAddress;\r
 \r


@@ -647,6 +740,7 @@ PeiConfigureUsbDevice (
     DEBUG ((EFI_D_ERROR, "PeiUsbGetDescriptor First Failed\n"));\r
     return Status;\r
   }\r
     DEBUG ((EFI_D_ERROR, "PeiUsbGetDescriptor First Failed\n"));\r
     return Status;\r
   }\r

+\r

   //\r
   // Get its default configuration and its first interface\r
   //\r
   //\r
   // Get its default configuration and its first interface\r
   //\r


@@ -654,10 +748,10 @@ PeiConfigureUsbDevice (
             PeiServices,\r
             PeiUsbDevice\r
             );\r
             PeiServices,\r
             PeiUsbDevice\r
             );\r

-\r

   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r

+  MicroSecondDelay (USB_GET_CONFIG_DESCRIPTOR_STALL);\r

 \r
   Status = PeiUsbSetConfiguration (\r
             PeiServices,\r
 \r
   Status = PeiUsbSetConfiguration (\r
             PeiServices,\r


@@ -673,7 +767,7 @@ PeiConfigureUsbDevice (
 \r
 /**\r
   Get all configurations from a detected usb device.\r
 \r
 /**\r
   Get all configurations from a detected usb device.\r

-  \r
+\r

   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
 \r
   @param  PeiServices            Describes the list of possible PEI Services.\r
   @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.\r
 \r


@@ -695,6 +789,7 @@ PeiUsbGetAllConfiguration (
   UINT8                     *Ptr;\r
   UINTN                     SkipBytes;\r
   UINTN                     LengthLeft;\r
   UINT8                     *Ptr;\r
   UINTN                     SkipBytes;\r
   UINTN                     LengthLeft;\r

+  UINTN                     InterfaceIndex;\r

   UINTN                     Index;\r
   UINTN                     NumOfEndpoint;\r
 \r
   UINTN                     Index;\r
   UINTN                     NumOfEndpoint;\r
 \r


@@ -716,6 +811,7 @@ PeiUsbGetAllConfiguration (
     DEBUG ((EFI_D_ERROR, "PeiUsbGet Config Descriptor First Failed\n"));\r
     return Status;\r
   }\r
     DEBUG ((EFI_D_ERROR, "PeiUsbGet Config Descriptor First Failed\n"));\r
     return Status;\r
   }\r

+  MicroSecondDelay (USB_GET_CONFIG_DESCRIPTOR_STALL);\r

 \r
   ConfigDesc        = (EFI_USB_CONFIG_DESCRIPTOR *) PeiUsbDevice->ConfigurationData;\r
   ConfigDescLength  = ConfigDesc->TotalLength;\r
 \r
   ConfigDesc        = (EFI_USB_CONFIG_DESCRIPTOR *) PeiUsbDevice->ConfigurationData;\r
   ConfigDescLength  = ConfigDesc->TotalLength;\r


@@ -758,43 +854,16 @@ PeiUsbGetAllConfiguration (
   Ptr += sizeof (EFI_USB_CONFIG_DESCRIPTOR);\r
   LengthLeft = ConfigDescLength - SkipBytes - sizeof (EFI_USB_CONFIG_DESCRIPTOR);\r
 \r
   Ptr += sizeof (EFI_USB_CONFIG_DESCRIPTOR);\r
   LengthLeft = ConfigDescLength - SkipBytes - sizeof (EFI_USB_CONFIG_DESCRIPTOR);\r
 \r

-  //\r
-  // Get the first interface descriptor\r
-  //\r
-  Status = GetExpectedDescriptor (\r
-            Ptr,\r
-            LengthLeft,\r
-            USB_DT_INTERFACE,\r
-            (UINT8) sizeof (EFI_USB_INTERFACE_DESCRIPTOR),\r
-            &SkipBytes\r
-            );\r
-\r
-  if (EFI_ERROR (Status)) {\r
-    return Status;\r
-  }\r
-\r
-  Ptr += SkipBytes;\r
-  PeiUsbDevice->InterfaceDesc = (EFI_USB_INTERFACE_DESCRIPTOR *) Ptr;\r
-\r
-  Ptr += sizeof (EFI_USB_INTERFACE_DESCRIPTOR);\r
-  LengthLeft -= SkipBytes;\r
-  LengthLeft -= sizeof (EFI_USB_INTERFACE_DESCRIPTOR);\r
+  for (InterfaceIndex = 0; InterfaceIndex < PeiUsbDevice->ConfigDesc->NumInterfaces; InterfaceIndex++) {\r

 \r
 \r

-  //\r
-  // Parse all the endpoint descriptor within this interface\r
-  //\r
-  NumOfEndpoint = PeiUsbDevice->InterfaceDesc->NumEndpoints;\r
-  ASSERT (NumOfEndpoint <= MAX_ENDPOINT);\r
-\r
-  for (Index = 0; Index < NumOfEndpoint; Index++) {\r

     //\r
     //\r

-    // Get the endpoint descriptor\r
+    // Get the interface descriptor\r

     //\r
     Status = GetExpectedDescriptor (\r
               Ptr,\r
               LengthLeft,\r
     //\r
     Status = GetExpectedDescriptor (\r
               Ptr,\r
               LengthLeft,\r

-              USB_DT_ENDPOINT,\r
-              (UINT8) sizeof (EFI_USB_ENDPOINT_DESCRIPTOR),\r
+              USB_DT_INTERFACE,\r
+              (UINT8) sizeof (EFI_USB_INTERFACE_DESCRIPTOR),\r

               &SkipBytes\r
               );\r
 \r
               &SkipBytes\r
               );\r
 \r


@@ -803,11 +872,47 @@ PeiUsbGetAllConfiguration (
     }\r
 \r
     Ptr += SkipBytes;\r
     }\r
 \r
     Ptr += SkipBytes;\r

-    PeiUsbDevice->EndpointDesc[Index] = (EFI_USB_ENDPOINT_DESCRIPTOR *) Ptr;\r
+    if (InterfaceIndex == 0) {\r
+      PeiUsbDevice->InterfaceDesc = (EFI_USB_INTERFACE_DESCRIPTOR *) Ptr;\r
+    }\r
+    PeiUsbDevice->InterfaceDescList[InterfaceIndex] = (EFI_USB_INTERFACE_DESCRIPTOR *) Ptr;\r

 \r
 \r

-    Ptr += sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);\r
+    Ptr += sizeof (EFI_USB_INTERFACE_DESCRIPTOR);\r

     LengthLeft -= SkipBytes;\r
     LengthLeft -= SkipBytes;\r

-    LengthLeft -= sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);\r
+    LengthLeft -= sizeof (EFI_USB_INTERFACE_DESCRIPTOR);\r
+\r
+    //\r
+    // Parse all the endpoint descriptor within this interface\r
+    //\r
+    NumOfEndpoint = PeiUsbDevice->InterfaceDescList[InterfaceIndex]->NumEndpoints;\r
+    ASSERT (NumOfEndpoint <= MAX_ENDPOINT);\r
+\r
+    for (Index = 0; Index < NumOfEndpoint; Index++) {\r
+      //\r
+      // Get the endpoint descriptor\r
+      //\r
+      Status = GetExpectedDescriptor (\r
+                Ptr,\r
+                LengthLeft,\r
+                USB_DT_ENDPOINT,\r
+                (UINT8) sizeof (EFI_USB_ENDPOINT_DESCRIPTOR),\r
+                &SkipBytes\r
+                );\r
+\r
+      if (EFI_ERROR (Status)) {\r
+        return Status;\r
+      }\r
+\r
+      Ptr += SkipBytes;\r
+      if (InterfaceIndex == 0) {\r
+        PeiUsbDevice->EndpointDesc[Index] = (EFI_USB_ENDPOINT_DESCRIPTOR *) Ptr;\r
+      }\r
+      PeiUsbDevice->EndpointDescList[InterfaceIndex][Index] = (EFI_USB_ENDPOINT_DESCRIPTOR *) Ptr;\r
+\r
+      Ptr += sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);\r
+      LengthLeft -= SkipBytes;\r
+      LengthLeft -= sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);\r
+    }\r

   }\r
 \r
   return EFI_SUCCESS;\r
   }\r
 \r
   return EFI_SUCCESS;\r


@@ -815,7 +920,7 @@ PeiUsbGetAllConfiguration (
 \r
 /**\r
   Get the start position of next wanted descriptor.\r
 \r
 /**\r
   Get the start position of next wanted descriptor.\r

-  \r
+\r

   @param  Buffer            Buffer containing data to parse.\r
   @param  Length            Buffer length.\r
   @param  DescType          Descriptor type.\r
   @param  Buffer            Buffer containing data to parse.\r
   @param  Length            Buffer length.\r
   @param  DescType          Descriptor type.\r


@@ -835,65 +940,70 @@ GetExpectedDescriptor (
   OUT UINTN       *ParsedBytes\r
   )\r
 {\r
   OUT UINTN       *ParsedBytes\r
   )\r
 {\r

-  UINT16  DescriptorHeader;\r
-  UINT8   Len;\r
-  UINT8   *Ptr;\r
-  UINTN   Parsed;\r
+  USB_DESC_HEAD   *Head;\r
+  UINTN           Offset;\r

 \r
 \r

-  Parsed  = 0;\r
-  Ptr     = Buffer;\r
+  //\r
+  // Total length is too small that cannot hold the single descriptor header plus data. \r
+  //\r
+  if (Length <= sizeof (USB_DESC_HEAD)) {\r
+    DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: met mal-format descriptor, total length = %d!\n", Length));\r
+    return EFI_DEVICE_ERROR;\r
+  }\r

 \r
 \r

-  while (TRUE) {\r
+  //\r
+  // All the descriptor has a common LTV (Length, Type, Value)\r
+  // format. Skip the descriptor that isn't of this Type\r
+  //\r
+  Offset = 0;\r
+  Head   = (USB_DESC_HEAD *)Buffer;\r
+  while (Offset < Length - sizeof (USB_DESC_HEAD)) {\r

     //\r
     //\r

-    // Buffer length should not less than Desc length\r
+    // Above condition make sure Head->Len and Head->Type are safe to access\r

     //\r
     //\r

-    if (Length < DescLength) {\r
-      return EFI_DEVICE_ERROR;\r
-    }\r
-\r
-    DescriptorHeader  = (UINT16) (*Ptr + ((*(Ptr + 1)) << 8));\r
-\r
-    Len               = Buffer[0];\r
+    Head = (USB_DESC_HEAD *)&Buffer[Offset];\r

 \r
 \r

-    //\r
-    // Check to see if it is a start of expected descriptor\r
-    //\r
-    if (DescriptorHeader == ((DescType << 8) | DescLength)) {\r
-      break;\r
+    if (Head->Len == 0) {\r
+      DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: met mal-format descriptor, Head->Len = 0!\n"));\r
+      return EFI_DEVICE_ERROR;\r

     }\r
 \r
     }\r
 \r

-    if ((UINT8) (DescriptorHeader >> 8) == DescType) {\r
-      if (Len > DescLength) {\r
-        return EFI_DEVICE_ERROR;\r
-      }\r
-    }\r

     //\r
     //\r

-    // Descriptor length should be at least 2\r
-    // and should not exceed the buffer length\r
+    // Make sure no overflow when adding Head->Len to Offset.\r

     //\r
     //\r

-    if (Len < 2) {\r
+    if (Head->Len > MAX_UINTN - Offset) {\r
+      DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: met mal-format descriptor, Head->Len = %d!\n", Head->Len));\r

       return EFI_DEVICE_ERROR;\r
     }\r
 \r
       return EFI_DEVICE_ERROR;\r
     }\r
 \r

-    if (Len > Length) {\r
-      return EFI_DEVICE_ERROR;\r
+    if (Head->Type == DescType) {\r
+      break;\r

     }\r
     }\r

-    //\r
-    // Skip this mismatch descriptor\r
-    //\r
-    Length -= Len;\r
-    Ptr += Len;\r
-    Parsed += Len;\r
+\r
+    Offset += Head->Len;\r
+  }\r
+\r
+  //\r
+  // Head->Len is invalid resulting data beyond boundary, or\r
+  // Descriptor cannot be found: No such type.\r
+  //\r
+  if (Length < Offset) {\r
+    DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: met mal-format descriptor, Offset/Len = %d/%d!\n", Offset, Length));\r
+    return EFI_DEVICE_ERROR;\r

   }\r
 \r
   }\r
 \r

-  *ParsedBytes = Parsed;\r
+  if ((Head->Type != DescType) || (Head->Len < DescLength)) {\r
+    DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: descriptor cannot be found, Header(T/L) = %d/%d!\n", Head->Type, Head->Len));\r
+    return EFI_DEVICE_ERROR;\r
+  }\r

 \r
 \r

+  *ParsedBytes = Offset;\r

   return EFI_SUCCESS;\r
 }\r
 \r
 /**\r
   Send reset signal over the given root hub port.\r
   return EFI_SUCCESS;\r
 }\r
 \r
 /**\r
   Send reset signal over the given root hub port.\r

-  \r
+\r

   @param  PeiServices       Describes the list of possible PEI Services.\r
   @param  UsbHcPpi          The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.\r
   @param  Usb2HcPpi         The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.\r
   @param  PeiServices       Describes the list of possible PEI Services.\r
   @param  UsbHcPpi          The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.\r
   @param  Usb2HcPpi         The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.\r


@@ -911,11 +1021,13 @@ ResetRootPort (
   )\r
 {\r
   EFI_STATUS             Status;\r
   )\r
 {\r
   EFI_STATUS             Status;\r

+  UINTN                  Index;\r
+  EFI_USB_PORT_STATUS    PortStatus;\r

 \r
 \r
   if (Usb2HcPpi != NULL) {\r
     MicroSecondDelay (200 * 1000);\r
 \r
 \r
   if (Usb2HcPpi != NULL) {\r
     MicroSecondDelay (200 * 1000);\r

-    \r
+\r

     //\r
     // reset root port\r
     //\r
     //\r
     // reset root port\r
     //\r


@@ -925,14 +1037,18 @@ ResetRootPort (
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r

-    \r
+\r

     if (EFI_ERROR (Status)) {\r
       DEBUG ((EFI_D_ERROR, "SetRootHubPortFeature EfiUsbPortReset Failed\n"));\r
       return;\r
     }\r
     if (EFI_ERROR (Status)) {\r
       DEBUG ((EFI_D_ERROR, "SetRootHubPortFeature EfiUsbPortReset Failed\n"));\r
       return;\r
     }\r

-    \r
-    MicroSecondDelay (200 * 1000);\r
-    \r
+\r
+    //\r
+    // Drive the reset signal for at least 50ms. Check USB 2.0 Spec\r
+    // section 7.1.7.5 for timing requirements.\r
+    //\r
+    MicroSecondDelay (USB_SET_ROOT_PORT_RESET_STALL);\r
+\r

     //\r
     // clear reset root port\r
     //\r
     //\r
     // clear reset root port\r
     //\r


@@ -942,21 +1058,57 @@ ResetRootPort (
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r

-    \r
+\r

     if (EFI_ERROR (Status)) {\r
       DEBUG ((EFI_D_ERROR, "ClearRootHubPortFeature EfiUsbPortReset Failed\n"));\r
       return;\r
     }\r
     if (EFI_ERROR (Status)) {\r
       DEBUG ((EFI_D_ERROR, "ClearRootHubPortFeature EfiUsbPortReset Failed\n"));\r
       return;\r
     }\r

-    \r
-    MicroSecondDelay (1 * 1000);\r
-    \r
+\r
+    MicroSecondDelay (USB_CLR_ROOT_PORT_RESET_STALL);\r
+\r
+    //\r
+    // USB host controller won't clear the RESET bit until\r
+    // reset is actually finished.\r
+    //\r
+    ZeroMem (&PortStatus, sizeof (EFI_USB_PORT_STATUS));\r
+\r
+    for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {\r
+      Status = Usb2HcPpi->GetRootHubPortStatus (\r
+                            PeiServices,\r
+                            Usb2HcPpi,\r
+                            PortNum,\r
+                            &PortStatus\r
+                            );\r
+      if (EFI_ERROR (Status)) {\r
+        return;\r
+      }\r
+\r
+      if (!USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_RESET)) {\r
+        break;\r
+      }\r
+\r
+      MicroSecondDelay (USB_WAIT_PORT_STS_CHANGE_STALL);\r
+    }\r
+\r
+    if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {\r
+      DEBUG ((EFI_D_ERROR, "ResetRootPort: reset not finished in time on port %d\n", PortNum));\r
+      return;\r
+    }\r
+\r
+    Usb2HcPpi->ClearRootHubPortFeature (\r
+                PeiServices,\r
+                Usb2HcPpi,\r
+                PortNum,\r
+                EfiUsbPortResetChange\r
+                );\r
+\r

     Usb2HcPpi->ClearRootHubPortFeature (\r
                 PeiServices,\r
                 Usb2HcPpi,\r
                 PortNum,\r
                 EfiUsbPortConnectChange\r
                 );\r
     Usb2HcPpi->ClearRootHubPortFeature (\r
                 PeiServices,\r
                 Usb2HcPpi,\r
                 PortNum,\r
                 EfiUsbPortConnectChange\r
                 );\r

-    \r
+\r

     //\r
     // Set port enable\r
     //\r
     //\r
     // Set port enable\r
     //\r


@@ -966,18 +1118,18 @@ ResetRootPort (
                 PortNum,\r
                 EfiUsbPortEnable\r
                 );\r
                 PortNum,\r
                 EfiUsbPortEnable\r
                 );\r

-    \r
+\r

     Usb2HcPpi->ClearRootHubPortFeature (\r
                 PeiServices,\r
                 Usb2HcPpi,\r
                 PortNum,\r
                 EfiUsbPortEnableChange\r
                 );\r
     Usb2HcPpi->ClearRootHubPortFeature (\r
                 PeiServices,\r
                 Usb2HcPpi,\r
                 PortNum,\r
                 EfiUsbPortEnableChange\r
                 );\r

-    \r
+\r

     MicroSecondDelay ((RetryIndex + 1) * 50 * 1000);\r
   } else {\r
     MicroSecondDelay (200 * 1000);\r
     MicroSecondDelay ((RetryIndex + 1) * 50 * 1000);\r
   } else {\r
     MicroSecondDelay (200 * 1000);\r

-    \r
+\r

     //\r
     // reset root port\r
     //\r
     //\r
     // reset root port\r
     //\r


@@ -987,14 +1139,18 @@ ResetRootPort (
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r

-    \r
+\r

     if (EFI_ERROR (Status)) {\r
       DEBUG ((EFI_D_ERROR, "SetRootHubPortFeature EfiUsbPortReset Failed\n"));\r
       return;\r
     }\r
     if (EFI_ERROR (Status)) {\r
       DEBUG ((EFI_D_ERROR, "SetRootHubPortFeature EfiUsbPortReset Failed\n"));\r
       return;\r
     }\r

-    \r
-    MicroSecondDelay (200 * 1000);\r
-    \r
+\r
+    //\r
+    // Drive the reset signal for at least 50ms. Check USB 2.0 Spec\r
+    // section 7.1.7.5 for timing requirements.\r
+    //\r
+    MicroSecondDelay (USB_SET_ROOT_PORT_RESET_STALL);\r
+\r

     //\r
     // clear reset root port\r
     //\r
     //\r
     // clear reset root port\r
     //\r


@@ -1004,21 +1160,57 @@ ResetRootPort (
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r

-    \r
+\r

     if (EFI_ERROR (Status)) {\r
       DEBUG ((EFI_D_ERROR, "ClearRootHubPortFeature EfiUsbPortReset Failed\n"));\r
       return;\r
     }\r
     if (EFI_ERROR (Status)) {\r
       DEBUG ((EFI_D_ERROR, "ClearRootHubPortFeature EfiUsbPortReset Failed\n"));\r
       return;\r
     }\r

-    \r
-    MicroSecondDelay (1 * 1000);\r
-    \r
+\r
+    MicroSecondDelay (USB_CLR_ROOT_PORT_RESET_STALL);\r
+\r
+    //\r
+    // USB host controller won't clear the RESET bit until\r
+    // reset is actually finished.\r
+    //\r
+    ZeroMem (&PortStatus, sizeof (EFI_USB_PORT_STATUS));\r
+\r
+    for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {\r
+      Status = UsbHcPpi->GetRootHubPortStatus (\r
+                           PeiServices,\r
+                           UsbHcPpi,\r
+                           PortNum,\r
+                           &PortStatus\r
+                           );\r
+      if (EFI_ERROR (Status)) {\r
+        return;\r
+      }\r
+\r
+      if (!USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_RESET)) {\r
+        break;\r
+      }\r
+\r
+      MicroSecondDelay (USB_WAIT_PORT_STS_CHANGE_STALL);\r
+    }\r
+\r
+    if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {\r
+      DEBUG ((EFI_D_ERROR, "ResetRootPort: reset not finished in time on port %d\n", PortNum));\r
+      return;\r
+    }\r
+\r
+    UsbHcPpi->ClearRootHubPortFeature (\r
+                PeiServices,\r
+                UsbHcPpi,\r
+                PortNum,\r
+                EfiUsbPortResetChange\r
+                );\r
+\r

     UsbHcPpi->ClearRootHubPortFeature (\r
                 PeiServices,\r
                 UsbHcPpi,\r
                 PortNum,\r
                 EfiUsbPortConnectChange\r
                 );\r
     UsbHcPpi->ClearRootHubPortFeature (\r
                 PeiServices,\r
                 UsbHcPpi,\r
                 PortNum,\r
                 EfiUsbPortConnectChange\r
                 );\r

-    \r
+\r

     //\r
     // Set port enable\r
     //\r
     //\r
     // Set port enable\r
     //\r


@@ -1028,14 +1220,14 @@ ResetRootPort (
                 PortNum,\r
                 EfiUsbPortEnable\r
                 );\r
                 PortNum,\r
                 EfiUsbPortEnable\r
                 );\r

-    \r
+\r

     UsbHcPpi->ClearRootHubPortFeature (\r
                 PeiServices,\r
                 UsbHcPpi,\r
                 PortNum,\r
                 EfiUsbPortEnableChange\r
                 );\r
     UsbHcPpi->ClearRootHubPortFeature (\r
                 PeiServices,\r
                 UsbHcPpi,\r
                 PortNum,\r
                 EfiUsbPortEnableChange\r
                 );\r

-    \r
+\r

     MicroSecondDelay ((RetryIndex + 1) * 50 * 1000);\r
   }\r
   return;\r
     MicroSecondDelay ((RetryIndex + 1) * 50 * 1000);\r
   }\r
   return;\r