From 2ad98be9cc49ce2ef3cd6f65ab2fd070355a688c Mon Sep 17 00:00:00 2001
From: Yoshihiro Shimoda <yoshihiro.shimoda.uh@renesas.com>
Date: Fri, 8 Jul 2011 14:51:33 +0900
Subject: [PATCH] --- yaml --- r: 259301 b: refs/heads/master c:
 ceaa0a6eeadfd2f53df121210d99a1f80ee7645e h: refs/heads/master i:   259299:
 9e1cfe134fe240462da9cb086e4a3c42b2a59cc6 v: v3

---
 [refs]                                |  2 +-
 trunk/drivers/usb/gadget/m66592-udc.c | 23 ++++++++++++++++++++++-
 2 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/[refs] b/[refs]
index bf2a345c3ca9..2932219620ac 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 5154e9f126c1d2ee8f5f93d9954f83d82b2d5e64
+refs/heads/master: ceaa0a6eeadfd2f53df121210d99a1f80ee7645e
diff --git a/trunk/drivers/usb/gadget/m66592-udc.c b/trunk/drivers/usb/gadget/m66592-udc.c
index dd9f460a7ef4..40c2f7a4f8fb 100644
--- a/trunk/drivers/usb/gadget/m66592-udc.c
+++ b/trunk/drivers/usb/gadget/m66592-udc.c
@@ -691,6 +691,7 @@ static void init_controller(struct m66592 *m66592)
 
 static void disable_controller(struct m66592 *m66592)
 {
+	m66592_bclr(m66592, M66592_UTST, M66592_TESTMODE);
 	if (!m66592->pdata->on_chip) {
 		m66592_bclr(m66592, M66592_SCKE, M66592_SYSCFG);
 		udelay(1);
@@ -1048,10 +1049,30 @@ static void clear_feature(struct m66592 *m66592, struct usb_ctrlrequest *ctrl)
 
 static void set_feature(struct m66592 *m66592, struct usb_ctrlrequest *ctrl)
 {
+	u16 tmp;
+	int timeout = 3000;
 
 	switch (ctrl->bRequestType & USB_RECIP_MASK) {
 	case USB_RECIP_DEVICE:
-		control_end(m66592, 1);
+		switch (le16_to_cpu(ctrl->wValue)) {
+		case USB_DEVICE_TEST_MODE:
+			control_end(m66592, 1);
+			/* Wait for the completion of status stage */
+			do {
+				tmp = m66592_read(m66592, M66592_INTSTS0) &
+								M66592_CTSQ;
+				udelay(1);
+			} while (tmp != M66592_CS_IDST || timeout-- > 0);
+
+			if (tmp == M66592_CS_IDST)
+				m66592_bset(m66592,
+					    le16_to_cpu(ctrl->wIndex >> 8),
+					    M66592_TESTMODE);
+			break;
+		default:
+			pipe_stall(m66592, 0);
+			break;
+		}
 		break;
 	case USB_RECIP_INTERFACE:
 		control_end(m66592, 1);