]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c
MdeModulePkg/UsbBusPei: Fix out-of-bound read access to descriptors
[mirror_edk2.git] / MdeModulePkg / Bus / Usb / UsbBusPei / UsbPeim.c
index 23090f68f39f07ee6cd66dfebb9fe8e58efd121b..86734f2f7362b4bc0c9f25705e8c9d65086bb2c8 100644 (file)
@@ -1,8 +1,8 @@
 /** @file\r
 The module to produce Usb Bus PPI.\r
 \r
-Copyright (c) 2006 - 2014, 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
@@ -37,7 +37,7 @@ EFI_PEI_PPI_DESCRIPTOR mUsbIoPpiList = {
 \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
@@ -56,7 +56,7 @@ PeiUsbEnumeration (
 \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
@@ -77,7 +77,7 @@ PeiConfigureUsbDevice (
 \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
@@ -94,7 +94,7 @@ PeiUsbGetAllConfiguration (
 \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
@@ -116,7 +116,7 @@ GetExpectedDescriptor (
 \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
@@ -142,7 +142,7 @@ PeimInitializeUsb (
   }\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
@@ -176,18 +176,18 @@ PeimInitializeUsb (
                  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
-      }   \r
-      PeiUsbEnumeration ((EFI_PEI_SERVICES **) PeiServices, NULL, Usb2HcPpi);   \r
+      }\r
+      PeiUsbEnumeration ((EFI_PEI_SERVICES **) PeiServices, NULL, Usb2HcPpi);\r
       Index++;\r
     }\r
   }\r
-  \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
+\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
@@ -228,6 +228,8 @@ PeiHubEnumeration (
 \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
@@ -241,25 +243,14 @@ PeiHubEnumeration (
       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
-\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
@@ -294,19 +285,44 @@ PeiHubEnumeration (
         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
-        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
-        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
-        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
@@ -330,6 +346,7 @@ PeiHubEnumeration (
         if (EFI_ERROR (Status)) {\r
           continue;\r
         }\r
+        DEBUG ((EFI_D_INFO, "PeiHubEnumeration: PeiConfigureUsbDevice Success\n"));\r
 \r
         Status = PeiServicesInstallPpi (&NewPeiUsbDevice->UsbIoPpiList);\r
 \r
@@ -389,7 +406,7 @@ PeiHubEnumeration (
 \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
@@ -423,7 +440,7 @@ PeiUsbEnumeration (
                 PeiServices,\r
                 Usb2HcPpi,\r
                 (UINT8 *) &NumOfRootPort\r
-                );    \r
+                );\r
   } else if (UsbHcPpi != NULL) {\r
     UsbHcPpi->GetRootHubPortNumber (\r
                 PeiServices,\r
@@ -435,6 +452,8 @@ PeiUsbEnumeration (
     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
@@ -445,7 +464,7 @@ PeiUsbEnumeration (
                   Usb2HcPpi,\r
                   (UINT8) Index,\r
                   &PortStatus\r
-                  );      \r
+                  );\r
     } else {\r
       UsbHcPpi->GetRootHubPortStatus (\r
                   PeiServices,\r
@@ -454,48 +473,14 @@ PeiUsbEnumeration (
                   &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 (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
@@ -530,33 +515,65 @@ PeiUsbEnumeration (
         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
-        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
-          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
-        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
+        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
@@ -570,7 +587,7 @@ PeiUsbEnumeration (
         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
@@ -633,7 +650,7 @@ PeiUsbEnumeration (
 \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
@@ -665,9 +682,6 @@ PeiConfigureUsbDevice (
   //\r
 \r
   for (Retry = 0; Retry < 3; Retry ++) {\r
-\r
-    PeiUsbDevice->MaxPacketSize0 = 8;\r
-\r
     Status = PeiUsbGetDescriptor (\r
                PeiServices,\r
                UsbIoPpi,\r
@@ -678,17 +692,21 @@ PeiConfigureUsbDevice (
                );\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
-    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
-  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
@@ -699,9 +717,10 @@ PeiConfigureUsbDevice (
             );\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
+  MicroSecondDelay (USB_SET_DEVICE_ADDRESS_STALL);\r
 \r
   PeiUsbDevice->DeviceAddress = *DeviceAddress;\r
 \r
@@ -732,6 +751,7 @@ PeiConfigureUsbDevice (
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
+  MicroSecondDelay (USB_GET_CONFIG_DESCRIPTOR_STALL);\r
 \r
   Status = PeiUsbSetConfiguration (\r
             PeiServices,\r
@@ -747,7 +767,7 @@ PeiConfigureUsbDevice (
 \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
@@ -791,6 +811,7 @@ PeiUsbGetAllConfiguration (
     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
@@ -899,7 +920,7 @@ PeiUsbGetAllConfiguration (
 \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
@@ -919,65 +940,70 @@ GetExpectedDescriptor (
   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
-  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
-  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
-    // Buffer length should not less than Desc length\r
+    // Above condition make sure Head->Len and Head->Type are safe to access\r
     //\r
-    if (Length < DescLength) {\r
-      return EFI_DEVICE_ERROR;\r
-    }\r
-\r
-    DescriptorHeader  = (UINT16) (*Ptr + ((*(Ptr + 1)) << 8));\r
+    Head = (USB_DESC_HEAD *)&Buffer[Offset];\r
 \r
-    Len               = Buffer[0];\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
-    if ((UINT8) (DescriptorHeader >> 8) == DescType) {\r
-      if (Len > DescLength) {\r
-        return EFI_DEVICE_ERROR;\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
-    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
-    if (Len > Length) {\r
-      return EFI_DEVICE_ERROR;\r
+    if (Head->Type == DescType) {\r
+      break;\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
-  *ParsedBytes = Parsed;\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
+  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
+  *ParsedBytes = Offset;\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
@@ -995,11 +1021,13 @@ ResetRootPort (
   )\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
     //\r
     // reset root port\r
     //\r
@@ -1009,14 +1037,18 @@ ResetRootPort (
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r
-    \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
@@ -1026,21 +1058,57 @@ ResetRootPort (
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r
-    \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
-    \r
+\r
     //\r
     // Set port enable\r
     //\r
@@ -1050,18 +1118,18 @@ ResetRootPort (
                 PortNum,\r
                 EfiUsbPortEnable\r
                 );\r
-    \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
-    \r
+\r
     //\r
     // reset root port\r
     //\r
@@ -1071,14 +1139,18 @@ ResetRootPort (
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r
-    \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
@@ -1088,21 +1160,57 @@ ResetRootPort (
                          PortNum,\r
                          EfiUsbPortReset\r
                          );\r
-    \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
-    \r
+\r
     //\r
     // Set port enable\r
     //\r
@@ -1112,14 +1220,14 @@ ResetRootPort (
                 PortNum,\r
                 EfiUsbPortEnable\r
                 );\r
-    \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