Merge branch 'master' of git://git.denx.de/u-boot-usb

12 files changed, 423 insertions, 158 deletions

diff --git a/common/cmd_dfu.c b/common/cmd_dfu.c
index db066acc36..793c422123 100644
--- a/common/cmd_dfu.c
+++ b/common/cmd_dfu.c
@@ -19,8 +19,8 @@ static int do_dfu(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	const char *str_env;
 	char *s = "dfu";
+	int ret, i = 0;
 	char *env_bkp;
-	int ret;
 
 	if (argc < 3)
 		return CMD_RET_USAGE;
@@ -49,6 +49,15 @@ static int do_dfu(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 
 	g_dnl_register(s);
 	while (1) {
+		if (dfu_reset())
+			/*
+			 * This extra number of usb_gadget_handle_interrupts()
+			 * calls is necessary to assure correct transmission
+			 * completion with dfu-util
+			 */
+			if (++i == 10)
+				goto exit;
+
 		if (ctrlc())
 			goto exit;
 
@@ -60,6 +69,9 @@ done:
 	dfu_free_entities();
 	free(env_bkp);
 
+	if (dfu_reset())
+		run_command("reset", 0);
+
 	return CMD_RET_SUCCESS;
 }
 
diff --git a/common/usb_hub.c b/common/usb_hub.c
index 754d436ad4..a11b401e62 100644
--- a/common/usb_hub.c
+++ b/common/usb_hub.c
@@ -110,7 +110,7 @@ static void usb_hub_power_on(struct usb_hub_device *hub)
 		ret = usb_get_port_status(dev, i + 1, portsts);
 		if (ret < 0) {
 			debug("port %d: get_port_status failed\n", i + 1);
-			return;
+			continue;
 		}
 
 		/*
@@ -125,7 +125,7 @@ static void usb_hub_power_on(struct usb_hub_device *hub)
 		portstatus = le16_to_cpu(portsts->wPortStatus);
 		if (portstatus & (USB_PORT_STAT_POWER << 1)) {
 			debug("port %d: Port power change failed\n", i + 1);
-			return;
+			continue;
 		}
 	}
 
diff --git a/drivers/dfu/dfu.c b/drivers/dfu/dfu.c
index b8870ecf73..d73d510391 100644
--- a/drivers/dfu/dfu.c
+++ b/drivers/dfu/dfu.c
@@ -16,9 +16,20 @@
 #include <linux/list.h>
 #include <linux/compiler.h>
 
+static bool dfu_reset_request;
 static LIST_HEAD(dfu_list);
 static int dfu_alt_num;
 
+bool dfu_reset(void)
+{
+	return dfu_reset_request;
+}
+
+void dfu_trigger_reset()
+{
+	dfu_reset_request = true;
+}
+
 static int dfu_find_alt_num(const char *s)
 {
 	int i = 0;
diff --git a/drivers/serial/usbtty.h b/drivers/serial/usbtty.h
index bbabb325f8..e243a8e3b2 100644
--- a/drivers/serial/usbtty.h
+++ b/drivers/serial/usbtty.h
@@ -22,6 +22,8 @@
 #include <usb/pxa27x_udc.h>
 #elif defined(CONFIG_DW_UDC)
 #include <usb/designware_udc.h>
+#elif defined(CONFIG_MV_UDC)
+#include <usb/mv_udc.h>
 #endif
 
 #include <version.h>
diff --git a/drivers/usb/gadget/f_dfu.c b/drivers/usb/gadget/f_dfu.c
index 5321a689d4..37d04a1928 100644
--- a/drivers/usb/gadget/f_dfu.c
+++ b/drivers/usb/gadget/f_dfu.c
@@ -312,6 +312,8 @@ static int state_dfu_idle(struct f_dfu *f_dfu,
 			DFU_STATE_dfuMANIFEST_WAIT_RST;
 		to_runtime_mode(f_dfu);
 		f_dfu->dfu_state = DFU_STATE_appIDLE;
+
+		dfu_trigger_reset();
 		break;
 	default:
 		f_dfu->dfu_state = DFU_STATE_dfuERROR;
diff --git a/drivers/usb/gadget/gadget_chips.h b/drivers/usb/gadget/gadget_chips.h
index f038747e63..aa54b8547e 100644
--- a/drivers/usb/gadget/gadget_chips.h
+++ b/drivers/usb/gadget/gadget_chips.h
@@ -144,7 +144,7 @@
 #define	gadget_is_m66592(g)	0
 #endif
 
-#ifdef CONFIG_USB_GADGET_MV
+#ifdef CONFIG_MV_UDC
 #define gadget_is_mv(g)        (!strcmp("mv_udc", (g)->name))
 #else
 #define gadget_is_mv(g)        0
diff --git a/drivers/usb/gadget/mv_udc.c b/drivers/usb/gadget/mv_udc.c
index 7fa5288803..7574e31a8f 100644
--- a/drivers/usb/gadget/mv_udc.c
+++ b/drivers/usb/gadget/mv_udc.c
@@ -17,6 +17,20 @@
 #include <linux/types.h>
 #include <usb/mv_udc.h>
 
+#if CONFIG_USB_MAX_CONTROLLER_COUNT > 1
+#error This driver only supports one single controller.
+#endif
+
+/*
+ * Check if the system has too long cachelines. If the cachelines are
+ * longer then 128b, the driver will not be able flush/invalidate data
+ * cache over separate QH entries. We use 128b because one QH entry is
+ * 64b long and there are always two QH list entries for each endpoint.
+ */
+#if ARCH_DMA_MINALIGN > 128
+#error This driver can not work on systems with caches longer than 128b
+#endif
+
 #ifndef DEBUG
 #define DBG(x...) do {} while (0)
 #else
@@ -39,8 +53,6 @@ static const char *reqname(unsigned r)
 }
 #endif
 
-#define PAGE_SIZE	4096
-#define QH_MAXNUM	32
 static struct usb_endpoint_descriptor ep0_out_desc = {
 	.bLength = sizeof(struct usb_endpoint_descriptor),
 	.bDescriptorType = USB_DT_ENDPOINT,
@@ -55,8 +67,6 @@ static struct usb_endpoint_descriptor ep0_in_desc = {
 	.bmAttributes =	USB_ENDPOINT_XFER_CONTROL,
 };
 
-struct ept_queue_head *epts;
-struct ept_queue_item *items[2 * NUM_ENDPOINTS];
 static int mv_pullup(struct usb_gadget *gadget, int is_on);
 static int mv_ep_enable(struct usb_ep *ep,
 		const struct usb_endpoint_descriptor *desc);
@@ -79,14 +89,115 @@ static struct usb_ep_ops mv_ep_ops = {
 	.free_request   = mv_ep_free_request,
 };
 
-static struct mv_ep ep[2 * NUM_ENDPOINTS];
+/* Init values for USB endpoints. */
+static const struct usb_ep mv_ep_init[2] = {
+	[0] = {	/* EP 0 */
+		.maxpacket	= 64,
+		.name		= "ep0",
+		.ops		= &mv_ep_ops,
+	},
+	[1] = {	/* EP 1..n */
+		.maxpacket	= 512,
+		.name		= "ep-",
+		.ops		= &mv_ep_ops,
+	},
+};
+
 static struct mv_drv controller = {
-	.gadget = {
-		.ep0 = &ep[0].ep,
-		.name = "mv_udc",
+	.gadget	= {
+		.name	= "mv_udc",
+		.ops	= &mv_udc_ops,
 	},
 };
 
+/**
+ * mv_get_qh() - return queue head for endpoint
+ * @ep_num:	Endpoint number
+ * @dir_in:	Direction of the endpoint (IN = 1, OUT = 0)
+ *
+ * This function returns the QH associated with particular endpoint
+ * and it's direction.
+ */
+static struct ept_queue_head *mv_get_qh(int ep_num, int dir_in)
+{
+	return &controller.epts[(ep_num * 2) + dir_in];
+}
+
+/**
+ * mv_get_qtd() - return queue item for endpoint
+ * @ep_num:	Endpoint number
+ * @dir_in:	Direction of the endpoint (IN = 1, OUT = 0)
+ *
+ * This function returns the QH associated with particular endpoint
+ * and it's direction.
+ */
+static struct ept_queue_item *mv_get_qtd(int ep_num, int dir_in)
+{
+	return controller.items[(ep_num * 2) + dir_in];
+}
+
+/**
+ * mv_flush_qh - flush cache over queue head
+ * @ep_num:	Endpoint number
+ *
+ * This function flushes cache over QH for particular endpoint.
+ */
+static void mv_flush_qh(int ep_num)
+{
+	struct ept_queue_head *head = mv_get_qh(ep_num, 0);
+	const uint32_t start = (uint32_t)head;
+	const uint32_t end = start + 2 * sizeof(*head);
+
+	flush_dcache_range(start, end);
+}
+
+/**
+ * mv_invalidate_qh - invalidate cache over queue head
+ * @ep_num:	Endpoint number
+ *
+ * This function invalidates cache over QH for particular endpoint.
+ */
+static void mv_invalidate_qh(int ep_num)
+{
+	struct ept_queue_head *head = mv_get_qh(ep_num, 0);
+	uint32_t start = (uint32_t)head;
+	uint32_t end = start + 2 * sizeof(*head);
+
+	invalidate_dcache_range(start, end);
+}
+
+/**
+ * mv_flush_qtd - flush cache over queue item
+ * @ep_num:	Endpoint number
+ *
+ * This function flushes cache over qTD pair for particular endpoint.
+ */
+static void mv_flush_qtd(int ep_num)
+{
+	struct ept_queue_item *item = mv_get_qtd(ep_num, 0);
+	const uint32_t start = (uint32_t)item;
+	const uint32_t end_raw = start + 2 * sizeof(*item);
+	const uint32_t end = roundup(end_raw, ARCH_DMA_MINALIGN);
+
+	flush_dcache_range(start, end);
+}
+
+/**
+ * mv_invalidate_qtd - invalidate cache over queue item
+ * @ep_num:	Endpoint number
+ *
+ * This function invalidates cache over qTD pair for particular endpoint.
+ */
+static void mv_invalidate_qtd(int ep_num)
+{
+	struct ept_queue_item *item = mv_get_qtd(ep_num, 0);
+	const uint32_t start = (uint32_t)item;
+	const uint32_t end_raw = start + 2 * sizeof(*item);
+	const uint32_t end = roundup(end_raw, ARCH_DMA_MINALIGN);
+
+	invalidate_dcache_range(start, end);
+}
+
 static struct usb_request *
 mv_ep_alloc_request(struct usb_ep *ep, unsigned int gfp_flags)
 {
@@ -102,9 +213,9 @@ static void mv_ep_free_request(struct usb_ep *ep, struct usb_request *_req)
 static void ep_enable(int num, int in)
 {
 	struct ept_queue_head *head;
-	struct mv_udc *udc = controller.udc;
+	struct mv_udc *udc = (struct mv_udc *)controller.ctrl->hcor;
 	unsigned n;
-	head = epts + 2*num + in;
+	head = mv_get_qh(num, in);
 
 	n = readl(&udc->epctrl[num]);
 	if (in)
@@ -112,8 +223,10 @@ static void ep_enable(int num, int in)
 	else
 		n |= (CTRL_RXE | CTRL_RXR | CTRL_RXT_BULK);
 
-	if (num != 0)
+	if (num != 0) {
 		head->config = CONFIG_MAX_PKT(EP_MAX_PACKET_SIZE) | CONFIG_ZLT;
+		mv_flush_qh(num);
+	}
 	writel(n, &udc->epctrl[num]);
 }
 
@@ -134,40 +247,108 @@ static int mv_ep_disable(struct usb_ep *ep)
 	return 0;
 }
 
+static int mv_bounce(struct mv_ep *ep)
+{
+	uint32_t addr = (uint32_t)ep->req.buf;
+	uint32_t ba;
+
+	/* Input buffer address is not aligned. */
+	if (addr & (ARCH_DMA_MINALIGN - 1))
+		goto align;
+
+	/* Input buffer length is not aligned. */
+	if (ep->req.length & (ARCH_DMA_MINALIGN - 1))
+		goto align;
+
+	/* The buffer is well aligned, only flush cache. */
+	ep->b_len = ep->req.length;
+	ep->b_buf = ep->req.buf;
+	goto flush;
+
+align:
+	/* Use internal buffer for small payloads. */
+	if (ep->req.length <= 64) {
+		ep->b_len = 64;
+		ep->b_buf = ep->b_fast;
+	} else {
+		ep->b_len = roundup(ep->req.length, ARCH_DMA_MINALIGN);
+		ep->b_buf = memalign(ARCH_DMA_MINALIGN, ep->b_len);
+		if (!ep->b_buf)
+			return -ENOMEM;
+	}
+
+	memcpy(ep->b_buf, ep->req.buf, ep->req.length);
+
+flush:
+	ba = (uint32_t)ep->b_buf;
+	flush_dcache_range(ba, ba + ep->b_len);
+
+	return 0;
+}
+
+static void mv_debounce(struct mv_ep *ep)
+{
+	uint32_t addr = (uint32_t)ep->req.buf;
+	uint32_t ba = (uint32_t)ep->b_buf;
+
+	invalidate_dcache_range(ba, ba + ep->b_len);
+
+	/* Input buffer address is not aligned. */
+	if (addr & (ARCH_DMA_MINALIGN - 1))
+		goto copy;
+
+	/* Input buffer length is not aligned. */
+	if (ep->req.length & (ARCH_DMA_MINALIGN - 1))
+		goto copy;
+
+	/* The buffer is well aligned, only invalidate cache. */
+	return;
+
+copy:
+	memcpy(ep->req.buf, ep->b_buf, ep->req.length);
+
+	/* Large payloads use allocated buffer, free it. */
+	if (ep->req.length > 64)
+		free(ep->b_buf);
+}
+
 static int mv_ep_queue(struct usb_ep *ep,
 		struct usb_request *req, gfp_t gfp_flags)
 {
 	struct mv_ep *mv_ep = container_of(ep, struct mv_ep, ep);
-	struct mv_udc *udc = controller.udc;
+	struct mv_udc *udc = (struct mv_udc *)controller.ctrl->hcor;
 	struct ept_queue_item *item;
 	struct ept_queue_head *head;
-	unsigned phys;
-	int bit, num, len, in;
+	int bit, num, len, in, ret;
 	num = mv_ep->desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
 	in = (mv_ep->desc->bEndpointAddress & USB_DIR_IN) != 0;
-	item = items[2 * num + in];
-	head = epts + 2 * num + in;
-	phys = (unsigned)req->buf;
+	item = mv_get_qtd(num, in);
+	head = mv_get_qh(num, in);
 	len = req->length;
 
+	ret = mv_bounce(mv_ep);
+	if (ret)
+		return ret;
+
 	item->next = TERMINATE;
 	item->info = INFO_BYTES(len) | INFO_IOC | INFO_ACTIVE;
-	item->page0 = phys;
-	item->page1 = (phys & 0xfffff000) + 0x1000;
+	item->page0 = (uint32_t)mv_ep->b_buf;
+	item->page1 = ((uint32_t)mv_ep->b_buf & 0xfffff000) + 0x1000;
 
 	head->next = (unsigned) item;
 	head->info = 0;
 
-	DBG("ept%d %s queue len %x, buffer %x\n",
-			num, in ? "in" : "out", len, phys);
+	DBG("ept%d %s queue len %x, buffer %p\n",
+	    num, in ? "in" : "out", len, mv_ep->b_buf);
 
 	if (in)
 		bit = EPT_TX(num);
 	else
 		bit = EPT_RX(num);
 
-	flush_cache(phys, len);
-	flush_cache((unsigned long)item, sizeof(struct ept_queue_item));
+	mv_flush_qh(num);
+	mv_flush_qtd(num);
+
 	writel(bit, &udc->epprime);
 
 	return 0;
@@ -181,13 +362,17 @@ static void handle_ep_complete(struct mv_ep *ep)
 	in = (ep->desc->bEndpointAddress & USB_DIR_IN) != 0;
 	if (num == 0)
 		ep->desc = &ep0_out_desc;
-	item = items[2 * num + in];
-
+	item = mv_get_qtd(num, in);
+	mv_invalidate_qtd(num);
+	
 	if (item->info & 0xff)
 		printf("EP%d/%s FAIL nfo=%x pg0=%x\n",
 			num, in ? "in" : "out", item->info, item->page0);
 
 	len = (item->info >> 16) & 0x7fff;
+
+	mv_debounce(ep);
+
 	ep->req.length -= len;
 	DBG("ept%d %s complete %x\n",
 			num, in ? "in" : "out", len);
@@ -203,16 +388,16 @@ static void handle_ep_complete(struct mv_ep *ep)
 
 static void handle_setup(void)
 {
-	struct usb_request *req = &ep[0].req;
-	struct mv_udc *udc = controller.udc;
+	struct usb_request *req = &controller.ep[0].req;
+	struct mv_udc *udc = (struct mv_udc *)controller.ctrl->hcor;
 	struct ept_queue_head *head;
 	struct usb_ctrlrequest r;
 	int status = 0;
 	int num, in, _num, _in, i;
 	char *buf;
-	head = epts;
+	head = mv_get_qh(0, 0);	/* EP0 OUT */
 
-	flush_cache((unsigned long)head, sizeof(struct ept_queue_head));
+	mv_invalidate_qh(0);
 	memcpy(&r, head->setup_data, sizeof(struct usb_ctrlrequest));
 	writel(EPT_RX(0), &udc->epstat);
 	DBG("handle setup %s, %x, %x index %x value %x\n", reqname(r.bRequest),
@@ -226,11 +411,11 @@ static void handle_setup(void)
 		if ((r.wValue == 0) && (r.wLength == 0)) {
 			req->length = 0;
 			for (i = 0; i < NUM_ENDPOINTS; i++) {
-				if (!ep[i].desc)
+				if (!controller.ep[i].desc)
 					continue;
-				num = ep[i].desc->bEndpointAddress
-					& USB_ENDPOINT_NUMBER_MASK;
-				in = (ep[i].desc->bEndpointAddress
+				num = controller.ep[i].desc->bEndpointAddress
+						& USB_ENDPOINT_NUMBER_MASK;
+				in = (controller.ep[i].desc->bEndpointAddress
 						& USB_DIR_IN) != 0;
 				if ((num == _num) && (in == _in)) {
 					ep_enable(num, in);
@@ -277,7 +462,7 @@ static void stop_activity(void)
 {
 	int i, num, in;
 	struct ept_queue_head *head;
-	struct mv_udc *udc = controller.udc;
+	struct mv_udc *udc = (struct mv_udc *)controller.ctrl->hcor;
 	writel(readl(&udc->epcomp), &udc->epcomp);
 	writel(readl(&udc->epstat), &udc->epstat);
 	writel(0xffffffff, &udc->epflush);
@@ -286,19 +471,21 @@ static void stop_activity(void)
 	for (i = 0; i < NUM_ENDPOINTS; i++) {
 		if (i != 0)
 			writel(0, &udc->epctrl[i]);
-		if (ep[i].desc) {
-			num = ep[i].desc->bEndpointAddress
+		if (controller.ep[i].desc) {
+			num = controller.ep[i].desc->bEndpointAddress
 				& USB_ENDPOINT_NUMBER_MASK;
-			in = (ep[i].desc->bEndpointAddress & USB_DIR_IN) != 0;
-			head = epts + (num * 2) + (in);
+			in = (controller.ep[i].desc->bEndpointAddress
+				& USB_DIR_IN) != 0;
+			head = mv_get_qh(num, in);
 			head->info = INFO_ACTIVE;
+			mv_flush_qh(num);
 		}
 	}
 }
 
 void udc_irq(void)
 {
-	struct mv_udc *udc = controller.udc;
+	struct mv_udc *udc = (struct mv_udc *)controller.ctrl->hcor;
 	unsigned n = readl(&udc->usbsts);
 	writel(n, &udc->usbsts);
 	int bit, i, num, in;
@@ -320,8 +507,8 @@ void udc_irq(void)
 		if (bit == 2) {
 			controller.gadget.speed = USB_SPEED_HIGH;
 			for (i = 1; i < NUM_ENDPOINTS && n; i++)
-				if (ep[i].desc)
-					ep[i].ep.maxpacket = 512;
+				if (controller.ep[i].desc)
+					controller.ep[i].ep.maxpacket = 512;
 		} else {
 			controller.gadget.speed = USB_SPEED_FULL;
 		}
@@ -340,14 +527,14 @@ void udc_irq(void)
 			writel(n, &udc->epcomp);
 
 		for (i = 0; i < NUM_ENDPOINTS && n; i++) {
-			if (ep[i].desc) {
-				num = ep[i].desc->bEndpointAddress
+			if (controller.ep[i].desc) {
+				num = controller.ep[i].desc->bEndpointAddress
 					& USB_ENDPOINT_NUMBER_MASK;
-				in = (ep[i].desc->bEndpointAddress
+				in = (controller.ep[i].desc->bEndpointAddress
 						& USB_DIR_IN) != 0;
 				bit = (in) ? EPT_TX(num) : EPT_RX(num);
 				if (n & bit)
-					handle_ep_complete(&ep[i]);
+					handle_ep_complete(&controller.ep[i]);
 			}
 		}
 	}
@@ -356,7 +543,7 @@ void udc_irq(void)
 int usb_gadget_handle_interrupts(void)
 {
 	u32 value;
-	struct mv_udc *udc = controller.udc;
+	struct mv_udc *udc = (struct mv_udc *)controller.ctrl->hcor;
 
 	value = readl(&udc->usbsts);
 	if (value)
@@ -367,13 +554,13 @@ int usb_gadget_handle_interrupts(void)
 
 static int mv_pullup(struct usb_gadget *gadget, int is_on)
 {
-	struct mv_udc *udc = controller.udc;
+	struct mv_udc *udc = (struct mv_udc *)controller.ctrl->hcor;
 	if (is_on) {
 		/* RESET */
 		writel(USBCMD_ITC(MICRO_8FRAME) | USBCMD_RST, &udc->usbcmd);
 		udelay(200);
 
-		writel((unsigned) epts, &udc->epinitaddr);
+		writel((unsigned)controller.epts, &udc->epinitaddr);
 
 		/* select DEVICE mode */
 		writel(USBMODE_DEVICE, &udc->usbmode);
@@ -395,7 +582,7 @@ static int mv_pullup(struct usb_gadget *gadget, int is_on)
 
 void udc_disconnect(void)
 {
-	struct mv_udc *udc = controller.udc;
+	struct mv_udc *udc = (struct mv_udc *)controller.ctrl->hcor;
 	/* disable pullup */
 	stop_activity();
 	writel(USBCMD_FS2, &udc->usbcmd);
@@ -407,18 +594,48 @@ void udc_disconnect(void)
 static int mvudc_probe(void)
 {
 	struct ept_queue_head *head;
+	uint8_t *imem;
 	int i;
 
-	controller.gadget.ops = &mv_udc_ops;
-	controller.udc = (struct mv_udc *)CONFIG_USB_REG_BASE;
-	epts = memalign(PAGE_SIZE, QH_MAXNUM * sizeof(struct ept_queue_head));
-	memset(epts, 0, QH_MAXNUM * sizeof(struct ept_queue_head));
+	const int num = 2 * NUM_ENDPOINTS;
+
+	const int eplist_min_align = 4096;
+	const int eplist_align = roundup(eplist_min_align, ARCH_DMA_MINALIGN);
+	const int eplist_raw_sz = num * sizeof(struct ept_queue_head);
+	const int eplist_sz = roundup(eplist_raw_sz, ARCH_DMA_MINALIGN);
+
+	const int ilist_align = roundup(ARCH_DMA_MINALIGN, 32);
+	const int ilist_ent_raw_sz = 2 * sizeof(struct ept_queue_item);
+	const int ilist_ent_sz = roundup(ilist_ent_raw_sz, ARCH_DMA_MINALIGN);
+	const int ilist_sz = NUM_ENDPOINTS * ilist_ent_sz;
+
+	/* The QH list must be aligned to 4096 bytes. */
+	controller.epts = memalign(eplist_align, eplist_sz);
+	if (!controller.epts)
+		return -ENOMEM;
+	memset(controller.epts, 0, eplist_sz);
+
+	/*
+	 * Each qTD item must be 32-byte aligned, each qTD touple must be
+	 * cacheline aligned. There are two qTD items for each endpoint and
+	 * only one of them is used for the endpoint at time, so we can group
+	 * them together.
+	 */
+	controller.items_mem = memalign(ilist_align, ilist_sz);
+	if (!controller.items_mem) {
+		free(controller.epts);
+		return -ENOMEM;
+	}
+
 	for (i = 0; i < 2 * NUM_ENDPOINTS; i++) {
 		/*
-		 * For item0 and item1, they are served as ep0
-		 * out&in seperately
+		 * Configure QH for each endpoint. The structure of the QH list
+		 * is such that each two subsequent fields, N and N+1 where N is
+		 * even, in the QH list represent QH for one endpoint. The Nth
+		 * entry represents OUT configuration and the N+1th entry does
+		 * represent IN configuration of the endpoint.
 		 */
-		head = epts + i;
+		head = controller.epts + i;
 		if (i < 2)
 			head->config = CONFIG_MAX_PKT(EP0_MAX_PACKET_SIZE)
 				| CONFIG_ZLT | CONFIG_IOS;
@@ -428,49 +645,65 @@ static int mvudc_probe(void)
 		head->next = TERMINATE;
 		head->info = 0;
 
-		items[i] = memalign(PAGE_SIZE, sizeof(struct ept_queue_item));
+		imem = controller.items_mem + ((i >> 1) * ilist_ent_sz);
+		if (i & 1)
+			imem += sizeof(struct ept_queue_item);
+
+		controller.items[i] = (struct ept_queue_item *)imem;
+
+		if (i & 1) {
+			mv_flush_qh(i - 1);
+			mv_flush_qtd(i - 1);
+		}
 	}
 
 	INIT_LIST_HEAD(&controller.gadget.ep_list);
-	ep[0].ep.maxpacket = 64;
-	ep[0].ep.name = "ep0";
-	ep[0].desc = &ep0_in_desc;
+
+	/* Init EP 0 */
+	memcpy(&controller.ep[0].ep, &mv_ep_init[0], sizeof(*mv_ep_init));
+	controller.ep[0].desc = &ep0_in_desc;
+	controller.gadget.ep0 = &controller.ep[0].ep;
 	INIT_LIST_HEAD(&controller.gadget.ep0->ep_list);
-	for (i = 0; i < 2 * NUM_ENDPOINTS; i++) {
-		if (i != 0) {
-			ep[i].ep.maxpacket = 512;
-			ep[i].ep.name = "ep-";
-			list_add_tail(&ep[i].ep.ep_list,
-				      &controller.gadget.ep_list);
-			ep[i].desc = NULL;
-		}
-		ep[i].ep.ops = &mv_ep_ops;
+
+	/* Init EP 1..n */
+	for (i = 1; i < NUM_ENDPOINTS; i++) {
+		memcpy(&controller.ep[i].ep, &mv_ep_init[1],
+		       sizeof(*mv_ep_init));
+		list_add_tail(&controller.ep[i].ep.ep_list,
+			      &controller.gadget.ep_list);
 	}
+
 	return 0;
 }
 
 int usb_gadget_register_driver(struct usb_gadget_driver *driver)
 {
-	struct mv_udc *udc = controller.udc;
-	int             retval;
-
-	if (!driver
-			|| driver->speed < USB_SPEED_FULL
-			|| !driver->bind
-			|| !driver->setup) {
-		DBG("bad parameter.\n");
+	struct mv_udc *udc;
+	int ret;
+
+	if (!driver)
 		return -EINVAL;
-	}
+	if (!driver->bind || !driver->setup || !driver->disconnect)
+		return -EINVAL;
+	if (driver->speed != USB_SPEED_FULL && driver->speed != USB_SPEED_HIGH)
+		return -EINVAL;
+
+	ret = usb_lowlevel_init(0, (void **)&controller.ctrl);
+	if (ret)
+		return ret;
+
+	ret = mvudc_probe();
+	if (!ret) {
+		udc = (struct mv_udc *)controller.ctrl->hcor;
 
-	if (!mvudc_probe()) {
-		usb_lowlevel_init();
 		/* select ULPI phy */
 		writel(PTS(PTS_ENABLE) | PFSC, &udc->portsc);
 	}
-	retval = driver->bind(&controller.gadget);
-	if (retval) {
-		DBG("driver->bind() returned %d\n", retval);
-		return retval;
+
+	ret = driver->bind(&controller.gadget);
+	if (ret) {
+		DBG("driver->bind() returned %d\n", ret);
+		return ret;
 	}
 	controller.driver = driver;
 
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 706cf0cb7d..fdad739724 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -36,16 +36,7 @@
 #define CONFIG_USB_MAX_CONTROLLER_COUNT 1
 #endif
 
-static struct ehci_ctrl {
-	struct ehci_hccr *hccr;	/* R/O registers, not need for volatile */
-	struct ehci_hcor *hcor;
-	int rootdev;
-	uint16_t portreset;
-	struct QH qh_list __aligned(USB_DMA_MINALIGN);
-	struct QH periodic_queue __aligned(USB_DMA_MINALIGN);
-	uint32_t *periodic_list;
-	int ntds;
-} ehcic[CONFIG_USB_MAX_CONTROLLER_COUNT];
+static struct ehci_ctrl ehcic[CONFIG_USB_MAX_CONTROLLER_COUNT];
 
 #define ALIGN_END_ADDR(type, ptr, size)			\
 	((uint32_t)(ptr) + roundup((size) * sizeof(type), USB_DMA_MINALIGN))
@@ -954,7 +945,9 @@ int usb_lowlevel_init(int index, void **controller)
 	 *         Split Transactions will be spread across microframes using
 	 *         S-mask and C-mask.
 	 */
-	ehcic[index].periodic_list = memalign(4096, 1024*4);
+	if (ehcic[index].periodic_list == NULL)
+		ehcic[index].periodic_list = memalign(4096, 1024 * 4);
+
 	if (!ehcic[index].periodic_list)
 		return -ENOMEM;
 	for (i = 0; i < 1024; i++) {
diff --git a/drivers/usb/host/ehci-omap.c b/drivers/usb/host/ehci-omap.c
index a47e078f62..032d5e5ec2 100644
--- a/drivers/usb/host/ehci-omap.c
+++ b/drivers/usb/host/ehci-omap.c
@@ -28,18 +28,21 @@ static struct omap_ehci *const ehci = (struct omap_ehci *)OMAP_EHCI_BASE;
 
 static int omap_uhh_reset(void)
 {
-	unsigned long init = get_timer(0);
-
-	/* perform UHH soft reset, and wait until reset is complete */
-	writel(OMAP_UHH_SYSCONFIG_SOFTRESET, &uhh->sysc);
-
-	/* Wait for UHH reset to complete */
-	while (!(readl(&uhh->syss) & OMAP_UHH_SYSSTATUS_EHCI_RESETDONE))
-		if (get_timer(init) > CONFIG_SYS_HZ) {
-			debug("OMAP UHH error: timeout resetting ehci\n");
-			return -EL3RST;
-		}
-
+/*
+ * Soft resetting the UHH module causes instability issues on
+ * all OMAPs so we just avoid it.
+ *
+ * See OMAP36xx Errata
+ *  i571: USB host EHCI may stall when entering smart-standby mode
+ *  i660: USBHOST Configured In Smart-Idle Can Lead To a Deadlock
+ *
+ * On OMAP4/5, soft-resetting the UHH module will put it into
+ * Smart-Idle mode and lead to a deadlock.
+ *
+ * On OMAP3, this doesn't seem to be the case but still instabilities
+ * are observed on beagle (3530 ES1.0) if soft-reset is used.
+ * e.g. NFS root failures with Linux kernel.
+ */
 	return 0;
 }
 
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index d090f0a53e..bd52afe262 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -22,6 +22,8 @@
 #ifndef USB_EHCI_H
 #define USB_EHCI_H
 
+#include <usb.h>
+
 #if !defined(CONFIG_SYS_USB_EHCI_MAX_ROOT_PORTS)
 #define CONFIG_SYS_USB_EHCI_MAX_ROOT_PORTS	2
 #endif
@@ -252,6 +254,17 @@ struct QH {
 	};
 };
 
+struct ehci_ctrl {
+	struct ehci_hccr *hccr;	/* R/O registers, not need for volatile */
+	struct ehci_hcor *hcor;
+	int rootdev;
+	uint16_t portreset;
+	struct QH qh_list __aligned(USB_DMA_MINALIGN);
+	struct QH periodic_queue __aligned(USB_DMA_MINALIGN);
+	uint32_t *periodic_list;
+	int ntds;
+};
+
 /* Low level init functions */
 int ehci_hcd_init(int index, struct ehci_hccr **hccr, struct ehci_hcor **hcor);
 int ehci_hcd_stop(int index);
diff --git a/include/dfu.h b/include/dfu.h
index 6c7d227137..1d4006de8b 100644
--- a/include/dfu.h
+++ b/include/dfu.h
@@ -109,6 +109,8 @@ const char *dfu_get_dev_type(enum dfu_device_type t);
 const char *dfu_get_layout(enum dfu_layout l);
 struct dfu_entity *dfu_get_entity(int alt);
 char *dfu_extract_token(char** e, int *n);
+void dfu_trigger_reset(void);
+bool dfu_reset(void);
 
 int dfu_read(struct dfu_entity *de, void *buf, int size, int blk_seq_num);
 int dfu_write(struct dfu_entity *de, void *buf, int size, int blk_seq_num);
diff --git a/include/usb/mv_udc.h b/include/usb/mv_udc.h
index c7696e6a3e..c71516cf6d 100644
--- a/include/usb/mv_udc.h
+++ b/include/usb/mv_udc.h
@@ -14,39 +14,19 @@
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
 
-/* Endpoint 0 states */
-#define EP0_IDLE		0
-#define EP0_IN_DATA		1
-#define EP0_OUT_DATA		2
-#define EP0_XFER_COMPLETE	3
+#include "../../drivers/usb/host/ehci.h"
 
+#define NUM_ENDPOINTS		6
 
 /* Endpoint parameters */
 #define MAX_ENDPOINTS		4
-#define EP_MAX_PACKET_SIZE	0x200
 
+#define EP_MAX_PACKET_SIZE	0x200
 #define EP0_MAX_PACKET_SIZE	64
-#define UDC_OUT_ENDPOINT        0x02
-#define UDC_OUT_PACKET_SIZE	EP_MAX_PACKET_SIZE
-#define UDC_IN_ENDPOINT         0x01
-#define UDC_IN_PACKET_SIZE      EP_MAX_PACKET_SIZE
-#define UDC_INT_ENDPOINT        0x05
-#define UDC_INT_PACKET_SIZE     EP_MAX_PACKET_SIZE
-#define UDC_BULK_PACKET_SIZE    EP_MAX_PACKET_SIZE
-
-#define        NUM_ENDPOINTS   6
-#define		REQ_COUNT	12
-struct mv_ep {
-	struct usb_ep ep;
-	struct usb_request req;
-	struct list_head queue;
-	const struct usb_endpoint_descriptor *desc;
-};
 
 struct mv_udc {
-	u32 pad0[80];
 #define MICRO_8FRAME	0x8
-#define USBCMD_ITC(x)	(((x > 0xff) ? 0xff : x) << 16)
+#define USBCMD_ITC(x)	((((x) > 0xff) ? 0xff : x) << 16)
 #define USBCMD_FS2	(1 << 15)
 #define USBCMD_RST	(1 << 1)
 #define USBCMD_RUN	(1)
@@ -62,37 +42,52 @@ struct mv_udc {
 	u32 epinitaddr;		/* 0x158 */
 	u32 pad2[10];
 #define PTS_ENABLE	2
-#define PTS(x)		((x & 0x3) << 30)
+#define PTS(x)		(((x) & 0x3) << 30)
 #define PFSC		(1 << 24)
 	u32 portsc;		/* 0x184 */
 	u32 pad3[8];
 #define USBMODE_DEVICE	2
 	u32 usbmode;		/* 0x1a8 */
 	u32 epstat;		/* 0x1ac */
-#define EPT_TX(x)	(1 << ((x & 0xffff) + 16))
-#define EPT_RX(x)	(1 << (x & 0xffff))
+#define EPT_TX(x)	(1 << (((x) & 0xffff) + 16))
+#define EPT_RX(x)	(1 << ((x) & 0xffff))
 	u32 epprime;		/* 0x1b0 */
 	u32 epflush;		/* 0x1b4 */
 	u32 pad4;
 	u32 epcomp;		/* 0x1bc */
-#define CTRL_TXE              (1 << 23)
-#define CTRL_TXR              (1 << 22)
-#define CTRL_RXE              (1 << 7)
-#define CTRL_RXR              (1 << 6)
-#define CTRL_TXT_BULK         (2 << 18)
-#define CTRL_RXT_BULK         (2 << 2)
+#define CTRL_TXE	(1 << 23)
+#define CTRL_TXR	(1 << 22)
+#define CTRL_RXE	(1 << 7)
+#define CTRL_RXR	(1 << 6)
+#define CTRL_TXT_BULK	(2 << 18)
+#define CTRL_RXT_BULK	(2 << 2)
 	u32 epctrl[16];		/* 0x1c0 */
 };
 
+struct mv_ep {
+	struct usb_ep ep;
+	struct list_head queue;
+	const struct usb_endpoint_descriptor *desc;
+
+	struct usb_request req;
+	uint8_t *b_buf;
+	uint32_t b_len;
+	uint8_t b_fast[64] __aligned(ARCH_DMA_MINALIGN);
+};
+
 struct mv_drv {
 	struct usb_gadget		gadget;
-	struct usb_gadget_driver		*driver;
-	struct mv_udc			*udc;
+	struct usb_gadget_driver	*driver;
+	struct ehci_ctrl		*ctrl;
+	struct ept_queue_head		*epts;
+	struct ept_queue_item		*items[2 * NUM_ENDPOINTS];
+	uint8_t				*items_mem;
+	struct mv_ep			ep[NUM_ENDPOINTS];
 };
 
 struct ept_queue_head {
 	unsigned config;
-	unsigned current; /* read-only */
+	unsigned current;	/* read-only */
 
 	unsigned next;
 	unsigned info;
@@ -111,9 +106,9 @@ struct ept_queue_head {
 	unsigned reserved_4;
 };
 
-#define CONFIG_MAX_PKT(n)     ((n) << 16)
-#define CONFIG_ZLT            (1 << 29)    /* stop on zero-len xfer */
-#define CONFIG_IOS            (1 << 15)    /* IRQ on setup */
+#define CONFIG_MAX_PKT(n)	((n) << 16)
+#define CONFIG_ZLT		(1 << 29)	/* stop on zero-len xfer */
+#define CONFIG_IOS		(1 << 15)	/* IRQ on setup */
 
 struct ept_queue_item {
 	unsigned next;
@@ -127,12 +122,11 @@ struct ept_queue_item {
 };
 
 #define TERMINATE 1
-#define INFO_BYTES(n)         ((n) << 16)
-#define INFO_IOC              (1 << 15)
-#define INFO_ACTIVE           (1 << 7)
-#define INFO_HALTED           (1 << 6)
-#define INFO_BUFFER_ERROR     (1 << 5)
-#define INFO_TX_ERROR         (1 << 3)
-
-extern int usb_lowlevel_init(int index, void **controller);
+#define INFO_BYTES(n)		((n) << 16)
+#define INFO_IOC		(1 << 15)
+#define INFO_ACTIVE		(1 << 7)
+#define INFO_HALTED		(1 << 6)
+#define INFO_BUFFER_ERROR	(1 << 5)
+#define INFO_TX_ERROR		(1 << 3)
+
 #endif /* __MV_UDC_H__ */