1 files changed, 30 insertions, 10 deletions

diff --git a/sys/dev/usb/controller/xhci_pci.c b/sys/dev/usb/controller/xhci_pci.c
index da16f51..2548fbb 100644
--- a/sys/dev/usb/controller/xhci_pci.c
+++ b/sys/dev/usb/controller/xhci_pci.c
@@ -146,6 +146,25 @@ xhci_interrupt_poll(void *_sc)
 }
 
 static int
+xhci_pci_port_route(device_t self, uint32_t set, uint32_t clear)
+{
+	uint32_t temp;
+
+	temp = pci_read_config(self, PCI_XHCI_INTEL_USB3_PSSEN, 4) |
+	    pci_read_config(self, PCI_XHCI_INTEL_XUSB2PR, 4);
+
+	temp |= set;
+	temp &= ~clear;
+
+	pci_write_config(self, PCI_XHCI_INTEL_USB3_PSSEN, temp, 4);
+	pci_write_config(self, PCI_XHCI_INTEL_XUSB2PR, temp, 4);
+
+	device_printf(self, "Port routing mask set to 0x%08x\n", temp);
+
+	return (0);
+}
+
+static int
 xhci_pci_attach(device_t self)
 {
 	struct xhci_softc *sc = device_get_softc(self);
@@ -189,7 +208,7 @@ xhci_pci_attach(device_t self)
 	    &sc->sc_irq_rid, RF_SHAREABLE | RF_ACTIVE);
 	if (sc->sc_irq_res == NULL) {
 		device_printf(self, "Could not allocate IRQ\n");
-		goto error;
+		/* goto error; FALLTHROUGH - use polling */
 	}
 	sc->sc_bus.bdev = device_add_child(self, "usbus", -1);
 	if (sc->sc_bus.bdev == NULL) {
@@ -216,6 +235,16 @@ xhci_pci_attach(device_t self)
 		USB_BUS_UNLOCK(&sc->sc_bus);
 	}
 
+	/* On Intel chipsets reroute ports from EHCI to XHCI controller. */
+	switch (pci_get_devid(self)) {
+	case 0x1e318086:	/* Panther Point */
+	case 0x8c318086:	/* Lynx Point */
+		sc->sc_port_route = &xhci_pci_port_route;
+		break;
+	default:
+		break;
+	}
+
 	xhci_pci_take_controller(self);
 
 	err = xhci_halt_controller(sc);
@@ -284,7 +313,6 @@ static int
 xhci_pci_take_controller(device_t self)
 {
 	struct xhci_softc *sc = device_get_softc(self);
-	uint32_t device_id = pci_get_devid(self);
 	uint32_t cparams;
 	uint32_t eecp;
 	uint32_t eec;
@@ -325,13 +353,5 @@ xhci_pci_take_controller(device_t self)
 			usb_pause_mtx(NULL, hz / 100);	/* wait 10ms */
 		}
 	}
-
-	/* On Intel chipsets reroute ports from EHCI to XHCI controller. */
-	if (device_id == 0x1e318086 /* Panther Point */ ||
-	    device_id == 0x8c318086 /* Lynx Point */) {
-		uint32_t temp = xhci_get_port_route();
-		pci_write_config(self, PCI_XHCI_INTEL_USB3_PSSEN, temp, 4);
-		pci_write_config(self, PCI_XHCI_INTEL_XUSB2PR, temp, 4);
-	}
 	return (0);
 }