libpayload: Add USB device mode driver

Add a framework for USB device mode controllers
and a driver for the ChipIdea controller which
is part of the tegra platform.

TODO:
- fix USB detach/attach
- implement zero length packet handling properly

BUG=chrome-os-partner:35861
TEST=none
BRANCH=none

Change-Id: I8defeea78b5a3bdbf9c1b1222c2702eaf3256b81
Signed-off-by: Patrick Georgi <pgeorgi@chromium.org>
Original-Commit-Id: 542332291880c4026a05a960ceb91d37891ee018
Original-Change-Id: Ib4068d201dd63ebeda80157bd3130f3059919cdd
Original-Signed-off-by: Patrick Georgi <pgeorgi@chromium.org>
Original-Reviewed-on: https://chromium-review.googlesource.com/243272
Original-Reviewed-by: Aaron Durbin <adurbin@chromium.org>
Reviewed-on: http://review.coreboot.org/8756
Tested-by: build bot (Jenkins)
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>

3 files changed, 967 insertions, 0 deletions

diff --git a/payloads/libpayload/drivers/udc/chipidea.c b/payloads/libpayload/drivers/udc/chipidea.c
new file mode 100644
index 0000000000..e06f0d2824
--- /dev/null
+++ b/payloads/libpayload/drivers/udc/chipidea.c
@@ -0,0 +1,472 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2015 Google Inc.
+ *
+ * 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 <libpayload.h>
+#include <arch/cache.h>
+#include <assert.h>
+#include <endian.h>
+#include <queue.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <usb/usb.h>
+
+#include <udc/udc.h>
+#include <udc/chipidea.h>
+#include "chipidea_priv.h"
+
+#ifdef DEBUG
+#define debug(x...) printf(x)
+#else
+#define debug(x...) do {} while (0)
+#endif
+
+#define min(a, b) (((a) < (b)) ? (a) : (b))
+
+static struct qh *get_qh(struct chipidea_pdata *p, int endpoint, int in_dir)
+{
+	assert(in_dir <= 1);
+	return &p->qhlist[2 * endpoint + in_dir];
+}
+
+static unsigned int ep_to_bits(int ep, int in_dir)
+{
+	return ep + (in_dir ? 16 : 0);
+}
+
+static void clear_setup_ep(struct chipidea_pdata *p, int endpoint)
+{
+	writel(1 << endpoint, &p->opreg->epsetupstat);
+}
+
+static void clear_ep(struct chipidea_pdata *p, int endpoint, int in_dir)
+{
+	writel(1 << ep_to_bits(endpoint, in_dir), &p->opreg->epcomplete);
+}
+
+static int chipidea_hw_init(struct usbdev_ctrl *this, void *_opreg,
+	const device_descriptor_t *dd)
+{
+	struct chipidea_opreg *opreg = _opreg;
+	struct chipidea_pdata *p = CI_PDATA(this);
+
+	p->opreg = phys_to_virt(opreg);
+	p->qhlist = dma_memalign(4096, sizeof(struct qh) * CI_QHELEMENTS);
+	memcpy(&this->device_descriptor, dd, sizeof(*dd));
+
+	if (p->qhlist == NULL)
+		die("failed to allocate memory for usb device mode");
+
+	memset(p->qhlist, 0, sizeof(struct qh) * CI_QHELEMENTS);
+
+	SLIST_INIT(&this->configs);
+
+	int i;
+	for (i = 0; i < 16; i++) {
+		SIMPLEQ_INIT(&p->job_queue[i][0]);
+		SIMPLEQ_INIT(&p->job_queue[i][1]);
+	}
+
+	for (i = 0; i < CI_QHELEMENTS; i++) {
+		p->qhlist[i].config = QH_MPS(512) | QH_NO_AUTO_ZLT | QH_IOS;
+		p->qhlist[i].td.next = TD_TERMINATE;
+	}
+	/* EP0 in/out are hardwired for SETUP */
+	p->qhlist[0].config = QH_MPS(64) | QH_NO_AUTO_ZLT | QH_IOS;
+	p->qhlist[1].config = QH_MPS(64) | QH_NO_AUTO_ZLT | QH_IOS;
+
+	do {
+		debug("waiting for usb phy clk valid: %x\n",
+			readl(&p->opreg->susp_ctrl));
+		mdelay(1);
+	} while ((readl(&p->opreg->susp_ctrl) & (1 << 7)) == 0);
+
+	writel(USBCMD_8MICRO | USBCMD_RST, &p->opreg->usbcmd);
+	mdelay(1);
+
+	/* enable device mode */
+	writel(2, &p->opreg->usbmode);
+
+	dcache_clean_by_mva(p->qhlist, sizeof(struct qh) * CI_QHELEMENTS);
+
+	writel(virt_to_phys(p->qhlist), &p->opreg->epbase);
+	writel(0xffffffff, &p->opreg->epflush);
+
+	/* enable EP0 */
+	writel((1 << 23) | (1 << 22) | (1 << 7) | (1 << 6),
+		&p->opreg->epctrl[0]);
+
+	/* clear status register */
+	writel(readl(&p->opreg->usbsts), &p->opreg->usbsts);
+
+	debug("taking controller out of reset\n");
+	writel(USBCMD_8MICRO | USBCMD_RUN, &p->opreg->usbcmd);
+
+	return 1;
+}
+
+static void chipidea_halt_ep(struct usbdev_ctrl *this, int ep, int in_dir)
+{
+	struct chipidea_pdata *p = CI_PDATA(this);
+	writel(1 << ep_to_bits(ep, in_dir), &p->opreg->epflush);
+	while (readl(&p->opreg->epflush))
+		;
+	clrbits_le32(&p->opreg->epctrl[ep], 1 << (7 + (in_dir ? 16 : 0)));
+}
+
+static void chipidea_start_ep(struct usbdev_ctrl *this,
+	int ep, int in_dir, int ep_type, int mps)
+{
+	struct chipidea_pdata *p = CI_PDATA(this);
+	struct qh *qh = get_qh(p, ep, in_dir);
+	qh->config = (mps << 16) | QH_NO_AUTO_ZLT | QH_IOS;
+	dcache_clean_by_mva(qh, sizeof(*qh));
+	in_dir = in_dir ? 1 : 0;
+	debug("enabling %d-%d (type %d)\n", ep, in_dir, ep_type);
+	/* enable endpoint, reset data toggle */
+	setbits_le32(&p->opreg->epctrl[ep],
+		((1 << 7) | (1 << 6) | (ep_type << 2)) << (in_dir*16));
+	p->ep_busy[ep][in_dir] = 0;
+}
+
+static void advance_endpoint(struct chipidea_pdata *p, int endpoint, int in_dir)
+{
+	if (p->ep_busy[endpoint][in_dir])
+		return;
+	if (SIMPLEQ_EMPTY(&p->job_queue[endpoint][in_dir]))
+		return;
+
+	struct job *job = SIMPLEQ_FIRST(&p->job_queue[endpoint][in_dir]);
+	struct qh *qh = get_qh(p, endpoint, in_dir);
+
+	uint32_t start = (uint32_t)(uintptr_t)job->data;
+	uint32_t offset = (start & 0xfff);
+	/* unlike with typical EHCI controllers,
+	 * a full TD transfers either 0x5000 bytes if
+	 * page aligned or 0x4000 bytes if not.
+	 */
+	int maxsize = 0x5000;
+	if (offset > 0)
+		maxsize = 0x4000;
+	uint32_t td_count = (job->length + maxsize - 1) / maxsize;
+
+	/* special case for zero length packets */
+	if (td_count == 0)
+		td_count = 1;
+
+	if (job->zlp)
+		td_count++;
+
+	struct td *tds = dma_memalign(32, sizeof(struct td) * td_count);
+	memset(tds, 0, sizeof(struct td) * td_count);
+
+	int i;
+	int remaining = job->length;
+	for (i = 0; i < td_count; i++) {
+		int datacount = min(maxsize, remaining);
+
+		debug("td %d, %d bytes\n", i, datacount);
+		tds[i].next = (uint32_t)virt_to_phys(&tds[i+1]);
+		tds[i].info = TD_INFO_LEN(datacount) | TD_INFO_ACTIVE;
+		tds[i].page0 = start;
+		tds[i].page1 = (start & 0xfffff000) + 0x1000;
+		tds[i].page2 = (start & 0xfffff000) + 0x2000;
+		tds[i].page3 = (start & 0xfffff000) + 0x3000;
+		tds[i].page4 = (start & 0xfffff000) + 0x4000;
+		remaining -= datacount;
+		start = start + datacount;
+	}
+	tds[td_count - 1].next = TD_TERMINATE;
+	tds[td_count - 1].info |= TD_INFO_IOC;
+
+	qh->td.next = (uint32_t)virt_to_phys(tds);
+	qh->td.info = 0;
+
+	job->tds = tds;
+	job->td_count = td_count;
+
+	dcache_clean_by_mva(tds, sizeof(struct td) * td_count);
+	dcache_clean_by_mva(job->data, job->length);
+	dcache_clean_by_mva(qh, sizeof(*qh));
+
+	debug("priming EP %d-%d with %zx bytes starting at %x (%p)\n", endpoint,
+		in_dir, job->length, tds[0].page0, job->data);
+	writel(1 << ep_to_bits(endpoint, in_dir), &p->opreg->epprime);
+	while (readl(&p->opreg->epprime))
+		;
+	p->ep_busy[endpoint][in_dir] = 1;
+}
+
+static void handle_endpoint(struct usbdev_ctrl *this, int endpoint, int in_dir)
+{
+	struct chipidea_pdata *p = CI_PDATA(this);
+	struct job *job = SIMPLEQ_FIRST(&p->job_queue[endpoint][in_dir]);
+	SIMPLEQ_REMOVE_HEAD(&p->job_queue[endpoint][in_dir], queue);
+
+	if (in_dir)
+		dcache_invalidate_by_mva(job->data, job->length);
+
+	int length = job->length;
+
+	int i = 0;
+	do {
+		int active;
+		do {
+			dcache_invalidate_by_mva(&job->tds[i],
+				sizeof(struct td));
+			active = job->tds[i].info & TD_INFO_ACTIVE;
+			debug("%d-%d: info %08x, page0 %x, next %x\n",
+				endpoint, in_dir, job->tds[i].info,
+				job->tds[i].page0, job->tds[i].next);
+		} while (active);
+		/*
+		 * The controller writes back the length field in info
+		 * with the number of bytes it did _not_ process.
+		 * Hence, take the originally scheduled length and
+		 * subtract whatever lengths we still find - that gives
+		 * us the data that the controller did transfer.
+		 */
+		int remaining = job->tds[i].info >> 16;
+		length -= remaining;
+	} while (job->tds[i++].next != TD_TERMINATE);
+	debug("%d-%d: scheduled %zd, now %d bytes\n", endpoint, in_dir,
+		job->length, length);
+
+	if (this->current_config &&
+	    this->current_config->interfaces[0].handle_packet)
+		this->current_config->interfaces[0].handle_packet(this,
+			endpoint, in_dir, job->data, length);
+
+	free(job->tds);
+	if (job->autofree)
+		free(job->data);
+	free(job);
+	p->ep_busy[endpoint][in_dir] = 0;
+
+	advance_endpoint(p, endpoint, in_dir);
+}
+
+static void start_setup(struct usbdev_ctrl *this, int ep)
+{
+	dev_req_t dr;
+	struct chipidea_pdata *p = CI_PDATA(this);
+	struct qh *qh = get_qh(p, ep, 0);
+
+	dcache_invalidate_by_mva(qh, sizeof(*qh));
+	memcpy(&dr, qh->setup_data, sizeof(qh->setup_data));
+	clear_setup_ep(p, ep);
+
+#ifdef DEBUG
+	hexdump((unsigned long)&dr, sizeof(dr));
+#endif
+
+	udc_handle_setup(this, ep, &dr);
+}
+
+
+static void chipidea_enqueue_packet(struct usbdev_ctrl *this, int endpoint,
+	int in_dir, void *data, int len, int zlp, int autofree)
+{
+	struct chipidea_pdata *p = CI_PDATA(this);
+	struct job *job = malloc(sizeof(*job));
+
+	job->data = data;
+	job->length = len;
+	job->zlp = zlp;
+	job->autofree = autofree;
+
+	debug("adding job of %d bytes to EP %d-%d\n", len, endpoint, in_dir);
+	SIMPLEQ_INSERT_TAIL(&p->job_queue[endpoint][in_dir], job, queue);
+
+	if ((endpoint == 0) || (this->initialized))
+		advance_endpoint(p, endpoint, in_dir);
+}
+
+static int chipidea_poll(struct usbdev_ctrl *this)
+{
+	struct chipidea_pdata *p = CI_PDATA(this);
+	uint32_t sts = readl(&p->opreg->usbsts);
+	writel(sts, &p->opreg->usbsts); /* clear */
+
+	/* new information if the bus is high speed or not */
+	if (sts & USBSTS_PCI) {
+		debug("USB speed negotiation: ");
+		if ((readl(&p->opreg->devlc) & DEVLC_HOSTSPEED_MASK)
+		   == DEVLC_HOSTSPEED(2)) {
+			debug("high speed\n");
+			// TODO: implement
+		} else {
+			debug("full speed\n");
+			// TODO: implement
+		}
+	}
+
+	/* reset requested. stop all activities */
+	if (sts & USBSTS_URI) {
+		int i;
+		debug("USB reset requested\n");
+		if (this->initialized) {
+			writel(readl(&p->opreg->epstat), &p->opreg->epstat);
+			writel(readl(&p->opreg->epsetupstat),
+				&p->opreg->epsetupstat);
+			writel(0xffffffff, &p->opreg->epflush);
+			for (i = 1; i < 16; i++)
+				writel(0, &p->opreg->epctrl[i]);
+			this->initialized = 0;
+		}
+		writel((1 << 22) | (1 << 6), &p->opreg->epctrl[0]);
+		p->qhlist[0].config = QH_MPS(64) | QH_NO_AUTO_ZLT | QH_IOS;
+		p->qhlist[1].config = QH_MPS(64) | QH_NO_AUTO_ZLT | QH_IOS;
+		dcache_clean_by_mva(p->qhlist, 2 * sizeof(struct qh));
+	}
+
+	if (sts & (USBSTS_UEI | USBSTS_UI)) {
+		uint32_t bitmap = readl(&p->opreg->epsetupstat);
+		int ep = 0;
+		while (bitmap) {
+			if (bitmap & 1) {
+				debug("incoming packet on EP %d (setup)\n", ep);
+				start_setup(this, ep);
+			}
+			bitmap >>= 1;
+			ep++;
+		}
+		bitmap = readl(&p->opreg->epcomplete);
+		ep = 0;
+		int dir_in = 0;
+		while (bitmap) {
+			if (bitmap & 1) {
+				debug("incoming packet on EP %d (%s)\n",
+					ep, dir_in ? "intr/in" : "out");
+				handle_endpoint(this, ep & 0xf, dir_in);
+				clear_ep(p, ep & 0xf, dir_in);
+			}
+			bitmap >>= 1;
+			ep++;
+			if (ep == 16)
+				dir_in = 1;
+		}
+	}
+
+	return 1;
+}
+
+static void chipidea_shutdown(struct usbdev_ctrl *this)
+{
+	struct chipidea_pdata *p = CI_PDATA(this);
+	int i, j;
+	int is_empty = 0;
+	while (!is_empty) {
+		is_empty = 1;
+		this->poll(this);
+		for (i = 0; i < 16; i++)
+			for (j = 0; j < 2; j++)
+				if (!SIMPLEQ_EMPTY(&p->job_queue[i][j]))
+					is_empty = 0;
+	}
+	writel(0xffffffff, &p->opreg->epflush);
+	writel(USBCMD_8MICRO | USBCMD_RST, &p->opreg->usbcmd);
+	writel(0, &p->opreg->usbmode);
+	writel(USBCMD_8MICRO, &p->opreg->usbcmd);
+	free(p->qhlist);
+	free(p);
+	free(this);
+}
+
+static void chipidea_set_address(struct usbdev_ctrl *this, int address)
+{
+	struct chipidea_pdata *p = CI_PDATA(this);
+	writel((address << 25) | (1 << 24), &p->opreg->usbadr);
+}
+
+static void chipidea_stall(struct usbdev_ctrl *this,
+	uint8_t ep, int in_dir, int set)
+{
+	struct chipidea_pdata *p = CI_PDATA(this);
+	assert(ep < 16);
+	uint32_t *ctrl = &p->opreg->epctrl[ep];
+	in_dir = in_dir ? 1 : 0;
+	if (set) {
+		if (in_dir)
+			setbits_le32(ctrl, 1 << 16);
+		else
+			setbits_le32(ctrl, 1 << 0);
+	} else {
+		/* reset STALL bit, reset data toggle */
+		if (in_dir) {
+			setbits_le32(ctrl, 1 << 22);
+			clrbits_le32(ctrl, 1 << 16);
+		} else {
+			setbits_le32(ctrl, 1 << 6);
+			setbits_le32(ctrl, 1 << 0);
+		}
+	}
+	this->ep_halted[ep][in_dir] = set;
+}
+
+static void *chipidea_malloc(size_t size)
+{
+	return dma_malloc(size);
+}
+
+static void chipidea_free(void *ptr)
+{
+	free(ptr);
+}
+
+struct usbdev_ctrl *chipidea_init(device_descriptor_t *dd)
+{
+	struct usbdev_ctrl *ctrl = calloc(1, sizeof(*ctrl));
+	if (ctrl == NULL)
+		return NULL;
+	ctrl->pdata = calloc(1, sizeof(struct chipidea_pdata));
+	if (ctrl->pdata == NULL) {
+		free(ctrl);
+		return NULL;
+	}
+
+	ctrl->poll = chipidea_poll;
+	ctrl->add_gadget = udc_add_gadget;
+	ctrl->enqueue_packet = chipidea_enqueue_packet;
+	ctrl->shutdown = chipidea_shutdown;
+	ctrl->set_address = chipidea_set_address;
+	ctrl->stall = chipidea_stall;
+	ctrl->halt_ep = chipidea_halt_ep;
+	ctrl->start_ep = chipidea_start_ep;
+	ctrl->alloc_data = chipidea_malloc;
+	ctrl->free_data = chipidea_free;
+	ctrl->initialized = 0;
+
+	if (!chipidea_hw_init(ctrl, (void *)0x7d000000, dd)) {
+		free(ctrl->pdata);
+		free(ctrl);
+		return NULL;
+	}
+	return ctrl;
+}
diff --git a/payloads/libpayload/drivers/udc/chipidea_priv.h b/payloads/libpayload/drivers/udc/chipidea_priv.h
new file mode 100644
index 0000000000..ede97ab264
--- /dev/null
+++ b/payloads/libpayload/drivers/udc/chipidea_priv.h
@@ -0,0 +1,168 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2015 Google Inc.
+ *
+ * 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.
+ */
+
+#ifndef __CHIPIDEA_PRIV_H__
+#define __CHIPIDEA_PRIV_H__
+
+#include <queue.h>
+
+struct chipidea_opreg {
+	uint8_t pad0[0x130];
+	uint32_t usbcmd; // 0x130
+	uint32_t usbsts; // 0x134
+	uint32_t pad138[3];
+	uint32_t usbadr; // 0x144
+		/* 31:25: address
+		 * 24: staging: 1 -> commit new address after
+		 *                   next ctrl-in on ep0
+		 */
+	uint32_t epbase; // 0x148
+	uint32_t pad14c[10];
+	uint32_t portsc; // 0x174
+	uint32_t pad178[15];
+	uint32_t devlc; // 0x1b4
+		/* 25:26: host-desired usb version
+		 * 23: force full speed */
+	uint32_t pad1b8[16];
+	uint32_t usbmode; // 0x1f8
+		/* 0:1: 2 -> device mode */
+	uint32_t pad1fc[3];
+	uint32_t epsetupstat; // 0x208
+		/* 0:15: 1 -> epX received setup packet */
+	uint32_t epprime; // 0x20c
+		/*  0:15: 1 -> rx buffer for epX (OUT) is primed
+		 *             (ie. ready for controller-side processing)
+		 * 16:31: 1 -> tx buffer for ep(X-16) (IN/INTR) is primed
+		 *             (ie. ready for controller-side processing)
+		 *
+		 * controller will read new td from qh and process it,
+		 * then set the bit to 0
+		 */
+	uint32_t epflush; // 0x210
+		/* 0:31: 1 -> flush buffer (as defined in epprime),
+		 *            so it's uninitialized again.
+		 * controller resets to 0 when done
+		 */
+	uint32_t epstat; // 0x214
+		/* 0:31: 1 -> command in epprime is done, EP is ready
+		 *           (which may be later than epprime reset)
+		 */
+	uint32_t epcomplete; // 0x218
+		/*  0:15: 1 -> incoming out/setup packet for epX was handled.
+		 *             software should check QH state
+		 * 16:31: 1 -> incoming intr/in packet for ep(X-16) was
+		 *             handled. software should check QH state
+		 */
+	uint32_t epctrl[16]; // 0x21c
+		/* epctrl[0] is hardcoded as enabled control endpoint.
+		 * TXS/RXS for stalling can be written.
+		 *
+		 * 23: TXE tx endpoint enable
+		 * 22: TXR reset tx data toggle (for every configuration event)
+		 * 18:19: 0=ctrl, 1=isoc, 2=bulk, 3=intr endpoint
+		 * 16: TXS stall tx
+		 *
+		 *  7: RXE rx endpoint enable
+		 *  6: RXR reset rx data toggle (for every configuration event)
+		 *  2:3: endpoint type (like 18:19)
+		 *  0: RXS stall rx
+		 */
+	uint32_t pad25c[0x69]; // 0x25c
+	uint32_t susp_ctrl; // 0x400
+};
+
+#define CI_PDATA(ctrl) ((struct chipidea_pdata *)((ctrl)->pdata))
+#define CI_QHELEMENTS 32
+
+#define QH_NO_AUTO_ZLT (1 << 29) /* no automatic ZLT handling by chipset */
+#define QH_MPS(x) ((x) << 16)
+#define QH_IOS (1 << 15) /* IRQ on setup */
+
+#define TD_INFO_LEN(x) ((x) << 16)
+#define TD_INFO_IOC (1 << 15)
+#define TD_INFO_ACTIVE (1 << 7)
+#define TD_TERMINATE 1
+
+#define USBCMD_8MICRO (8 << 16)
+#define USBCMD_RST 2
+#define USBCMD_RUN 1
+
+#define USBSTS_SLI (1 << 8)
+#define USBSTS_URI (1 << 6)
+#define USBSTS_PCI (1 << 2)
+#define USBSTS_UEI (1 << 1)
+#define USBSTS_UI  (1 << 0)
+
+#define DEVLC_HOSTSPEED(x) (x << 25)
+#define DEVLC_HOSTSPEED_MASK DEVLC_HOSTSPEED(3)
+
+struct td {
+	/* points to next td */
+	uint32_t next;
+	uint32_t info;
+	/* page0..4 are like EHCI pages: up to 4k each
+	 * page0 from addr to page end, page4 to its length
+	 */
+	uint32_t page0;
+	uint32_t page1;
+	uint32_t page2;
+	uint32_t page3;
+	uint32_t page4;
+	uint32_t res;
+};
+
+struct qh {
+	uint32_t config;
+	uint32_t current;
+	struct td td;
+	/* contains the data of a setup request */
+	uint8_t setup_data[8];
+	uint32_t res[4];
+};
+
+struct job {
+	SIMPLEQ_ENTRY(job) queue; // linkage
+	struct td *tds; // for later free()ing
+	int td_count;
+	void *data;
+	size_t length;
+	int zlp; // append zero length packet?
+	int autofree; // free after processing?
+};
+
+SIMPLEQ_HEAD(job_queue, job);
+
+struct chipidea_pdata {
+	struct chipidea_opreg *opreg;
+	struct qh *qhlist;
+	struct job_queue job_queue[16][2];
+	int ep_busy[16][2];
+};
+
+#endif
diff --git a/payloads/libpayload/drivers/udc/udc.c b/payloads/libpayload/drivers/udc/udc.c
new file mode 100644
index 0000000000..cdc2b29f95
--- /dev/null
+++ b/payloads/libpayload/drivers/udc/udc.c
@@ -0,0 +1,327 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2015 Google Inc.
+ *
+ * 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 <libpayload.h>
+#include <arch/cache.h>
+#include <assert.h>
+#include <endian.h>
+#include <queue.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <usb/usb.h>
+
+#include <udc/udc.h>
+
+#ifdef DEBUG
+#define debug(x...) printf(x)
+#else
+#define debug(x...) do {} while (0)
+#endif
+
+#define min(a, b) (((a) < (b)) ? (a) : (b))
+
+// TODO: make this right
+#define ZLP(len, explen) 0
+
+static struct usbdev_configuration *fetch_config(struct usbdev_ctrl *this,
+	int id)
+{
+	struct usbdev_configuration *config;
+	SLIST_FOREACH(config, &this->configs, list) {
+		debug("checking descriptor %d\n",
+			config->descriptor.bConfigurationValue);
+		if (config->descriptor.bConfigurationValue == id)
+			return config;
+	}
+	return NULL;
+}
+
+static void enable_interface(struct usbdev_ctrl *this, int iface_num)
+{
+	struct usbdev_configuration *config = this->current_config;
+	struct usbdev_interface *iface = &config->interfaces[iface_num];
+
+	/* first: shut down all endpoints except EP0 */
+	int i;
+	for (i = 1; i < 16; i++) {
+		/* disable endpoints */
+		this->halt_ep(this, i, 0);
+		this->halt_ep(this, i, 1);
+	}
+
+	/* now enable all configured endpoints */
+	int epcount = iface->descriptor.bNumEndpoints;
+	for (i = 0; i < epcount; i++) {
+		int ep = iface->eps[i].bEndpointAddress;
+		int mps = iface->eps[i].wMaxPacketSize;
+		int in_dir = 0;
+		if (ep & 0x80) {
+			in_dir = 1;
+			ep &= 0x7f;
+		}
+		int ep_type = iface->eps[i].bmAttributes & 0x3;
+		this->start_ep(this, ep, in_dir, ep_type, mps);
+	}
+
+	// gadget specific configuration
+	if (iface->init)
+		iface->init(this);
+}
+
+/**
+ * handle default control transfers on EP 0
+ *
+ * returns 1 if transfer was handled
+ */
+static int setup_ep0(struct usbdev_ctrl *this, dev_req_t *dr)
+{
+	if ((dr->bmRequestType == 0x00) &&
+	    (dr->bRequest == SET_ADDRESS)) {
+		this->set_address(this, dr->wValue & 0x7f);
+
+		/* status phase IN */
+		this->enqueue_packet(this, 0, 1, NULL, 0, 0, 0);
+		return 1;
+	} else
+	if ((dr->bmRequestType == 0x00) &&
+	    (dr->bRequest == SET_CONFIGURATION)) {
+		struct usbdev_configuration *config =
+			fetch_config(this, dr->wValue);
+
+		this->current_config = config;
+		this->current_config_id = dr->wValue;
+
+		if (dr->wValue == 0)
+			// TODO: reset device
+			return 1;
+
+		if (config == NULL) {
+			this->stall(this, 0, 0, 1);
+			this->stall(this, 0, 1, 1);
+			return 1;
+		}
+
+		/* status phase IN */
+		this->enqueue_packet(this, 0, 1, NULL, 0, 0, 0);
+
+		/* automatically configure endpoints in interface 0 */
+		enable_interface(this, 0);
+		this->initialized = 1;
+		return 1;
+	} else
+	if ((dr->bmRequestType == 0x80) &&
+	    (dr->bRequest == GET_CONFIGURATION)) {
+		unsigned char *res = dma_malloc(1);
+		res[0] = this->current_config_id;
+
+		/* data phase IN */
+		this->enqueue_packet(this, 0, 1, res, 1, 0, 1);
+
+		// status phase OUT
+		this->enqueue_packet(this, 0, 0, NULL, 0, 0, 0);
+		return 1;
+	} else
+	// ENDPOINT_HALT
+	if ((dr->bmRequestType == 0x02) && // endpoint
+	    (dr->bRequest == CLEAR_FEATURE) &&
+	    (dr->wValue == 0)) {
+		int ep = dr->wIndex;
+		/* clear STALL */
+		this->stall(this, ep & 0xf, ep & 0x80, 0);
+
+		/* status phase IN */
+		this->enqueue_packet(this, 0, 1, NULL, 0, 0, 0);
+		return 1;
+	} else
+	// ENDPOINT_HALT
+	if ((dr->bmRequestType == 0x02) && // endpoint
+	    (dr->bRequest == SET_FEATURE) &&
+	    (dr->wValue == 0)) {
+		int ep = dr->wIndex;
+		/* set STALL */
+		this->stall(this, ep & 0xf, ep & 0x80, 1);
+
+		/* status phase IN */
+		this->enqueue_packet(this, 0, 1, NULL, 0, 0, 0);
+		return 1;
+	} else
+	// DEVICE_REMOTE_WAKEUP
+	if ((dr->bmRequestType == 0x00) &&
+	    (dr->bRequest == CLEAR_FEATURE) &&
+	    (dr->wValue == 1)) {
+		this->remote_wakeup = 0;
+
+		/* status phase IN */
+		this->enqueue_packet(this, 0, 1, NULL, 0, 0, 0);
+		return 1;
+	} else
+	// DEVICE_REMOTE_WAKEUP
+	if ((dr->bmRequestType == 0x00) &&
+	    (dr->bRequest == SET_FEATURE) &&
+	    (dr->wValue == 1)) {
+		this->remote_wakeup = 1;
+
+		/* status phase IN */
+		this->enqueue_packet(this, 0, 1, NULL, 0, 0, 0);
+		return 1;
+	} else
+	if ((dr->bmRequestType == 0x82) && // endpoint
+	    (dr->bRequest == GET_STATUS)) {
+		unsigned char *res = dma_malloc(2);
+		int ep = dr->wIndex;
+		/* is EP halted? */
+		res[0] = this->ep_halted[ep & 0xf][(ep & 0x80) ? 1 : 0];
+		res[1] = 0;
+
+		/* data phase IN */
+		this->enqueue_packet(this, 0, 1, res,
+			min(2, dr->wLength), 0, 1);
+
+		// status phase OUT
+		this->enqueue_packet(this, 0, 0, NULL, 0, 0, 0);
+		return 1;
+	} else
+	if ((dr->bmRequestType == 0x80) &&
+	    (dr->bRequest == GET_STATUS)) {
+		unsigned char *res = dma_malloc(2);
+		res[0] = 1; // self powered
+		if (this->remote_wakeup)
+			res[0] |= 2;
+
+		res[1] = 0;
+
+		/* data phase IN */
+		this->enqueue_packet(this, 0, 1, res,
+			min(2, dr->wLength), 0, 1);
+
+		// status phase OUT
+		this->enqueue_packet(this, 0, 0, NULL, 0, 0, 0);
+		return 1;
+	} else
+	if ((dr->bmRequestType == 0x80) &&
+	    (dr->bRequest == GET_DESCRIPTOR) &&
+	    ((dr->wValue & 0xff00) == 0x0200)) {
+		int i, j;
+		/* config descriptor #id
+		 * since config = 0 is undefined, but descriptors
+		 * should start at 0, add 1 to have them match up.
+		 */
+		int id = (dr->wValue & 0xff) + 1;
+		struct usbdev_configuration *config = fetch_config(this, id);
+		if (config == NULL) {
+			this->stall(this, 0, 0, 1);
+			this->stall(this, 0, 1, 1);
+			return 1;
+		}
+		debug("descriptor found, should be %d bytes\n",
+			config->descriptor.wTotalLength);
+
+		uint8_t *data = dma_malloc(config->descriptor.wTotalLength);
+		uint8_t *head = data;
+
+		memcpy(head, &config->descriptor,
+			sizeof(configuration_descriptor_t));
+		head += sizeof(configuration_descriptor_t);
+
+		for (i = 0; i < config->descriptor.bNumInterfaces; i++) {
+			memcpy(head, &config->interfaces[i].descriptor,
+				sizeof(interface_descriptor_t));
+			head += sizeof(interface_descriptor_t);
+			for (j = 0;
+			     j < config->interfaces[i].descriptor.bNumEndpoints;
+			     j++) {
+				memcpy(head, &config->interfaces[i].eps[j],
+					sizeof(endpoint_descriptor_t));
+				head += sizeof(endpoint_descriptor_t);
+			}
+		}
+		int size = config->descriptor.wTotalLength;
+		assert((head - data) == config->descriptor.wTotalLength);
+
+		/* data phase IN */
+		this->enqueue_packet(this, 0, 1, data,
+			min(size, dr->wLength),
+			ZLP(size, dr->wLength), 1);
+
+		/* status phase OUT */
+		this->enqueue_packet(this, 0, 0, NULL, 0, 0, 0);
+		return 1;
+	} else
+	if ((dr->bmRequestType == 0x80) &&
+	    (dr->bRequest == GET_DESCRIPTOR) &&
+	    ((dr->wValue & 0xff00) == 0x0100)) {
+		device_descriptor_t *dd = dma_malloc(sizeof(*dd));
+		memcpy(dd, &this->device_descriptor, sizeof(*dd));
+		dd->bNumConfigurations = this->config_count;
+
+		/* data phase IN */
+		this->enqueue_packet(this, 0, 1, (void *)dd,
+			min(sizeof(*dd), dr->wLength),
+			ZLP(sizeof(*dd), dr->wLength), 1);
+
+		/* status phase OUT */
+		this->enqueue_packet(this, 0, 0, NULL, 0, 0, 0);
+		return 1;
+	}
+	return 0;
+}
+
+void udc_add_gadget(struct usbdev_ctrl *this,
+	struct usbdev_configuration *config)
+{
+	int i, size;
+	SLIST_INSERT_HEAD(&this->configs, config, list);
+
+	size = sizeof(configuration_descriptor_t);
+
+	for (i = 0; i < config->descriptor.bNumInterfaces; i++) {
+		size += sizeof(config->interfaces[i].descriptor);
+		size += config->interfaces[i].descriptor.bNumEndpoints *
+			sizeof(endpoint_descriptor_t);
+	}
+	config->descriptor.wTotalLength = size;
+	config->descriptor.bConfigurationValue = ++this->config_count;
+}
+
+void udc_handle_setup(struct usbdev_ctrl *this, int ep, dev_req_t *dr)
+{
+	if ((ep == 0) && setup_ep0(this, dr))
+		return;
+
+	if (this->current_config &&
+	    this->current_config->interfaces[0].handle_setup &&
+	    this->current_config->interfaces[0].handle_setup(this, ep, dr))
+		return;
+
+	/* no successful SETUP transfer should end up here, report error */
+	this->halt_ep(this, ep, 0);
+	this->halt_ep(this, ep, 1);
+}
+