[coreboot-gerrit] Patch set updated for coreboot: ehci_debug: Remove dependency on dbg_pipe (and cbmem) in romstage

Kimarie Hoot (kimarie.hoot@se-eng.com) gerrit at coreboot.org
Fri Jun 26 19:53:06 CEST 2015


Kimarie Hoot (kimarie.hoot at se-eng.com) just uploaded a new patch set to gerrit, which you can find at http://review.coreboot.org/10646

-gerrit

commit b2a1a914b94c27f50d8eaaeb73ff5fbc2e7c4433
Author: Kimarie Hoot <kimarie.hoot at se-eng.com>
Date:   Tue Jun 23 09:44:00 2015 -0600

    ehci_debug: Remove dependency on dbg_pipe (and cbmem) in romstage
    
    Completely re-write/remove the usbdebug_init_ function with
    all of its labels and goto statements.  Created smaller
    functions to manage the USB debug device class enumeration
    steps to make the code more readable.
    
    Added checks to see if the hardware and/or debug device are
    already initialized and skip the reset/enumeration steps.
    There is no HW reason that requires the EHCI host controller
    to be re-initialized upon entry to ramstage.
    
    This change also updates the EHCI driver to read the MMIO BAR
    from the PCI config register for each USB transaction.  This
    enables the EHCI driver to automatically adjust when the BAR
    is moved without needing to be re-initialized.  By automatically
    adjusting the USB debug should be able to continue seemless
    operation from romstage through ramstage and PCI resource
    allocation.
    
    Left dbg_pipe support in ramstage so the driver can buffer up to
    the max USB packet size for a debug port (8 bytes) before
    transmitting.  In romstage the driver will transmit each byte
    over USB as it is received.  This may be slower, but it will
    still operate correctly regardless of when console_init is called,
    whether cache-as-ram is setup correctly, or whether cbmem is
    initialized.
    
    Removed usbdebug_init() after FSP/MRC initialization because the EHCI
    debug driver has been re-written to remove the global variable that was
    holding the cached EHCI BAR value.  The driver does not need to be
    re-initialized after FSP calls, even if the FSP changes the value in the
    EHCI BAR.  The EHCI debug driver will now automatically pick up the new
    value.
    
    Change-Id: If742c1b393fe4655696d8e201fb0ff87dc8d712f
    Signed-off-by: Kimarie Hoot <kimarie.hoot at se-eng.com>
---
 src/Kconfig                                        |  11 +-
 src/drivers/usb/Kconfig                            |   1 -
 src/drivers/usb/console.c                          | 186 +++-
 src/drivers/usb/ehci.h                             |  10 +-
 src/drivers/usb/ehci_debug.c                       | 938 ++++++++++++---------
 src/drivers/usb/ehci_debug.h                       |  48 +-
 src/drivers/usb/gadget.c                           | 305 ++++---
 src/drivers/usb/pci_ehci.c                         | 166 +++-
 src/include/device/pci_ehci.h                      |  10 +-
 src/mainboard/intel/cougar_canyon2/romstage.c      |   5 -
 src/northbridge/intel/sandybridge/raminit.c        |   5 -
 src/soc/intel/fsp_baytrail/romstage/romstage.c     |   5 -
 src/southbridge/amd/agesa/hudson/enable_usbdebug.c |   7 +-
 src/southbridge/amd/pi/hudson/enable_usbdebug.c    |   7 +-
 src/southbridge/amd/sb600/enable_usbdebug.c        |   7 +-
 src/southbridge/amd/sb700/enable_usbdebug.c        |   7 +-
 src/southbridge/amd/sb800/enable_usbdebug.c        |   7 +-
 src/southbridge/intel/common/usb_debug.c           |   7 +-
 src/southbridge/intel/fsp_rangeley/romstage.c      |   5 -
 src/southbridge/nvidia/ck804/enable_usbdebug.c     |   7 +-
 src/southbridge/nvidia/mcp55/enable_usbdebug.c     |   7 +-
 src/southbridge/sis/sis966/enable_usbdebug.c       |   7 +-
 22 files changed, 1085 insertions(+), 673 deletions(-)

diff --git a/src/Kconfig b/src/Kconfig
index 91a3b29..762b878 100644
--- a/src/Kconfig
+++ b/src/Kconfig
@@ -3,6 +3,7 @@
 ##
 ## Copyright (C) 2012 Alexandru Gagniuc <mr.nuke.me at gmail.com>
 ## Copyright (C) 2009-2010 coresystems GmbH
+## Copyright (C) 2015 Sage Electronic Engineering, LLC.
 ##
 ## This program is free software; you can redistribute it and/or modify
 ## it under the terms of the GNU General Public License as published by
@@ -1174,16 +1175,6 @@ config DEBUG_SPI_FLASH
 	help
 	  This option enables additional SPI flash related debug messages.
 
-config DEBUG_USBDEBUG
-	bool "Output verbose USB 2.0 EHCI debug dongle messages"
-	default n
-	depends on USBDEBUG
-	help
-	  This option enables additional USB 2.0 debug dongle related messages.
-
-	  Select this to debug the connection of usbdebug dongle. Note that
-	  you need some other working console to receive the messages.
-
 if SOUTHBRIDGE_INTEL_BD82X6X && DEFAULT_CONSOLE_LOGLEVEL_8
 # Only visible with the right southbridge and loglevel.
 config DEBUG_INTEL_ME
diff --git a/src/drivers/usb/Kconfig b/src/drivers/usb/Kconfig
index c5c1ec7..c05705e 100644
--- a/src/drivers/usb/Kconfig
+++ b/src/drivers/usb/Kconfig
@@ -35,7 +35,6 @@ if USBDEBUG
 config USBDEBUG_IN_ROMSTAGE
 	bool "Enable early (pre-RAM) usbdebug"
 	default y
-	depends on EARLY_CBMEM_INIT
 	help
 	   Configuring USB controllers in system-agent binary may cause
 	   problems to usbdebug. Disabling this option delays usbdebug to
diff --git a/src/drivers/usb/console.c b/src/drivers/usb/console.c
index 8f07dbc..3ec9f3e 100644
--- a/src/drivers/usb/console.c
+++ b/src/drivers/usb/console.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2006 Eric Biederman (ebiederm at xmission.com)
  * Copyright (C) 2007 AMD
+ * Copyright (C) 2014-2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -18,65 +19,232 @@
  * Foundation, Inc.
  */
 
+#include <string.h>
+#include <console/console.h>
 #include <console/usb.h>
+#include <device/pci.h>
+#include <device/pci_ehci.h>
 #include "ehci_debug.h"
 
-static void usbdebug_tx_byte(struct dbgp_pipe *pipe, unsigned char data)
+#ifndef __PRE_RAM__
+#define DBGP_MAX_ENDPOINTS  (4)
+#define DBGP_SETUP_EP0      (0)
+#define DBGP_CONSOLE_EPOUT  (1)
+#define DBGP_CONSOLE_EPIN   (2)
+
+struct dbgp_info {
+	/*
+	 * Storing the EHCI PCI device instead of the MMIO base address for the
+	 * debug registers allows usbdebug to automatically continue even if the
+	 * BAR changes during the boot sequence.
+	 */
+	pci_devfn_t ehci_dev; /* EHCI controller device */
+
+	struct dbgp_pipe ep_pipe[DBGP_MAX_ENDPOINTS];
+};
+
+static struct dbgp_info glob_dbg_info;
+
+static void dbgp_put(struct dbgp_pipe *pipe);
+static int dbgp_try_get(struct dbgp_pipe *pipe);
+static int dbgp_ep_is_active(struct dbgp_pipe *pipe);
+
+static inline struct dbgp_info *dbgp_get_info(void)
 {
+	return &glob_dbg_info;
+}
+#endif
+
+static pci_devfn_t dbgp_get_dev(void);
+static struct dbgp_pipe *dbgp_console_output(void);
+static struct dbgp_pipe *dbgp_console_input(void);
+
+static void usbdebug_tx_byte(pci_devfn_t dev, void *_pipe, unsigned char data)
+{
+	/*
+	 * _pipe is not used in romstage, in ramstage it is cast to
+	 * (struct dbgp_pipe*) and used to buffer up to the maximum packet size of
+	 * 8 bytes of data that can be transmitted over the EHCI debug interface.
+	 * This will maximize bus utilization when there is a lot more debug output
+	 * expected as there tends to be significantly more debug spew in general
+	 * in ramstage.
+	 */
+#ifndef __PRE_RAM__
+	struct dbgp_pipe *pipe = (struct dbgp_pipe *)_pipe;
 	if (!dbgp_try_get(pipe))
 		return;
 	pipe->buf[pipe->bufidx++] = data;
 	if (pipe->bufidx >= 8) {
-		dbgp_bulk_write_x(pipe, pipe->buf, pipe->bufidx);
+		ehci_debug_bulk_write_x(dev, pipe->buf, pipe->bufidx);
 		pipe->bufidx = 0;
 	}
 	dbgp_put(pipe);
+#else
+	/* in romstage, transmit every byte as it is received, no buffering */
+	ehci_debug_bulk_write_x(dev, (char *)&data, 1);
+#endif
 }
 
-static void usbdebug_tx_flush(struct dbgp_pipe *pipe)
+static void usbdebug_tx_flush(pci_devfn_t dev, void *_pipe)
 {
+#ifndef __PRE_RAM__
+	struct dbgp_pipe *pipe = (struct dbgp_pipe *)_pipe;
 	if (!dbgp_try_get(pipe))
 		return;
 	if (pipe->bufidx > 0) {
-		dbgp_bulk_write_x(pipe, pipe->buf, pipe->bufidx);
+		ehci_debug_bulk_write_x(dev, pipe->buf, pipe->bufidx);
 		pipe->bufidx = 0;
 	}
 	dbgp_put(pipe);
+#else
+	/*
+	 * This is a no-op in romstage because the byte was transmitted as soon
+	 * as we received it.
+	 */
+#endif
 }
 
-static unsigned char usbdebug_rx_byte(struct dbgp_pipe *pipe)
+static unsigned char usbdebug_rx_byte(pci_devfn_t dev, void *_pipe)
 {
+#ifndef __PRE_RAM__
+	struct dbgp_pipe *pipe = (struct dbgp_pipe *)_pipe;
 	unsigned char data = 0xff;
 	if (!dbgp_try_get(pipe))
 		return 0xff;
 	while (pipe->bufidx >= pipe->buflen) {
 		pipe->buflen = 0;
 		pipe->bufidx = 0;
-		int count = dbgp_bulk_read_x(pipe, pipe->buf, 8);
+		int count = ehci_debug_bulk_read_x(dev, pipe->buf, 8);
 		if (count>0)
 			pipe->buflen = count;
 	}
 	data = pipe->buf[pipe->bufidx++];
 	dbgp_put(pipe);
 	return data;
+#else
+	u8  data  = 0;
+	int count = ehci_debug_bulk_read_x(dev, &data, 1);
+	if (count > 0)
+		return data;
+	return 0;
+#endif
 }
 
 void usb_tx_byte(int idx, unsigned char data)
 {
-	usbdebug_tx_byte(dbgp_console_output(), data);
+	pci_devfn_t dev = dbgp_get_dev();
+	usbdebug_tx_byte(dev, dbgp_console_output(), data);
 }
 
 void usb_tx_flush(int idx)
 {
-	usbdebug_tx_flush(dbgp_console_output());
+	pci_devfn_t dev = dbgp_get_dev();
+	usbdebug_tx_flush(dev, dbgp_console_output());
 }
 
 unsigned char usb_rx_byte(int idx)
 {
-	return usbdebug_rx_byte(dbgp_console_input());
+	pci_devfn_t dev = dbgp_get_dev();
+	return usbdebug_rx_byte(dev, dbgp_console_input());
 }
 
 int usb_can_rx_byte(int idx)
 {
+#ifndef __PRE_RAM__
 	return dbgp_ep_is_active(dbgp_console_input());
+#else
+	return 1;
+#endif
+}
+
+#ifndef __PRE_RAM__
+static int dbgp_try_get(struct dbgp_pipe *pipe)
+{
+	struct dbgp_pipe *globals = &dbgp_get_info()->ep_pipe[DBGP_SETUP_EP0];
+	if (!dbgp_ep_is_active(pipe) || (globals->status & DBGP_EP_BUSY))
+		return 0;
+	globals->status |= DBGP_EP_BUSY;
+	pipe->status |= DBGP_EP_BUSY;
+	return 1;
+}
+
+static void dbgp_put(struct dbgp_pipe *pipe)
+{
+	struct dbgp_pipe *globals = &dbgp_get_info()->ep_pipe[DBGP_SETUP_EP0];
+	globals->status &= ~DBGP_EP_BUSY;
+	pipe->status &= ~DBGP_EP_BUSY;
+}
+
+static int dbgp_ep_is_active(struct dbgp_pipe *pipe)
+{
+	return (pipe->status & DBGP_EP_STATMASK) == (DBGP_EP_VALID | DBGP_EP_ENABLED);
+}
+#endif /* __PRE_RAM__ */
+
+static pci_devfn_t dbgp_get_dev(void)
+{
+#ifndef __PRE_RAM__
+	return dbgp_get_info()->ehci_dev;
+#else
+	pci_devfn_t usb_hcdid_devfn_t = chipset_ehci_debug_get_dev(CONFIG_USBDEBUG_HCD_INDEX);
+	const struct device *usb_dev = dev_find_slot(PCI_DEV2SEGBUS(usb_hcdid_devfn_t), PCI_DEV2DEVFN(usb_hcdid_devfn_t));
+	u32 usb_devfn = usb_dev->path.pci.devfn;
+	return PCI_DEV(0, PCI_SLOT(usb_devfn), PCI_FUNC(usb_devfn));
+#endif
+}
+
+static struct dbgp_pipe *dbgp_console_output(void)
+{
+#ifndef __PRE_RAM__
+	return &dbgp_get_info()->ep_pipe[DBGP_CONSOLE_EPOUT];
+#else
+	return NULL;
+#endif
+}
+
+static struct dbgp_pipe *dbgp_console_input(void)
+{
+#ifndef __PRE_RAM__
+	return &dbgp_get_info()->ep_pipe[DBGP_CONSOLE_EPIN];
+#else
+	return NULL;
+#endif
+}
+
+#if !defined(__PRE_RAM__) && !defined(__SMM__)
+void usbdebug_re_enable(unsigned ehci_base)
+{
+	struct dbgp_info *dbg_info = dbgp_get_info();
+	int idx;
+
+	for (idx = 0; idx < DBGP_MAX_ENDPOINTS; idx++)
+		dbg_info->ep_pipe[idx].status |= DBGP_EP_ENABLED;
+}
+
+void usbdebug_disable(void)
+{
+	struct dbgp_info *dbg_info = dbgp_get_info();
+	int i;
+	for (i=0; i<DBGP_MAX_ENDPOINTS; i++)
+		dbg_info->ep_pipe[i].status &= ~DBGP_EP_ENABLED;
+}
+
+#endif
+
+int usbdebug_init(void)
+{
+#ifndef __PRE_RAM__
+	struct dbgp_info *dbg_info = dbgp_get_info();
+	dbg_info->ehci_dev = ehci_debug_init();
+	if (dbg_info->ehci_dev != 0) {
+		memset(&dbg_info->ep_pipe, 0, sizeof (dbg_info->ep_pipe));
+		dbg_info->ep_pipe[DBGP_SETUP_EP0].status |= DBGP_EP_ENABLED | DBGP_EP_VALID;
+		dbg_info->ep_pipe[DBGP_CONSOLE_EPOUT].status |= DBGP_EP_ENABLED | DBGP_EP_VALID;
+		dbg_info->ep_pipe[DBGP_CONSOLE_EPIN].status |= DBGP_EP_ENABLED | DBGP_EP_VALID;
+	}
+#else
+	(void) ehci_debug_init();
+#endif
+
+	return 0;
 }
