[U-Boot] [PATCH RFC V1 16/18] usb: gadget: mv_udc: fix full speed connections

[Troy Kisky]

Set maximum packet length in queue header to wMaxPacketSize
of endpoint.

Signed-off-by: Troy Kisky <troy.kisky at boundarydevices.com>
---
 drivers/usb/gadget/mv_udc.c | 40 +++++++++++++++++++++++++++++-----------
 1 file changed, 29 insertions(+), 11 deletions(-)

diff --git a/drivers/usb/gadget/mv_udc.c b/drivers/usb/gadget/mv_udc.c
index 5c10a8b..b9ff306 100644
--- a/drivers/usb/gadget/mv_udc.c
+++ b/drivers/usb/gadget/mv_udc.c
@@ -32,6 +32,7 @@
 #include <asm/byteorder.h>
 #include <asm/io.h>
 #include <asm/errno.h>
+#include <asm/unaligned.h>
 #include <linux/types.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
@@ -128,7 +129,7 @@ static void mv_ep_free_request(struct usb_ep *ep, struct usb_request *_req)
 	return;
 }
 
-static void ep_enable(int num, int in)
+static void ep_enable(int num, int in, int maxpacket)
 {
 	struct ept_queue_head *head;
 	struct mv_udc *udc = controller.udc;
@@ -142,7 +143,7 @@ static void ep_enable(int num, int in)
 		n |= (CTRL_RXE | CTRL_RXR | CTRL_RXT_BULK);
 
 	if (num != 0)
-		head->config = CONFIG_MAX_PKT(EP_MAX_PACKET_SIZE) | CONFIG_ZLT;
+		head->config = CONFIG_MAX_PKT(maxpacket) | CONFIG_ZLT;
 	writel(n, &udc->epctrl[num]);
 }
 
@@ -153,8 +154,21 @@ static int mv_ep_enable(struct usb_ep *ep,
 	int num, in;
 	num = desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
 	in = (desc->bEndpointAddress & USB_DIR_IN) != 0;
-	ep_enable(num, in);
 	mv_ep->desc = desc;
+
+	if (num) {
+		int max = get_unaligned_le16(&desc->wMaxPacketSize);
+
+		if ((max > 64) && (controller.gadget.speed == USB_SPEED_FULL))
+			max = 64;
+		if (ep->maxpacket != max) {
+			DBG("%s: from %d to %d\n", __func__,
+			    ep->maxpacket, max);
+			ep->maxpacket = max;
+		}
+	}
+	ep_enable(num, in, ep->maxpacket);
+	DBG("%s: num=%d maxpacket=%d\n", __func__, num, ep->maxpacket);
 	return 0;
 }
 
@@ -262,7 +276,7 @@ static void handle_setup(void)
 				in = (ep[i].desc->bEndpointAddress
 						& USB_DIR_IN) != 0;
 				if ((num == _num) && (in == _in)) {
-					ep_enable(num, in);
+					ep_enable(num, in, ep[i].ep.maxpacket);
 					usb_ep_queue(controller.gadget.ep0,
 							req, 0);
 					break;
@@ -344,15 +358,19 @@ void udc_irq(void)
 		DBG("-- suspend --\n");
 
 	if (n & STS_PCI) {
-		DBG("-- portchange --\n");
+		int max = 64;
+		int speed = USB_SPEED_FULL;
+
 		bit = (readl(&udc->portsc) >> 26) & 3;
+		DBG("-- portchange %x %s\n", bit, (bit == 2) ? "High" : "Full");
 		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;
-		} else {
-			controller.gadget.speed = USB_SPEED_FULL;
+			speed = USB_SPEED_HIGH;
+			max = 512;
+		}
+		controller.gadget.speed = speed;
+		for (i = 1; i < NUM_ENDPOINTS && n; i++) {
+			if (ep[i].ep.maxpacket > max)
+				ep[i].ep.maxpacket = max;
 		}
 	}
 
-- 
1.8.1.2