libpayload: Fill gaps in the xHCI driver

Well, it turned out to be more as some gaps ;)
but we finally have xHCI running. It's well tested against a QM77 Ivy
Bridge board.

We have no SuperSpeed support (yet). On Ivy Bridge, SuperSpeed is not
advertised and USB 3 devices will just work at HighSpeed.

There are still some bit fields in xhci_private.h, so this might need
little more work to run on ARM.

Change-Id: I7a2cb3f226d24573659142565db38b13acdc218c
Signed-off-by: Nico Huber <nico.huber@secunet.com>
Signed-off-by: Patrick Georgi <patrick.georgi@secunet.com>
Reviewed-on: http://review.coreboot.org/3452
Tested-by: build bot (Jenkins)
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>

9 files changed, 2274 insertions, 398 deletions

diff --git a/payloads/libpayload/Config.in b/payloads/libpayload/Config.in
index 0a29bd5025..6610f8f785 100644
--- a/payloads/libpayload/Config.in
+++ b/payloads/libpayload/Config.in
@@ -380,8 +380,8 @@ config USB_MSC
 
 config USB_GEN_HUB
 	bool
-	default n if (!USB_HUB)
-	default y if (USB_HUB)
+	default n if (!USB_HUB && !USB_XHCI)
+	default y if (USB_HUB || USB_XHCI)
 
 endmenu
 
diff --git a/payloads/libpayload/drivers/Makefile.inc b/payloads/libpayload/drivers/Makefile.inc
index 0f014d647d..a1891beb73 100644
--- a/payloads/libpayload/drivers/Makefile.inc
+++ b/payloads/libpayload/drivers/Makefile.inc
@@ -73,6 +73,10 @@ libc-$(CONFIG_USB_OHCI) += usb/ohci_rh.c
 libc-$(CONFIG_USB_EHCI) += usb/ehci.c
 libc-$(CONFIG_USB_EHCI) += usb/ehci_rh.c
 libc-$(CONFIG_USB_XHCI) += usb/xhci.c
+libc-$(CONFIG_USB_XHCI) += usb/xhci_debug.c
+libc-$(CONFIG_USB_XHCI) += usb/xhci_devconf.c
+libc-$(CONFIG_USB_XHCI) += usb/xhci_events.c
+libc-$(CONFIG_USB_XHCI) += usb/xhci_commands.c
 libc-$(CONFIG_USB_XHCI) += usb/xhci_rh.c
 libc-$(CONFIG_USB_HID) += usb/usbhid.c
 libc-$(CONFIG_USB_MSC) += usb/usbmsc.c
diff --git a/payloads/libpayload/drivers/usb/xhci.c b/payloads/libpayload/drivers/usb/xhci.c
index 083b331548..c29d323175 100644
--- a/payloads/libpayload/drivers/usb/xhci.c
+++ b/payloads/libpayload/drivers/usb/xhci.c
@@ -2,6 +2,7 @@
  * This file is part of the libpayload project.
  *
  * Copyright (C) 2010 Patrick Georgi
+ * Copyright (C) 2013 secunet Security Networks AG
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -27,15 +28,17 @@
  * SUCH DAMAGE.
  */
 
-#define USB_DEBUG
+//#define XHCI_SPEW_DEBUG
 
+#include <inttypes.h>
 #include <arch/virtual.h>
-#include "xhci.h"
 #include "xhci_private.h"
+#include "xhci.h"
 
 static void xhci_start (hci_t *controller);
 static void xhci_stop (hci_t *controller);
 static void xhci_reset (hci_t *controller);
