1 files changed, 23 insertions, 7 deletions
diff --git a/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c b/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c
index 9d50ef824..a66d7cd95 100644
--- a/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c
+++ b/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c
@@ -543,6 +543,10 @@ XhcSetBiosOwnership (
 {
   UINT32                    Buffer;
 
+  if (Xhc->UsbLegSupOffset == 0xFFFFFFFF) {
+    return;
+  }
+
   DEBUG ((EFI_D_INFO, "XhcSetBiosOwnership: called to set BIOS ownership\n"));
 
   Buffer = XhcReadExtCapReg (Xhc, Xhc->UsbLegSupOffset);
@@ -563,6 +567,10 @@ XhcClearBiosOwnership (
 {
   UINT32                    Buffer;
 
+  if (Xhc->UsbLegSupOffset == 0xFFFFFFFF) {
+    return;
+  }
+
   DEBUG ((EFI_D_INFO, "XhcClearBiosOwnership: called to clear BIOS ownership\n"));
 
   Buffer = XhcReadExtCapReg (Xhc, Xhc->UsbLegSupOffset);
@@ -571,16 +579,18 @@ XhcClearBiosOwnership (
 }
 
 /**
-  Calculate the XHCI legacy support capability register offset.
+  Calculate the offset of the XHCI capability.
 
   @param  Xhc     The XHCI Instance.
+  @param  CapId   The XHCI Capability ID.
 
   @return The offset of XHCI legacy support capability register.
 
 **/
 UINT32
-XhcGetLegSupCapAddr (
-  IN USB_XHCI_INSTANCE    *Xhc
+XhcGetCapabilityAddr (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT8                CapId
   )
 {
   UINT32 ExtCapOffset;
@@ -594,7 +604,7 @@ XhcGetLegSupCapAddr (
     // Check if the extended capability register's capability id is USB Legacy Support.
     //
     Data = XhcReadExtCapReg (Xhc, ExtCapOffset);
-    if ((Data & 0xFF) == 0x1) {
+    if ((Data & 0xFF) == CapId) {
       return ExtCapOffset;
     }
     //
@@ -604,7 +614,7 @@ XhcGetLegSupCapAddr (
     ExtCapOffset += (NextExtCapReg << 2);
   } while (NextExtCapReg != 0);
 
-  return 0;
+  return 0xFFFFFFFF;
 }
 
 /**
@@ -660,6 +670,8 @@ XhcResetHC (
 {
   EFI_STATUS              Status;
 
+  Status = EFI_SUCCESS;
+
   DEBUG ((EFI_D_INFO, "XhcResetHC!\n"));
   //
   // Host can only be reset when it is halt. If not so, halt it
@@ -672,8 +684,12 @@ XhcResetHC (
     }
   }
 
-  XhcSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET);
-  Status = XhcWaitOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET, FALSE, Timeout);
+  if ((Xhc->DebugCapSupOffset == 0xFFFFFFFF) || ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset) & 0xFF) != XHC_CAP_USB_DEBUG) ||
+      ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset + XHC_DC_DCCTRL) & BIT0) == 0)) {
+    XhcSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET);
+    Status = XhcWaitOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET, FALSE, Timeout);
+  }
+
   return Status;
 }