3 files changed, 30 insertions, 7 deletions
diff --git a/src/drivers/usb/ehci_debug.c b/src/drivers/usb/ehci_debug.c
index b1ae27dbe9..c60fbaaaad 100644
--- a/src/drivers/usb/ehci_debug.c
+++ b/src/drivers/usb/ehci_debug.c
@@ -569,6 +569,8 @@ static int usbdebug_init_(unsigned ehci_bar, unsigned offset, struct ehci_debug_
 	int port_map_tried;
 	int playtimes = 3;
 
+	dprintk(BIOS_INFO, "ehci_bar: 0x%x debug_offset 0x%x\n", ehci_bar, offset);
+
 	ehci_caps  = (struct ehci_caps *)ehci_bar;
 	ehci_regs  = (struct ehci_regs *)(ehci_bar +
 			HC_LENGTH(read32((unsigned long)&ehci_caps->hc_capbase)));
@@ -589,7 +591,6 @@ try_next_port:
 	debug_port = HCS_DEBUG_PORT(hcs_params);
 	n_ports    = HCS_N_PORTS(hcs_params);
 
-	dprintk(BIOS_INFO, "ehci_bar: 0x%x\n", ehci_bar);
 	dprintk(BIOS_INFO, "debug_port: %d\n", debug_port);
 	dprintk(BIOS_INFO, "n_ports:    %d\n", n_ports);
 
@@ -926,11 +927,13 @@ struct dbgp_pipe *dbgp_console_input(void)
 int usbdebug_init(void)
 {
 	struct ehci_debug_info *dbg_info = dbgp_ehci_info();
+	unsigned int ehci_base, dbg_offset;
 
 #if !defined(__PRE_RAM__) && !defined(__SMM__)
 	if (!get_usbdebug_from_cbmem(dbg_info))
 		return 0;
 #endif
-	ehci_debug_hw_enable();
-	return usbdebug_init_(CONFIG_EHCI_BAR, CONFIG_EHCI_DEBUG_OFFSET, dbg_info);
+	if (ehci_debug_hw_enable(&ehci_base, &dbg_offset))
+		return -1;
+	return usbdebug_init_(ehci_base, dbg_offset, dbg_info);
 }
diff --git a/src/drivers/usb/ehci_debug.h b/src/drivers/usb/ehci_debug.h
index 7cfac011e1..30bf724cb8 100644
--- a/src/drivers/usb/ehci_debug.h
+++ b/src/drivers/usb/ehci_debug.h
@@ -24,10 +24,12 @@
 void usbdebug_re_enable(unsigned ehci_base);
 void usbdebug_disable(void);
 
-void ehci_debug_hw_enable(void);
+/* Returns 0 on success and sets MMIO base and dbg_offset if EHCI debug
+ * capability was found and enabled. Returns non-zero on error.
+ */
+int ehci_debug_hw_enable(unsigned *base, unsigned *dbg_offset);
 void ehci_debug_select_port(unsigned int port);
 
-
 #define DBGP_EP_VALID		(1<<0)
 #define DBGP_EP_ENABLED		(1<<1)
 #define DBGP_EP_BUSY		(1<<2)
diff --git a/src/drivers/usb/pci_ehci.c b/src/drivers/usb/pci_ehci.c
index 7c715f67bd..c7e8afb06f 100644
--- a/src/drivers/usb/pci_ehci.c
+++ b/src/drivers/usb/pci_ehci.c
@@ -34,12 +34,30 @@ static struct device_operations *ehci_drv_ops;
 static struct device_operations ehci_dbg_ops;
 #endif
 
-void ehci_debug_hw_enable(void)
+int ehci_debug_hw_enable(unsigned int *base, unsigned int *dbg_offset)
 {
-#if defined(__PRE_RAM__) || !CONFIG_USBDEBUG_IN_ROMSTAGE
 	pci_devfn_t dbg_dev = pci_ehci_dbg_dev(CONFIG_USBDEBUG_HCD_INDEX);
 	pci_ehci_dbg_enable(dbg_dev, CONFIG_EHCI_BAR);
+#ifdef __SIMPLE_DEVICE__
+	pci_devfn_t dev = dbg_dev;
+#else
+	device_t dev = dev_find_slot(PCI_DEV2SEGBUS(dbg_dev), PCI_DEV2DEVFN(dbg_dev));
 #endif
+
+	u8 pos = pci_find_capability(dev, PCI_CAP_ID_EHCI_DEBUG);
+	if (!pos)
+		return -1;
+
+	u32 cap = pci_read_config32(dev, pos);
+
+	/* FIXME: We should remove static EHCI_BAR_INDEX. */
+	u8 dbg_bar = 0x10 + 4 * ((cap >> 29) - 1);
+	if (dbg_bar != EHCI_BAR_INDEX)
+		return -1;
+
+	*base = CONFIG_EHCI_BAR;
+	*dbg_offset = (cap>>16) & 0x1ffc;
+	return 0;
 }
 
 void ehci_debug_select_port(unsigned int port)