+static void xhci_reinit (hci_t *controller);
 static void xhci_shutdown (hci_t *controller);
 static int xhci_bulk (endpoint_t *ep, int size, u8 *data, int finalize);
 static int xhci_control (usbdev_t *dev, direction_t dir, int drlen, void *devreq,
@@ -44,226 +47,828 @@ static void* xhci_create_intr_queue (endpoint_t *ep, int reqsize, int reqcount,
 static void xhci_destroy_intr_queue (endpoint_t *ep, void *queue);
 static u8* xhci_poll_intr_queue (void *queue);
 
-static void
-xhci_reset (hci_t *controller)
+/*
+ * Some structures must not cross page boundaries. To get this,
+ * we align them by their size (or the next greater power of 2).
+ */
+void *
+xhci_align(const size_t min_align, const size_t size)
 {
+	size_t align;
+	if (!(size & (size - 1)))
+		align = size; /* It's a power of 2 */
+	else
+		align = 1 << ((sizeof(unsigned) << 3) - __builtin_clz(size));
+	if (align < min_align)
+		align = min_align;
+	xhci_spew("Aligning %zu to %zu\n", size, align);
+	return memalign(align, size);
 }
 
+void
+xhci_clear_trb(trb_t *const trb, const int pcs)
+{
+	trb->ptr_low	= 0;
+	trb->ptr_high	= 0;
+	trb->status	= 0;
+	trb->control	= !pcs;
+}
+
+void
+xhci_init_cycle_ring(transfer_ring_t *const tr, const size_t ring_size)
+{
+	memset((void *)tr->ring, 0, ring_size * sizeof(*tr->ring));
+	TRB_SET(TT, &tr->ring[ring_size - 1], TRB_LINK);
+	TRB_SET(TC, &tr->ring[ring_size - 1], 1);
+	/* only one segment that points to itself */
+	tr->ring[ring_size - 1].ptr_low = virt_to_phys(tr->ring);
+
+	tr->pcs = 1;
+	tr->cur = tr->ring;
+}
+
+/* On Panther Point: switch ports shared with EHCI to xHCI */
 static void
-xhci_reinit (hci_t *controller)
+xhci_switch_ppt_ports(pcidev_t addr)
+{
+	if (pci_read_config32(addr, 0x00) == 0x1e318086) {
+		u32 reg32 = pci_read_config32(addr, 0xdc) & 0xf;
+		xhci_debug("Ports capable of SuperSpeed: 0x%"PRIx32"\n", reg32);
+
+		/* For now, do not enable SuperSpeed on any ports */
+		//pci_write_config32(addr, 0xd8, reg32);
+		pci_write_config32(addr, 0xd8, 0x00000000);
+		reg32 = pci_read_config32(addr, 0xd8) & 0xf;
+		xhci_debug("Configured for SuperSpeed:   0x%"PRIx32"\n", reg32);
+
+		reg32 = pci_read_config32(addr, 0xd4) & 0xf;
+		xhci_debug("Trying to switch over:       0x%"PRIx32"\n", reg32);
+
+		pci_write_config32(addr, 0xd0, reg32);
+		reg32 = pci_read_config32(addr, 0xd0) & 0xf;
+		xhci_debug("Actually switched over:      0x%"PRIx32"\n", reg32);
+	}
+}
+
+static long
+xhci_handshake(volatile u32 *const reg, u32 mask, u32 wait_for, long timeout_us)
+{
+	while ((*reg & mask) != wait_for && timeout_us--) udelay(1);
+	return timeout_us;
+}
+
+static int
+xhci_wait_ready(xhci_t *const xhci)
 {
+	xhci_debug("Waiting for controller to be ready... ");
+	if (!xhci_handshake(&xhci->opreg->usbsts, USBSTS_CNR, 0, 100000L)) {
+		usb_debug("timeout!\n");
+		return -1;
+	}
+	usb_debug("ok.\n");
+	return 0;
 }
 
 hci_t *
-xhci_init (pcidev_t addr)
+xhci_init (const pcidev_t addr)
 {
 	int i;
 
-	hci_t *controller = new_controller ();
-
-	if (!controller)
-		fatal("Could not create USB controller instance.\n");
-
-	controller->instance = malloc (sizeof (xhci_t));
-	if(!controller->instance)
-		fatal("Not enough memory creating USB controller instance.\n");
-
-	controller->type = XHCI;
-
-	controller->start = xhci_start;
-	controller->stop = xhci_stop;
-	controller->reset = xhci_reset;
-	controller->init = xhci_reinit;
-	controller->shutdown = xhci_shutdown;
-	controller->bulk = xhci_bulk;
-	controller->control = xhci_control;
-	controller->create_intr_queue = xhci_create_intr_queue;
-	controller->destroy_intr_queue = xhci_destroy_intr_queue;
-	controller->poll_intr_queue = xhci_poll_intr_queue;
-	for (i = 0; i < 128; i++) {
-		controller->devices[i] = 0;
-	}
-	init_device_entry (controller, 0);
-	XHCI_INST (controller)->roothub = controller->devices[0];
-
-	controller->bus_address = addr;
-	controller->reg_base = (u32)phys_to_virt(pci_read_config32 (controller->bus_address, 0x10) & ~0xf);
-	//controller->reg_base = pci_read_config32 (controller->bus_address, 0x14) & ~0xf;
-	if (pci_read_config32 (controller->bus_address, 0x14) > 0) {
-		fatal("We don't do 64bit addressing.\n");
-	}
-	usb_debug("regbase: %lx\n", controller->reg_base);
-
-	XHCI_INST (controller)->capreg = (void*)controller->reg_base;
-	XHCI_INST (controller)->opreg = (void*)(controller->reg_base + XHCI_INST (controller)->capreg->caplength);
-	XHCI_INST (controller)->hcrreg = (void*)(controller->reg_base + XHCI_INST (controller)->capreg->rtsoff);
-	XHCI_INST (controller)->dbreg = (void*)(controller->reg_base + XHCI_INST (controller)->capreg->dboff);
-	usb_debug("caplen: %lx\nrtsoff: %lx\ndboff: %lx\n", XHCI_INST (controller)->capreg->caplength, XHCI_INST (controller)->capreg->rtsoff, XHCI_INST (controller)->capreg->dboff);
-	usb_debug("caplength: %x\n", XHCI_INST (controller)->capreg->caplength);
-	usb_debug("hciversion: %x.%x\n", XHCI_INST (controller)->capreg->hciver_hi, XHCI_INST (controller)->capreg->hciver_lo);
-	if ((XHCI_INST (controller)->capreg->hciversion < 0x96) || (XHCI_INST (controller)->capreg->hciversion > 0x100)) {
-		fatal("Unsupported xHCI version\n");
-	}
-	usb_debug("maxslots: %x\n", XHCI_INST (controller)->capreg->MaxSlots);
-	usb_debug("maxports: %x\n", XHCI_INST (controller)->capreg->MaxPorts);
-	int pagesize = XHCI_INST (controller)->opreg->pagesize << 12;
-	usb_debug("pagesize: %x\n", pagesize);
-
-	XHCI_INST (controller)->dcbaa = memalign(64, (XHCI_INST (controller)->capreg->MaxSlots+1)*sizeof(devctxp_t));
-	memset((void*)XHCI_INST (controller)->dcbaa, 0, (XHCI_INST (controller)->capreg->MaxSlots+1)*sizeof(devctxp_t));
-
-	usb_debug("max scratchpad bufs: %x\n", XHCI_INST (controller)->capreg->Max_Scratchpad_Bufs);
-	if (XHCI_INST (controller)->capreg->Max_Scratchpad_Bufs > 0) {
-		XHCI_INST (controller)->dcbaa->ptr = memalign(64, XHCI_INST (controller)->capreg->Max_Scratchpad_Bufs * 8);
-	}
-
-	XHCI_INST (controller)->opreg->dcbaap_lo = virt_to_phys(XHCI_INST (controller)->dcbaa);
-	XHCI_INST (controller)->opreg->dcbaap_hi = 0;
-
-	usb_debug("waiting for controller to be ready - ");
-	while ((XHCI_INST (controller)->opreg->usbsts & USBSTS_CNR) != 0) mdelay(1);
-	usb_debug("ok.\n");
+	/* First, allocate and initialize static controller structures */
+
+	hci_t *const controller = new_controller();
+	if (!controller) {
+		xhci_debug("Could not create USB controller instance\n");
+		return controller;
+	}
+
+	controller->type		= XHCI;
+	controller->start		= xhci_start;
+	controller->stop		= xhci_stop;
+	controller->reset		= xhci_reset;
+	controller->init		= xhci_reinit;
+	controller->shutdown		= xhci_shutdown;
+	controller->bulk		= xhci_bulk;
+	controller->control		= xhci_control;
+	controller->set_address		= xhci_set_address;
+	controller->finish_device_config= xhci_finish_device_config;
+	controller->destroy_device	= xhci_destroy_dev;
+	controller->create_intr_queue	= xhci_create_intr_queue;
+	controller->destroy_intr_queue	= xhci_destroy_intr_queue;
+	controller->poll_intr_queue	= xhci_poll_intr_queue;
+	for (i = 0; i < 128; ++i) {
+		controller->devices[i] = NULL;
+	}
+
+	controller->instance = malloc(sizeof(xhci_t));
+	if (!controller->instance) {
+		xhci_debug("Out of memory creating xHCI controller instance\n");
+		goto _free_controller;
+	}
+	xhci_t *const xhci = (xhci_t *)controller->instance;
+	memset(xhci, 0x00, sizeof(*xhci));
+
+	init_device_entry(controller, 0);
+	xhci->roothub = controller->devices[0];
+	xhci->cr.ring = xhci_align(64, COMMAND_RING_SIZE * sizeof(trb_t));
+	xhci->er.ring = xhci_align(64, EVENT_RING_SIZE * sizeof(trb_t));
+	xhci->ev_ring_table = xhci_align(64, sizeof(erst_entry_t));
+	if (!xhci->roothub || !xhci->cr.ring ||
+			!xhci->er.ring || !xhci->ev_ring_table) {
+		xhci_debug("Out of memory\n");
+		goto _free_xhci;
+	}
+
+	/* Now, gather information and check for compatibility */
+
+	controller->bus_address	= addr;
+	controller->reg_base	= pci_read_config32(addr, REG_BAR0) & ~0xf;
+	if (pci_read_config32(addr, REG_BAR1) > 0) {
+		xhci_debug("We don't do 64bit addressing\n");
+		goto _free_xhci;
+	}
+
+	xhci->capreg	= phys_to_virt(controller->reg_base);
+	xhci->opreg	= ((void *)xhci->capreg) + xhci->capreg->caplength;
+	xhci->hcrreg	= ((void *)xhci->capreg) + xhci->capreg->rtsoff;
+	xhci->dbreg	= ((void *)xhci->capreg) + xhci->capreg->dboff;
+	xhci_debug("regbase: 0x%"PRIx32"\n", controller->reg_base);
+	xhci_debug("caplen:  0x%"PRIx32"\n", xhci->capreg->caplength);
+	xhci_debug("rtsoff:  0x%"PRIx32"\n", xhci->capreg->rtsoff);
+	xhci_debug("dboff:   0x%"PRIx32"\n", xhci->capreg->dboff);
+
+	xhci_debug("hciversion: %"PRIx8".%"PRIx8"\n",
+		   xhci->capreg->hciver_hi, xhci->capreg->hciver_lo);
+	if ((xhci->capreg->hciversion < 0x96) ||
+			(xhci->capreg->hciversion > 0x100)) {
+		xhci_debug("Unsupported xHCI version\n");
+		goto _free_xhci;
+	}
+
+	xhci_debug("context size: %dB\n", xhci->capreg->csz ? 64 : 32);
+	if (xhci->capreg->csz) {
+		xhci_debug("Only 32B contexts are supported\n");
+		goto _free_xhci;
+	}
+
+	xhci_debug("maxslots: 0x%02lx\n", xhci->capreg->MaxSlots);
+	xhci_debug("maxports: 0x%02lx\n", xhci->capreg->MaxPorts);
+	const unsigned pagesize = xhci->opreg->pagesize << 12;
+	xhci_debug("pagesize: 0x%04x\n", pagesize);
+
+	/*
+	 * We haven't touched the hardware yet. So we allocate all dynamic
+	 * structures at first and can still chicken out easily if we run out
+	 * of memory.
+	 */
+	const size_t dcbaa_size = (xhci->capreg->MaxSlots + 1) * sizeof(u64);
+	xhci->dcbaa = xhci_align(64, dcbaa_size);
+	if (!xhci->dcbaa) {
+		xhci_debug("Out of memory\n");
+		goto _free_xhci;
+	}
+	memset((void*)xhci->dcbaa, 0x00, dcbaa_size);
+
+	/*
+	 * Let dcbaa[0] point to another array of pointers, sp_ptrs.
+	 * The pointers therein point to scratchpad buffers (pages).
+	 */
+	const size_t max_sp_bufs = xhci->capreg->Max_Scratchpad_Bufs;
+	xhci_debug("max scratchpad bufs: 0x%zx\n", max_sp_bufs);
+	if (max_sp_bufs) {
+		const size_t sp_ptrs_size = max_sp_bufs * sizeof(u64);
+		xhci->sp_ptrs = xhci_align(64, sp_ptrs_size);
+		if (!xhci->sp_ptrs) {
+			xhci_debug("Out of memory\n");
+			goto _free_xhci_structs;
+		}
+		memset(xhci->sp_ptrs, 0x00, sp_ptrs_size);
+		for (i = 0; i < max_sp_bufs; ++i) {
+			/* Could use mmap() here if we had it.
+			   Maybe there is another way. */
+			void *const page = memalign(pagesize, pagesize);
+			if (!page) {
+				xhci_debug("Out of memory\n");
+				goto _free_xhci_structs;
+			}
+			xhci->sp_ptrs[i] = virt_to_phys(page);
+		}
+		xhci->dcbaa[0] = virt_to_phys(xhci->sp_ptrs);
+	}
+
+	/* Now start working on the hardware */
+
+	if (xhci_wait_ready(xhci))
+		goto _free_xhci;
+
+	/* TODO: Check if BIOS claims ownership (and hand over) */
 
-	usb_debug("ERST Max: %lx -> %lx entries\n", XHCI_INST (controller)->capreg->ERST_Max, 1<<(XHCI_INST (controller)->capreg->ERST_Max));
-
-	// enable all available slots
-	XHCI_INST (controller)->opreg->config = XHCI_INST (controller)->capreg->MaxSlots & CONFIG_MASK_MaxSlotsEn;
-
-	XHCI_INST (controller)->cmd_ring = memalign(64, 16*sizeof(trb_t)); /* TODO: make sure not to cross 64k page boundary */
-	memset((void*)XHCI_INST (controller)->cmd_ring, 0, 16*sizeof(trb_t));
-
-	XHCI_INST (controller)->ev_ring = memalign(64, 16*sizeof(trb_t)); /* TODO: make sure not to cross 64k page boundary */
-	memset((void*)XHCI_INST (controller)->ev_ring, 0, 16*sizeof(trb_t));
-
-	XHCI_INST (controller)->ev_ring_table = memalign(64, sizeof(erst_entry_t));
-	memset((void*)XHCI_INST (controller)->ev_ring_table, 0, sizeof(erst_entry_t));
-	XHCI_INST (controller)->ev_ring_table[0].seg_base_lo = virt_to_phys(XHCI_INST (controller)->ev_ring);
-	XHCI_INST (controller)->ev_ring_table[0].seg_base_hi = 0;
-	XHCI_INST (controller)->ev_ring_table[0].seg_size = 16;
-
-	// init command ring
-	XHCI_INST (controller)->opreg->crcr_lo = virt_to_phys(XHCI_INST (controller)->cmd_ring) | CRCR_RCS;
-	XHCI_INST (controller)->opreg->crcr_hi = 0;
-	XHCI_INST (controller)->cmd_ccs = 1;
-	XHCI_INST (controller)->ev_ccs = 1;
-
-	// init primary interrupter
-	XHCI_INST (controller)->hcrreg->intrrs[0].erstsz = 1;
-	XHCI_INST (controller)->hcrreg->intrrs[0].erdp_lo = virt_to_phys(XHCI_INST (controller)->ev_ring);
-	XHCI_INST (controller)->hcrreg->intrrs[0].erdp_hi = 0;
-	XHCI_INST (controller)->hcrreg->intrrs[0].erstba_lo = virt_to_phys(XHCI_INST (controller)->ev_ring_table);
-	XHCI_INST (controller)->hcrreg->intrrs[0].erstba_hi = 0;
-
-	XHCI_INST (controller)->opreg->usbcmd |= USBCMD_RS; /* start USB controller */
-	XHCI_INST (controller)->dbreg[0] = 0; // and tell controller to consume commands
-
-	/* TODO: TEST */
-	// setup noop command
-	trb_t *cmd = &XHCI_INST (controller)->cmd_ring[0];
-	((u32*)cmd)[3] = 1-XHCI_INST (controller)->cmd_ccs; // disable command descriptor
-	((u32*)cmd)[0] = 0;
-	((u32*)cmd)[1] = 0;
-	((u32*)cmd)[2] = 0;
-	cmd->cmd_No_Op.TRB_Type = TRB_CMD_NOOP;
-
-	// ring the HC doorbell
-	usb_debug("Posting command at %lx\n", virt_to_phys(cmd));
-	cmd->cmd_No_Op.C = XHCI_INST (controller)->cmd_ccs; // enable command
-	XHCI_INST (controller)->dbreg[0] = 0; // and tell controller to consume commands
-
-	// wait for result in event ring
-	trb_t *ev = &XHCI_INST (controller)->ev_ring[0];
-	trb_t *ev1 = &XHCI_INST (controller)->ev_ring[1];
-	while (ev->event_cmd_cmpl.C != XHCI_INST (controller)->ev_ccs) {
-		usb_debug("CRCR: %lx, USBSTS: %lx\n",  XHCI_INST (controller)->opreg->crcr_lo, XHCI_INST (controller)->opreg->usbsts);
-		usb_debug("ev0.C %x, ev1.C %x\n", ev->event_cmd_cmpl.C, ev1->event_cmd_cmpl.C);
-		mdelay(100);
-	}
-	usb_debug("command ring is %srunning\n", (XHCI_INST (controller)->opreg->crcr_lo & CRCR_CRR)?"":"not ");
-	switch (ev->event_cmd_cmpl.TRB_Type) {
-		case TRB_EV_CMD_CMPL:
-			usb_debug("Completed command TRB at %lx. Code: %d\n",
-				ev->event_cmd_cmpl.Cmd_TRB_Pointer_lo, ev->event_cmd_cmpl.Completion_Code);
-			break;
-		case TRB_EV_PORTSC:
-			usb_debug("Port Status Change Event. Completion Code: %d\n Port: %d. Ignoring.\n",
-				ev->event_cmd_cmpl.Completion_Code, ev->event_portsc.Port);
-			// we ignore the event as we look for the PORTSC registers instead, at a time when it suits _us_
-			break;
-		default:
-			usb_debug("Unknown event: %d, Completion Code: %d\n", ev->event_cmd_cmpl.TRB_Type, ev->event_cmd_cmpl.Completion_Code);
-			break;
-	}
-	usb_debug("CRCR: %lx, USBSTS: %lx\n",  XHCI_INST (controller)->opreg->crcr_lo, XHCI_INST (controller)->opreg->usbsts);
-	usb_debug("ev0.C %x, ev1.C %x, ev1.CC %d\n", ev->event_cmd_cmpl.C, ev1->event_cmd_cmpl.C, ev1->event_cmd_cmpl.Completion_Code);
-
-	controller->devices[0]->controller = controller;
-	controller->devices[0]->init = xhci_rh_init;
-	controller->devices[0]->init (controller->devices[0]);
+	xhci_reset(controller);
+	xhci_reinit(controller);
+
+	xhci_switch_ppt_ports(addr);
+
+	xhci->roothub->controller = controller;
+	xhci->roothub->init = xhci_rh_init;
+	xhci->roothub->init(xhci->roothub);
 
 	return controller;
+
+_free_xhci_structs:
+	if (xhci->sp_ptrs) {
+		for (i = 0; i < max_sp_bufs; ++i) {
+			if (xhci->sp_ptrs[i])
+				free(phys_to_virt(xhci->sp_ptrs[i]));
+		}
+	}
+	free(xhci->sp_ptrs);
+	free(xhci->dcbaa);
+_free_xhci:
+	free((void *)xhci->ev_ring_table);
+	free((void *)xhci->er.ring);
+	free((void *)xhci->cr.ring);
+	free(xhci->roothub);
+	free(xhci);
+_free_controller:
+	detach_controller(controller);
+	free(controller);
+	return NULL;
+}
+
+static void
+xhci_reset(hci_t *const controller)
+{
+	xhci_t *const xhci = XHCI_INST(controller);
+
+	xhci_stop(controller);
+
+	xhci->opreg->usbcmd |= USBCMD_HCRST;
+	xhci_debug("Resetting controller... ");
+	if (!xhci_handshake(&xhci->opreg->usbcmd, USBCMD_HCRST, 0, 1000000L))
+		usb_debug("timeout!\n");
+	else
+		usb_debug("ok.\n");
 }
 
 static void
-xhci_shutdown (hci_t *controller)
+xhci_reinit (hci_t *controller)
 {
+	xhci_t *const xhci = XHCI_INST(controller);
+
+	if (xhci_wait_ready(xhci))
+		return;
+
+	/* Enable all available slots */
+	xhci->opreg->config = xhci->capreg->MaxSlots & CONFIG_MASK_MaxSlotsEn;
+	xhci->max_slots_en = xhci->capreg->MaxSlots & CONFIG_MASK_MaxSlotsEn;
+
+	/* Set DCBAA */
+	xhci->opreg->dcbaap_lo = virt_to_phys(xhci->dcbaa);
+	xhci->opreg->dcbaap_hi = 0;
+
+	/* Initialize command ring */
+	xhci_init_cycle_ring(&xhci->cr, COMMAND_RING_SIZE);
+	xhci_debug("command ring @%p (0x%08x)\n",
+		   xhci->cr.ring, virt_to_phys(xhci->cr.ring));
+	xhci->opreg->crcr_lo = virt_to_phys(xhci->cr.ring) | CRCR_RCS;
+	xhci->opreg->crcr_hi = 0;
+
+	/* Make sure interrupts are disabled */
+	xhci->opreg->usbcmd &= ~USBCMD_INTE;
+
+	/* Initialize event ring */
+	xhci_reset_event_ring(&xhci->er);
+	xhci_debug("event ring @%p (0x%08x)\n",
+		   xhci->er.ring, virt_to_phys(xhci->er.ring));
+	xhci_debug("ERST Max: 0x%lx ->  0x%lx entries\n",
+		   xhci->capreg->ERST_Max, 1 << xhci->capreg->ERST_Max);
+	memset((void*)xhci->ev_ring_table, 0x00, sizeof(erst_entry_t));
+	xhci->ev_ring_table[0].seg_base_lo = virt_to_phys(xhci->er.ring);
+	xhci->ev_ring_table[0].seg_base_hi = 0;
+	xhci->ev_ring_table[0].seg_size = EVENT_RING_SIZE;
+
+	/* Initialize primary interrupter */
+	xhci->hcrreg->intrrs[0].erstsz = 1;
+	xhci_update_event_dq(xhci);
+	/* erstba has to be written at last */
+	xhci->hcrreg->intrrs[0].erstba_lo = virt_to_phys(xhci->ev_ring_table);
+	xhci->hcrreg->intrrs[0].erstba_hi = 0;
+
+	xhci_start(controller);
+
+#ifdef USB_DEBUG
+	int i;
+	for (i = 0; i < 32; ++i) {
+		xhci_debug("NOOP run #%d\n", i);
+		trb_t *const cmd = xhci_next_command_trb(xhci);
+		TRB_SET(TT, cmd, TRB_CMD_NOOP);
+
+		xhci_post_command(xhci);
+
+		/* Wait for result in event ring */
+		xhci_wait_for_command_done(xhci, cmd, 1);
+		xhci_debug("Command ring is %srunning\n",
+			   (xhci->opreg->crcr_lo & CRCR_CRR) ? "" : "not ");
+	}
+#endif
+}
+
+static void
+xhci_shutdown(hci_t *const controller)
+{
+	int i;
+
 	if (controller == 0)
 		return;
-	detach_controller (controller);
-	XHCI_INST (controller)->roothub->destroy (XHCI_INST (controller)->
-						  roothub);
-	/* TODO: stop hardware, kill data structures */
-	free (XHCI_INST (controller));
-	free (controller);
+	xhci_t *const xhci = XHCI_INST(controller);
+
+	detach_controller(controller);
+
+	/* Detach device hierarchy (starting at root hub) */
+	usb_detach_device(controller, 0);
+
+	xhci_stop(controller);
+
+	if (xhci->sp_ptrs) {
+		const size_t max_sp_bufs = xhci->capreg->Max_Scratchpad_Bufs;
+		for (i = 0; i < max_sp_bufs; ++i) {
+			if (xhci->sp_ptrs[i])
+				free(phys_to_virt(xhci->sp_ptrs[i]));
+		}
+	}
+	free(xhci->sp_ptrs);
+	free(xhci->dcbaa);
+	free((void *)xhci->ev_ring_table);
+	free((void *)xhci->er.ring);
+	free((void *)xhci->cr.ring);
+	free(xhci);
+	free(controller);
 }
 
 static void
 xhci_start (hci_t *controller)
 {
+	xhci_t *const xhci = XHCI_INST(controller);
+
+	xhci->opreg->usbcmd |= USBCMD_RS;
+	if (!xhci_handshake(&xhci->opreg->usbsts, USBSTS_HCH, 0, 1000000L))
+		xhci_debug("Controller didn't start within 1s\n");
 }
 
 static void
 xhci_stop (hci_t *controller)
 {
+	xhci_t *const xhci = XHCI_INST(controller);
+
+	xhci->opreg->usbcmd &= ~USBCMD_RS;
+	if (!xhci_handshake(&xhci->opreg->usbsts,
+			    USBSTS_HCH, USBSTS_HCH, 1000000L))
+		xhci_debug("Controller didn't halt within 1s\n");
 }
 
 static int
-xhci_control (usbdev_t *dev, direction_t dir, int drlen, void *devreq, int dalen,
-	      unsigned char *data)
+xhci_reset_endpoint(usbdev_t *const dev, endpoint_t *const ep,
+		    const int clear_halt)
+{
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	devinfo_t *const di = DEVINFO_FROM_XHCI(xhci, dev->address);
+	const int slot_id = dev->address;
+	const int ep_id = ep ? xhci_ep_id(ep) : 1;
+
+	xhci_debug("Resetting ID %d EP %d (ep state: %d)\n",
+		   slot_id, ep_id, EC_GET(STATE, di->devctx.eps[ep_id]));
+
+	/* Run Reset Endpoint Command if the EP is in Halted state */
+	if (EC_GET(STATE, di->devctx.eps[ep_id]) == 2) {
+		const int cc = xhci_cmd_reset_endpoint(xhci, slot_id, ep_id);
+		if (cc != CC_SUCCESS) {
+			xhci_debug("Reset Endpoint Command failed: %d\n", cc);
+			return 1;
+		}
+	}
+
+	/* Clear TT buffer for bulk and control endpoints behind a TT */
+	const int hub = dev->hub;
+	if (hub && dev->speed < HIGH_SPEED &&
+			dev->controller->devices[hub]->speed == HIGH_SPEED)
+		/* TODO */;
+
+	/* Try clearing the device' halt condition on non-control endpoints */
+	if (clear_halt && ep)
+		clear_stall(ep);
+
+	/* Reset transfer ring if the endpoint is in the right state */
+	const unsigned ep_state = EC_GET(STATE, di->devctx.eps[ep_id]);
+	if (ep_state == 3 || ep_state == 4) {
+		transfer_ring_t *const tr = di->transfer_rings[ep_id];
+		const int cc = xhci_cmd_set_tr_dq(xhci, slot_id, ep_id,
+						  tr->ring, 1);
+		if (cc != CC_SUCCESS) {
+			xhci_debug("Set TR Dequeue Command failed: %d\n", cc);
+			return 1;
+		}
+		xhci_init_cycle_ring(tr, TRANSFER_RING_SIZE);
+	}
+
+	xhci_debug("Finished resetting ID %d EP %d (ep state: %d)\n",
+		   slot_id, ep_id, EC_GET(STATE, di->devctx.eps[ep_id]));
+
+	return 0;
+}
+
+static void
+xhci_enqueue_trb(transfer_ring_t *const tr)
+{
+	const int chain = TRB_GET(CH, tr->cur);
+	TRB_SET(C, tr->cur, tr->pcs);
+	++tr->cur;
+
+	while (TRB_GET(TT, tr->cur) == TRB_LINK) {
+		xhci_spew("Handling LINK pointer\n");
+		const int tc = TRB_GET(TC, tr->cur);
+		TRB_SET(CH, tr->cur, chain);
+		TRB_SET(C, tr->cur, tr->pcs);
+		tr->cur = phys_to_virt(tr->cur->ptr_low);
+		if (tc)
+			tr->pcs ^= 1;
+	}
+}
+
+static void
+xhci_enqueue_td(transfer_ring_t *const tr, const int ep, const size_t mps,
+		const int dalen, void *const data, const int dir)
 {
-	return 1;
+	trb_t *trb = NULL;				/* cur TRB */
+	u8 *cur_start = data;				/* cur data pointer */
+	size_t length = dalen;				/* remaining bytes */
+	size_t packets = (length + mps - 1) / mps;	/* remaining packets */
+	size_t residue = 0;				/* residue from last TRB */
+	size_t trb_count = 0;				/* TRBs added so far */
+
+	while (length || !trb_count /* enqueue at least one */) {
+		const size_t cur_end = ((size_t)cur_start + 0x10000) & ~0xffff;
+		size_t cur_length = cur_end - (size_t)cur_start;
+		if (length < cur_length) {
+			cur_length = length;
+			packets = 0;
+			length = 0;
+		} else {
+			packets -= (residue + cur_length) / mps;
+			residue = (residue + cur_length) % mps;
+			length -= cur_length;
+		}
+
+		trb = tr->cur;
+		xhci_clear_trb(trb, tr->pcs);
+		trb->ptr_low = virt_to_phys(cur_start);
+		TRB_SET(TL, trb, cur_length);
+		TRB_SET(TDS, trb, packets);
+
+		/* Check for first, data stage TRB */
+		if (!trb_count && ep == 1) {
+			TRB_SET(DIR, trb, dir);
+			TRB_SET(TT, trb, TRB_DATA_STAGE);
+		} else {
+			TRB_SET(TT, trb, TRB_NORMAL);
+		}
+
+		/* Check for last TRB */
+		if (!length)
+			TRB_SET(IOC, trb, 1);
+		else
+			TRB_SET(CH, trb, 1);
+
+		xhci_enqueue_trb(tr);
+
+		cur_start += cur_length;
+		++trb_count;
+	}
+}
+
+static int
+xhci_control(usbdev_t *const dev, const direction_t dir,
+	     const int drlen, void *const devreq,
+	     const int dalen, unsigned char *const data)
+{
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	devinfo_t *const di = DEVINFO_FROM_XHCI(xhci, dev->address);
+	transfer_ring_t *const tr = di->transfer_rings[1];
+
+	const size_t off = (size_t)data & 0xffff;
+	if ((off + dalen) > ((TRANSFER_RING_SIZE - 3) << 16)) {
+		xhci_debug("Unsupported transfer size\n");
+		return 1;
+	}
+
+	/* Reset endpoint if it's halted */
+	const unsigned ep_state = EC_GET(STATE, di->devctx.ep0);
+	if (ep_state == 2 || ep_state == 4) {
+		if (xhci_reset_endpoint(dev, NULL, 0))
+			return 1;
+	}
+
+	/* Fill and enqueue setup TRB */
+	trb_t *const setup = tr->cur;
+	xhci_clear_trb(setup, tr->pcs);
+	setup->ptr_low = ((u32 *)devreq)[0];
+	setup->ptr_high = ((u32 *)devreq)[1];
+	TRB_SET(TL, setup, 8);
+	TRB_SET(TRT, setup, (dalen)
+			? ((dir == OUT) ? TRB_TRT_OUT_DATA : TRB_TRT_IN_DATA)
+			: TRB_TRT_NO_DATA);
+	TRB_SET(TT, setup, TRB_SETUP_STAGE);
+	TRB_SET(IDT, setup, 1);
+	TRB_SET(IOC, setup, 1);
+	xhci_enqueue_trb(tr);
+
+	/* Fill and enqueue data TRBs (if any) */
+	if (dalen) {
+		const unsigned mps = EC_GET(MPS, di->devctx.ep0);
+		const unsigned dt_dir = (dir == OUT) ? TRB_DIR_OUT : TRB_DIR_IN;
+		xhci_enqueue_td(tr, 1, mps, dalen, data, dt_dir);
+	}
+
+	/* Fill status TRB */
+	trb_t *const status = tr->cur;
+	xhci_clear_trb(status, tr->pcs);
+	TRB_SET(DIR, status, (dir == OUT) ? TRB_DIR_IN : TRB_DIR_OUT);
+	TRB_SET(TT, status, TRB_STATUS_STAGE);
+	TRB_SET(IOC, status, 1);
+	xhci_enqueue_trb(tr);
+
+	/* Ring doorbell for EP0 */
+	xhci->dbreg[dev->address] = 1;
+
+	/* Wait for transfer events */
+	int i;
+	const int n_stages = 2 + !!dalen;
+	for (i = 0; i < n_stages; ++i) {
+		const int ret = xhci_wait_for_transfer(xhci, dev->address, 1);
+		if (ret != CC_SUCCESS) {
+			if (ret == TIMEOUT) {
+				xhci_debug("Stopping ID %d EP 1\n",
+					   dev->address);
+				xhci_cmd_stop_endpoint(xhci, dev->address, 1);
+			}
+			xhci_debug("Stage %d/%d failed: %d\n"
+				   "  trb ring:   @%p\n"
+				   "  setup trb:  @%p\n"
+				   "  status trb: @%p\n"
+				   "  ep state:   %d -> %d\n"
+				   "  usbsts:     0x%08"PRIx32"\n",
+				   i, n_stages, ret,
+				   tr->ring, setup, status,
+				   ep_state, EC_GET(STATE, di->devctx.ep0),
+				   xhci->opreg->usbsts);
+			return 1;
+		}
+	}
+
+	return 0;
 }
 
 /* finalize == 1: if data is of packet aligned size, add a zero length packet */
 static int
-xhci_bulk (endpoint_t *ep, int size, u8 *data, int finalize)
+xhci_bulk(endpoint_t *const ep,
+	  const int size, u8 *const data,
+	  const int finalize)
 {
-	int maxpsize = ep->maxpacketsize;
-	if (maxpsize == 0)
-		fatal("MaxPacketSize == 0!!!");
-	return 1;
+	/* finalize: Hopefully the xHCI controller always does this.
+		     We have no control over the packets. */
+
+	xhci_t *const xhci = XHCI_INST(ep->dev->controller);
+	const int ep_id = xhci_ep_id(ep);
+	devinfo_t *const di = DEVINFO_FROM_XHCI(xhci, ep->dev->address);
+	transfer_ring_t *const tr = di->transfer_rings[ep_id];
+
+	const size_t off = (size_t)data & 0xffff;
+	if ((off + size) > ((TRANSFER_RING_SIZE - 1) << 16)) {
+		xhci_debug("Unsupported transfer size\n");
+		return 1;
+	}
+
+	/* Reset endpoint if it's halted */
+	const unsigned ep_state = EC_GET(STATE, di->devctx.eps[ep_id]);
+	if (ep_state == 2 || ep_state == 4) {
+		if (xhci_reset_endpoint(ep->dev, ep, 0))
+			return 1;
+	}
+
+	/* Enqueue transfer and ring doorbell */
+	const unsigned mps = EC_GET(MPS, di->devctx.eps[ep_id]);
+	const unsigned dir = (ep->direction == OUT) ? TRB_DIR_OUT : TRB_DIR_IN;
+	xhci_enqueue_td(tr, ep_id, mps, size, data, dir);
+	xhci->dbreg[ep->dev->address] = ep_id;
+
+	/* Wait for transfer event */
+	const int ret = xhci_wait_for_transfer(xhci, ep->dev->address, ep_id);
+	if (ret != CC_SUCCESS) {
+		if (ret == TIMEOUT) {
+			xhci_debug("Stopping ID %d EP %d\n",
+				   ep->dev->address, ep_id);
+			xhci_cmd_stop_endpoint(xhci, ep->dev->address, ep_id);
+		} else if (ret == CC_STALL_ERROR) {
+			xhci_reset_endpoint(ep->dev, ep, 1);
+		}
+		xhci_debug("Bulk transfer failed: %d\n"
+			   "  ep state: %d -> %d\n"
+			   "  usbsts:   0x%08"PRIx32"\n",
+			   ret, ep_state,
+			   EC_GET(STATE, di->devctx.eps[ep_id]),
+			   xhci->opreg->usbsts);
+		return 1;
+	}
+
+	return 0;
+}
+
+static trb_t *
+xhci_next_trb(trb_t *cur, int *const pcs)
+{
+	++cur;
+	while (TRB_GET(TT, cur) == TRB_LINK) {
+		if (pcs && TRB_GET(TC, cur))
+			*pcs ^= 1;
+		cur = phys_to_virt(cur->ptr_low);
+	}
+	return cur;
 }
 
 /* create and hook-up an intr queue into device schedule */
-static void*
-xhci_create_intr_queue (endpoint_t *ep, int reqsize, int reqcount, int reqtiming)
+static void *
+xhci_create_intr_queue(endpoint_t *const ep,
+		       const int reqsize, const int reqcount,
+		       const int reqtiming)
 {
+	/* reqtiming: We ignore it and use the interval from the
+		      endpoint descriptor configured earlier. */
+
+	xhci_t *const xhci = XHCI_INST(ep->dev->controller);
+	const int ep_id = xhci_ep_id(ep);
+	devinfo_t *const di = DEVINFO_FROM_XHCI(xhci, ep->dev->address);
+	transfer_ring_t *const tr = di->transfer_rings[ep_id];
+
+	if (reqcount > (TRANSFER_RING_SIZE - 2)) {
+		xhci_debug("reqcount is too high, at most %d supported\n",
+			   TRANSFER_RING_SIZE - 2);
+		return NULL;
+	}
+	if (reqsize > 0x10000) {
+		xhci_debug("reqsize is too large, at most 64KiB supported\n");
+		return NULL;
+	}
+	if (di->interrupt_queues[ep_id]) {
+		xhci_debug("Only one interrupt queue per endpoint supported\n");
+		return NULL;
+	}
+
+	/* Allocate intrq structure and reqdata chunks */
+
+	intrq_t *const intrq = malloc(sizeof(*intrq));
+	if (!intrq) {
+		xhci_debug("Out of memory\n");
+		return NULL;
+	}
+
+	int i;
+	int pcs = tr->pcs;
+	trb_t *cur = tr->cur;
+	for (i = 0; i < reqcount; ++i) {
+		if (TRB_GET(C, cur) == pcs) {
+			xhci_debug("Not enough empty TRBs\n");
+			goto _free_return;
+		}
+		void *const reqdata = xhci_align(1, reqsize);
+		if (!reqdata) {
+			xhci_debug("Out of memory\n");
+			goto _free_return;
+		}
+		xhci_clear_trb(cur, pcs);
+		cur->ptr_low = virt_to_phys(reqdata);
+		cur->ptr_high = 0;
+		TRB_SET(TL,	cur, reqsize);
+		TRB_SET(TT,	cur, TRB_NORMAL);
+		TRB_SET(ISP,	cur, 1);
+		TRB_SET(IOC,	cur, 1);
+
+		cur = xhci_next_trb(cur, &pcs);
+	}
+
+	intrq->size	= reqsize;
+	intrq->count	= reqcount;
+	intrq->next	= tr->cur;
+	intrq->ready	= NULL;
+	intrq->ep	= ep;
+	di->interrupt_queues[ep_id] = intrq;
+
+	/* Now enqueue all the prepared TRBs but the last
+	   and ring the doorbell. */
+	for (i = 0; i < (reqcount - 1); ++i)
+		xhci_enqueue_trb(tr);
+	xhci->dbreg[ep->dev->address] = ep_id;
+
+	return intrq;
+
+_free_return:
+	cur = tr->cur;
+	for (--i; i >= 0; --i) {
+		free(phys_to_virt(cur->ptr_low));
+		cur = xhci_next_trb(cur, NULL);
+	}
+	free(intrq);
 	return NULL;
 }
 
 /* remove queue from device schedule, dropping all data that came in */
 static void
-xhci_destroy_intr_queue (endpoint_t *ep, void *q_)
+xhci_destroy_intr_queue(endpoint_t *const ep, void *const q)
 {
-	//free(q);
+	xhci_t *const xhci = XHCI_INST(ep->dev->controller);
+	const int ep_id = xhci_ep_id(ep);
+	devinfo_t *const di = DEVINFO_FROM_XHCI(xhci, ep->dev->address);
+	transfer_ring_t *const tr = di->transfer_rings[ep_id];
+
+	intrq_t *const intrq = (intrq_t *)q;
+
+	/* Make sure the endpoint is stopped */
+	if (EC_GET(STATE, di->devctx.eps[ep_id]) == 1) {
+		const int cc = xhci_cmd_stop_endpoint(
+				xhci, ep->dev->address, ep_id);
+		if (cc != CC_SUCCESS)
+			xhci_debug("Warning: Failed to stop endpoint\n");
+	}
+
+	/* Process all remaining transfer events */
+	xhci_handle_events(xhci);
+
+	/* Free all pending transfers and the interrupt queue structure */
+	int i;
+	for (i = 0; i < intrq->count; ++i) {
+		free(phys_to_virt(intrq->next->ptr_low));
+		intrq->next = xhci_next_trb(intrq->next, NULL);
+	}
+	di->interrupt_queues[ep_id] = NULL;
+	free((void *)intrq);
+
+	/* Reset the controller's dequeue pointer and reinitialize the ring */
+	xhci_cmd_set_tr_dq(xhci, ep->dev->address, ep_id, tr->ring, 1);
+	xhci_init_cycle_ring(tr, TRANSFER_RING_SIZE);
 }
 
 /* read one intr-packet from queue, if available. extend the queue for new input.
    return NULL if nothing new available.
    Recommended use: while (data=poll_intr_queue(q)) process(data);
  */
-static u8*
-xhci_poll_intr_queue (void *q_)
+static u8 *
+xhci_poll_intr_queue(void *const q)
 {
-	return NULL;
+	if (!q)
+		return NULL;
+
+	intrq_t *const intrq = (intrq_t *)q;
+	endpoint_t *const ep = intrq->ep;
+	xhci_t *const xhci = XHCI_INST(ep->dev->controller);
+
+	/* TODO: Reset interrupt queue if it gets halted? */
+
+	xhci_handle_events(xhci);
+
+	u8 *reqdata = NULL;
+	while (!reqdata && intrq->ready) {
+		const int ep_id = xhci_ep_id(ep);
+		devinfo_t *const di = DEVINFO_FROM_XHCI(xhci, ep->dev->address);
+		transfer_ring_t *const tr = di->transfer_rings[ep_id];
+
+		/* Fetch the request's buffer */
+		reqdata = phys_to_virt(intrq->next->ptr_low);
+
+		/* Enqueue the last (spare) TRB and ring doorbell */
+		xhci_enqueue_trb(tr);
+		xhci->dbreg[ep->dev->address] = ep_id;
+
+		/* Reuse the current buffer for the next spare TRB */
+		xhci_clear_trb(tr->cur, tr->pcs);
+		tr->cur->ptr_low = virt_to_phys(reqdata);
+		tr->cur->ptr_high = 0;
+		TRB_SET(TL,	tr->cur, intrq->size);
+		TRB_SET(TT,	tr->cur, TRB_NORMAL);
+		TRB_SET(ISP,	tr->cur, 1);
+		TRB_SET(IOC,	tr->cur, 1);
+
+		/* Check if anything was transferred */
+		const size_t read = TRB_GET(TL, intrq->next);
+		if (!read)
+			reqdata = NULL;
+		else if (read < intrq->size)
+			/* At least zero it, poll interface is rather limited */
+			memset(reqdata + read, 0x00, intrq->size - read);
+
+		/* Advance the interrupt queue */
+		if (intrq->ready == intrq->next)
+			/* This was last TRB being ready */
+			intrq->ready = NULL;
+		intrq->next = xhci_next_trb(intrq->next, NULL);
+	}
+
+	return reqdata;
 }
diff --git a/payloads/libpayload/drivers/usb/xhci_commands.c b/payloads/libpayload/drivers/usb/xhci_commands.c
new file mode 100644
index 0000000000..3a744b391e
--- /dev/null
+++ b/payloads/libpayload/drivers/usb/xhci_commands.c
@@ -0,0 +1,204 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2013 secunet Security Networks AG
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <inttypes.h>
+#include <arch/virtual.h>
+#include "xhci_private.h"
+
+trb_t *
+xhci_next_command_trb(xhci_t *const xhci)
+{
+	xhci_clear_trb(xhci->cr.cur, xhci->cr.pcs);
+	return xhci->cr.cur;
+}
+
+void
+xhci_post_command(xhci_t *const xhci)
+{
+	xhci_debug("Command %d (@%p)\n",
+		   TRB_GET(TT, xhci->cr.cur), xhci->cr.cur);
+
+	TRB_SET(C, xhci->cr.cur, xhci->cr.pcs);
+	++xhci->cr.cur;
+
+	/* Ring the doorbell */
+	xhci->dbreg[0] = 0;
+
+	while (TRB_GET(TT, xhci->cr.cur) == TRB_LINK) {
+		xhci_debug("Handling LINK pointer (@%p)\n", xhci->cr.cur);
+		const int tc = TRB_GET(TC, xhci->cr.cur);
+		TRB_SET(C, xhci->cr.cur, xhci->cr.pcs);
+		xhci->cr.cur = phys_to_virt(xhci->cr.cur->ptr_low);
+		if (tc)
+			xhci->cr.pcs ^= 1;
+	}
+}
+
+static int
+xhci_wait_for_command(xhci_t *const xhci,
+		      const trb_t *const cmd_trb,
+		      const int clear_event)
+{
+	int cc;
+
+	cc = xhci_wait_for_command_done(xhci, cmd_trb, clear_event);
+	if (cc != TIMEOUT)
+		return cc;
+
+	/* Abort command on timeout */
+	xhci_debug("Aborting command (@%p), CRCR: 0x%"PRIx32"\n",
+		   cmd_trb, xhci->opreg->crcr_lo);
+	xhci->opreg->crcr_lo |= CRCR_CS | CRCR_CA;
+	xhci->opreg->crcr_hi = 0;
+	cc = xhci_wait_for_command_aborted(xhci, cmd_trb);
+
+	if (xhci->opreg->crcr_lo & CRCR_CRR)
+		fatal("xhci_wait_for_command: Command ring still running\n");
+
+	return cc;
+}
+
+/*
+ * xhci_cmd_* return >= 0: xhci completion code (cc)
+ *		      < 0: driver error code
+ */
+
+int
+xhci_cmd_enable_slot(xhci_t *const xhci, int *const slot_id)
+{
+	trb_t *const cmd = xhci_next_command_trb(xhci);
+	TRB_SET(TT, cmd, TRB_CMD_ENABLE_SLOT);
+	xhci_post_command(xhci);
+
+	int cc = xhci_wait_for_command(xhci, cmd, 0);
+	if (cc >= 0) {
+		if (cc == CC_SUCCESS) {
+			*slot_id = TRB_GET(ID, xhci->er.cur);
+			if (*slot_id > xhci->max_slots_en)
+				cc = CONTROLLER_ERROR;
+		}
+		xhci_advance_event_ring(xhci);
+		xhci_handle_events(xhci);
+	}
+	return cc;
+}
+
+int
+xhci_cmd_disable_slot(xhci_t *const xhci, const int slot_id)
+{
+	trb_t *const cmd = xhci_next_command_trb(xhci);
+	TRB_SET(TT, cmd, TRB_CMD_DISABLE_SLOT);
+	TRB_SET(ID, cmd, slot_id);
+	xhci_post_command(xhci);
+
+	return xhci_wait_for_command(xhci, cmd, 1);
+}
+
+int
+xhci_cmd_address_device(xhci_t *const xhci,
+			const int slot_id,
+			inputctx_t *const ic)
+{
+	trb_t *const cmd = xhci_next_command_trb(xhci);
+	TRB_SET(TT, cmd, TRB_CMD_ADDRESS_DEV);
+	TRB_SET(ID, cmd, slot_id);
+	cmd->ptr_low = virt_to_phys(ic);
+	xhci_post_command(xhci);
+
+	return xhci_wait_for_command(xhci, cmd, 1);
+}
+
+int
+xhci_cmd_configure_endpoint(xhci_t *const xhci,
+			    const int slot_id,
+			    const int config_id,
+			    inputctx_t *const ic)
+{
+	trb_t *const cmd = xhci_next_command_trb(xhci);
+	TRB_SET(TT, cmd, TRB_CMD_CONFIGURE_EP);
+	TRB_SET(ID, cmd, slot_id);
+	cmd->ptr_low = virt_to_phys(ic);
+	if (config_id == 0)
+		TRB_SET(DC, cmd, 1);
+	xhci_post_command(xhci);
+
+	return xhci_wait_for_command(xhci, cmd, 1);
+}
+
+int
+xhci_cmd_evaluate_context(xhci_t *const xhci,
+			  const int slot_id,
+			  inputctx_t *const ic)
+{
+	trb_t *const cmd = xhci_next_command_trb(xhci);
+	TRB_SET(TT, cmd, TRB_CMD_EVAL_CTX);
+	TRB_SET(ID, cmd, slot_id);
+	cmd->ptr_low = virt_to_phys(ic);
+	xhci_post_command(xhci);
+
+	return xhci_wait_for_command(xhci, cmd, 1);
+}
+
+int
+xhci_cmd_reset_endpoint(xhci_t *const xhci, const int slot_id, const int ep)
+{
+	trb_t *const cmd = xhci_next_command_trb(xhci);
+	TRB_SET(TT, cmd, TRB_CMD_RESET_EP);
+	TRB_SET(ID, cmd, slot_id);
+	TRB_SET(EP, cmd, ep);
+	xhci_post_command(xhci);
+
+	return xhci_wait_for_command(xhci, cmd, 1);
+}
+
+int
+xhci_cmd_stop_endpoint(xhci_t *const xhci, const int slot_id, const int ep)
+{
+	trb_t *const cmd = xhci_next_command_trb(xhci);
+	TRB_SET(TT, cmd, TRB_CMD_STOP_EP);
+	TRB_SET(ID, cmd, slot_id);
+	TRB_SET(EP, cmd, ep);
+	xhci_post_command(xhci);
+
+	return xhci_wait_for_command(xhci, cmd, 1);
+}
+
+int
+xhci_cmd_set_tr_dq(xhci_t *const xhci, const int slot_id, const int ep,
+		   trb_t *const dq_trb, const int dcs)
+{
+	trb_t *const cmd = xhci_next_command_trb(xhci);
+	TRB_SET(TT, cmd, TRB_CMD_SET_TR_DQ);
+	TRB_SET(ID, cmd, slot_id);
+	TRB_SET(EP, cmd, ep);
+	cmd->ptr_low = virt_to_phys(dq_trb) | dcs;
+	xhci_post_command(xhci);
+
+	return xhci_wait_for_command(xhci, cmd, 1);
+}
diff --git a/payloads/libpayload/drivers/usb/xhci_debug.c b/payloads/libpayload/drivers/usb/xhci_debug.c
new file mode 100644
index 0000000000..ba644c62cf
--- /dev/null
+++ b/payloads/libpayload/drivers/usb/xhci_debug.c
@@ -0,0 +1,136 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2013 secunet Security Networks AG
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <inttypes.h>
+#include "xhci_private.h"
+
+#ifdef XHCI_DUMPS
+
+void
+xhci_dump_slotctx(const slotctx_t *const sc)
+{
+	xhci_debug("Slot Context (@%p):\n", sc);
+	usb_debug(" FIELD1\t0x%08"PRIx32"\n", sc->f1);
+	usb_debug(" FIELD2\t0x%08"PRIx32"\n", sc->f2);
+	usb_debug(" FIELD3\t0x%08"PRIx32"\n", sc->f3);
+	usb_debug(" FIELD4\t0x%08"PRIx32"\n", sc->f4);
+	SC_DUMP(ROUTE,  *sc);
+	SC_DUMP(SPEED,  *sc);
+	SC_DUMP(MTT,    *sc);
+	SC_DUMP(HUB,    *sc);
+	SC_DUMP(CTXENT, *sc);
+	SC_DUMP(RHPORT, *sc);
+	SC_DUMP(NPORTS, *sc);
+	SC_DUMP(TTID,   *sc);
+	SC_DUMP(TTPORT, *sc);
+	SC_DUMP(TTT,    *sc);
+	SC_DUMP(UADDR,  *sc);
+	SC_DUMP(STATE,  *sc);
+}
+
+void
+xhci_dump_epctx(const epctx_t *const ec)
+{
+	xhci_debug("Endpoint Context (@%p):\n", ec);
+	usb_debug(" FIELD1\t0x%08"PRIx32"\n", ec->f1);
+	usb_debug(" FIELD2\t0x%08"PRIx32"\n", ec->f2);
+	usb_debug(" TRDQ_L\t0x%08"PRIx32"\n", ec->tr_dq_low);
+	usb_debug(" TRDQ_H\t0x%08"PRIx32"\n", ec->tr_dq_high);
+	usb_debug(" FIELD5\t0x%08"PRIx32"\n", ec->f5);
+	EC_DUMP(STATE,  *ec);
+	EC_DUMP(INTVAL, *ec);
+	EC_DUMP(CERR,   *ec);
+	EC_DUMP(TYPE,   *ec);
+	EC_DUMP(MBS,    *ec);
+	EC_DUMP(MPS,    *ec);
+	EC_DUMP(DCS,    *ec);
+	EC_DUMP(AVRTRB, *ec);
+	EC_DUMP(MXESIT, *ec);
+}
+
+void
+xhci_dump_devctx(const devctx_t *const dc, const u32 ctx_mask)
+{
+	int i;
+	if (ctx_mask & 1)
+		xhci_dump_slotctx(&dc->slot);
+	for (i = 0; i < SC_GET(CTXENT, dc->slot); ++i) {
+		if (ctx_mask & (2 << i))
+			xhci_dump_epctx(&dc->all_eps[i]);
+	}
+}
+
+void
+xhci_dump_inputctx(const inputctx_t *const ic)
+{
+	xhci_debug("Input Control  add: 0x%08"PRIx32"\n", ic->control.add);
+	xhci_debug("Input Control drop: 0x%08"PRIx32"\n", ic->control.drop);
+	xhci_dump_devctx(&ic->dev, ic->control.add);
+}
+
+void
+xhci_dump_transfer_trb(const trb_t *const cur)
+{
+	xhci_debug("Transfer TRB (@%p):\n", cur);
+	usb_debug(" PTR_L\t0x%08"PRIx32"\n", cur->ptr_low);
+	usb_debug(" PTR_H\t0x%08"PRIx32"\n", cur->ptr_high);
+	usb_debug(" STATUS\t0x%08"PRIx32"\n", cur->status);
+	usb_debug(" CNTRL\t0x%08"PRIx32"\n", cur->control);
+	TRB_DUMP(TL,	cur);
+	TRB_DUMP(TDS,	cur);
+	TRB_DUMP(C,	cur);
+	TRB_DUMP(ISP,	cur);
+	TRB_DUMP(CH,	cur);
+	TRB_DUMP(IOC,	cur);
+	TRB_DUMP(IDT,	cur);
+	TRB_DUMP(TT,	cur);
+	TRB_DUMP(DIR,	cur);
+}
+
+static const trb_t *
+xhci_next_trb(const trb_t *const cur)
+{
+	if (TRB_GET(TT, cur) == TRB_LINK)
+		return (!cur->ptr_low) ? NULL : phys_to_virt(cur->ptr_low);
+	else
+		return cur + 1;
+}
+
+void
+xhci_dump_transfer_trbs(const trb_t *const first, const trb_t *const last)
+{
+	const trb_t *cur;
+	for (cur = first; cur; cur = xhci_next_trb(cur)) {
+		xhci_dump_transfer_trb(cur);
+		if (cur == last)
+			break;
+	}
+}
+
+#endif
diff --git a/payloads/libpayload/drivers/usb/xhci_devconf.c b/payloads/libpayload/drivers/usb/xhci_devconf.c
new file mode 100644
index 0000000000..34bac8ff47
--- /dev/null
+++ b/payloads/libpayload/drivers/usb/xhci_devconf.c
@@ -0,0 +1,425 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2013 secunet Security Networks AG
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+//#define XHCI_SPEW_DEBUG
+
+#include <arch/virtual.h>
+#include "xhci_private.h"
+
+static u32
+xhci_gen_route(xhci_t *const xhci, const int hubport, const int hubaddr)
+{
+	if (!hubaddr)
+		return 0;
+	volatile const devctx_t *const devctx =
+		phys_to_virt(xhci->dcbaa[hubaddr]);
+	u32 route_string = SC_GET(ROUTE, devctx->slot);
+	int i;
+	for (i = 0; i < 20; i += 4) {
+		if (!(route_string & (0xf << i))) {
+			route_string |= (hubport & 0xf) << i;
+			break;
+		}
+	}
+	return route_string;
+}
+
+static int
+xhci_get_rh_port(xhci_t *const xhci, const int hubport, const int hubaddr)
+{
+	if (!hubaddr)
+		return hubport;
+	volatile const devctx_t *const devctx =
+		phys_to_virt(xhci->dcbaa[hubaddr]);
+	return SC_GET(RHPORT, devctx->slot);
+}
+
+static int
+xhci_get_tt(xhci_t *const xhci, const int xhci_speed,
+	    const int hubport, const int hubaddr,
+	    int *const tt, int *const tt_port)
+{
+	if (!hubaddr)
+		return 0;
+	volatile const devctx_t *const devctx =
+		phys_to_virt(xhci->dcbaa[hubaddr]);
+	if ((*tt = SC_GET(TTID, devctx->slot))) {
+		*tt_port = SC_GET(TTPORT, devctx->slot);
+	} else if (xhci_speed < XHCI_HIGH_SPEED &&
+			SC_GET(SPEED, devctx->slot) == XHCI_HIGH_SPEED) {
+		*tt = hubaddr;
+		*tt_port = hubport;
+	}
+	return *tt != 0;
+}
+
+static long
+xhci_decode_mps0(const int xhci_speed, const u8 b_mps)
+{
+	switch (xhci_speed) {
+	case XHCI_LOW_SPEED:
+	case XHCI_FULL_SPEED:
+	case XHCI_HIGH_SPEED:
+		switch (b_mps) {
+		case 8: case 16: case 32: case 64:
+			return b_mps;
+		default:
+			xhci_debug("Invalid MPS0: 0x%02x\n", b_mps);
+			return 8;
+		}
+		break;
+	case XHCI_SUPER_SPEED:
+		if (b_mps == 9) {
+			return 2 << b_mps;
+		} else {
+			xhci_debug("Invalid MPS0: 0x%02x\n", b_mps);
+			return 2 << 9;
+		}
+		break;
+	default:
+		xhci_debug("Invalid speed for MPS0: %d\n", xhci_speed);
+		return 8;
+	}
+}
+
+
+static long
+xhci_get_mps0(usbdev_t *const dev, const int xhci_speed)
+{
+	u8 buf[8];
+	dev_req_t dr = {
+		.bmRequestType	= gen_bmRequestType(
+					device_to_host, standard_type, dev_recp),
+		.data_dir	= device_to_host,
+		.bRequest	= GET_DESCRIPTOR,
+		.wValue		= (1 << 8),
+		.wIndex		= 0,
+		.wLength	= 8,
+	};
+	if (dev->controller->control(dev, IN, sizeof(dr), &dr, 8, buf)) {
+		xhci_debug("Failed to read MPS0\n");
+		return COMMUNICATION_ERROR;
+	} else {
+		return xhci_decode_mps0(xhci_speed, buf[7]);
+	}
+}
+
+int
+xhci_set_address (hci_t *controller, int speed, int hubport, int hubaddr)
+{
+	xhci_t *const xhci = XHCI_INST(controller);
+	const int xhci_speed = speed + 1;
+
+	int ret = -1;
+
+	inputctx_t *const ic = xhci_align(64, sizeof(*ic));
+	devinfo_t *const di = memalign(sizeof(di->devctx), sizeof(*di));
+	transfer_ring_t *const tr = malloc(sizeof(*tr));
+	if (tr)
+		tr->ring = xhci_align(16, TRANSFER_RING_SIZE * sizeof(trb_t));
+	if (!ic || !di || !tr || !tr->ring) {
+		xhci_debug("Out of memory\n");
+		goto _free_return;
+	}
+
+	int slot_id;
+	int cc = xhci_cmd_enable_slot(xhci, &slot_id);
+	if (cc != CC_SUCCESS) {
+		xhci_debug("Enable slot failed: %d\n", cc);
+		goto _free_return;
+	} else {
+		xhci_debug("Enabled slot %d\n", slot_id);
+	}
+
+	memset(ic, 0x00, sizeof(*ic));
+	ic->control.add = (1 << 0) /* Slot Context */ |
+			  (1 << 1) /* EP0 Context */ ;
+
+	SC_SET(ROUTE,	ic->dev.slot, xhci_gen_route(xhci, hubport, hubaddr));
+	SC_SET(SPEED,	ic->dev.slot, xhci_speed);
+	SC_SET(CTXENT,	ic->dev.slot, 1); /* the endpoint 0 context */
+	SC_SET(RHPORT,	ic->dev.slot, xhci_get_rh_port(xhci, hubport, hubaddr));
+
+	int tt, tt_port;
+	if (xhci_get_tt(xhci, xhci_speed, hubport, hubaddr, &tt, &tt_port)) {
+		xhci_debug("TT for %d: %d[%d]\n", slot_id, tt, tt_port);
+		volatile const devctx_t *const ttctx =
+			phys_to_virt(xhci->dcbaa[tt]);
+		SC_SET(MTT, ic->dev.slot, SC_GET(MTT, ttctx->slot));
+		SC_SET(TTID, ic->dev.slot, tt);
+		SC_SET(TTPORT, ic->dev.slot, tt_port);
+	}
+
+	memset(di, 0x00, sizeof(*di));
+	di->transfer_rings[1] = tr;
+	xhci_init_cycle_ring(tr, TRANSFER_RING_SIZE);
+
+	ic->dev.ep0.tr_dq_low	= virt_to_phys(tr->ring);
+	ic->dev.ep0.tr_dq_high	= 0;
+	EC_SET(TYPE,	ic->dev.ep0, EP_CONTROL);
+	EC_SET(AVRTRB,	ic->dev.ep0, 8);
+	EC_SET(MPS,	ic->dev.ep0, 8);
+	EC_SET(CERR,	ic->dev.ep0, 3);
+	EC_SET(DCS,	ic->dev.ep0, 1);
+
+	volatile devctx_t *const oc = &di->devctx;
+	xhci->dcbaa[slot_id] = virt_to_phys(oc);
+
+	cc = xhci_cmd_address_device(xhci, slot_id, ic);
+	if (cc != CC_SUCCESS) {
+		xhci_debug("Address device failed: %d\n", cc);
+		goto _disable_return;
+	} else {
+		xhci_debug("Addressed device %d (USB: %d)\n",
+			  slot_id, SC_GET(UADDR, oc->slot));
+	}
+	mdelay(2); /* SetAddress() recovery interval (usb20 spec 9.2.6.3) */
+
+	init_device_entry(controller, slot_id);
+	controller->devices[slot_id]->address = slot_id;
+
+	const long mps0 = xhci_get_mps0(
+			controller->devices[slot_id], xhci_speed);
+	if (mps0 < 0) {
+		goto _disable_return;
+	} else if (mps0 != 8) {
+		memset(&ic->control, 0x00, sizeof(ic->control));
+		memset(&ic->dev.ep0, 0x00, sizeof(ic->dev.ep0));
+		ic->control.add = (1 << 1); /* EP0 Context */
+		EC_SET(MPS, ic->dev.ep0, mps0);
+		cc = xhci_cmd_evaluate_context(xhci, slot_id, ic);
+		if (cc != CC_SUCCESS) {
+			xhci_debug("Context evaluation failed: %d\n", cc);
+			goto _disable_return;
+		} else {
+			xhci_debug("Set MPS0 to %dB\n", mps0);
+		}
+	}
+
+	ret = slot_id;
+	goto _free_ic_return;
+
+_disable_return:
+	xhci_cmd_disable_slot(xhci, slot_id);
+	xhci->dcbaa[slot_id] = 0;
+_free_return:
+	if (tr)
+		free((void *)tr->ring);
+	free(tr);
+	free((void *)di);
+_free_ic_return:
+	free(ic);
+	return ret;
+}
+
+static int
+xhci_finish_hub_config(usbdev_t *const dev, inputctx_t *const ic)
+{
+	hub_descriptor_t *const descriptor = (hub_descriptor_t *)
+		get_descriptor(
+			dev,
+			gen_bmRequestType(device_to_host, class_type, dev_recp),
+			0x29, 0, 0);
+	if (!descriptor) {
+		xhci_debug("Failed to fetch hub descriptor\n");
+		return COMMUNICATION_ERROR;
+	}
+
+	SC_SET(HUB,	ic->dev.slot, 1);
+	SC_SET(MTT,	ic->dev.slot, 0); /* No support for Multi-TT */
+	SC_SET(NPORTS,	ic->dev.slot, descriptor->bNbrPorts);
+	if (dev->speed == HIGH_SPEED)
+		SC_SET(TTT, ic->dev.slot,
+		       (descriptor->wHubCharacteristics >> 5) & 0x0003);
+
+	free(descriptor);
+	return 0;
+}
+
+static size_t
+xhci_bound_interval(const endpoint_t *const ep)
+{
+	if (	(ep->dev->speed == LOW_SPEED &&
+			(ep->type == ISOCHRONOUS ||
+			 ep->type == INTERRUPT)) ||
+		(ep->dev->speed == FULL_SPEED &&
+			 ep->type == INTERRUPT))
+	{
+		if (ep->interval < 3)
+			return 3;
+		else if (ep->interval > 11)
+			return 11;
+		else
+			return ep->interval;
+	} else {
+		if (ep->interval < 0)
+			return 0;
+		else if (ep->interval > 15)
+			return 15;
+		else
+			return ep->interval;
+	}
+}
+
+static int
+xhci_finish_ep_config(const endpoint_t *const ep, inputctx_t *const ic)
+{
+	xhci_t *const xhci = XHCI_INST(ep->dev->controller);
+	devinfo_t *const di = phys_to_virt(xhci->dcbaa[ep->dev->address]
+					   - offsetof(devinfo_t, devctx));
+	const int ep_id = xhci_ep_id(ep);
+	xhci_debug("ep_id: %d\n", ep_id);
+	if (ep_id <= 1 || 32 <= ep_id)
+		return DRIVER_ERROR;
+
+	transfer_ring_t *const tr = malloc(sizeof(*tr));
+	if (tr)
+		tr->ring = xhci_align(16, TRANSFER_RING_SIZE * sizeof(trb_t));
+	if (!tr || !tr->ring) {
+		free(tr);
+		xhci_debug("Out of memory\n");
+		return OUT_OF_MEMORY;
+	}
+	di->transfer_rings[ep_id] = tr;
+	xhci_init_cycle_ring(tr, TRANSFER_RING_SIZE);
+
+	ic->control.add |= (1 << ep_id);
+	if (SC_GET(CTXENT, ic->dev.slot) < ep_id)
+		SC_SET(CTXENT, ic->dev.slot, ep_id);
+
+	epctx_t *const epctx = &ic->dev.eps[ep_id];
+	xhci_debug("Filling epctx (@%p)\n", epctx);
+	epctx->tr_dq_low	= virt_to_phys(tr->ring);
+	epctx->tr_dq_high	= 0;
+	EC_SET(INTVAL,	*epctx, xhci_bound_interval(ep));
+	EC_SET(CERR,	*epctx, 3);
+	EC_SET(TYPE,	*epctx, ep->type | ((ep->direction != OUT) << 2));
+	EC_SET(MPS,	*epctx, ep->maxpacketsize);
+	EC_SET(DCS,	*epctx, 1);
+	size_t avrtrb;
+	switch (ep->type) {
+		case BULK: case ISOCHRONOUS:	avrtrb = 3 * 1024; break;
+		case INTERRUPT:			avrtrb =     1024; break;
+		default:			avrtrb =        8; break;
+	}
+	EC_SET(AVRTRB,	*epctx, avrtrb);
+	EC_SET(MXESIT,  *epctx, EC_GET(MPS, *epctx) * EC_GET(MBS, *epctx));
+
+	return 0;
+}
+
+int
+xhci_finish_device_config(usbdev_t *const dev)
+{
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	devinfo_t *const di = phys_to_virt(xhci->dcbaa[dev->address]
+					   - offsetof(devinfo_t, devctx));
+
+	int i, ret = 0;
+
+	inputctx_t *const ic = xhci_align(64, sizeof(*ic));
+	if (!ic) {
+		xhci_debug("Out of memory\n");
+		return OUT_OF_MEMORY;
+	}
+	memset(ic, 0x00, sizeof(*ic));
+
+	ic->control.add = (1 << 0); /* Slot Context */
+
+	xhci_dump_slotctx((const slotctx_t *)&di->devctx.slot);
+	ic->dev.slot.f1 = di->devctx.slot.f1;
+	ic->dev.slot.f2 = di->devctx.slot.f2;
+	ic->dev.slot.f3 = di->devctx.slot.f3;
+
+	if (((device_descriptor_t *)dev->descriptor)->bDeviceClass == 0x09) {
+		ret = xhci_finish_hub_config(dev, ic);
+		if (ret)
+			goto _free_return;
+	}
+
+	for (i = 1; i < dev->num_endp; ++i) {
+		ret = xhci_finish_ep_config(&dev->endpoints[i], ic);
+		if (ret)
+			goto _free_ep_ctx_return;
+	}
+
+	xhci_dump_inputctx(ic);
+
+	const int config_id = ((configuration_descriptor_t *)
+				dev->configuration)->bConfigurationValue;
+	xhci_debug("config_id: %d\n", config_id);
+	const int cc =
+		xhci_cmd_configure_endpoint(xhci, dev->address, config_id, ic);
+	if (cc != CC_SUCCESS) {
+		xhci_debug("Configure endpoint failed: %d\n", cc);
+		ret = CONTROLLER_ERROR;
+		goto _free_ep_ctx_return;
+	} else {
+		xhci_debug("Endpoints configured\n");
+	}
+
+	goto _free_return;
+
+_free_ep_ctx_return:
+	for (i = 2; i < 31; ++i) {
+		if (di->transfer_rings[i])
+			free((void *)di->transfer_rings[i]->ring);
+		free(di->transfer_rings[i]);
+		di->transfer_rings[i] = NULL;
+	}
+_free_return:
+	free(ic);
+	return ret;
+}
+
+void
+xhci_destroy_dev(hci_t *const controller, const int slot_id)
+{
+	xhci_t *const xhci = XHCI_INST(controller);
+
+	if (slot_id <= 0 || xhci->max_slots_en > slot_id)
+		return;
+
+	int i;
+
+	const int cc = xhci_cmd_disable_slot(xhci, slot_id);
+	if (cc != CC_SUCCESS)
+		xhci_debug("Failed to disable slot %d: %d\n", slot_id, cc);
+
+	devinfo_t *const di = DEVINFO_FROM_XHCI(xhci, slot_id);
+	for (i = 1; i < 31; ++i) {
+		if (di->transfer_rings[i])
+			free((void *)di->transfer_rings[i]->ring);
+		free(di->transfer_rings[i]);
+
+		free(di->interrupt_queues[i]);
+	}
+	free(di);
+	xhci->dcbaa[slot_id] = 0;
+}
diff --git a/payloads/libpayload/drivers/usb/xhci_events.c b/payloads/libpayload/drivers/usb/xhci_events.c
new file mode 100644
index 0000000000..b04ecda267
--- /dev/null
+++ b/payloads/libpayload/drivers/usb/xhci_events.c
@@ -0,0 +1,333 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2013 secunet Security Networks AG
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+//#define XHCI_SPEW_DEBUG
+
+#include <inttypes.h>
+#include <arch/virtual.h>
+#include "xhci_private.h"
+
+void
+xhci_reset_event_ring(event_ring_t *const er)
+{
+	int i;
+	for (i = 0; i < EVENT_RING_SIZE; ++i)
+		er->ring[i].control &= ~TRB_CYCLE;
+	er->cur		= er->ring;
+	er->last	= er->ring + EVENT_RING_SIZE;
+	er->ccs		= 1;
+	er->adv		= 1;
+}
+
+static inline int
+xhci_event_ready(const event_ring_t *const er)
+{
+	return (er->cur->control & TRB_CYCLE) == er->ccs;
+}
+
+void
+xhci_update_event_dq(xhci_t *const xhci)
+{
+	if (xhci->er.adv) {
+		xhci_spew("Updating dq ptr: @%p(0x%08"PRIx32") -> %p\n",
+			  phys_to_virt(xhci->hcrreg->intrrs[0].erdp_lo),
+			  xhci->hcrreg->intrrs[0].erdp_lo, xhci->er.cur);
+		xhci->hcrreg->intrrs[0].erdp_lo = virt_to_phys(xhci->er.cur);
+		xhci->hcrreg->intrrs[0].erdp_hi = 0;
+		xhci->er.adv = 0;
+	}
+}
+
+void
+xhci_advance_event_ring(xhci_t *const xhci)
+{
+	xhci->er.cur++;
+	xhci->er.adv = 1;
+	if (xhci->er.cur == xhci->er.last) {
+		xhci_spew("Roll over in event ring\n");
+		xhci->er.cur = xhci->er.ring;
+		xhci->er.ccs ^= 1;
+		xhci_update_event_dq(xhci);
+	}
+}
+
+static void
+xhci_handle_transfer_event(xhci_t *const xhci)
+{
+	const trb_t *const ev = xhci->er.cur;
+
+	const int cc = TRB_GET(CC, ev);
+	const int id = TRB_GET(ID, ev);
+	const int ep = TRB_GET(EP, ev);
+
+	devinfo_t *di;
+	intrq_t *intrq;
+
+	if (id && id <= xhci->max_slots_en &&
+			(di = DEVINFO_FROM_XHCI(xhci, id)) &&
+			(intrq = di->interrupt_queues[ep])) {
+		/* It's a running interrupt endpoint */
+		intrq->ready = phys_to_virt(ev->ptr_low);
+		if (cc == CC_SUCCESS || cc == CC_SHORT_PACKET) {
+			TRB_SET(TL, intrq->ready,
+				intrq->size - TRB_GET(EVTL, ev));
+		} else {
+			xhci_debug("Interrupt Transfer failed: %d\n",
+				   cc);
+			TRB_SET(TL, intrq->ready, 0);
+		}
+	} else if (cc == CC_STOPPED || cc == CC_STOPPED_LENGTH_INVALID) {
+		/* Ignore 'Forced Stop Events' */
+	} else {
+		xhci_debug("Warning: "
+			   "Spurious transfer event for ID %d, EP %d:\n"
+			   "  Pointer: 0x%08x%08x\n"
+			   "       TL: 0x%06x\n"
+			   "       CC: %d\n",
+			   id, ep,
+			   ev->ptr_high, ev->ptr_low,
+			   TRB_GET(EVTL, ev), cc);
+	}
+	xhci_advance_event_ring(xhci);
+}
+
+static void
+xhci_handle_command_completion_event(xhci_t *const xhci)
+{
+	const trb_t *const ev = xhci->er.cur;
+
+	xhci_debug("Warning: Spurious command completion event:\n"
+		   "  Pointer: 0x%08x%08x\n"
+		   "       CC: %d\n"
+		   "  Slot ID: %d\n"
+		   "    Cycle: %d\n",
+		   ev->ptr_high, ev->ptr_low,
+		   TRB_GET(CC, ev), TRB_GET(ID, ev), ev->control & TRB_CYCLE);
+	xhci_advance_event_ring(xhci);
+}
+
+static void
+xhci_handle_host_controller_event(xhci_t *const xhci)
+{
+	const trb_t *const ev = xhci->er.cur;
+
+	const int cc = TRB_GET(CC, ev);
+	switch (cc) {
+	case CC_EVENT_RING_FULL_ERROR:
+		xhci_debug("Event ring full! (@%p)\n", xhci->er.cur);
+		/*
+		 * If we get here, we have processed the whole queue:
+		 * xHC pushes this event, when it sees the ring full,
+		 * full of other events.
+		 * IMO it's save and necessary to update the dequeue
+		 * pointer here.
+		 */
+		xhci_advance_event_ring(xhci);
+		xhci_update_event_dq(xhci);
+		break;
+	default:
+		xhci_debug("Warning: Spurious host controller event: %d\n", cc);
+		xhci_advance_event_ring(xhci);
+		break;
+	}
+}
+
+/* handle standard types:
+ * - command completion event
+ * - port status change event
+ * - transfer event
+ * - host controller event
+ */
+static void
+xhci_handle_event(xhci_t *const xhci)
+{
+	const trb_t *const ev = xhci->er.cur;
+
+	const int trb_type = TRB_GET(TT, ev);
+	switch (trb_type) {
+		/* Either pass along the event or advance event ring */
+	case TRB_EV_TRANSFER:
+		xhci_handle_transfer_event(xhci);
+		break;
+	case TRB_EV_CMD_CMPL:
+		xhci_handle_command_completion_event(xhci);
+		break;
+	case TRB_EV_PORTSC:
+		xhci_debug("Port Status Change Event for %d: %d\n",
+			   TRB_GET(PORT, ev), TRB_GET(CC, ev));
+		/* We ignore the event as we look for the PORTSC
+		   registers instead, at a time when it suits _us_. */
+		xhci_advance_event_ring(xhci);
+		break;
+	case TRB_EV_HOST:
+		xhci_handle_host_controller_event(xhci);
+		break;
+	default:
+		xhci_debug("Warning: Spurious event: %d, Completion Code: %d\n",
+			   trb_type, TRB_GET(CC, ev));
+		xhci_advance_event_ring(xhci);
+		break;
+	}
+}
+
+void
+xhci_handle_events(xhci_t *const xhci)
+{
+	while (xhci_event_ready(&xhci->er))
+		xhci_handle_event(xhci);
+	xhci_update_event_dq(xhci);
+}
+
+static unsigned long
+xhci_wait_for_event(const event_ring_t *const er,
+		    unsigned long *const timeout_us)
+{
+	while (!xhci_event_ready(er) && *timeout_us) {
+		--*timeout_us;
+		udelay(1);
+	}
+	return *timeout_us;
+}
+
+static unsigned long
+xhci_wait_for_event_type(xhci_t *const xhci,
+		    const int trb_type,
+		    unsigned long *const timeout_us)
+{
+	while (xhci_wait_for_event(&xhci->er, timeout_us)) {
+		if (TRB_GET(TT, xhci->er.cur) == trb_type)
+			break;
+
+		xhci_handle_event(xhci);
+	}
+	return *timeout_us;
+}
+
+/* returns cc of command in question (pointed to by `address`) */
+int
+xhci_wait_for_command_aborted(xhci_t *const xhci, const trb_t *const address)
+{
+	/*
+	 * Specification says that something might be seriously wrong, if
+	 * we don't get a response after 5s. Still, let the caller decide,
+	 * what to do then.
+	 */
+	unsigned long timeout_us = 5 * 1000 * 1000; /* 5s */
+	int cc = TIMEOUT;
+	/*
+	 * Expects two command completion events:
+	 * The first with CC == COMMAND_ABORTED should point to address,
+	 * the second with CC == COMMAND_RING_STOPPED should point to new dq.
+	 */
+	while (xhci_wait_for_event_type(xhci, TRB_EV_CMD_CMPL, &timeout_us)) {
+		if ((xhci->er.cur->ptr_low == virt_to_phys(address)) &&
+				(xhci->er.cur->ptr_high == 0)) {
+			cc = TRB_GET(CC, xhci->er.cur);
+			xhci_advance_event_ring(xhci);
+			break;
+		}
+
+		xhci_handle_command_completion_event(xhci);
+	}
+	if (!timeout_us)
+		xhci_debug("Warning: Timed out waiting for COMMAND_ABORTED.\n");
+	while (xhci_wait_for_event_type(xhci, TRB_EV_CMD_CMPL, &timeout_us)) {
+		if (TRB_GET(CC, xhci->er.cur) == CC_COMMAND_RING_STOPPED) {
+			xhci->cr.cur = phys_to_virt(xhci->er.cur->ptr_low);
+			xhci_advance_event_ring(xhci);
+			break;
+		}
+
+		xhci_handle_command_completion_event(xhci);
+	}
+	if (!timeout_us)
+		xhci_debug("Warning: Timed out "
+			   "waiting for COMMAND_RING_STOPPED.\n");
+	xhci_update_event_dq(xhci);
+	return cc;
+}
+
+/*
+ * returns cc of command in question (pointed to by `address`)
+ * caller should abort command if cc is TIMEOUT
+ */
+int
+xhci_wait_for_command_done(xhci_t *const xhci,
+			   const trb_t *const address,
+			   const int clear_event)
+{
+	/*
+	 * The Address Device Command should take most time, as it has to
+	 * communicate with the USB device. Set address processing shouldn't
+	 * take longer than 50ms (at the slave). Let's take a timeout of
+	 * 100ms.
+	 */
+	unsigned long timeout_us = 100 * 1000; /* 100ms */
+	int cc = TIMEOUT;
+	while (xhci_wait_for_event_type(xhci, TRB_EV_CMD_CMPL, &timeout_us)) {
+		if ((xhci->er.cur->ptr_low == virt_to_phys(address)) &&
+				(xhci->er.cur->ptr_high == 0)) {
+			cc = TRB_GET(CC, xhci->er.cur);
+			break;
+		}
+
+		xhci_handle_command_completion_event(xhci);
+	}
+	if (!timeout_us) {
+		xhci_debug("Warning: Timed out waiting for TRB_EV_CMD_CMPL.\n");
+	} else if (clear_event) {
+		xhci_advance_event_ring(xhci);
+	}
+	xhci_update_event_dq(xhci);
+	return cc;
+}
+
+/* returns cc of transfer for given slot/endpoint pair */
+int
+xhci_wait_for_transfer(xhci_t *const xhci, const int slot_id, const int ep_id)
+{
+	xhci_spew("Waiting for transfer on ID %d EP %d\n", slot_id, ep_id);
+	/* 2s for all types of transfers */ /* TODO: test, wait longer? */
+	unsigned long timeout_us = 2 * 1000 * 1000;
+	int cc = TIMEOUT;
+	while (xhci_wait_for_event_type(xhci, TRB_EV_TRANSFER, &timeout_us)) {
+		if (TRB_GET(ID, xhci->er.cur) == slot_id &&
+				TRB_GET(EP, xhci->er.cur) == ep_id) {
+			cc = TRB_GET(CC, xhci->er.cur);
+			xhci_advance_event_ring(xhci);
+			break;
+		}
+
+		xhci_handle_transfer_event(xhci);
+	}
+	if (!timeout_us)
+		xhci_debug("Warning: Timed out waiting for TRB_EV_TRANSFER.\n");
+	xhci_update_event_dq(xhci);
+	return cc;
+}
diff --git a/payloads/libpayload/drivers/usb/xhci_private.h b/payloads/libpayload/drivers/usb/xhci_private.h
index 16834f77cc..6f7893ee31 100644
--- a/payloads/libpayload/drivers/usb/xhci_private.h
+++ b/payloads/libpayload/drivers/usb/xhci_private.h
@@ -2,6 +2,7 @@
  * This file is part of the libpayload project.
  *
  * Copyright (C) 2010 Patrick Georgi
+ * Copyright (C) 2013 secunet Security Networks AG
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -30,159 +31,268 @@
 #ifndef __XHCI_PRIVATE_H
 #define __XHCI_PRIVATE_H
 
+//#define USB_DEBUG
 #include <usb/usb.h>
 
+//#define XHCI_DUMPS
+#define xhci_debug(fmt, args...) usb_debug("%s: " fmt, __func__, ## args)
+#ifdef XHCI_SPEW_DEBUG
+# define xhci_spew(fmt, args...) xhci_debug(fmt, ##args)
+#else
+# define xhci_spew(fmt, args...) do {} while(0)
+#endif
+
 #define MASK(startbit, lenbit) (((1<<(lenbit))-1)<<(startbit))
 
-typedef volatile union trb {
-	// transfer
+enum { XHCI_FULL_SPEED = 1, XHCI_LOW_SPEED = 2, XHCI_HIGH_SPEED = 3, XHCI_SUPER_SPEED = 4 };
 
-	// events
-#define TRB_EV_CMD_CMPL 33
-	struct {
-		u32 Cmd_TRB_Pointer_lo;
-		u32 Cmd_TRB_Pointer_hi;
-		struct {
-			unsigned long:24;
-			unsigned long Completion_Code:8;
-		} __attribute__ ((packed));
-		struct {
-			unsigned long C:1;
-			unsigned long:9;
-			unsigned long TRB_Type:6;
-			unsigned long VF_ID:8;
-			unsigned long Slot_ID:8;
-		} __attribute__ ((packed));
-	} __attribute__ ((packed)) event_cmd_cmpl;
+#define TIMEOUT			-1
+#define CONTROLLER_ERROR	-2
+#define COMMUNICATION_ERROR	-3
+#define OUT_OF_MEMORY		-4
+#define DRIVER_ERROR		-5
 
-#define TRB_EV_PORTSC 34
-	struct {
-		struct {
-			unsigned long:24;
-			unsigned long Port:8;
-		} __attribute__ ((packed));
-		u32 rsvd;
-		struct {
-			unsigned long:24;
-			unsigned long Completion_Code:8;
-		} __attribute__ ((packed));
-		struct {
-			unsigned long C:1;
-			unsigned long:9;
-			unsigned long TRB_Type:6;
-			unsigned long:16;
-		} __attribute__ ((packed));
-	} __attribute__ ((packed)) event_portsc;
+#define CC_SUCCESS			 1
+#define CC_TRB_ERROR			 5
+#define CC_STALL_ERROR			 6
+#define CC_SHORT_PACKET			13
+#define CC_EVENT_RING_FULL_ERROR	21
+#define CC_COMMAND_RING_STOPPED		24
+#define CC_COMMAND_ABORTED		25
+#define CC_STOPPED			26
+#define CC_STOPPED_LENGTH_INVALID	27
 
-	// commands
-#define TRB_CMD_NOOP 23
-	struct {
-		u32 rsvd[3];
-		struct {
-			unsigned long C:1;
-			unsigned long:9;
-			unsigned long TRB_Type:6;
-			unsigned long:16;
-		} __attribute__ ((packed));
-	} __attribute__ ((packed)) cmd_No_Op;
+enum {
+	TRB_NORMAL = 1,
+	TRB_SETUP_STAGE = 2, TRB_DATA_STAGE = 3, TRB_STATUS_STAGE = 4,
+	TRB_LINK = 6,
+	TRB_CMD_ENABLE_SLOT = 9, TRB_CMD_DISABLE_SLOT = 10, TRB_CMD_ADDRESS_DEV = 11,
+	TRB_CMD_CONFIGURE_EP = 12, TRB_CMD_EVAL_CTX = 13, TRB_CMD_RESET_EP = 14,
+	TRB_CMD_STOP_EP = 15, TRB_CMD_SET_TR_DQ = 16, TRB_CMD_NOOP = 23,
+	TRB_EV_TRANSFER = 32, TRB_EV_CMD_CMPL = 33, TRB_EV_PORTSC = 34, TRB_EV_HOST = 37,
+};
+enum { TRB_TRT_NO_DATA = 0, TRB_TRT_OUT_DATA = 2, TRB_TRT_IN_DATA = 3 };
+enum { TRB_DIR_OUT = 0, TRB_DIR_IN = 1 };
 
-	// "others"
-	struct {
-		u32 Ring_Segment_Ptr_lo;
-		u32 Ring_Segment_Ptr_hi;
-		struct {
-			unsigned long:22;
-			unsigned long Interrupter_Target;
-		} __attribute__ ((packed));
-		struct {
-			unsigned long C:1;
-			unsigned long TC:1;
-			unsigned long:2;
-			unsigned long CH:1;
-			unsigned long IOC:1;
-			unsigned long:4;
-			unsigned long TRB_Type:6;
-			unsigned long:16;
-		} __attribute__ ((packed));
-	} __attribute__ ((packed)) link;
+#define TRB_PORT_FIELD		ptr_low
+#define TRB_PORT_START		24
+#define TRB_PORT_LEN		8
+#define TRB_TL_FIELD		status		/* TL - Transfer Length */
+#define TRB_TL_START		0
+#define TRB_TL_LEN		17
+#define TRB_EVTL_FIELD		status		/* EVTL - (Event TRB) Transfer Length */
+#define TRB_EVTL_START		0
+#define TRB_EVTL_LEN		24
+#define TRB_TDS_FIELD		status		/* TDS - TD Size */
+#define TRB_TDS_START		17
+#define TRB_TDS_LEN		5
+#define TRB_CC_FIELD		status		/* CC - Completion Code */
+#define TRB_CC_START		24
+#define TRB_CC_LEN		8
+#define TRB_C_FIELD		control		/* C - Cycle Bit */
+#define TRB_C_START		0
+#define TRB_C_LEN		1
+#define TRB_TC_FIELD		control		/* TC - Toggle Cycle */
+#define TRB_TC_START		1
+#define TRB_TC_LEN		1
+#define TRB_ISP_FIELD		control		/* ISP - Interrupt-on Short Packet */
+#define TRB_ISP_START		2
+#define TRB_ISP_LEN		1
+#define TRB_CH_FIELD		control		/* CH - Chain Bit */
+#define TRB_CH_START		4
+#define TRB_CH_LEN		1
+#define TRB_IOC_FIELD		control		/* IOC - Interrupt On Completion */
+#define TRB_IOC_START		5
+#define TRB_IOC_LEN		1
+#define TRB_IDT_FIELD		control		/* IDT - Immediate Data */
+#define TRB_IDT_START		6
+#define TRB_IDT_LEN		1
+#define TRB_DC_FIELD		control		/* DC - Deconfigure */
+#define TRB_DC_START		9
+#define TRB_DC_LEN		1
+#define TRB_TT_FIELD		control		/* TT - TRB Type */
+#define TRB_TT_START		10
+#define TRB_TT_LEN		6
+#define TRB_TRT_FIELD		control		/* TRT - Transfer Type */
+#define TRB_TRT_START		16
+#define TRB_TRT_LEN		2
+#define TRB_DIR_FIELD		control		/* DIR - Direction */
+#define TRB_DIR_START		16
+#define TRB_DIR_LEN		1
+#define TRB_EP_FIELD		control		/* EP - Endpoint ID */
+#define TRB_EP_START		16
+#define TRB_EP_LEN		5
+#define TRB_ID_FIELD		control		/* ID - Slot ID */
+#define TRB_ID_START		24
+#define TRB_ID_LEN		8
+#define TRB_MASK(tok)		MASK(TRB_##tok##_START, TRB_##tok##_LEN)
+#define TRB_GET(tok, trb)	(((trb)->TRB_##tok##_FIELD & TRB_MASK(tok)) \
+				 >> TRB_##tok##_START)
+#define TRB_SET(tok, trb, to)	(trb)->TRB_##tok##_FIELD = \
+				(((trb)->TRB_##tok##_FIELD & ~TRB_MASK(tok)) | \
+				 (((to) << TRB_##tok##_START) & TRB_MASK(tok)))
+#define TRB_DUMP(tok, trb)	usb_debug(" "#tok"\t0x%04"PRIx32"\n", TRB_GET(tok, trb))
+
+#define TRB_CYCLE		(1 << 0)
+typedef volatile struct trb {
+	u32 ptr_low;
+	u32 ptr_high;
+	u32 status;
+	u32 control;
 } trb_t;
 
+#define EVENT_RING_SIZE 64
+typedef struct {
+	trb_t *ring;
+	trb_t *cur;
+	trb_t *last;
+	u8 ccs;
+	u8 adv;
+} event_ring_t;
+
+#define TRANSFER_RING_SIZE 32
+typedef struct {
+	trb_t *ring;
+	trb_t *cur;
+	u8 pcs;
+} __attribute__ ((packed)) transfer_ring_t;
+
+#define COMMAND_RING_SIZE 4
+typedef transfer_ring_t command_ring_t;
+
+#define SC_ROUTE_FIELD		f1		/* ROUTE - Route String */
+#define SC_ROUTE_START		0
+#define SC_ROUTE_LEN		20
+#define SC_SPEED_FIELD		f1
+#define SC_SPEED_START		20
+#define SC_SPEED_LEN		4
+#define SC_MTT_FIELD		f1		/* MTT - Multi Transaction Translator */
+#define SC_MTT_START		25
+#define SC_MTT_LEN		1
+#define SC_HUB_FIELD		f1		/* HUB - Is this a hub? */
+#define SC_HUB_START		26
+#define SC_HUB_LEN		1
+#define SC_CTXENT_FIELD		f1		/* CTXENT - Context Entries (number of following ep contexts) */
+#define SC_CTXENT_START		27
+#define SC_CTXENT_LEN		5
+#define SC_RHPORT_FIELD		f2		/* RHPORT - Root Hub Port Number */
+#define SC_RHPORT_START		16
+#define SC_RHPORT_LEN		8
+#define SC_NPORTS_FIELD		f2		/* NPORTS - Number of Ports */
+#define SC_NPORTS_START		24
+#define SC_NPORTS_LEN		8
+#define SC_TTID_FIELD		f3		/* TTID - TT Hub Slot ID */
+#define SC_TTID_START		0
+#define SC_TTID_LEN		8
+#define SC_TTPORT_FIELD		f3		/* TTPORT - TT Port Number */
+#define SC_TTPORT_START		8
+#define SC_TTPORT_LEN		8
+#define SC_TTT_FIELD		f3		/* TTT - TT Think Time */
+#define SC_TTT_START		16
+#define SC_TTT_LEN		2
+#define SC_UADDR_FIELD		f4		/* UADDR - USB Device Address */
+#define SC_UADDR_START		0
+#define SC_UADDR_LEN		8
+#define SC_STATE_FIELD		f4		/* STATE - Slot State */
+#define SC_STATE_START		27
+#define SC_STATE_LEN		8
+#define SC_MASK(tok)		MASK(SC_##tok##_START, SC_##tok##_LEN)
+#define SC_GET(tok, sc)		(((sc).SC_##tok##_FIELD & SC_MASK(tok)) \
+				 >> SC_##tok##_START)
+#define SC_SET(tok, sc, to)	(sc).SC_##tok##_FIELD = \
+				(((sc).SC_##tok##_FIELD & ~SC_MASK(tok)) | \
+				 (((to) << SC_##tok##_START) & SC_MASK(tok)))
+#define SC_DUMP(tok, sc)	usb_debug(" "#tok"\t0x%04"PRIx32"\n", SC_GET(tok, sc))
 typedef struct slotctx {
-	struct {
-		unsigned long Route_String:20;
-		unsigned long Speed:4;
-		unsigned long:1;
-		unsigned long MTT:1;
-		unsigned long Hub:1;
-		unsigned long Context_Entries:5;
-	} __attribute__ ((packed));
-	struct {
-		unsigned long Max_Exit_Latency:16;
-		unsigned long Root_Hub_Port_Number:8;
-		unsigned long Number_of_Ports:8;
-	} __attribute__ ((packed));
-	struct {
-		unsigned long TT_Hub_Slot_ID:8;
-		unsigned long TT_Port_Number:8;
-		unsigned long TTT:2;
-		unsigned long:4;
-		unsigned long Interrupter_Target:10;
-	} __attribute__ ((packed));
-	struct {
-		unsigned long USB_Device_Address:8;
-		unsigned long:19;
-		unsigned long Slot_State:5;
-	} __attribute__ ((packed));
+	u32 f1;
+	u32 f2;
+	u32 f3;
+	u32 f4;
 	u32 rsvd[4];
 } slotctx_t;
 
+#define EC_STATE_FIELD		f1		/* STATE - Endpoint State */
+#define EC_STATE_START		0
+#define EC_STATE_LEN		3
+#define EC_INTVAL_FIELD		f1		/* INTVAL - Interval */
+#define EC_INTVAL_START		16
+#define EC_INTVAL_LEN		8
+#define EC_CERR_FIELD		f2		/* CERR - Error Count */
+#define EC_CERR_START		1
+#define EC_CERR_LEN		2
+#define EC_TYPE_FIELD		f2		/* TYPE - EP Type */
+#define EC_TYPE_START		3
+#define EC_TYPE_LEN		3
+#define EC_MBS_FIELD		f2		/* MBS - Max Burst Size */
+#define EC_MBS_START		8
+#define EC_MBS_LEN		8
+#define EC_MPS_FIELD		f2		/* MPS - Max Packet Size */
+#define EC_MPS_START		16
+#define EC_MPS_LEN		16
+#define EC_DCS_FIELD		tr_dq_low	/* DCS - Dequeue Cycle State */
+#define EC_DCS_START		0
+#define EC_DCS_LEN		1
+#define EC_AVRTRB_FIELD		f5		/* AVRTRB - Average TRB Length */
+#define EC_AVRTRB_START		0
+#define EC_AVRTRB_LEN		16
+#define EC_MXESIT_FIELD		f5		/* MXESIT - Max ESIT Payload */
+#define EC_MXESIT_START		16
+#define EC_MXESIT_LEN		16
+#define EC_MASK(tok)		MASK(EC_##tok##_START, EC_##tok##_LEN)
+#define EC_GET(tok, ec)		(((ec).EC_##tok##_FIELD & EC_MASK(tok)) \
+				 >> EC_##tok##_START)
+#define EC_SET(tok, ec, to)	(ec).EC_##tok##_FIELD = \
+				(((ec).EC_##tok##_FIELD & ~EC_MASK(tok)) | \
+				 (((to) << EC_##tok##_START) & EC_MASK(tok)))
+#define EC_DUMP(tok, ec)	usb_debug(" "#tok"\t0x%04"PRIx32"\n", EC_GET(tok, ec))
+enum { EP_ISOC_OUT = 1, EP_BULK_OUT = 2, EP_INTR_OUT = 3,
+	EP_CONTROL = 4, EP_ISOC_IN = 5, EP_BULK_IN = 6, EP_INTR_IN = 7 };
 typedef struct epctx {
-	struct {
-		unsigned long EP_State:3;
-		unsigned long:5;
-		unsigned long Mult:2;
-		unsigned long MaxPStreams:5;
-		unsigned long LSA:1;
-		unsigned long Interval:8;
-		unsigned long:8;
-	} __attribute__ ((packed));
-	struct {
-		unsigned long:1;
-		unsigned long CErr:2;
-		unsigned long EP_Type:3;
-		unsigned long:1;
-		unsigned long HID:1;
-		unsigned long Max_Burst_Size:8;
-		unsigned long Max_Packet_Size:16;
-	} __attribute__ ((packed));
-	union {
-		u32 TR_Dequeue_Pointer_lo;
-		struct {
-			unsigned long DCS:1;
-			unsigned long:3;
-		} __attribute__ ((packed));
-	} __attribute__ ((packed));
-	u32 TR_Dequeue_Pointer_hi;
-	struct {
-		unsigned long Average_TRB_Length:16;
-		unsigned long Max_ESIT_Payload:16;
-	} __attribute__ ((packed));
+	u32 f1;
+	u32 f2;
+	u32 tr_dq_low;
+	u32 tr_dq_high;
+	u32 f5;
 	u32 rsvd[3];
 } epctx_t;
 
-typedef struct devctx {
-	slotctx_t slot;
-	epctx_t ep0;
+typedef union devctx {
 	struct {
-		epctx_t out;
-		epctx_t in;
-	} eps[15];
+		slotctx_t slot;
+		epctx_t ep0;
+		epctx_t eps1_30[30];
+	};
+	epctx_t eps[32]; /* At index 0 it's actually the slotctx,
+			    we have it like that so we can use
+			    the ep_id directly as index. */
 } devctx_t;
 
-typedef struct devctxp {
-	devctx_t *ptr;
-	void *upper;
-} devctxp_t;
+typedef struct inputctx {
+	struct {
+		u32 drop;
+		u32 add;
+		u32 reserved[6];
+	} control;
+	devctx_t dev;
+} inputctx_t;
+
+typedef struct intrq {
+	size_t size;	/* Size of each transfer */
+	size_t count;	/* The number of TRBs to fill at once */
+	trb_t *next;	/* The next TRB expected to be processed by the controller */
+	trb_t *ready;	/* The last TRB in the transfer ring processed by the controller */
+	endpoint_t *ep;
+} intrq_t;
+
+typedef struct devinfo {
+	volatile devctx_t devctx;
+	transfer_ring_t *transfer_rings[32];
+	intrq_t *interrupt_queues[32];
+} devinfo_t;
+#define DEVINFO_FROM_XHCI(xhci, slot_id) \
+	(((xhci)->dcbaa[slot_id]) \
+	 ? phys_to_virt((xhci)->dcbaa[slot_id] - offsetof(devinfo_t, devctx)) \
+	 : NULL)
 
 typedef struct erst_entry {
 	u32 seg_base_lo;
@@ -258,12 +368,14 @@ typedef struct xhci {
 		u32 usbcmd;
 #define USBCMD_RS 1<<0
 #define USBCMD_HCRST 1<<1
+#define USBCMD_INTE 1<<2
 		u32 usbsts;
 #define USBSTS_HCH 1<<0
 #define USBSTS_HSE 1<<2
 #define USBSTS_EINT 1<<3
 #define USBSTS_PCD 1<<4
 #define USBSTS_CNR 1<<11
+#define USBSTS_PRSRV_MASK ((1 << 1) | 0xffffe000)
 		u32 pagesize;
 		u8 res1[0x13-0x0c+1];
 		u32 dnctrl;
@@ -281,34 +393,35 @@ typedef struct xhci {
 		u8 res3[0x3ff-0x3c+1];
 		struct {
 			u32 portsc;
-#define PORTSC_CCS 1<<0
-#define PORTSC_PED 1<<1
+#define PORTSC_CCS (1<<0)
+#define PORTSC_PED (1<<1)
 	// BIT 2 rsvdZ
-#define PORTSC_OCA 1<<3
-#define PORTSC_PR 1<<4
-#define PORTSC_PLS 1<<5
+#define PORTSC_OCA (1<<3)
+#define PORTSC_PR (1<<4)
+#define PORTSC_PLS (1<<5)
 #define PORTSC_PLS_MASK MASK(5, 4)
-#define PORTSC_PP 1<<9
-#define PORTSC_PORT_SPEED 1<<10
-#define PORTSC_PORT_SPEED_MASK MASK(10, 4)
-#define PORTSC_PIC 1<<14
+#define PORTSC_PP (1<<9)
+#define PORTSC_PORT_SPEED_START 10
+#define PORTSC_PORT_SPEED (1<<PORTSC_PORT_SPEED_START)
+#define PORTSC_PORT_SPEED_MASK MASK(PORTSC_PORT_SPEED_START, 4)
+#define PORTSC_PIC (1<<14)
 #define PORTSC_PIC_MASK MASK(14, 2)
-#define PORTSC_LWS 1<<16
-#define PORTSC_CSC 1<<17
-#define PORTSC_PEC 1<<18
-#define PORTSC_WRC 1<<19
-#define PORTSC_OCC 1<<20
-#define PORTSC_PRC 1<<21
-#define PORTSC_PLC 1<<22
-#define PORTSC_CEC 1<<23
-#define PORTSC_CAS 1<<24
-#define PORTSC_WCE 1<<25
-#define PORTSC_WDE 1<<26
-#define PORTSC_WOE 1<<27
+#define PORTSC_LWS (1<<16)
+#define PORTSC_CSC (1<<17)
+#define PORTSC_PEC (1<<18)
+#define PORTSC_WRC (1<<19)
+#define PORTSC_OCC (1<<20)
+#define PORTSC_PRC (1<<21)
+#define PORTSC_PLC (1<<22)
+#define PORTSC_CEC (1<<23)
+#define PORTSC_CAS (1<<24)
+#define PORTSC_WCE (1<<25)
+#define PORTSC_WDE (1<<26)
+#define PORTSC_WOE (1<<27)
 	// BIT 29:28 rsvdZ
-#define PORTSC_DR 1<<30
-#define PORTSC_WPR 1<<31
-#define PORTSC_RW_MASK PORTSC_PR | PORTSC_PLS_MASK | PORTSC_PP | PORTSC_PIC_MASK | PORTSC_LWS | PORTSC_WCE | PORTSC_WDE | PORTSC_WOE
+#define PORTSC_DR (1<<30)
+#define PORTSC_WPR (1<<31)
+#define PORTSC_RW_MASK (PORTSC_PR | PORTSC_PLS_MASK | PORTSC_PP | PORTSC_PIC_MASK | PORTSC_LWS | PORTSC_WCE | PORTSC_WDE | PORTSC_WOE)
 			u32 portpmsc;
 			u32 portli;
 			u32 res;
@@ -335,16 +448,66 @@ typedef struct xhci {
 	volatile u32 *dbreg;
 
 	/* R/W, volatile, Memory -> bitfields allowed */
-	volatile devctxp_t *dcbaa;
+	u64 *dcbaa;	/* pointers to sp_ptrs and output (device) contexts */
+	u64 *sp_ptrs;	/* pointers to scratchpad buffers */
 
-	trb_t *cmd_ring;
-	trb_t *ev_ring;
+	command_ring_t cr;
+	event_ring_t er;
 	volatile erst_entry_t *ev_ring_table;
-	int cmd_ccs, ev_ccs;
 
 	usbdev_t *roothub;
+
+	u8 max_slots_en;
 } xhci_t;
 
 #define XHCI_INST(controller) ((xhci_t*)((controller)->instance))
 
+void *xhci_align(const size_t min_align, const size_t size);
+void xhci_init_cycle_ring(transfer_ring_t *, const size_t ring_size);
+int xhci_set_address (hci_t *, int speed, int hubport, int hubaddr);
+int xhci_finish_device_config(usbdev_t *);
+void xhci_destroy_dev(hci_t *, int slot_id);
+
+void xhci_reset_event_ring(event_ring_t *);
+void xhci_advance_event_ring(xhci_t *);
+void xhci_update_event_dq(xhci_t *);
+void xhci_handle_events(xhci_t *);
+int xhci_wait_for_command_aborted(xhci_t *, const trb_t *);
+int xhci_wait_for_command_done(xhci_t *, const trb_t *, int clear_event);
+int xhci_wait_for_transfer(xhci_t *, const int slot_id, const int ep_id);
+
+void xhci_clear_trb(trb_t *, int pcs);
+
+trb_t *xhci_next_command_trb(xhci_t *);
+void xhci_post_command(xhci_t *);
+int xhci_cmd_enable_slot(xhci_t *, int *slot_id);
+int xhci_cmd_disable_slot(xhci_t *, int slot_id);
+int xhci_cmd_address_device(xhci_t *, int slot_id, inputctx_t *);
+int xhci_cmd_configure_endpoint(xhci_t *, int slot_id, int config_id, inputctx_t *);
+int xhci_cmd_evaluate_context(xhci_t *, int slot_id, inputctx_t *);
+int xhci_cmd_reset_endpoint(xhci_t *, int slot_id, int ep);
+int xhci_cmd_stop_endpoint(xhci_t *, int slot_id, int ep);
+int xhci_cmd_set_tr_dq(xhci_t *, int slot_id, int ep, trb_t *, int dcs);
+
+static inline int xhci_ep_id(const endpoint_t *const ep) {
+	return ((ep->endpoint & 0x7f) << 1) + (ep->direction == IN);
+}
+
+
+#ifdef XHCI_DUMPS
+void xhci_dump_slotctx(const slotctx_t *);
+void xhci_dump_epctx(const epctx_t *);
+void xhci_dump_devctx(const devctx_t *, const u32 ctx_mask);
+void xhci_dump_inputctx(const inputctx_t *);
+void xhci_dump_transfer_trb(const trb_t *);
+void xhci_dump_transfer_trbs(const trb_t *first, const trb_t *last);
+#else
+#define xhci_dump_slotctx(args...)		do {} while(0)
+#define xhci_dump_epctx(args...)		do {} while(0)
+#define xhci_dump_devctx(args...)		do {} while(0)
+#define xhci_dump_inputctx(args...)		do {} while(0)
+#define xhci_dump_transfer_trb(args...)		do {} while(0)
+#define xhci_dump_transfer_trbs(args...)	do {} while(0)
+#endif
+
 #endif
diff --git a/payloads/libpayload/drivers/usb/xhci_rh.c b/payloads/libpayload/drivers/usb/xhci_rh.c
index 58eb6c328d..e6052be251 100644
--- a/payloads/libpayload/drivers/usb/xhci_rh.c
+++ b/payloads/libpayload/drivers/usb/xhci_rh.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the libpayload project.
  *
- * Copyright (C) 2010 Patrick Georgi
+ * Copyright (C) 2013 secunet Security Networks AG
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -29,105 +29,111 @@
 
 #define USB_DEBUG
 
-#include <libpayload.h>
+#include <usb/usb.h>
+#include "generic_hub.h"
 #include "xhci_private.h"
 #include "xhci.h"
 
-typedef struct {
-	int numports;
-	int *port;
-} rh_inst_t;
-
-#define RH_INST(dev) ((rh_inst_t*)(dev)->data)
-
-static void
-xhci_rh_enable_port (usbdev_t *dev, int port)
+static int
+xhci_rh_hub_status_changed(usbdev_t *const dev)
 {
-	// FIXME: check power situation?
-	// enable slot
-	// attach device context to slot
-	// address device
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	const int changed = !!(xhci->opreg->usbsts & USBSTS_PCD);
+	if (changed)
+		xhci->opreg->usbsts =
+			(xhci->opreg->usbsts & USBSTS_PRSRV_MASK) | USBSTS_PCD;
+	return changed;
 }
 
-/* disable root hub */
-static void
-xhci_rh_disable_port (usbdev_t *dev, int port)
+static int
+xhci_rh_port_status_changed(usbdev_t *const dev, const int port)
 {
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	volatile u32 *const portsc = &xhci->opreg->prs[port - 1].portsc;
+
+	const int changed = !!(*portsc & PORTSC_CSC);
+	/* always clear all the status change bits */
+	*portsc = (*portsc & PORTSC_RW_MASK) | 0x00ef0000;
+	return changed;
 }
 
-static void
-xhci_rh_scanport (usbdev_t *dev, int port)
+static int
+xhci_rh_port_connected(usbdev_t *const dev, const int port)
 {
-	// clear CSC
-	int val = XHCI_INST (dev->controller)->opreg->prs[port].portsc;
-	val &= PORTSC_RW_MASK;
-	val |= PORTSC_CSC;
-	XHCI_INST (dev->controller)->opreg->prs[port].portsc = val;
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	volatile u32 *const portsc = &xhci->opreg->prs[port - 1].portsc;
 
-	usb_debug("device attach status on port %x: %x\n", port, XHCI_INST (dev->controller)->opreg->prs[port].portsc & PORTSC_CCS);
+	return *portsc & PORTSC_CCS;
 }
 
 static int
-xhci_rh_report_port_changes (usbdev_t *dev)
+xhci_rh_port_in_reset(usbdev_t *const dev, const int port)
 {
-	int i;
-	// no change
-	if (!(XHCI_INST (dev->controller)->opreg->usbsts & USBSTS_PCD))
-		return -1;
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	volatile u32 *const portsc = &xhci->opreg->prs[port - 1].portsc;
 
-	for (i = 0; i < RH_INST (dev)->numports; i++) {
-		if (XHCI_INST (dev->controller)->opreg->prs[i].portsc & PORTSC_CSC) {
-			usb_debug("found connect status change on port %d\n", i);
-			return i;
-		}
-	}
-
-	return -1; // shouldn't ever happen
+	return !!(*portsc & PORTSC_PR);
 }
 
-static void
-xhci_rh_destroy (usbdev_t *dev)
+static int
+xhci_rh_port_enabled(usbdev_t *const dev, const int port)
 {
-	int i;
-	for (i = 0; i < RH_INST (dev)->numports; i++)
-		xhci_rh_disable_port (dev, i);
-	free (RH_INST (dev));
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	volatile u32 *const portsc = &xhci->opreg->prs[port - 1].portsc;
+
+	return !!(*portsc & PORTSC_PED);
 }
 
-static void
-xhci_rh_poll (usbdev_t *dev)
+static int
+xhci_rh_port_speed(usbdev_t *const dev, const int port)
 {
-	int port;
-	while ((port = xhci_rh_report_port_changes (dev)) != -1)
-		xhci_rh_scanport (dev, port);
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	volatile u32 *const portsc = &xhci->opreg->prs[port - 1].portsc;
+
+	if (*portsc & PORTSC_PED) {
+		return ((*portsc & PORTSC_PORT_SPEED_MASK)
+				>> PORTSC_PORT_SPEED_START)
+			- 1;
+	} else {
+		return -1;
+	}
 }
 
-void
-xhci_rh_init (usbdev_t *dev)
+static int
+xhci_rh_start_port_reset(usbdev_t *const dev, const int port)
 {
-	int i;
-
-	dev->destroy = xhci_rh_destroy;
-	dev->poll = xhci_rh_poll;
-
-	dev->data = malloc (sizeof (rh_inst_t));
-	if (!dev->data)
-		fatal("Not enough memory for XHCI RH.\n");
+	xhci_t *const xhci = XHCI_INST(dev->controller);
+	volatile u32 *const portsc = &xhci->opreg->prs[port - 1].portsc;
 
-	RH_INST (dev)->numports = XHCI_INST (dev->controller)->capreg->MaxPorts;
-	RH_INST (dev)->port = malloc(sizeof(int) * RH_INST (dev)->numports);
-	usb_debug("%d ports registered\n", RH_INST (dev)->numports);
+	*portsc = (*portsc & PORTSC_RW_MASK) | PORTSC_PR;
+	return 0;
+}
 
-	for (i = 0; i < RH_INST (dev)->numports; i++) {
-		xhci_rh_enable_port (dev, i);
-		RH_INST (dev)->port[i] = -1;
-	}
+static const generic_hub_ops_t xhci_rh_ops = {
+	.hub_status_changed	= xhci_rh_hub_status_changed,
+	.port_status_changed	= xhci_rh_port_status_changed,
+	.port_connected		= xhci_rh_port_connected,
+	.port_in_reset		= xhci_rh_port_in_reset,
+	.port_enabled		= xhci_rh_port_enabled,
+	.port_speed		= xhci_rh_port_speed,
+	.enable_port		= NULL,
+	.disable_port		= NULL,
+	.start_port_reset	= xhci_rh_start_port_reset,
+	.reset_port		= generic_hub_rh_resetport,
+};
 
+void
+xhci_rh_init (usbdev_t *dev)
+{
 	/* we can set them here because a root hub _really_ shouldn't
 	   appear elsewhere */
 	dev->address = 0;
 	dev->hub = -1;
 	dev->port = -1;
 
-	usb_debug("rh init done\n");
+	const int num_ports = /* TODO: maybe we need to read extended caps */
+		(XHCI_INST(dev->controller)->capreg->hcsparams1 >> 24) & 0xff;
+	generic_hub_init(dev, num_ports, &xhci_rh_ops);
+
+	usb_debug("xHCI: root hub init done\n");
 }