diff --git a/src/drivers/usb/ehci.h b/src/drivers/usb/ehci.h
index da58813..d555ac5 100644
--- a/src/drivers/usb/ehci.h
+++ b/src/drivers/usb/ehci.h
@@ -4,6 +4,7 @@
  * It was taken from the Linux kernel (include/linux/usb/ehci_def.h).
  *
  * Copyright (C) 2001-2002 David Brownell
+ * Copyright (C) 2014-2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License as published by the
@@ -190,14 +191,13 @@ struct ehci_dbg_port {
 #define DBGP_OUT	(1<<4)
 #define DBGP_LEN(x)	(((x)>>0)&0x0f)
 	u32	pids;
-#define DBGP_PID_GET(x)		(((x)>>16)&0xff)
-#define DBGP_PID_SET(data, tok)	(((data)<<8)|(tok))
+#define DBGP_RCVD_PID_GET(pids)         (((pids) >> 16) & 0xFF)
+#define DBGP_DATA_PID_GET(pids)         (((pids) >> 8) & 0xFF)
+#define DBGP_DATA_PID_UPDATE(pids, tok) (((pids) & ~0xFF00) | ((tok) << 8))
+#define DBGP_SEND_PID_UPDATE(pids, tok) (((pids) & ~0xFF) | (tok))
 	u32	data03;
 	u32	data47;
 	u32	address;
 #define DBGP_EPADDR(dev, ep)	(((dev)<<8)|(ep))
 } __attribute__ ((packed));
-
-#define USB_DEBUG_DEVNUM 127
-
 #endif
diff --git a/src/drivers/usb/ehci_debug.c b/src/drivers/usb/ehci_debug.c
index 5098f97..e459721 100644
--- a/src/drivers/usb/ehci_debug.c
+++ b/src/drivers/usb/ehci_debug.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2006 Eric Biederman (ebiederm at xmission.com)
  * Copyright (C) 2007 AMD
+ * Copyright (C) 2014-2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -22,31 +23,16 @@
 #include <console/console.h>
 #include <console/usb.h>
 #include <arch/io.h>
-#include <arch/early_variables.h>
+#include <arch/byteorder.h>
 #include <string.h>
-#include <cbmem.h>
+#include <device/pci.h>
+#include <device/pci_ehci.h>
+#include <device/device.h>
 
 #include "ehci_debug.h"
 #include "usb_ch9.h"
 #include "ehci.h"
 
