1 files changed, 9 insertions, 85 deletions

diff --git a/src/southbridge/intel/lynxpoint/usb_xhci.c b/src/southbridge/intel/lynxpoint/usb_xhci.c
index f6409b4381..e09134081c 100644
--- a/src/southbridge/intel/lynxpoint/usb_xhci.c
+++ b/src/southbridge/intel/lynxpoint/usb_xhci.c
@@ -37,6 +37,8 @@ static u32 usb_xhci_mem_base(device_t dev)
 	return mem_base & ~0xf;
 }
 
+#ifdef __SMM__
+
 static int usb_xhci_port_count_usb3(device_t dev)
 {
 	if (pch_is_lp()) {
@@ -78,8 +80,6 @@ static void usb_xhci_reset_port_usb3(u32 mem_base, int port)
 #define XHCI_RESET_DELAY_US	1000 /* 1ms */
 #define XHCI_RESET_TIMEOUT	100  /* 100ms */
 
-#ifdef __SMM__
-
 /*
  * 1) Wait until port is done polling
  * 2) If port is disconnected
@@ -87,7 +87,7 @@ static void usb_xhci_reset_port_usb3(u32 mem_base, int port)
  *  b) Poll for warm reset complete
  *  c) Write 1 to port change status bits
  */
-static void usb_xhci_reset_usb3(device_t dev)
+static void usb_xhci_reset_usb3(device_t dev, int all)
 {
 	u32 status, port_disabled;
 	int timeout, port;
@@ -127,7 +127,10 @@ static void usb_xhci_reset_usb3(device_t dev)
 			continue;
 		status = read32(portsc) & XHCI_USB3_PORTSC_PLS;
 		/* Reset all or only disconnected ports */
-		usb_xhci_reset_port_usb3(mem_base, port);
+		if (all || status == XHCI_PLSR_RXDETECT)
+			usb_xhci_reset_port_usb3(mem_base, port);
+		else
+			port_disabled |= 1 << port;
 	}
 
 	/* Wait for warm reset complete on all reset ports */
@@ -181,7 +184,7 @@ void usb_xhci_sleep_prepare(device_t dev, u8 slp_typ)
 		write32(mem_base + 0x816c, reg32);
 
 		/* Reset disconnected USB3 ports */
-		usb_xhci_reset_usb3(dev);
+		usb_xhci_reset_usb3(dev, 0);
 
 		/* Set MMIO 0x80e0[15] */
 		reg32 = read32(mem_base + 0x80e0);
@@ -233,7 +236,7 @@ void usb_xhci_route_all(void)
 		usb_ehci_disable(PCH_EHCI2_DEV);
 
 	/* Reset and clear port change status */
-	usb_xhci_reset_usb3(PCH_XHCI_DEV);
+	usb_xhci_reset_usb3(PCH_XHCI_DEV, 1);
 }
 
 #else /* !__SMM__ */
@@ -285,81 +288,6 @@ static void usb_xhci_clock_gating(device_t dev)
 	pci_write_config32(dev, 0xa4, reg32);
 }
 
-/* Re-enable ports that are disabled */
-static void usb_xhci_enable_ports_usb3(device_t dev)
-{
-#if CONFIG_FINALIZE_USB_ROUTE_XHCI
-	int port;
-	u32 portsc, status, disabled;
-	u32 mem_base = usb_xhci_mem_base(dev);
-	int port_count = usb_xhci_port_count_usb3(dev);
-	u8 port_reset = 0;
-	int timeout;
-
-	if (!mem_base || !port_count)
-		return;
-
-	/* Get port disable override map */
-	disabled = pci_read_config32(dev, XHCI_USB3PDO);
-
-	for (port = 0; port < port_count; port++) {
-		/* Skip overridden ports */
-		if (disabled & (1 << port))
-			continue;
-		portsc = mem_base + XHCI_USB3_PORTSC(port);
-		status = read32(portsc) & XHCI_USB3_PORTSC_PLS;
-		switch (status) {
-		case XHCI_PLSR_RXDETECT:
-			/* Clear change status */
-			printk(BIOS_DEBUG, "usb_xhci reset status %d\n", port);
-			usb_xhci_reset_status_usb3(mem_base, port);
-			break;
-		case XHCI_PLSR_DISABLED:
-		default:
-			/* Reset port */
-			printk(BIOS_DEBUG, "usb_xhci reset port %d\n", port);
-			usb_xhci_reset_port_usb3(mem_base, port);
-			port_reset |= 1 << port;
-			break;
-		}
-	}
-
-	if (!port_reset)
-		return;
-
-	/* Wait for warm reset complete on all reset ports */
-	for (timeout = XHCI_RESET_TIMEOUT; timeout; timeout--) {
-		int complete = 1;
-		for (port = 0; port < port_count; port++) {
-			/* Only check ports that were reset */
-			if (!(port_reset & (1 << port)))
-				continue;
-			/* Check if warm reset is complete */
-			status = read32(mem_base + XHCI_USB3_PORTSC(port));
-			if (!(status & XHCI_USB3_PORTSC_WRC))
-				complete = 0;
-		}
-		/* Check for warm reset complete in any port */
-		if (complete)
-			break;
-		udelay(XHCI_RESET_DELAY_US);
-	}
-
-	/* Enable ports that were reset */
-	for (port = 0; port < port_count; port++) {
-		/* Only check ports that were reset */
-		if (!(port_reset & (1 << port)))
-			continue;
-		/* Transition to enabled */
-		portsc = mem_base + XHCI_USB3_PORTSC(port);
-		status = read32(portsc);
-		status &= ~(XHCI_USB3_PORTSC_PLS | XHCI_USB3_PORTSC_PED);
-		status |= XHCI_PLSW_ENABLE | XHCI_USB3_PORTSC_LWS;
-		write32(portsc, status);
-	}
-#endif
-}
-
 static void usb_xhci_init(device_t dev)
 {
 	u32 reg32;
@@ -422,10 +350,6 @@ static void usb_xhci_init(device_t dev)
 	reg32 &= ~(1 << 23); /* unsupported request */
 	reg32 |= (1 << 31);
 	pci_write_config32(dev, 0x40, reg32);
-
-	/* Enable ports that are disabled before returning to OS */
-	if (acpi_is_wakeup_s3())
-		usb_xhci_enable_ports_usb3(dev);
 }
 
 static void usb_xhci_set_subsystem(device_t dev, unsigned vendor,