-struct ehci_debug_info {
-	void *ehci_base;
-	void *ehci_debug;
-
-	struct dbgp_pipe ep_pipe[DBGP_MAX_ENDPOINTS];
-};
-
-#if CONFIG_DEBUG_USBDEBUG
-static void dbgp_print_data(struct ehci_dbg_port *ehci_debug);
-static int dbgp_enabled(void);
-# define dprintk(LEVEL, args...) \
-	do { if (!dbgp_enabled()) printk(LEVEL, ##args); } while (0)
-#else
-# define dbgp_print_data(x)	do {} while(0)
-# define dprintk(LEVEL, args...)   do {} while(0)
-#endif
-
 #define DBGP_LEN_UPDATE(x, len) (((x) & ~0x0f) | ((len) & 0x0f))
 
 #define DBGP_CLAIM (DBGP_OWNER | DBGP_ENABLED | DBGP_INUSE)
@@ -60,18 +46,21 @@ static int dbgp_enabled(void);
 #define DBGP_MICROFRAME_RETRIES		10
 #define DBGP_MAX_PACKET		8
 
-static struct ehci_debug_info glob_dbg_info CAR_GLOBAL;
-static struct ehci_debug_info * glob_dbg_info_p CAR_GLOBAL;
-
-static inline struct ehci_debug_info *dbgp_ehci_info(void)
+void ehci_mdelay(int ms)
 {
-	if (car_get_var(glob_dbg_info_p) == NULL)
-		car_set_var(glob_dbg_info_p, &glob_dbg_info);
+	int i;
 
-	return car_get_var(glob_dbg_info_p);
+	/* NOTE: using udelay here can cause the system to lock up depending on
+	 *       how the udelay function is implemented, which can differ
+	 *       depending on your build configuration. */
+	while (ms--) {
+		for (i = 0; i < 1000; i++)
+			inb(0xED);
+	}
 }
 
-static int dbgp_wait_until_complete(struct ehci_dbg_port *ehci_debug)
+/* Poll the EHCI debug port control/status register until DONE bit is set */
+static int ehci_debug_wait_until_complete(struct ehci_dbg_port *ehci_debug)
 {
 	u32 ctrl;
 	int loop = 0;
@@ -83,10 +72,8 @@ static int dbgp_wait_until_complete(struct ehci_dbg_port *ehci_debug)
 			break;
 	} while (++loop < DBGP_MICROFRAME_TIMEOUT_LOOPS);
 
-	if (! (ctrl & DBGP_DONE)) {
-		dprintk(BIOS_ERR, "dbgp_wait_until_complete: retry timeout.\n");
+	if (! (ctrl & DBGP_DONE))
 		return -DBGP_ERR_SIGNAL;
-	}
 
 	/* Now that we have observed the completed transaction,
 	 * clear the done bit.
@@ -95,42 +82,38 @@ static int dbgp_wait_until_complete(struct ehci_dbg_port *ehci_debug)
 	return (ctrl & DBGP_ERROR) ? -DBGP_ERRCODE(ctrl) : DBGP_LEN(ctrl);
 }
 
-static void dbgp_breath(void)
-{
-	/* Sleep to give the debug port a chance to breathe */
-}
-
-static int dbgp_wait_until_done(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe,
-	unsigned ctrl, const int timeout)
+/* Monitor USB responses and handle retries until an ACK response is received
+ * or until the expected data (device->host) has been transmitted. */
+static int ehci_debug_wait_until_done(struct ehci_dbg_port *ehci_debug, int endpoint)
 {
 	u32 rd_ctrl, rd_pids;
 	u32 ctrl_prev = 0, pids_prev = 0;
-	u8 lpid;
+	u8 lpid, epid;
 	int ret, host_retries;
 	int loop;
+	u32 ctrl;
 
 	loop = 0;
 device_retry:
 	host_retries = 0;
-	if (loop++ >= timeout)
+	if (loop++ >= DBGP_MICROFRAME_TIMEOUT_LOOPS)
 		return -DBGP_ERR_BAD;
 
 host_retry:
 	if (host_retries++ >= DBGP_MICROFRAME_RETRIES)
 		return -DBGP_ERR_BAD;
-	if (loop == 1 || host_retries > 1)
-		dprintk(BIOS_SPEW, "dbgp:  start (@ %3d,%d) ctrl=%08x\n",
-			loop, host_retries, ctrl | DBGP_GO);
-	write32(&ehci_debug->control, ctrl | DBGP_GO);
-	ret = dbgp_wait_until_complete(ehci_debug);
+
+	ctrl = read32(&ehci_debug->control);
+
+	if (loop > 1 || host_retries > 1)
+		write32(&ehci_debug->control, ctrl | DBGP_GO);
+	ret = ehci_debug_wait_until_complete(ehci_debug);
 	rd_ctrl = read32(&ehci_debug->control);
 	rd_pids = read32(&ehci_debug->pids);
 
 	if (rd_ctrl != ctrl_prev || rd_pids != pids_prev || (ret<0)) {
 		ctrl_prev = rd_ctrl;
 		pids_prev = rd_pids;
-		dprintk(BIOS_SPEW, "dbgp: status (@ %3d,%d) ctrl=%08x pids=%08x ret=%d\n",
-			loop, host_retries, rd_ctrl, rd_pids, ret);
 	}
 
 	/* Controller hardware failure. */
@@ -142,37 +125,44 @@ host_retry:
 		goto host_retry;
 	}
 
-	lpid = DBGP_PID_GET(rd_pids);
+	lpid = DBGP_RCVD_PID_GET(rd_pids);
+	epid = DBGP_DATA_PID_GET(rd_pids);
 
 	/* If I get an ACK or in-sync DATA PID, we are done. */
-	if ((lpid == USB_PID_ACK) || (lpid == pipe->pid)) {
-		pipe->pid ^= USB_PID_DATA_TOGGLE;
+	if ((lpid == USB_PID_ACK) || (lpid == epid)) {
+		/*
+		 * This updates the USB data toggle in the pids register.  The data
+		 * toggle is only updated when packets are received correctly.  If
+		 * the next received packet does not match the data toggle, that
+		 * indicates a transmission error occurred on the bus and the host
+		 * and device are now out of sync.
+		 */
+		rd_pids = DBGP_DATA_PID_UPDATE(rd_pids, epid ^ USB_PID_DATA_TOGGLE);
+		write32(&ehci_debug->pids, rd_pids);
 	}
 
 	/* If the port is getting full or it has dropped data
 	 * start pacing ourselves, not necessary but it's friendly.
 	 */
 	else if (lpid == USB_PID_NYET) {
-		dbgp_breath();
 		goto device_retry;
 	}
 
 	/* If I get a NACK or out-of-sync DATA PID, reissue the transmission. */
-	else if ((lpid == USB_PID_NAK) || (lpid == (pipe->pid ^ USB_PID_DATA_TOGGLE))) {
+	else if ((lpid == USB_PID_NAK) || (lpid == (epid ^ USB_PID_DATA_TOGGLE))) {
 		goto device_retry;
 	}
 
 	/* Abort on STALL handshake for endpoint 0.*/
-	else if ((lpid == USB_PID_STALL) && (pipe->endpoint == 0x0)) {
+	else if ((lpid == USB_PID_STALL) && (endpoint == USB_DEBUG_SETUP_EP0)) {
 		ret = -DBGP_ERR_BAD;
 	}
 
-	dbgp_print_data(ehci_debug);
-
 	return ret;
 }
 
-static void dbgp_set_data(struct ehci_dbg_port *ehci_debug, const void *buf, int size)
+/* Load requested data into the data buffer register */
+static void ehci_debug_set_data(struct ehci_dbg_port *ehci_debug, const void *buf, int size)
 {
 	const unsigned char *bytes = buf;
 	u32 lo, hi;
@@ -187,7 +177,8 @@ static void dbgp_set_data(struct ehci_dbg_port *ehci_debug, const void *buf, int
 	write32(&ehci_debug->data47, hi);
 }
 
-static void dbgp_get_data(struct ehci_dbg_port *ehci_debug, void *buf, int size)
+/* Read received data from the data buffer register */
+static void ehci_debug_get_data(struct ehci_dbg_port *ehci_debug, void *buf, int size)
 {
 	unsigned char *bytes = buf;
 	u32 lo, hi;
@@ -201,57 +192,69 @@ static void dbgp_get_data(struct ehci_dbg_port *ehci_debug, void *buf, int size)
 		bytes[i] = (hi >> (8*(i - 4))) & 0xff;
 }
 
-#if CONFIG_DEBUG_USBDEBUG
-static void dbgp_print_data(struct ehci_dbg_port *ehci_debug)
+/* Returns TRUE if the EHCI controller has already been configured.  Per the
+ * EHCI spec, host software is supposed to set the configured flag (CF) bit as
+ * the last action in the process of configuring the Host Controller.  When
+ * this bit is set, all ports are routed to the EHCI controller by default. */
+static int ehci_is_initialized(pci_devfn_t dev)
 {
-	u32 ctrl = read32(&ehci_debug->control);
-	u32	lo = read32(&ehci_debug->data03);
-	u32	hi = read32(&ehci_debug->data47);
-	int len = DBGP_LEN(ctrl);
-	if (len) {
-		int i;
-		dprintk(BIOS_SPEW, "dbgp:    buf:");
-		for (i = 0; i < 4 && i < len; i++)
-			dprintk(BIOS_SPEW, " %02x", (lo >> (8*i)) & 0xff);
-		for (; i < 8 && i < len; i++)
-			dprintk(BIOS_SPEW, " %02x", (hi >> (8*(i - 4))) & 0xff);
-		dprintk(BIOS_SPEW, "\n");
+	int initialized = 0;
+	struct ehci_regs *ehci_regs = (struct ehci_regs *)pci_ehci_base_regs(dev);
+
+	if (ehci_regs != NULL) {
+		u32 cfg_flag = read32(&ehci_regs->configured_flag);
+
+		/* Determine if the EHCI controller is already initialized */
+		if (cfg_flag == FLAG_CF)
+			initialized = 1;
 	}
+
+	return initialized;
 }
-#endif
 
-static int dbgp_bulk_write(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe,
+/* Perform a USB bulk OUT transaction (host->device) */
+int ehci_debug_bulk_write(void *_ehci_debug, int devnum, int endpoint,
 	const char *bytes, int size)
 {
+	struct ehci_dbg_port *ehci_debug = (struct ehci_dbg_port *)_ehci_debug;
 	u32 pids, addr, ctrl;
-	int ret;
 
 	if (size > DBGP_MAX_PACKET)
 		return -1;
 
-	addr = DBGP_EPADDR(pipe->devnum, pipe->endpoint);
-	pids = DBGP_PID_SET(pipe->pid, USB_PID_OUT);
-
-	ctrl = read32(&ehci_debug->control);
-	ctrl = DBGP_LEN_UPDATE(ctrl, size);
-	ctrl |= DBGP_OUT;
-
-	dbgp_set_data(ehci_debug, bytes, size);
+	addr = DBGP_EPADDR(devnum, endpoint);
 	write32(&ehci_debug->address, addr);
+
+	/* Read-modify-write pids reg */
+	pids = read32(&ehci_debug->pids);
+	pids = DBGP_SEND_PID_UPDATE(pids, USB_PID_OUT);
 	write32(&ehci_debug->pids, pids);
 
-	ret = dbgp_wait_until_done(ehci_debug, pipe, ctrl, pipe->timeout);
+	ehci_debug_set_data(ehci_debug, bytes, size);
 
-	return ret;
+	ctrl = read32(&ehci_debug->control);
+	ctrl = DBGP_LEN_UPDATE(ctrl, size);
+	ctrl |= DBGP_OUT | DBGP_GO;
+	write32(&ehci_debug->control, ctrl);
+
+	return ehci_debug_wait_until_done(ehci_debug, endpoint);
 }
 
-int dbgp_bulk_write_x(struct dbgp_pipe *pipe, const char *bytes, int size)
+/* Public API for USB bulk OUT transaction - called from usb/console.c */
+int ehci_debug_bulk_write_x(pci_devfn_t dev, const char *bytes, int size)
 {
-	struct ehci_debug_info *dbg_info = dbgp_ehci_info();
-	return dbgp_bulk_write(dbg_info->ehci_debug, pipe, bytes, size);
+	/* If the EHCI controller is not initialized just drop this write request
+	 * into the bit bucket. */
+	if (ehci_is_initialized(dev)) {
+		struct ehci_dbg_port *ehci_debug = (struct ehci_dbg_port *)pci_ehci_get_debug_regs(dev);
+		(void) ehci_debug_bulk_write(ehci_debug, USB_DEBUG_DEVNUM, USB_DEBUG_CONSOLE_EPOUT, bytes, size);
+	}
+
+	return size;
 }
 
-static int dbgp_bulk_read(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe,
+/* Perform a USB bulk IN transaction (device->host) */
+static int ehci_debug_bulk_read(struct ehci_dbg_port *ehci_debug, int devnum, int endpoint,
 	void *data, int size)
 {
 	u32 pids, addr, ctrl;
@@ -260,46 +263,51 @@ static int dbgp_bulk_read(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pi
 	if (size > DBGP_MAX_PACKET)
 		return -1;
 
-	addr = DBGP_EPADDR(pipe->devnum, pipe->endpoint);
-	pids = DBGP_PID_SET(pipe->pid, USB_PID_IN);
+	addr = DBGP_EPADDR(devnum, endpoint);
+	write32(&ehci_debug->address, addr);
+
+	/* Read-modify-write pids register */
+	pids = read32(&ehci_debug->pids);
+	pids = DBGP_SEND_PID_UPDATE(pids, USB_PID_IN);
+	write32(&ehci_debug->pids, pids);
 
 	ctrl = read32(&ehci_debug->control);
 	ctrl = DBGP_LEN_UPDATE(ctrl, size);
 	ctrl &= ~DBGP_OUT;
+	write32(&ehci_debug->control, ctrl | DBGP_GO);
 
-	write32(&ehci_debug->address, addr);
-	write32(&ehci_debug->pids, pids);
-	ret = dbgp_wait_until_done(ehci_debug, pipe, ctrl, pipe->timeout);
+	ret = ehci_debug_wait_until_done(ehci_debug, endpoint);
 	if (ret < 0)
 		return ret;
 
 	if (size > ret)
 		size = ret;
-	dbgp_get_data(ehci_debug, data, size);
+	ehci_debug_get_data(ehci_debug, data, size);
 	return ret;
 }
 
-int dbgp_bulk_read_x(struct dbgp_pipe *pipe, void *data, int size)
-{
-	struct ehci_debug_info *dbg_info = dbgp_ehci_info();
-	return dbgp_bulk_read(dbg_info->ehci_debug, pipe, data, size);
-}
-
-void dbgp_mdelay(int ms)
+/* Public API for USB bulk IN transaction - called from usb/console.c */
+int ehci_debug_bulk_read_x(pci_devfn_t dev, void *data, int size)
 {
-	int i;
+	/* If the EHCI controller is not initialized just drop this read request
+	 * into the bit bucket. */
+	int count = 0;
 
-	while (ms--) {
-		for (i = 0; i < 1000; i++)
-			inb(0x80);
+	if (ehci_is_initialized(dev)) {
+		struct ehci_dbg_port *ehci_debug = (struct ehci_dbg_port *)pci_ehci_get_debug_regs(dev);
+		count = ehci_debug_bulk_read(ehci_debug, USB_DEBUG_DEVNUM, USB_DEBUG_CONSOLE_EPIN, data, size);
 	}
+
+	return count;
 }
 
-int dbgp_control_msg(struct ehci_dbg_port *ehci_debug, unsigned devnum, int requesttype,
+/* Send a USB Control Transfer - the control endpoint is mainly used for
+ * enumerating the device.  Each control transfer has two or three stages:
+ * 1) Setup stage, 2) Optional data stage, 3) Status stage. */
+int ehci_debug_control_msg(void *_ehci_debug, int devnum, int requesttype,
 		int request, int value, int index, void *data, int size)
 {
-	struct ehci_debug_info *info = dbgp_ehci_info();
-	struct dbgp_pipe *pipe = &info->ep_pipe[DBGP_SETUP_EP0];
+	struct ehci_dbg_port *ehci_debug = (struct ehci_dbg_port *)_ehci_debug;
 	u32 pids, addr, ctrl;
 	struct usb_ctrlrequest req;
 	int read;
@@ -316,400 +324,522 @@ int dbgp_control_msg(struct ehci_dbg_port *ehci_debug, unsigned devnum, int requ
 	req.wIndex = cpu_to_le16(index);
 	req.wLength = cpu_to_le16(size);
 
-	pipe->devnum = devnum;
-	pipe->endpoint = 0;
-	pipe->pid = USB_PID_DATA0;
-	pipe->timeout = 1000;
-	addr = DBGP_EPADDR(pipe->devnum, pipe->endpoint);
-	pids = DBGP_PID_SET(pipe->pid, USB_PID_SETUP);
-
-	ctrl = read32(&ehci_debug->control);
-	ctrl = DBGP_LEN_UPDATE(ctrl, sizeof(req));
-	ctrl |= DBGP_OUT;
-
 	/* Setup stage */
-	dbgp_set_data(ehci_debug, &req, sizeof(req));
+	addr = DBGP_EPADDR(devnum, USB_DEBUG_SETUP_EP0);
 	write32(&ehci_debug->address, addr);
+
+	/*
+	 * NOTE: Per USB spec, a control transfer always starts with DATA0 for
+	 *       the data toggle.
+	 */
+	pids = read32(&ehci_debug->pids);
+	pids = DBGP_DATA_PID_UPDATE(pids, USB_PID_DATA0);
+	pids = DBGP_SEND_PID_UPDATE(pids, USB_PID_SETUP);
 	write32(&ehci_debug->pids, pids);
-	ret = dbgp_wait_until_done(ehci_debug, pipe, ctrl, 1);
+
+	ehci_debug_set_data(ehci_debug, &req, sizeof(req));
+
+	ctrl = read32(&ehci_debug->control);
+	ctrl = DBGP_LEN_UPDATE(ctrl, sizeof(req));
+	ctrl |= DBGP_OUT | DBGP_GO;
+	write32(&ehci_debug->control, ctrl);
+	ret = ehci_debug_wait_until_done(ehci_debug, USB_DEBUG_SETUP_EP0);
 	if (ret < 0)
 		return ret;
 
 	/* Data stage (optional) */
 	if (read && size)
-		ret = dbgp_bulk_read(ehci_debug, pipe, data, size);
+		ret = ehci_debug_bulk_read(ehci_debug, devnum, USB_DEBUG_SETUP_EP0, data, size);
 	else if (!read && size)
-		ret = dbgp_bulk_write(ehci_debug, pipe, data, size);
+		ret = ehci_debug_bulk_write(ehci_debug, devnum, USB_DEBUG_SETUP_EP0, data, size);
 
 	/* Status stage in opposite direction */
-	pipe->pid = USB_PID_DATA1;
 	ctrl = read32(&ehci_debug->control);
 	ctrl = DBGP_LEN_UPDATE(ctrl, 0);
+
+	/*
+	 * NOTE: Per USB spec, DATA1 is always used during the status stage of a
+	 *       control transfer.
+	 */
+	/* Read-modify-write pids register */
+	pids = read32(&ehci_debug->pids);
+	pids = DBGP_DATA_PID_UPDATE(pids, USB_PID_DATA1);
 	if (read) {
-		pids = DBGP_PID_SET(pipe->pid, USB_PID_OUT);
+		pids = DBGP_SEND_PID_UPDATE(pids, USB_PID_OUT);
 		ctrl |= DBGP_OUT;
 	} else {
-		pids = DBGP_PID_SET(pipe->pid, USB_PID_IN);
+		pids = DBGP_SEND_PID_UPDATE(pids, USB_PID_IN);
 		ctrl &= ~DBGP_OUT;
 	}
 
 	write32(&ehci_debug->pids, pids);
-	ret2 = dbgp_wait_until_done(ehci_debug, pipe, ctrl, pipe->timeout);
+	write32(&ehci_debug->control, ctrl | DBGP_GO);
+
+	ret2 = ehci_debug_wait_until_done(ehci_debug, USB_DEBUG_SETUP_EP0);
 	if (ret2 < 0)
 		return ret2;
 
 	return ret;
 }
 
-static int ehci_reset_port(struct ehci_regs *ehci_regs, int port)
+/* Issue a USB reset event - this causes the attached USB device to reset
+ * its state and start using address 0 so the host can communicate with it
+ * during the enumeration sequence. */
+static int ehci_port_reset(struct ehci_regs *ehci_regs, int port)
 {
-	u32 portsc;
-	int loop;
-
-	/* Reset the usb debug port */
-	portsc = read32(&ehci_regs->port_status[port - 1]);
-	portsc &= ~PORT_PE;
-	portsc |= PORT_RESET;
-	write32(&ehci_regs->port_status[port - 1], portsc);
-
-	dbgp_mdelay(HUB_ROOT_RESET_TIME);
-
-	portsc = read32(&ehci_regs->port_status[port - 1]);
-	write32(&ehci_regs->port_status[port - 1],
-			portsc & ~(PORT_RWC_BITS | PORT_RESET));
-
-	loop = 100;
-	do {
-		dbgp_mdelay(1);
-		portsc = read32(&ehci_regs->port_status[port - 1]);
-	} while ((portsc & PORT_RESET) && (--loop > 0));
-
-	/* Device went away? */
-	if (!(portsc & PORT_CONNECT))
-		return -1; //-ENOTCONN;
-
-	/* bomb out completely if something weird happened */
-	if ((portsc & PORT_CSC))
-		return -2; //-EINVAL;
+	/* Reset the usb debug port if a connection status change is detected
+	 * NOTE: This CSC bit is always set the first time so the host controller
+	 *       can use this bit to determine which port to start enumeration
+	 *       on. */
+	u32 portsc = read32(&ehci_regs->port_status[port]);
+	if ((portsc & PORT_CSC) != 0) {
+		u32 delay_time, delay_ms;
+		int loop;
+
+		portsc &= ~PORT_PE;
+		portsc |= PORT_RESET;
+		write32(&ehci_regs->port_status[port], portsc);
+
+		delay_ms = HUB_ROOT_RESET_TIME;
+		for (delay_time = 0; delay_time < HUB_RESET_TIMEOUT;
+				delay_time += delay_ms) {
+			ehci_mdelay(delay_ms);
+
+			portsc = read32(&ehci_regs->port_status[port]);
+			if (portsc & PORT_RESET) {
+				/* force reset to complete */
+				loop = 2;
+				write32(&ehci_regs->port_status[port],
+						portsc & ~(PORT_RWC_BITS | PORT_RESET));
+				do {
+					ehci_mdelay(delay_ms);
+					portsc = read32(&ehci_regs->port_status[port]);
+					delay_time += delay_ms;
+				} while ((portsc & PORT_RESET) && (--loop > 0));
+			}
+
+			/* Device went away? */
+			if (!(portsc & PORT_CONNECT))
+				return -1; /* -ENOTCONN; */
+
+			/* bomb out completely if something weird happened */
+			if ((portsc & PORT_CSC))
+				return -2; /* -EINVAL; */
+
+			/* If we've finished resetting, then break out of the loop */
+			if (!(portsc & PORT_RESET) && (portsc & PORT_PE))
+				return 0;
+		}
 
-	/* If we've finished resetting, then break out of the loop */
-	if (!(portsc & PORT_RESET) && (portsc & PORT_PE))
-		return 0;
+		return -3; /* -EBUSY; */
+	}
 
-	return -3; //-EBUSY;
+	return 0;
 }
 
-static int ehci_wait_for_port(struct ehci_regs *ehci_regs, int port)
+/* Wait for a port status change event following a host controller reset.  This
+ * is the definitive signal that the host controller has checked all ports and
+ * updated their connection status. */
+static int ehci_wait_for_port(pci_devfn_t dev)
 {
+	struct ehci_regs *ehci_regs = (struct ehci_regs *)pci_ehci_base_regs(dev);
 	u32 status;
-	int ret, reps;
+	int reps;
 
 	for (reps = 0; reps < 3; reps++) {
-		dbgp_mdelay(100);
 		status = read32(&ehci_regs->status);
-		if (status & STS_PCD) {
-			ret = ehci_reset_port(ehci_regs, port);
-			if (ret == 0)
-				return 0;
-		}
+		if ((status & STS_PCD) != 0)
+			return 0;
+		ehci_mdelay(100);
 	}
-	return -1; //-ENOTCONN;
+	return -1;
 }
 
-
-
-static int usbdebug_init_(unsigned ehci_bar, unsigned offset, struct ehci_debug_info *info)
+/* Halt the EHCI controller - this is required before resetting the EHCI
+ * controller.
+ *
+ * When software resets the controller, it resets the internal pipelines,
+ * timers, counters, state machines, and so on to their initial value. Any
+ * transaction currently in progress on USB is immediately terminated.
+ */
+static int ehci_controller_halt(struct ehci_regs *ehci_regs)
 {
-	struct ehci_caps *ehci_caps;
-	struct ehci_regs *ehci_regs;
-
-	u32 cmd, ctrl, status, portsc, hcs_params;
-	u32 debug_port, new_debug_port = 0, n_ports;
-	int ret, i;
-	int loop;
-	int port_map_tried;
-	int playtimes = 3;
-
-	/* Keep all endpoints disabled before any printk() call. */
-	memset(info, 0, sizeof (*info));
-	info->ehci_base = (void *)ehci_bar;
-	info->ehci_debug = (void *)(ehci_bar + offset);
+	int ret = 0;
+	u32 status = read32(&ehci_regs->status);
 
-	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(&ehci_caps->hc_capbase)));
-
-	struct ehci_dbg_port *ehci_debug = info->ehci_debug;
-
-	if (CONFIG_USBDEBUG_DEFAULT_PORT > 0)
-		ehci_debug_select_port(CONFIG_USBDEBUG_DEFAULT_PORT);
-	else
-		ehci_debug_select_port(1);
-
-try_next_time:
-	port_map_tried = 0;
-
-try_next_port:
-	hcs_params = read32(&ehci_caps->hcs_params);
-	debug_port = HCS_DEBUG_PORT(hcs_params);
-	n_ports    = HCS_N_PORTS(hcs_params);
-
-	dprintk(BIOS_INFO, "debug_port: %d\n", debug_port);
-	dprintk(BIOS_INFO, "n_ports:    %d\n", n_ports);
-
-        for (i = 1; i <= n_ports; i++) {
-                portsc = read32(&ehci_regs->port_status[i-1]);
-                dprintk(BIOS_INFO, "PORTSC #%d: %08x\n", i, portsc);
-        }
-
-	if(port_map_tried && (new_debug_port != debug_port)) {
-		if(--playtimes) {
-			ehci_debug_select_port(debug_port);
-			goto try_next_time;
-		}
-		return -1;
-	}
+	if ((status & STS_HALT) == 0) {
+		u32 loop = 100;
 
-	/* Wait until the controller is halted */
-	status = read32(&ehci_regs->status);
-	if (!(status & STS_HALT)) {
-		cmd = read32(&ehci_regs->command);
+		/* Clear Run/Stop bit in the EHCI command register */
+		u32 cmd = read32(&ehci_regs->command);
 		cmd &= ~CMD_RUN;
 		write32(&ehci_regs->command, cmd);
-		loop = 100;
-		do {
-			dbgp_mdelay(10);
-			status = read32(&ehci_regs->status);
-		} while (!(status & STS_HALT) && (--loop > 0));
-		if (status & STS_HALT)
-			dprintk(BIOS_INFO, "EHCI controller halted successfully.\n");
-		else
-			dprintk(BIOS_INFO, "EHCI controller is not halted. Reset may fail.\n");
-	}
 
-	loop = 100;
-	/* Reset the EHCI controller */
-	cmd = read32(&ehci_regs->command);
-	cmd |= CMD_RESET;
-	write32(&ehci_regs->command, cmd);
-	do {
-		dbgp_mdelay(10);
-		cmd = read32(&ehci_regs->command);
-	} while ((cmd & CMD_RESET) && (--loop > 0));
+		/* Wait until the Halted bit is set in the EHCI status register -
+		 * indicating that controller has responded to the change in Run/Stop
+		 * bit in the EHCI command register. */
+		while (loop-- > 0) {
+			ehci_mdelay(10);
+			status = read32(&ehci_regs->status);
+			if ((status & STS_HALT) != 0)
+				break;
+		}
 
-	if(!loop) {
-		dprintk(BIOS_INFO, "Could not reset EHCI controller.\n");
-		// on some systems it works without succeeding here.
-		// return -2;
-	} else {
-		dprintk(BIOS_INFO, "EHCI controller reset successfully.\n");
+		if (loop == 0)
+			ret = -1;
 	}
 
-	/* Claim ownership, but do not enable yet */
-	ctrl = read32(&ehci_debug->control);
-	ctrl |= DBGP_OWNER;
-	ctrl &= ~(DBGP_ENABLED | DBGP_INUSE);
-	write32(&ehci_debug->control, ctrl);
+	return ret;
+}
 
-	/* Start EHCI controller */
-	cmd = read32(&ehci_regs->command);
+/* Resume the host controller operation - this ensures that the hardware will
+ * automatically issue start-of-frame (SOF) packets which are used by the
+ * USB device to ensure that the bus is still active. */
+static int ehci_controller_resume(struct ehci_regs *ehci_regs)
+{
+	int ret = 0;
+	int loop = 10;
+	u32 status = 0;
+
+	u32 cmd = read32(&ehci_regs->command);
 	cmd &= ~(CMD_LRESET | CMD_IAAD | CMD_PSE | CMD_ASE | CMD_RESET);
 	cmd |= CMD_RUN;
 	write32(&ehci_regs->command, cmd);
 
-	/* Ensure everything is routed to the EHCI */
-	write32(&ehci_regs->configured_flag, FLAG_CF);
-
-	/* Wait until the controller is no longer halted */
-	loop = 10;
-	do {
-		dbgp_mdelay(10);
+	/* Wait until the controller is running again */
+	while (loop-- > 0) {
+		ehci_mdelay(10);
 		status = read32(&ehci_regs->status);
-	} while ((status & STS_HALT) && (--loop > 0));
-
-	if(!loop) {
-		dprintk(BIOS_INFO, "EHCI could not be started.\n");
-		return -3;
-	}
-	dprintk(BIOS_INFO, "EHCI started.\n");
-
-	/* Wait for a device to show up in the debug port */
-	ret = ehci_wait_for_port(ehci_regs, debug_port);
-	if (ret < 0) {
-		dprintk(BIOS_INFO, "No device found in debug port %d\n", debug_port);
-		goto next_debug_port;
+		if ((status & STS_HALT) == 0)
+			break;
 	}
-	dprintk(BIOS_INFO, "EHCI done waiting for port.\n");
 
+	if (loop == 0)
+		ret = -1;
 
-	/* Enable the debug port */
-	ctrl = read32(&ehci_debug->control);
-	ctrl |= DBGP_CLAIM;
-	write32(&ehci_debug->control, ctrl);
-	ctrl = read32(&ehci_debug->control);
-	if ((ctrl & DBGP_CLAIM) != DBGP_CLAIM) {
-		dprintk(BIOS_INFO, "No device in EHCI debug port.\n");
-		write32(&ehci_debug->control, ctrl & ~DBGP_CLAIM);
-		ret = -4;
-		goto err;
-	}
-	dprintk(BIOS_INFO, "EHCI debug port enabled.\n");
+	return ret;
+}
 
-#if 0
-	/* Completely transfer the debug device to the debug controller */
-	portsc = read32(&ehci_regs->port_status[debug_port - 1]);
-	portsc &= ~PORT_PE;
-	write32(&ehci_regs->port_status[debug_port - 1], portsc);
-#endif
+/* Reset the EHCI controller - this insures that the state of the EHCI
+ * controller is sane before the EHCI debug port is activated. */
+static int ehci_controller_reset(struct ehci_regs *ehci_regs)
+{
+	int ret = 0;
+	int loop = 100;
 
-	dbgp_mdelay(100);
+	u32 cmd = read32(&ehci_regs->command);
+	cmd |= CMD_RESET;
+	write32(&ehci_regs->command, cmd);
 
-	ret = dbgp_probe_gadget(info->ehci_debug, &info->ep_pipe[0]);
-	if (ret < 0) {
-		dprintk(BIOS_INFO, "Could not probe gadget on debug port.\n");
-		ret = -6;
-		goto err;
+	/* Wait for the reset to complete */
+	while (loop-- > 0) {
+		ehci_mdelay(10);
+		cmd = read32(&ehci_regs->command);
+		if ((cmd & CMD_RESET) == 0)
+			break;
 	}
 
-	return 0;
-err:
-	/* Things didn't work so remove my claim */
-	ctrl = read32(&ehci_debug->control);
-	ctrl &= ~(DBGP_CLAIM | DBGP_OUT);
-	write32(&ehci_debug->control, ctrl);
-	//return ret;
-
-next_debug_port:
-#if CONFIG_USBDEBUG_DEFAULT_PORT==0
-	port_map_tried |= (1 << (debug_port - 1));
-	new_debug_port = ((debug_port-1 + 1) % n_ports) + 1;
-	if (port_map_tried != ((1 << n_ports) - 1)) {
-		ehci_debug_select_port(new_debug_port);
-		goto try_next_port;
-	}
-	if (--playtimes) {
-		ehci_debug_select_port(new_debug_port);
-		goto try_next_time;
-	}
-#else
-	if (0)
-		goto try_next_port;
-	if (--playtimes)
-		goto try_next_time;
-#endif
+	if (loop == 0)
+		ret = -1;
 
-	return -10;
+	return ret;
 }
 
-#if CONFIG_DEBUG_USBDEBUG
-static int dbgp_enabled(void)
+/* Initialize the EHCI controller, this includes halting, resetting, and
+ * restarting the EHCI controller itself to insure the controller is in a sane
+ * state. */
+static int ehci_controller_init(pci_devfn_t dev)
 {
-	struct dbgp_pipe *globals = &dbgp_ehci_info()->ep_pipe[DBGP_SETUP_EP0];
-	return (globals->status & DBGP_EP_ENABLED);
-}
-#endif
+	int ret = 0;
+
+	/* Determine if the EHCI controller is already initialized */
+	if (!ehci_is_initialized(dev)) {
+		struct ehci_regs *ehci_regs = (struct ehci_regs *)pci_ehci_base_regs(dev);
+
+		/* Must be halted before resetting the EHCI controller */
+		ret = ehci_controller_halt(ehci_regs);
+
+		/* Reset the EHCI controller */
+		if (ret == 0)
+			ret = ehci_controller_reset(ehci_regs);
+
+		/* (Re-)start the EHCI controller
+		 *
+		 * Per the EHCI controller specification, we are using EHCI debug
+		 * operational model #2: the host is running (i.e. Run/Stop bit is 1),
+		 * in mode 2 the normal transmission of SOF packets will keep debug
+		 * devices from suspending. */
+		if (ret == 0)
+			ret = ehci_controller_resume(ehci_regs);
+
+		if (ret == 0) {
+			/* Ensure everything is routed to the EHCI */
+			write32(&ehci_regs->configured_flag, FLAG_CF);
+			printk(BIOS_DEBUG, "EHCI started.\n");
+		} else {
+			printk(BIOS_WARNING, "EHCI could not be started.\n");
+		}
+	}
 
-int dbgp_try_get(struct dbgp_pipe *pipe)
-{
-	struct dbgp_pipe *globals = &dbgp_ehci_info()->ep_pipe[DBGP_SETUP_EP0];
-	if (!dbgp_ep_is_active(pipe) || (globals->status & DBGP_EP_BUSY))
-		return 0;
-	globals->status |= DBGP_EP_BUSY;
-	pipe->status |= DBGP_EP_BUSY;
-	return 1;
+	return ret;
 }
 
-void dbgp_put(struct dbgp_pipe *pipe)
+/* Claim the EHCI debug port - this lets an EHCI host controller driver know
+ * that the debug port is activated and should not be managed as a standard
+ * EHCI port. */
+static int ehci_debug_claim_port(pci_devfn_t dev)
 {
-	struct dbgp_pipe *globals = &dbgp_ehci_info()->ep_pipe[DBGP_SETUP_EP0];
-	globals->status &= ~DBGP_EP_BUSY;
-	pipe->status &= ~DBGP_EP_BUSY;
+	struct ehci_dbg_port *ehci_debug = (struct ehci_dbg_port *)pci_ehci_get_debug_regs(dev);
+
+	u32 ctrl = read32(&ehci_debug->control);
+	ctrl |= DBGP_OWNER;
+	write32(&ehci_debug->control, ctrl);
+
+	return 0;
 }
 
-#if !defined(__PRE_RAM__) && !defined(__SMM__)
-void usbdebug_re_enable(unsigned ehci_base)
+/* Returns TRUE if the EHCI port is enabled for debug port operation */
+static int ehci_debug_is_claimed(pci_devfn_t dev)
 {
-	struct ehci_debug_info *dbg_info = dbgp_ehci_info();
-	unsigned diff;
-	int i;
+	int claimed = 0;
+	struct ehci_dbg_port *ehci_debug = (struct ehci_dbg_port *)pci_ehci_get_debug_regs(dev);
+	u32 ctrl = read32(&ehci_debug->control);
 
-	diff = (unsigned)dbg_info->ehci_base - ehci_base;
-	dbg_info->ehci_debug -= diff;
-	dbg_info->ehci_base = (void*)ehci_base;
+	if ((ctrl & DBGP_CLAIM) == DBGP_CLAIM)
+		claimed = 1;
 
-	for (i=0; i<DBGP_MAX_ENDPOINTS; i++)
-		if (dbg_info->ep_pipe[i].status & DBGP_EP_VALID)
-			dbg_info->ep_pipe[i].status |= DBGP_EP_ENABLED;
+	return claimed;
 }
 
-void usbdebug_disable(void)
+/* Returns TRUE if a USB device is connected to the selected port */
+static int ehci_device_connected(struct ehci_regs *ehci_regs, int port)
 {
-	struct ehci_debug_info *dbg_info = dbgp_ehci_info();
-	int i;
-	for (i=0; i<DBGP_MAX_ENDPOINTS; i++)
-		dbg_info->ep_pipe[i].status &= ~DBGP_EP_ENABLED;
+	u32 status = read32(&ehci_regs->port_status[port]);
+	return ((status & PORT_CONNECT) != 0);
 }
 
-#endif
-
-#if !defined(__PRE_RAM__) && !defined(__SMM__)
-static int get_usbdebug_from_cbmem(struct ehci_debug_info *info)
+/* Enable the EHCI debug port operation */
+static int ehci_debug_enable_port(pci_devfn_t dev, int port)
 {
-	struct ehci_debug_info *dbg_info_cbmem;
+	struct ehci_regs *ehci_regs = (struct ehci_regs *)pci_ehci_base_regs(dev);
+	struct ehci_dbg_port *ehci_debug = (struct ehci_dbg_port *)pci_ehci_get_debug_regs(dev);
 
-	dbg_info_cbmem = cbmem_find(CBMEM_ID_EHCI_DEBUG);
-	if (dbg_info_cbmem == NULL)
-		return -1;
+	u32 ctrl = read32(&ehci_debug->control);
+	ctrl |= DBGP_CLAIM;
+	write32(&ehci_debug->control, ctrl);
 
-	memcpy(info, dbg_info_cbmem, sizeof (*info));
-	printk(BIOS_DEBUG, "EHCI debug port found in CBMEM.\n");
+	/* Completely transfer the debug device to the debug controller */
+	u32 status = read32(&ehci_regs->port_status[port]);
+	status &= ~PORT_PE;
+	write32(&ehci_regs->port_status[port], status);
 
 	return 0;
 }
 
-#elif defined(__PRE_RAM__)
-static void migrate_ehci_debug(int is_recovery)
+/* Release the debug port - this allows an EHCI host controller driver know
+ * that the USB port is not active and the port can be managed as a standard
+ * port instead of as a special debug port. */
+static int ehci_debug_release_port(pci_devfn_t dev)
 {
-	struct ehci_debug_info *dbg_info = dbgp_ehci_info();
-	struct ehci_debug_info *dbg_info_cbmem;
+	struct ehci_dbg_port *ehci_debug = (struct ehci_dbg_port *)pci_ehci_get_debug_regs(dev);
 
-	dbg_info_cbmem = cbmem_add(CBMEM_ID_EHCI_DEBUG, sizeof(*dbg_info));
-	if (dbg_info_cbmem == NULL)
-		return;
+	u32 ctrl = read32(&ehci_debug->control);
+	ctrl &= ~(DBGP_CLAIM | DBGP_OUT);
+	write32(&ehci_debug->control, ctrl);
 
-	memcpy(dbg_info_cbmem, dbg_info, sizeof(*dbg_info));
-	car_set_var(glob_dbg_info_p, dbg_info_cbmem);
+	return 0;
 }
-ROMSTAGE_CBMEM_INIT_HOOK(migrate_ehci_debug);
-#endif
 
-int dbgp_ep_is_active(struct dbgp_pipe *pipe)
+/* Locates a debug port, this involves scanning one or more ports on the EHCI
+ * controller to determine if it can be used a debug port and/or if there is
+ * a USB debug class device attached.  When this function returns, a device,
+ * if found, has been enumerated and is ready for operation. */
+static int ehci_locate_debug_port(pci_devfn_t dev)
 {
-	return (pipe->status & DBGP_EP_STATMASK) == (DBGP_EP_VALID | DBGP_EP_ENABLED);
-}
+	int ret = 0;
+
+	/* If the debug port has not yet been claimed...
+	 * Scan ports to locate the debug port and enumerate an attached USB debug
+	 * device, if possible. */
+	if (!ehci_debug_is_claimed(dev)) {
+		struct ehci_regs *ehci_regs = (struct ehci_regs *)pci_ehci_base_regs(dev);
+		struct ehci_caps *ehci_caps = (struct ehci_caps *)pci_ehci_get_bar(dev);
+		u32    hcs_params = read32(&ehci_caps->hcs_params);
+		int    n_ports    = HCS_N_PORTS(hcs_params); /* Total # of ports on this EHCI controller */
+		int    port_idx   = 0;
+
+		printk(BIOS_DEBUG, "%s: debug_port = %d; n_ports = %d\n",
+				__FUNCTION__, HCS_DEBUG_PORT(hcs_params), n_ports);
+
+		/* Claim ownership of the debug port, but do not enable yet */
+		ret = ehci_debug_claim_port(dev);
+		if (ret < 0)
+			return -1;
+
+		/*
+		 * Per the EHCI spec...
+		 * If the controller has already been configured...
+		 * 1. Check Current Connect Status bit in PORTSC register
+		 * 2. Set Owner bit in Debug Port Control register
+		 * 3. Set Enabled bit in Debug Port Control register
+		 * ELSE
+		 * 1. Ensure the host controller is running (HCHalted bit in USBSTS register
+		 *    is a zero
+		 * 2. Set and clear Port Reset bit in PORTSC register
+		 * 3. Check for Port Enabled/Disabled bit in the PORTSC register
+		 * 4. Set Enabled bit in Debug Port Control register
+		 * 5. Reset Port Enabled/Disabled bit in the PORTSC register
+		 * 6. Turn off the host controller by setting Run/Stop bit to 0
+		 */
+
+		if (CONFIG_USBDEBUG_DEFAULT_PORT > 0)
+			port_idx = CONFIG_USBDEBUG_DEFAULT_PORT - 1;
+
+		for ( ; port_idx < n_ports; port_idx++) {
+
+			/* If there is a device connected to the current port, try to enumerate
+			 * it as a USB debug device */
+			if (ehci_device_connected(ehci_regs, port_idx)) {
+
+				/* For some chipset, the debug device can be routed to any physical
+				 * port, in those cases an extra register write is necessary to
+				 * connect the physical port to the EHCI debug port register
+				 * interface. */
+				pci_ehci_debug_select_port(dev, port_idx);
+
+				/* Reset debug port - this officially starts enumeration of an
+				 * attached USB device. */
+				ret = ehci_port_reset(ehci_regs, port_idx);
+
+				/* Enable the debug port */
+				if (ret == 0)
+					ret = ehci_debug_enable_port(dev, port_idx);
+
+				/* Enumerate the attached USB debug device */
+				if (ret == 0)
+					ret = gadget_probe_dev(dev);
+
+				if (ret >= 0)
+					return 0;
+			}
+
+			/* If we tried the specified port and failed, exit, otherwise continue
+			 * searching on the next port. */
+			if (CONFIG_USBDEBUG_DEFAULT_PORT != 0)
+				break;
+		}
 
-struct dbgp_pipe *dbgp_console_output(void)
-{
-	return &dbgp_ehci_info()->ep_pipe[DBGP_CONSOLE_EPOUT];
-}
+		/* If things didn't work, remove my ownership claim on the debug port */
+		(void) ehci_debug_release_port(dev);
+		ret = -1;
+	}
 
-struct dbgp_pipe *dbgp_console_input(void)
-{
-	return &dbgp_ehci_info()->ep_pipe[DBGP_CONSOLE_EPIN];
+	return ret;
 }
 
-int usbdebug_init(void)
+/* Initialize the EHCI debug port - this function may scan multiple EHCI
+ * controllers for an attached EHCI debug device. */
+pci_devfn_t ehci_debug_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;
+	ROMSTAGE_CONST struct device *usb_dev = NULL;
+	u32 usb_devfn = 0;
+	pci_devfn_t usb_devfn_t = 0;
+	pci_devfn_t usb_hcdid_devfn_t = 0;
+	int err = 0;
+	unsigned long ehci_base = CONFIG_EHCI_BAR;
+	unsigned long dbg_offset = 0;
+
+#if defined(__PRE_RAM__) && (CONFIG_USBDEBUG_HCD_INDEX == 0)
+# error "Cannot use dynamic PCI scan for EHCI debug port in romstage"
 #endif
-	if (ehci_debug_hw_enable(&ehci_base, &dbg_offset))
-		return -1;
-	return usbdebug_init_(ehci_base, dbg_offset, dbg_info);
+
+	/*
+	 * Get the starting point for the search loop
+	 * USBDEBUG_HCD_INDEX defines a specific controller
+	 * to use if non-zero, else use dev_root as a
+	 * starting point
+	 */
+	if (CONFIG_USBDEBUG_HCD_INDEX > 0) {
+		usb_hcdid_devfn_t = chipset_ehci_debug_get_dev(CONFIG_USBDEBUG_HCD_INDEX);
+		usb_dev = dev_find_slot(PCI_DEV2SEGBUS(usb_hcdid_devfn_t), PCI_DEV2DEVFN(usb_hcdid_devfn_t));
+	} else {
+		usb_hcdid_devfn_t = 0x0;
+		usb_dev = dev_find_slot(PCI_DEV2SEGBUS(usb_hcdid_devfn_t), PCI_DEV2DEVFN(usb_hcdid_devfn_t));
+	}
+
+	/*
+	 * Loop through all the PCI devices in the devicetree,
+	 * starting with either dev_root or dev defined by
+	 * USBDEBUG_HCD_INDEX, looking for EHCI devices
+	 */
+	for (; usb_dev; usb_dev = usb_dev->sibling) {
+		usb_devfn = usb_dev->path.pci.devfn;
+		usb_devfn_t = PCI_DEV(0, PCI_SLOT(usb_devfn), PCI_FUNC(usb_devfn));
+
+		/* Make sure the device is an EHCI device */
+		if (!pci_ehci_is_controller(usb_devfn_t))
+			continue;
+
+		printk(BIOS_DEBUG, "PCI: 00:%02x.%02x: Searching for EHCI debug device\n",
+				PCI_SLOT(usb_devfn), PCI_FUNC(usb_devfn));
+
+		/* Enable the controller and get the BAR and debug port register offset */
+		err = pci_ehci_debug_enable(usb_devfn_t, &ehci_base, &dbg_offset);
+
+		if (err == 0) {
+			/* This is not strictly required but seems like a good idea to
+			 * ensure that the EHCI controller is in a sane state before
+			 * beginning operation. */
+			if (ehci_controller_init(usb_devfn_t) < 0) {
+				printk(BIOS_WARNING, "Could not reset EHCI controller.\n");
+				/* on some systems it works without succeeding here. */
+			} else {
+				printk(BIOS_DEBUG, "EHCI controller reset successfully.\n");
+			}
+
+			/* After a host controller reset, need to wait to receive the port
+			 * change detect event before continuing to scan the ports for an
+			 * attached debug class device. */
+			err = ehci_wait_for_port(usb_devfn_t);
+		}
+
+		/* Locate the debug port(s) on this controller and determine if a USB
+		 * debug class device is attached.  Once a device has enumerated
+		 * successfully, all USB debug operations are sent through  a single set
+		 * of debug port registers. */
+		if (err == 0)
+			err = ehci_locate_debug_port(usb_devfn_t);
+
+		if (err == 0) {
+			printk(BIOS_DEBUG, "PCI: 00:%02x.%02x: Found and started EHCI debug device\n",
+					PCI_SLOT(usb_devfn), PCI_FUNC(usb_devfn));
+			return usb_devfn_t;
+		} else {
+			printk(BIOS_DEBUG, "PCI: 00:%02x.%02x: Could not find EHCI debug device\n",
+					PCI_SLOT(usb_devfn), PCI_FUNC(usb_devfn));
+
+			/*
+			 * Break out if the HCD_INDEX is non-zero, indicating
+			 * the user has selected a specific USB controller to
+			 * use for the debug port and it has failed here
+			 */
+			if ((usb_hcdid_devfn_t > 0) && (usb_devfn_t == usb_hcdid_devfn_t)) {
+				printk(BIOS_WARNING,
+						"Exiting: Could not find debug device on EHCI #%d controller\n",
+						CONFIG_USBDEBUG_HCD_INDEX);
+				break;
+			}
+		}
+
+		/*
+		 * If the current controller does not have an EHCI debug port or there
+		 * was no USB debug class device attached to a port on the current
+		 * controller, increment the ehci_base so that, if there are multiple
+		 * EHCI controllers in the chipset, they are programmed with unique
+		 * MMIO base addresses.
+		 */
+		ehci_base += 0x400;
+	}
+
+	return 0;
 }
diff --git a/src/drivers/usb/ehci_debug.h b/src/drivers/usb/ehci_debug.h
index 8f7d7eb..16ab6fe 100644
--- a/src/drivers/usb/ehci_debug.h
+++ b/src/drivers/usb/ehci_debug.h
@@ -2,6 +2,8 @@
  * This file is part of the coreboot project.
  *
  * Copyright (C) 2007 AMD
+ * Copyright (C) 2014-2015 Sage Electronic Engineering, LLC.
+ *
  * Written by Yinghai Lu <yinghai.lu at amd.com> for AMD.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -22,6 +24,12 @@
 #define _EHCI_DEBUG_H_
 
 #include <types.h>
+#include <device/device.h>
+
+#define USB_DEBUG_DEVNUM        (127)
+#define USB_DEBUG_SETUP_EP0     (0)
+#define USB_DEBUG_CONSOLE_EPOUT (1 | USB_DIR_OUT)
+#define USB_DEBUG_CONSOLE_EPIN  (2 | USB_DIR_IN)
 
 void usbdebug_re_enable(unsigned ehci_base);
 void usbdebug_disable(void);
@@ -29,47 +37,35 @@ void usbdebug_disable(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);
+int pci_ehci_debug_enable(pci_devfn_t dbg_dev, unsigned long *base, unsigned long *dbg_offset);
+void pci_ehci_debug_select_port(pci_devfn_t dbg_dev, unsigned int port);
+int pci_ehci_is_controller(pci_devfn_t dev);
+u32 pci_ehci_get_debug_regs(pci_devfn_t dev);
+u8* pci_ehci_get_bar(pci_devfn_t dev);
 
 #define DBGP_EP_VALID		(1<<0)
 #define DBGP_EP_ENABLED		(1<<1)
 #define DBGP_EP_BUSY		(1<<2)
 #define DBGP_EP_STATMASK	(DBGP_EP_VALID | DBGP_EP_ENABLED)
 
-#define DBGP_MAX_ENDPOINTS	4
-#define DBGP_SETUP_EP0		0	/* Compulsory endpoint 0. */
-#define DBGP_CONSOLE_EPOUT	1
-#define DBGP_CONSOLE_EPIN	2
-
-struct ehci_dbg_port;
-
 struct dbgp_pipe
 {
-	u8 devnum;
-	u8 endpoint;
-	u8 pid;
 	u8 status;
-	int timeout;
 
 	u8 bufidx;
 	u8 buflen;
 	char buf[8];
 };
 
-void dbgp_put(struct dbgp_pipe *pipe);
-int dbgp_try_get(struct dbgp_pipe *pipe);
-
-struct dbgp_pipe *dbgp_console_output(void);
-struct dbgp_pipe *dbgp_console_input(void);
-int dbgp_ep_is_active(struct dbgp_pipe *pipe);
-int dbgp_bulk_write_x(struct dbgp_pipe *pipe, const char *bytes, int size);
-int dbgp_bulk_read_x(struct dbgp_pipe *pipe, void *data, int size);
-
-int dbgp_control_msg(struct ehci_dbg_port *ehci_debug, unsigned devnum,
-	int requesttype, int request, int value, int index, void *data, int size);
-void dbgp_mdelay(int ms);
+void ehci_mdelay(int ms);
+int ehci_debug_bulk_write(void *_ehci_debug, int devnum, int endpoint,
+	const char *bytes, int size);
+int ehci_debug_bulk_write_x(pci_devfn_t dev, const char *bytes, int size);
+int ehci_debug_bulk_read_x(pci_devfn_t dev, void *data, int size);
+int ehci_debug_control_msg(void *_ehci_debug, int devnum, int requesttype,
+		int request, int value, int index, void *data, int size);
+pci_devfn_t ehci_debug_init(void);
 
-int dbgp_probe_gadget(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe);
+int gadget_probe_dev(pci_devfn_t dev);
 
 #endif /* _EHCI_DEBUG_H_ */
diff --git a/src/drivers/usb/gadget.c b/src/drivers/usb/gadget.c
index 782ab5e..8ddad7a 100644
--- a/src/drivers/usb/gadget.c
+++ b/src/drivers/usb/gadget.c
@@ -1,6 +1,8 @@
 /*
  * This file is part of the coreboot project.
  *
+ * Copyright (C) 2014-2015 Sage Electronic Engineering, LLC.
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation; version 2 of the License.
@@ -18,12 +20,13 @@
 #include <stddef.h>
 #include <console/console.h>
 #include <string.h>
+#include <device/pci.h>
 
 #include "ehci_debug.h"
 #include "usb_ch9.h"
 #include "ehci.h"
 
-#define dprintk printk
+/* This file provides USB debug device class protocol handling. */
 
 #define USB_HUB_PORT_CONNECTION		0
 #define USB_HUB_PORT_ENABLED		1
@@ -32,7 +35,6 @@
 #define USB_HUB_C_PORT_CONNECTION	16
 #define USB_HUB_C_PORT_RESET		20
 
-
 static int hub_port_status(const char * buf, int feature)
 {
 	return !!(buf[feature>>3] & (1<<(feature&0x7)));
@@ -41,28 +43,28 @@ static int hub_port_status(const char * buf, int feature)
 /* After USB port reset, treat device number 0 as an USB hub. Assign it with
  * a device number hub_addr. Then apply enable and reset on downstream port.
  */
- static int dbgp_hub_enable(struct ehci_dbg_port *ehci_debug, unsigned char hub_addr,
+ static int gadget_enumerate_hub(struct ehci_dbg_port *ehci_debug, unsigned char hub_addr,
 	unsigned char port)
 {
 	char status[8];
 	int ret, loop;
 
 	/* Assign a devicenumber for the hub. */
-	ret = dbgp_control_msg(ehci_debug, 0,
+	ret = ehci_debug_control_msg(ehci_debug, 0,
 		USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
 		USB_REQ_SET_ADDRESS, hub_addr, 0, NULL, 0);
 	if (ret < 0)
 		goto err;
 
 	/* Enter configured state on hub. */
-	ret = dbgp_control_msg(ehci_debug, hub_addr,
+	ret = ehci_debug_control_msg(ehci_debug, hub_addr,
 		USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
 		USB_REQ_SET_CONFIGURATION, 1, 0, NULL, 0);
 	if (ret < 0)
 		goto err;
 
 	/* Set PORT_POWER, poll for PORT_CONNECTION. */
-	ret = dbgp_control_msg(ehci_debug, hub_addr,
+	ret = ehci_debug_control_msg(ehci_debug, hub_addr,
 		USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_OTHER,
 		USB_REQ_SET_FEATURE, USB_HUB_PORT_POWER, port, NULL, 0);
 	if (ret < 0)
@@ -70,8 +72,8 @@ static int hub_port_status(const char * buf, int feature)
 
 	loop = 100;
 	do {
-		dbgp_mdelay(10);
-		ret = dbgp_control_msg(ehci_debug, hub_addr,
+		ehci_mdelay(10);
+		ret = ehci_debug_control_msg(ehci_debug, hub_addr,
 			USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_OTHER,
 			USB_REQ_GET_STATUS, 0, port, status, 4);
 		if (ret < 0)
@@ -82,7 +84,7 @@ static int hub_port_status(const char * buf, int feature)
 	if (! loop)
 		goto err;
 
-	ret = dbgp_control_msg(ehci_debug, hub_addr,
+	ret = ehci_debug_control_msg(ehci_debug, hub_addr,
 		USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_OTHER,
 		USB_REQ_CLEAR_FEATURE, USB_HUB_C_PORT_CONNECTION, port, NULL, 0);
 	if (ret < 0)
@@ -90,7 +92,7 @@ static int hub_port_status(const char * buf, int feature)
 
 
 	/* Set PORT_RESET, poll for C_PORT_RESET. */
-	ret = dbgp_control_msg(ehci_debug, hub_addr,
+	ret = ehci_debug_control_msg(ehci_debug, hub_addr,
 		USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_OTHER,
 		USB_REQ_SET_FEATURE, USB_HUB_PORT_RESET, port, NULL, 0);
 	if (ret < 0)
@@ -98,8 +100,8 @@ static int hub_port_status(const char * buf, int feature)
 
 	loop = 100;
 	do {
-		dbgp_mdelay(10);
-		ret = dbgp_control_msg(ehci_debug, hub_addr,
+		ehci_mdelay(10);
+		ret = ehci_debug_control_msg(ehci_debug, hub_addr,
 			USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_OTHER,
 			USB_REQ_GET_STATUS, 0, port, status, 4);
 		if (ret < 0)
@@ -110,7 +112,7 @@ static int hub_port_status(const char * buf, int feature)
 	if (! loop)
 		goto err;
 
-	ret = dbgp_control_msg(ehci_debug, hub_addr,
+	ret = ehci_debug_control_msg(ehci_debug, hub_addr,
 		USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_OTHER,
 		USB_REQ_CLEAR_FEATURE, USB_HUB_C_PORT_RESET, port, NULL, 0);
 	if (ret < 0)
@@ -122,106 +124,97 @@ err:
 	return -1;
 }
 
-static void ack_set_configuration(struct dbgp_pipe *pipe, u8 devnum, int timeout)
+/* Send USB request "GET DESCRIPTOR" to retrieve the Debug descriptor from
+ * the device.  If the device does not respond to the GET DESCRIPTOR request,
+ * it is not a USB debug class device. */
+static int gadget_get_debug_descriptor(struct ehci_dbg_port *ehci_debug, int devnum)
 {
-	int i = DBGP_SETUP_EP0;
-	while (++i < DBGP_MAX_ENDPOINTS) {
-		if (pipe[i].endpoint != 0) {
-			pipe[i].devnum = devnum;
-			pipe[i].pid = USB_PID_DATA0;
-			pipe[i].timeout = timeout;
+	int ret = -1;
+	int sz_descriptor = sizeof(struct usb_debug_descriptor);
+	struct usb_debug_descriptor descriptor;
+
+	printk(BIOS_DEBUG, "%s: GET DESCRIPTOR (type=DEBUG; devnum=%d)\n", __FUNCTION__, devnum);
+
+	memset(&descriptor, 0, sz_descriptor);
+
+	ret = ehci_debug_control_msg(ehci_debug, devnum,
+		USB_DIR_IN | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
+		USB_REQ_GET_DESCRIPTOR, (USB_DT_DEBUG << 8), 0,
+		&descriptor, sz_descriptor);
+
+	if (ret == sz_descriptor) {
+		if ((descriptor.bLength == sz_descriptor) &&
+			(descriptor.bDescriptorType==USB_DT_DEBUG)) {
+			if ((descriptor.bDebugInEndpoint != USB_DEBUG_CONSOLE_EPIN) ||
+				(descriptor.bDebugOutEndpoint != USB_DEBUG_CONSOLE_EPOUT)) {
+				printk(BIOS_ERR, "This USB debug device is not supported\n");
+				printk(BIOS_ERR, "Supported EPIN=0x%02x, Device EPIN=0x%02x\n",
+						USB_DEBUG_CONSOLE_EPIN, descriptor.bDebugInEndpoint);
+				printk(BIOS_ERR, "Supported EPOUT=0x%02x, Device EPOUT=0x%02x\n",
+						USB_DEBUG_CONSOLE_EPOUT, descriptor.bDebugOutEndpoint);
+			} else {
+				ret = 0;
+			}
 		}
 	}
+
+	return ret;
 }
 
-static void activate_endpoints(struct dbgp_pipe *pipe)
+/* Send USB request "SET ADDRESS" to set a valid address in the USB debug.
+ * Address "0" is the default address of ALL USB devices after a reset,
+ * for proper operation of all devices connected to USB, a device should be
+ * assigned a valid USB address as soon as possible. */
+static inline int gadget_set_debug_address(struct ehci_dbg_port *ehci_debug)
 {
-	int i = DBGP_SETUP_EP0;
-	pipe[i].status |= DBGP_EP_ENABLED | DBGP_EP_VALID;
-	while (++i < DBGP_MAX_ENDPOINTS) {
-		if (pipe[i].endpoint != 0)
-			pipe[i].status |= DBGP_EP_ENABLED | DBGP_EP_VALID;
-	}
+	printk(BIOS_DEBUG, "%s: SET ADDRESS (addr=%d)\n", __FUNCTION__, USB_DEBUG_DEVNUM);
+	return ehci_debug_control_msg(ehci_debug, 0,
+		USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
+		USB_REQ_SET_ADDRESS, USB_DEBUG_DEVNUM, 0, NULL, 0);
 }
 
-static int probe_for_debug_descriptor(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe)
+/* Send USB request "SET FEATURE" to enable the USB debug device.  This is
+ * part of the expected enumeration sequence for debug class devices and is
+ * functionally equivalent to the "SET CONFIG" request that normally terminates
+ * the USB enumeration sequence for all other USB device classes. */
+static inline int gadget_set_debug_feature(struct ehci_dbg_port *ehci_debug)
 {
-	struct usb_debug_descriptor dbgp_desc;
-	int configured = 0, ret;
-	u8 devnum = 0;
-
-	/* Find the debug device and make it device number 127 */
-debug_dev_retry:
-	memset(&dbgp_desc, 0, sizeof(dbgp_desc));
-	ret = dbgp_control_msg(ehci_debug, devnum,
-		USB_DIR_IN | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
-		USB_REQ_GET_DESCRIPTOR, (USB_DT_DEBUG << 8), 0,
-		&dbgp_desc, sizeof(dbgp_desc));
-	if (ret == sizeof(dbgp_desc)) {
-		if (dbgp_desc.bLength == sizeof(dbgp_desc) && dbgp_desc.bDescriptorType==USB_DT_DEBUG)
-			goto debug_dev_found;
-		else
-			dprintk(BIOS_INFO, "Invalid debug device descriptor.\n");
-	}
-	if (devnum == 0) {
-		devnum = USB_DEBUG_DEVNUM;
-		goto debug_dev_retry;
-	} else {
-		dprintk(BIOS_INFO, "Could not find attached debug device.\n");
-		return -1;
-	}
-debug_dev_found:
-
-	/* Move the device to 127 if it isn't already there */
-	if (devnum != USB_DEBUG_DEVNUM) {
-		ret = dbgp_control_msg(ehci_debug, devnum,
-			USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
-			USB_REQ_SET_ADDRESS, USB_DEBUG_DEVNUM, 0, NULL, 0);
-		if (ret < 0) {
-			dprintk(BIOS_INFO, "Could not move attached device to %d.\n",
-				USB_DEBUG_DEVNUM);
-			return -2;
-		}
-		devnum = USB_DEBUG_DEVNUM;
-		dprintk(BIOS_INFO, "EHCI debug device renamed to 127.\n");
-	}
-
-	/* Enable the debug interface */
-	ret = dbgp_control_msg(ehci_debug, USB_DEBUG_DEVNUM,
+	printk(BIOS_DEBUG, "%s: SET FEATURE (type=DEBUG)\n", __FUNCTION__);
+	return ehci_debug_control_msg(ehci_debug, USB_DEBUG_DEVNUM,
 		USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
 		USB_REQ_SET_FEATURE, USB_DEVICE_DEBUG_MODE, 0, NULL, 0);
-	if (ret < 0) {
-		dprintk(BIOS_INFO, "Could not enable EHCI debug device.\n");
-		return -3;
-	}
-	dprintk(BIOS_INFO, "EHCI debug interface enabled.\n");
+}
 
-	pipe[DBGP_CONSOLE_EPOUT].endpoint = dbgp_desc.bDebugOutEndpoint;
-	pipe[DBGP_CONSOLE_EPIN].endpoint = dbgp_desc.bDebugInEndpoint;
+/* Send USB request "SET CONFIG" to enable some USB debug devices.  Some
+ * implementations of the USB debug device class still expect to receive
+ * a SET CONFIG request as part of the enumeration sequence.  Some USB
+ * device controllers may also have a hardware requirement to receive a SET
+ * CONFIG request before they will enable full operation.  This function is
+ * provided as a fallback for a USB debug device that has otherwise succeeded
+ * the debug enumeration process, but is still not responding to the bulk write
+ * request. */
+static int gadget_set_debug_config(struct ehci_dbg_port *ehci_debug)
+{
+	printk(BIOS_DEBUG, "%s: Test write\n", __FUNCTION__);
 
-	ack_set_configuration(pipe, devnum, 1000);
+	/* Send a small test write. */
+	int ret = ehci_debug_bulk_write(ehci_debug, USB_DEBUG_DEVNUM, USB_DEBUG_CONSOLE_EPOUT, "USB\r\n", 5);
 
-	/* Perform a small write. */
-	configured = 0;
-small_write:
-	ret = dbgp_bulk_write_x(&pipe[DBGP_CONSOLE_EPOUT], "USB\r\n",5);
 	if (ret < 0) {
-		dprintk(BIOS_INFO, "dbgp_bulk_write failed: %d\n", ret);
-		if (!configured) {
-			/* Send Set Configure request to device. This is required for FX2
-			   (CY7C68013) to transfer from USB state Addressed to Configured,
-			   only then endpoints other than 0 are enabled. */
-			if (dbgp_control_msg(ehci_debug, USB_DEBUG_DEVNUM,
+		/* Send control request: SET CONFIG (config = 1)
+		 *
+		 * This is reportedly required for FX2 (CY7C68013) to transfer
+		 * from USB state Addressed to Configured, only then endpoints
+		 * other than 0 are enabled. */
+		printk(BIOS_DEBUG, "%s: SET CONFIG (type=DEBUG)\n", __FUNCTION__);
+		ret = ehci_debug_control_msg(ehci_debug, USB_DEBUG_DEVNUM,
 				USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
-				USB_REQ_SET_CONFIGURATION, 1, 0, NULL, 0) >= 0) {
-				configured = 1;
-				goto small_write;
-			}
-		}
-		return -4;
+				USB_REQ_SET_CONFIGURATION, 1, 0, NULL, 0);
+		if (ret == 0)
+			ret = ehci_debug_bulk_write(ehci_debug, USB_DEBUG_DEVNUM, USB_DEBUG_CONSOLE_EPOUT, "USB\r\n", 5);
 	}
-	dprintk(BIOS_INFO, "Test write done\n");
-	return 0;
+
+	return ret;
 }
 
 /* FTDI FT232H UART programming. */
@@ -230,103 +223,135 @@ small_write:
 #define FTDI_SIO_SET_DATA_REQUEST      0x04
 #define FTDI_SIO_SET_BITMODE_REQUEST   0x0b
 
-static int probe_for_ftdi(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe)
+static int gadget_enumerate_ftdi(struct ehci_dbg_port *ehci_debug)
 {
 	int ret;
-	u8 devnum = 0;
+	u8 devnum = USB_DEBUG_DEVNUM;
 	u8 uart_if = 1; /* FTDI_INTERFACE_A 1 */
 
 	/* Move the device to 127 if it isn't already there */
-	ret = dbgp_control_msg(ehci_debug, devnum,
-		USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
-		USB_REQ_SET_ADDRESS, USB_DEBUG_DEVNUM, 0, NULL, 0);
-	if (ret < 0) {
-		dprintk(BIOS_INFO, "Could not move attached device to %d.\n",
-			USB_DEBUG_DEVNUM);
-			return -2;
-	}
-	devnum = USB_DEBUG_DEVNUM;
-	dprintk(BIOS_INFO, "EHCI debug device renamed to %d.\n", devnum);
+	ret = gadget_set_debug_address(ehci_debug);
+	if (ret < 0)
+		return ret;
 
 	/* Send Set Configure request to device.  */
-	ret = dbgp_control_msg(ehci_debug, devnum,
-		USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
-		USB_REQ_SET_CONFIGURATION, 1, 0, NULL, 0);
-	if (ret < 0) {
-		dprintk(BIOS_INFO, "FTDI set configuration failed.\n");
-		return -2;
-	}
+	ret = ehci_debug_control_msg(ehci_debug, USB_DEBUG_DEVNUM,
+			USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
+			USB_REQ_SET_CONFIGURATION, 1, 0, NULL, 0);
+	if (ret < 0)
+		return ret;
 
-	ret = dbgp_control_msg(ehci_debug, devnum,
+	ret = ehci_debug_control_msg(ehci_debug, devnum,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 		FTDI_SIO_SET_BITMODE_REQUEST, 0, uart_if, NULL, 0);
 	if (ret < 0) {
-		dprintk(BIOS_INFO, "FTDI SET_BITMODE failed.\n");
+		printk(BIOS_DEBUG, "FTDI SET_BITMODE failed.\n");
 		return -3;
 	}
-	ret = dbgp_control_msg(ehci_debug, devnum,
+	ret = ehci_debug_control_msg(ehci_debug, devnum,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 		FTDI_SIO_SET_BAUDRATE_REQUEST,
 		0xc068, 0x0200 | uart_if, NULL, 0);
 	if (ret < 0) {
-		dprintk(BIOS_INFO, "FTDI SET_BAUDRATE failed.\n");
+		printk(BIOS_DEBUG, "FTDI SET_BAUDRATE failed.\n");
 		return -3;
 	}
-	ret = dbgp_control_msg(ehci_debug, devnum,
+	ret = ehci_debug_control_msg(ehci_debug, devnum,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 		FTDI_SIO_SET_DATA_REQUEST,
 		0x0008, uart_if, NULL, 0);
 	if (ret < 0) {
-		dprintk(BIOS_INFO, "FTDI SET_DATA failed.\n");
+		printk(BIOS_DEBUG, "FTDI SET_DATA failed.\n");
 		return -3;
 	}
-	ret = dbgp_control_msg(ehci_debug, devnum,
+	ret = ehci_debug_control_msg(ehci_debug, devnum,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 		FTDI_SIO_SET_FLOW_CTRL_REQUEST,
 		0x0000, uart_if, NULL, 0);
 	if (ret < 0) {
-		dprintk(BIOS_INFO, "FTDI SET_FLOW_CTRL failed.\n");
+		printk(BIOS_DEBUG, "FTDI SET_FLOW_CTRL failed.\n");
 		return -3;
 	}
 
-	pipe[DBGP_CONSOLE_EPOUT].endpoint = 0x02;
-	pipe[DBGP_CONSOLE_EPIN].endpoint = 0x81;
-
-	ack_set_configuration(pipe, devnum, 1000);
-
-	/* Perform a small write. */
-	ret = dbgp_bulk_write_x(&pipe[DBGP_CONSOLE_EPOUT], "USB\r\n", 5);
+	ret = ehci_debug_bulk_write(ehci_debug, USB_DEBUG_DEVNUM, USB_DEBUG_CONSOLE_EPOUT, "USB\r\n", 5);
 	if (ret < 0) {
-		dprintk(BIOS_INFO, "dbgp_bulk_write failed: %d\n", ret);
+		printk(BIOS_WARNING, "%s: Test write failed ret=%d\n", __FUNCTION__, ret);
 		return -4;
 	}
-	dprintk(BIOS_INFO, "Test write done\n");
+
 	return 0;
 }
 
-int dbgp_probe_gadget(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe)
+/* Send the series of USB requests necessary to "enumerate" the USB debug
+ * class device.  This step is required for both the host and device to
+ * synchronize and successfully establish communication.  The debug class
+ * enumeration is an abbreviated form of the standard USB device
+ * enumeration. */
+static int gadget_enumerate_debug_dev(struct ehci_dbg_port *ehci_debug)
 {
-	int ret;
+	int ret = -1;
+
+	/* Per the USB specification, all USB devices are supposed to default to
+	 * address=0 after a reset */
+	int devnum = 0;
+
+	while (1) {
+		/* Send control request: GET DESCRIPTOR (type = DEBUG) */
+		ret = gadget_get_debug_descriptor(ehci_debug, devnum);
+
+		/* Send control request: SET ADDRESS (addr = USB_DEBUG_DEVNUM) */
+		if ((ret >= 0) && (devnum == 0))
+			ret = gadget_set_debug_address(ehci_debug);
+
+		/* Send control request: SET FEATURE (type = DEBUG) */
+		if (ret >= 0)
+			ret = gadget_set_debug_feature(ehci_debug);
+
+		/* Workaround: Send SET CONFIG request to devices that depend on it for
+		 *             complete USB enumeration */
+		if (ret >= 0)
+			ret = gadget_set_debug_config(ehci_debug);
+
+		if (ret >= 0)
+			break;
+
+		/* Workaround: Some debug class devices are apparently hard-coded to
+		 * always use USB_DEBUG_DEVNUM without waiting for the SET ADDRESS
+		 * request.  Retry on USB_DEBUG_DEVNUM to see if the debug device will
+		 * respond on that address instead.  After trying USB_DEBUG_DEVNUM
+		 * without success, give up because this is obviously not a debug class
+		 * device. */
+		if (devnum == USB_DEBUG_DEVNUM)
+			break;
+		devnum = USB_DEBUG_DEVNUM;
+	}
+
+	return ret;
+}
+
+int gadget_probe_dev(pci_devfn_t dev)
+{
+	int ret = -1;
+	struct ehci_dbg_port *ehci_debug = (struct ehci_dbg_port *)pci_ehci_get_debug_regs(dev);
 
 	if (CONFIG_USBDEBUG_OPTIONAL_HUB_PORT != 0) {
-		ret = dbgp_hub_enable(ehci_debug, USB_DEBUG_DEVNUM-1,
+		ret = gadget_enumerate_hub(ehci_debug, USB_DEBUG_DEVNUM-1,
 			CONFIG_USBDEBUG_OPTIONAL_HUB_PORT);
 		if (ret < 0) {
-			printk(BIOS_INFO, "Could not enable USB hub on debug port.\n");
+			printk(BIOS_WARNING, "Could not enable USB hub on debug port.\n");
 			return ret;
 		}
 	}
 
 	if (IS_ENABLED(CONFIG_USBDEBUG_DONGLE_FTDI_FT232H)) {
-		ret = probe_for_ftdi(ehci_debug, pipe);
+		ret = gadget_enumerate_ftdi(ehci_debug);
 	} else {
-		ret = probe_for_debug_descriptor(ehci_debug, pipe);
+		ret = gadget_enumerate_debug_dev(ehci_debug);
 	}
 	if (ret < 0) {
-		dprintk(BIOS_INFO, "Could not enable debug dongle.\n");
+		printk(BIOS_WARNING, "Could not enable debug dongle.\n");
 		return ret;
 	}
 
-	activate_endpoints(pipe);
 	return 0;
 }
diff --git a/src/drivers/usb/pci_ehci.c b/src/drivers/usb/pci_ehci.c
index 8c0422f..4810d2d 100644
--- a/src/drivers/usb/pci_ehci.c
+++ b/src/drivers/usb/pci_ehci.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2006 Eric Biederman (ebiederm at xmission.com)
  * Copyright (C) 2007 AMD
+ * Copyright (C) 2014-2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -32,38 +33,86 @@
 #if !defined(__PRE_RAM__) && !defined(__SMM__)
 static struct device_operations *ehci_drv_ops;
 static struct device_operations ehci_dbg_ops;
+static int pci_ehci_debug_is_enabled(struct device *dev);
 #endif
 
-int ehci_debug_hw_enable(unsigned int *base, unsigned int *dbg_offset)
+static int pci_ehci_debug_offset(pci_devfn_t dbg_dev, unsigned long *offset);
+
+/* Enable the EHCI debug port - for all chipsets, this involves setting up the
+ * PCI BAR for the EHCI MMIO registers.  For some chipsets there may be
+ * additional register writes to enable the EHCI controller and/or turn on the
+ * debug port support. */
+int pci_ehci_debug_enable(pci_devfn_t dbg_dev, unsigned long *base, unsigned long *dbg_offset)
+{
+	chipset_ehci_debug_enable(dbg_dev, (unsigned long)base);
+	return pci_ehci_debug_offset(dbg_dev, dbg_offset);
+}
+
+/* Route the EHCI debug port to a physical port - for some chipsets this is a
+ * no-op because there is only one physical port that can operate as an EHCI
+ * debug port.  For other chipsets there is an additional register that can be
+ * used to make any EHCI port operate as the EHCI debug port. */
+void pci_ehci_debug_select_port(pci_devfn_t dbg_dev, unsigned int port)
+{
+	/* Chipset port enable registers assume a 1-based port number, but the
+	 * EHCI controller uses 0-based port numbers.  Convert from 0-based to
+	 * 1-based here before calling the chipset function. */
+	chipset_ehci_debug_set_port(dbg_dev, port + 1);
+}
+
+/* Returns TRUE if the PCI device class matches the EHCI class code */
+int pci_ehci_is_controller(pci_devfn_t dev)
 {
-	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;
+	pci_devfn_t pci_dev = dev;
 #else
-	device_t dev = dev_find_slot(PCI_DEV2SEGBUS(dbg_dev), PCI_DEV2DEVFN(dbg_dev));
+	device_t pci_dev = dev_find_slot(PCI_DEV2SEGBUS(dev), PCI_DEV2DEVFN(dev));
 #endif
 
-	u8 pos = pci_find_capability(dev, PCI_CAP_ID_EHCI_DEBUG);
-	if (!pos)
-		return -1;
+	int controller = 0; /* 1=is EHCI controller, 0=is NOT ECHI controller */
+	u32 class      = pci_read_config32(pci_dev, PCI_CLASS_REVISION) >> 8;
 
-	u32 cap = pci_read_config32(dev, pos);
+	if (class == PCI_EHCI_CLASSCODE)
+		controller = 1;
 
-	/* FIXME: We should remove static EHCI_BAR_INDEX. */
-	u8 dbg_bar = 0x10 + 4 * ((cap >> 29) - 1);
-	if (dbg_bar != EHCI_BAR_INDEX)
-		return -1;
+	return controller;
+}
 
-	*base = CONFIG_EHCI_BAR;
-	*dbg_offset = (cap>>16) & 0x1ffc;
-	return 0;
+/* Returns the MMIO address of the EHCI debug port register interface */
+u32 pci_ehci_get_debug_regs(pci_devfn_t dev)
+{
+	unsigned long offset = 0;
+	u32 ehci_debug = 0;
+
+	if (pci_ehci_debug_offset(dev, &offset) == 0) {
+		u32 bar = (u32)pci_ehci_get_bar(dev);
+		ehci_debug = bar + offset;
+	}
+	return ehci_debug;
 }
 
-void ehci_debug_select_port(unsigned int port)
+/* Returns the PCI BAR value for the EHCI controller */
+u8* pci_ehci_get_bar(pci_devfn_t dev)
 {
-	pci_devfn_t dbg_dev = pci_ehci_dbg_dev(CONFIG_USBDEBUG_HCD_INDEX);
-	pci_ehci_dbg_set_port(dbg_dev, port);
+	u32 pci_bar = 0;
+
+	/* This assumes that the EHCI controller will never be bus 0, device 0,
+	 * function 0 */
+	if (dev != 0) {
+#ifdef __SIMPLE_DEVICE__
+		pci_devfn_t pci_dev = dev;
+#else
+		device_t pci_dev = dev_find_slot(PCI_DEV2SEGBUS(dev), PCI_DEV2DEVFN(dev));
+#endif
+		u8 pci_cmd = pci_read_config8(pci_dev, PCI_COMMAND);
+
+		if ((pci_cmd & PCI_COMMAND_MEMORY) != 0) {
+			pci_bar = pci_read_config32(pci_dev, EHCI_BAR_INDEX);
+			pci_bar &= ~(0x0F);
+		}
+	}
+
+	return (u8*)pci_bar;
 }
 
 #if !defined(__PRE_RAM__) && !defined(__SMM__)
@@ -87,9 +136,7 @@ static void pci_ehci_set_resources(struct device *dev)
 
 void pci_ehci_read_resources(struct device *dev)
 {
-	pci_devfn_t dbg_dev = pci_ehci_dbg_dev(CONFIG_USBDEBUG_HCD_INDEX);
-
-	if (!ehci_drv_ops && pci_match_simple_dev(dev, dbg_dev)) {
+	if (!ehci_drv_ops && pci_ehci_debug_is_enabled(dev)) {
 		memcpy(&ehci_dbg_ops, dev->ops, sizeof(ehci_dbg_ops));
 		ehci_drv_ops = dev->ops;
 		ehci_dbg_ops.set_resources = pci_ehci_set_resources;
@@ -101,15 +148,80 @@ void pci_ehci_read_resources(struct device *dev)
 
 	pci_dev_read_resources(dev);
 }
+
+/*
+ * Given a device structure, make sure that it is an
+ * EHCI class device, that its BAR is non-zero, and
+ * that its EHCI Debug Port is enabled
+ */
+static int pci_ehci_debug_is_enabled(struct device *dev)
+{
+	struct ehci_dbg_port *ehci_debug;
+	int enabled = 0;	/* 1 = enabled, 0 = disabled */
+	u32 class, bar, cap, dbg_offset, ctrl;
+	u8 pos;
+
+	/* Make sure its an ECHI device */
+	class = pci_read_config32(dev, PCI_CLASS_REVISION) >> 8;
+	if (class != PCI_EHCI_CLASSCODE)
+		return 0;
+
+	/* Make sure the BAR is set */
+	bar = pci_read_config32(dev, EHCI_BAR_INDEX);
+	if (!bar)
+		return 0;
+
+	/* Find its capabilities */
+	pos = pci_find_capability(dev, PCI_CAP_ID_EHCI_DEBUG);
+	if (!pos)
+		return 0;
+
+	cap = pci_read_config32(dev, pos);
+	dbg_offset = (cap>>16) & 0x1ffc;
+	ehci_debug = (struct ehci_dbg_port *)(bar + dbg_offset);
+
+	ctrl = read32((unsigned long)&ehci_debug->control);
+	enabled = (ctrl & DBGP_ENABLED) == DBGP_ENABLED ? 1 : 0;
+
+	return enabled;
+}
+
+
 #endif
 
-u8 *pci_ehci_base_regs(pci_devfn_t sdev)
+/* Returns the offset to the EHCI debug port registers */
+static int pci_ehci_debug_offset(pci_devfn_t dbg_dev, unsigned long *dbg_offset)
 {
 #ifdef __SIMPLE_DEVICE__
-	u8 *base = (u8 *)(pci_read_config32(sdev, EHCI_BAR_INDEX) & ~0x0f);
+	pci_devfn_t dev = dbg_dev;
 #else
-	device_t dev = dev_find_slot(PCI_DEV2SEGBUS(sdev), PCI_DEV2DEVFN(sdev));
-	u8 *base = (u8 *)(pci_read_config32(dev, EHCI_BAR_INDEX) & ~0x0f);
+	device_t dev = dev_find_slot(PCI_DEV2SEGBUS(dbg_dev), PCI_DEV2DEVFN(dbg_dev));
 #endif
-	return base + HC_LENGTH(read32(base));
+
+	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;
+
+	*dbg_offset = (cap>>16) & 0x1ffc;
+
+	return 0;
+}
+
+/* Returns the address of the EHCI MMIO register interface */
+u8* pci_ehci_base_regs(pci_devfn_t sdev)
+{
+	u8* base = pci_ehci_get_bar(sdev);
+
+	if (base != NULL)
+		base += HC_LENGTH(read32(base));
+
+	return base;
 }
+
diff --git a/src/include/device/pci_ehci.h b/src/include/device/pci_ehci.h
index 33f42b1..b916d01 100644
--- a/src/include/device/pci_ehci.h
+++ b/src/include/device/pci_ehci.h
@@ -2,6 +2,8 @@
  * This file is part of the coreboot project.
  *
  * Copyright (C) 2007 AMD
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
+ * 
  * Written by Yinghai Lu <yinghai.lu at amd.com> for AMD.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -27,10 +29,10 @@
 #define EHCI_BAR_INDEX		0x10
 #define PCI_EHCI_CLASSCODE 	0x0c0320	/* USB2.0 with EHCI controller */
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned hcd_idx);
-u8 *pci_ehci_base_regs(pci_devfn_t dev);
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port);
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base);
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned hcd_idx);
+u8* pci_ehci_base_regs(pci_devfn_t dev);
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port);
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base);
 
 #ifndef __PRE_RAM__
 #if !CONFIG_USBDEBUG
diff --git a/src/mainboard/intel/cougar_canyon2/romstage.c b/src/mainboard/intel/cougar_canyon2/romstage.c
index 92d0518..304fd32 100644
--- a/src/mainboard/intel/cougar_canyon2/romstage.c
+++ b/src/mainboard/intel/cougar_canyon2/romstage.c
@@ -265,11 +265,6 @@ void romstage_main_continue(EFI_STATUS status, VOID *HobListPtr) {
 
 	post_code(0x49);
 
-#if CONFIG_USBDEBUG
-	/* FSP reconfigures USB, so reinit it to have debug */
-	early_usbdebug_init();
-#endif
-
 	/* For reference print FSP version */
 	u32 version = MCHBAR32(0x5034);
 	printk(BIOS_DEBUG, "FSP Version %d.%d.%d Build %d\n",
diff --git a/src/northbridge/intel/sandybridge/raminit.c b/src/northbridge/intel/sandybridge/raminit.c
index 053a487..72fcc5d 100644
--- a/src/northbridge/intel/sandybridge/raminit.c
+++ b/src/northbridge/intel/sandybridge/raminit.c
@@ -267,11 +267,6 @@ void sdram_initialize(struct pei_data *pei_data)
 		die("UEFI PEI System Agent not found.\n");
 	}
 
-#if CONFIG_USBDEBUG_IN_ROMSTAGE
-	/* mrc.bin reconfigures USB, so reinit it to have debug */
-	usbdebug_init();
-#endif
-
 	/* For reference print the System Agent version
 	 * after executing the UEFI PEI stage.
 	 */
diff --git a/src/soc/intel/fsp_baytrail/romstage/romstage.c b/src/soc/intel/fsp_baytrail/romstage/romstage.c
index c40f163..5b22361 100644
--- a/src/soc/intel/fsp_baytrail/romstage/romstage.c
+++ b/src/soc/intel/fsp_baytrail/romstage/romstage.c
@@ -230,11 +230,6 @@ void romstage_main_continue(EFI_STATUS status, void *hob_list_ptr) {
 	printk(BIOS_DEBUG, "%s status: %x  hob_list_ptr: %x\n",
 		__func__, (u32) status, (u32) hob_list_ptr);
 
-#if IS_ENABLED(CONFIG_USBDEBUG_IN_ROMSTAGE)
-	/* FSP reconfigures USB, so reinit it to have debug */
-	usbdebug_init();
-#endif	/* IS_ENABLED(CONFIG_USBDEBUG_IN_ROMSTAGE) */
-
 	printk(BIOS_DEBUG, "FSP Status: 0x%0x\n", (u32)status);
 
 	/* Get previous sleep state again and clear */
diff --git a/src/southbridge/amd/agesa/hudson/enable_usbdebug.c b/src/southbridge/amd/agesa/hudson/enable_usbdebug.c
index c699831..9bd5810 100644
--- a/src/southbridge/amd/agesa/hudson/enable_usbdebug.c
+++ b/src/southbridge/amd/agesa/hudson/enable_usbdebug.c
@@ -2,6 +2,7 @@
  * This file is part of the coreboot project.
  *
  * Copyright (C) 2010 Advanced Micro Devices, Inc.
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -28,7 +29,7 @@
 
 #define DEBUGPORT_MISC_CONTROL		0x80
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned int hcd_idx)
 {
 	if (hcd_idx==3)
 		return PCI_DEV(0, 0x16, 2);
@@ -38,7 +39,7 @@ pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
 		return PCI_DEV(0, 0x12, 2);
 }
 
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port)
 {
 	u8 *base_regs = pci_ehci_base_regs(dev);
 	u32 reg32;
@@ -52,7 +53,7 @@ void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
 }
 
 
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base)
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base)
 {
 	/* Enable all of the USB controllers */
 	outb(0xEF, PM_INDEX);
diff --git a/src/southbridge/amd/pi/hudson/enable_usbdebug.c b/src/southbridge/amd/pi/hudson/enable_usbdebug.c
index c699831..9bd5810 100644
--- a/src/southbridge/amd/pi/hudson/enable_usbdebug.c
+++ b/src/southbridge/amd/pi/hudson/enable_usbdebug.c
@@ -2,6 +2,7 @@
  * This file is part of the coreboot project.
  *
  * Copyright (C) 2010 Advanced Micro Devices, Inc.
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -28,7 +29,7 @@
 
 #define DEBUGPORT_MISC_CONTROL		0x80
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned int hcd_idx)
 {
 	if (hcd_idx==3)
 		return PCI_DEV(0, 0x16, 2);
@@ -38,7 +39,7 @@ pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
 		return PCI_DEV(0, 0x12, 2);
 }
 
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port)
 {
 	u8 *base_regs = pci_ehci_base_regs(dev);
 	u32 reg32;
@@ -52,7 +53,7 @@ void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
 }
 
 
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base)
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base)
 {
 	/* Enable all of the USB controllers */
 	outb(0xEF, PM_INDEX);
diff --git a/src/southbridge/amd/sb600/enable_usbdebug.c b/src/southbridge/amd/sb600/enable_usbdebug.c
index be17ca6..bf0aed1 100644
--- a/src/southbridge/amd/sb600/enable_usbdebug.c
+++ b/src/southbridge/amd/sb600/enable_usbdebug.c
@@ -2,6 +2,7 @@
  * This file is part of the coreboot project.
  *
  * Copyright (C) 2008 Advanced Micro Devices, Inc.
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -26,18 +27,18 @@
 #include <device/pci_def.h>
 #include "sb600.h"
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned int hcd_idx)
 {
 	return  PCI_DEV(0, 0x13, 5); /* USB EHCI, D19:F5 */
 }
 
 /* Required for successful build, but currently empty. */
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port)
 {
 	/* TODO: Allow changing the physical USB port used as Debug Port. */
 }
 
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base)
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base)
 {
 	/* Set the EHCI BAR address. */
 	pci_write_config32(dev, EHCI_BAR_INDEX, base);
diff --git a/src/southbridge/amd/sb700/enable_usbdebug.c b/src/southbridge/amd/sb700/enable_usbdebug.c
index 37e529a..168b8bc 100644
--- a/src/southbridge/amd/sb700/enable_usbdebug.c
+++ b/src/southbridge/amd/sb700/enable_usbdebug.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2010 Advanced Micro Devices, Inc.
  * Copyright (C) 2010 Uwe Hermann <uwe at hermann-uwe.de>
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -29,7 +30,7 @@
 
 #define DEBUGPORT_MISC_CONTROL		0x80
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned int hcd_idx)
 {
 	if (hcd_idx==2)
 		return PCI_DEV(0, 0x13, 2);
@@ -37,7 +38,7 @@ pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
 		return PCI_DEV(0, 0x12, 2);
 }
 
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port)
 {
 	u8 *base_regs = pci_ehci_base_regs(dev);
 	u32 reg32;
@@ -50,7 +51,7 @@ void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
 	write32(base_regs + DEBUGPORT_MISC_CONTROL, reg32);
 }
 
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base)
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base)
 {
 	/* Set the EHCI BAR address. */
 	pci_write_config32(dev, EHCI_BAR_INDEX, base);
diff --git a/src/southbridge/amd/sb800/enable_usbdebug.c b/src/southbridge/amd/sb800/enable_usbdebug.c
index 4965762..f127fb3 100644
--- a/src/southbridge/amd/sb800/enable_usbdebug.c
+++ b/src/southbridge/amd/sb800/enable_usbdebug.c
@@ -2,6 +2,7 @@
  * This file is part of the coreboot project.
  *
  * Copyright (C) 2010 Advanced Micro Devices, Inc.
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -28,7 +29,7 @@
 
 #define DEBUGPORT_MISC_CONTROL		0x80
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned int hcd_idx)
 {
 	if (hcd_idx==3)
 		return PCI_DEV(0, 0x16, 2);
@@ -38,7 +39,7 @@ pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
 		return PCI_DEV(0, 0x12, 2);
 }
 
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port)
 {
 	u8 *base_regs = pci_ehci_base_regs(dev);
 	u32 reg32;
@@ -52,7 +53,7 @@ void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
 }
 
 
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base)
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base)
 {
 	/* Enable all of the USB controllers */
 	outb(0xEF, PM_INDEX);
diff --git a/src/southbridge/intel/common/usb_debug.c b/src/southbridge/intel/common/usb_debug.c
index 3c81986..419b5e1 100644
--- a/src/southbridge/intel/common/usb_debug.c
+++ b/src/southbridge/intel/common/usb_debug.c
@@ -2,6 +2,7 @@
  * This file is part of the coreboot project.
  *
  * Copyright (C) 2009 coresystems GmbH
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -26,7 +27,7 @@
 #include <device/pci_ehci.h>
 #include <device/pci_def.h>
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned int hcd_idx)
 {
 	u32 class;
 	pci_devfn_t dev;
@@ -57,12 +58,12 @@ pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
 }
 
 /* Required for successful build, but currently empty. */
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port)
 {
 	/* Not needed, the ICH* southbridges hardcode physical USB port 1. */
 }
 
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base)
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base)
 {
 	/* Bail out. No console to complain in. */
 	if (!dev)
diff --git a/src/southbridge/intel/fsp_rangeley/romstage.c b/src/southbridge/intel/fsp_rangeley/romstage.c
index 3500dfd..13317b8 100644
--- a/src/southbridge/intel/fsp_rangeley/romstage.c
+++ b/src/southbridge/intel/fsp_rangeley/romstage.c
@@ -105,11 +105,6 @@ void romstage_main_continue(EFI_STATUS status, void *hob_list_ptr) {
 	printk(BIOS_DEBUG, "%s status: %x  hob_list_ptr: %x\n",
 		__func__, (u32) status, (u32) hob_list_ptr);
 
-#if IS_ENABLED(CONFIG_USBDEBUG_IN_ROMSTAGE)
-	/* FSP reconfigures USB, so reinit it to have debug */
-	usbdebug_init();
-#endif	/* IS_ENABLED(CONFIG_USBDEBUG_IN_ROMSTAGE) */
-
 	printk(BIOS_DEBUG, "FSP Status: 0x%0x\n", (u32)status);
 
 	post_code(0x4b);
diff --git a/src/southbridge/nvidia/ck804/enable_usbdebug.c b/src/southbridge/nvidia/ck804/enable_usbdebug.c
index baf260b..0c72d82 100644
--- a/src/southbridge/nvidia/ck804/enable_usbdebug.c
+++ b/src/southbridge/nvidia/ck804/enable_usbdebug.c
@@ -5,6 +5,7 @@
  * Written by Yinghai Lu <yhlu at tyan.com> for Tyan Computer.
  * Copyright (C) 2006,2007 AMD
  * Written by Yinghai Lu <yinghai.lu at amd.com> for AMD.
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -30,12 +31,12 @@
 #include <device/pci_def.h>
 #include "ck804.h"
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned int hcd_idx)
 {
 	return PCI_DEV(0, CK804_DEVN_BASE + 2, 1); /* USB EHCI */
 }
 
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port)
 {
 	u32 dword;
 
@@ -46,7 +47,7 @@ void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
 	pci_write_config32(dev, 0x74, dword);
 }
 
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base)
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base)
 {
 	/* Set the EHCI BAR address. */
 	pci_write_config32(dev, EHCI_BAR_INDEX, base);
diff --git a/src/southbridge/nvidia/mcp55/enable_usbdebug.c b/src/southbridge/nvidia/mcp55/enable_usbdebug.c
index 02d5f34..ad52036 100644
--- a/src/southbridge/nvidia/mcp55/enable_usbdebug.c
+++ b/src/southbridge/nvidia/mcp55/enable_usbdebug.c
@@ -5,6 +5,7 @@
  * Written by Yinghai Lu <yhlu at tyan.com> for Tyan Computer.
  * Copyright (C) 2006,2007 AMD
  * Written by Yinghai Lu <yinghai.lu at amd.com> for AMD.
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -30,12 +31,12 @@
 #include <device/pci_def.h>
 #include "mcp55.h"
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned int hcd_idx)
 {
 	return PCI_DEV(0, MCP55_DEVN_BASE + 2, 1); /* USB EHCI */
 }
 
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port)
 {
 	u32 dword;
 
@@ -46,7 +47,7 @@ void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
 	pci_write_config32(dev, 0x74, dword);
 }
 
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base)
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base)
 {
 	/* Set the EHCI BAR address. */
 	pci_write_config32(dev, EHCI_BAR_INDEX, base);
diff --git a/src/southbridge/sis/sis966/enable_usbdebug.c b/src/southbridge/sis/sis966/enable_usbdebug.c
index 88ae772..fab9865 100644
--- a/src/southbridge/sis/sis966/enable_usbdebug.c
+++ b/src/southbridge/sis/sis966/enable_usbdebug.c
@@ -5,6 +5,7 @@
  * Written by Yinghai Lu <yhlu at tyan.com> for Tyan Computer.
  * Copyright (C) 2006,2007 AMD
  * Written by Yinghai Lu <yinghai.lu at amd.com> for AMD.
+ * Copyright (C) 2015 Sage Electronic Engineering, LLC.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -32,12 +33,12 @@
 #include <device/pci_def.h>
 #include "sis966.h"
 
-pci_devfn_t pci_ehci_dbg_dev(unsigned int hcd_idx)
+pci_devfn_t chipset_ehci_debug_get_dev(unsigned int hcd_idx)
 {
 	return PCI_DEV(0, SIS966_DEVN_BASE + 2, 1); /* USB EHCI */
 }
 
-void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
+void chipset_ehci_debug_set_port(pci_devfn_t dev, unsigned int port)
 {
 	u32 dword;
 
@@ -48,7 +49,7 @@ void pci_ehci_dbg_set_port(pci_devfn_t dev, unsigned int port)
 	pci_write_config32(dev, 0x74, dword);
 }
 
-void pci_ehci_dbg_enable(pci_devfn_t dev, unsigned long base)
+void chipset_ehci_debug_enable(pci_devfn_t dev, unsigned long base)
 {
 	/* Set the EHCI BAR address. */
 	pci_write_config32(dev, EHCI_BAR_INDEX, base);



More information about the coreboot-gerrit mailing list