From 6ee1a4897c7737997848c5e38f4d3d3b84df2bd8 Mon Sep 17 00:00:00 2001
From: Gerard Cauvy <g-cauvy1@ti.com>
Date: Fri, 10 Feb 2012 12:21:18 +0200
Subject: [PATCH] --- yaml --- r: 289624 b: refs/heads/master c:
 3b637367ae40b6d3c20e30cb0cdd059e67bbf848 h: refs/heads/master v: v3

---
 [refs]                          |  2 +-
 trunk/drivers/usb/dwc3/core.h   |  3 +++
 trunk/drivers/usb/dwc3/ep0.c    | 21 +++++++++++++--------
 trunk/drivers/usb/dwc3/gadget.c |  1 +
 4 files changed, 18 insertions(+), 9 deletions(-)

diff --git a/[refs] b/[refs]
index 1f94c51e9fe6..bc5c4999bd7d 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 090725431b9636a0a59516ff0fe94933cf09a82b
+refs/heads/master: 3b637367ae40b6d3c20e30cb0cdd059e67bbf848
diff --git a/trunk/drivers/usb/dwc3/core.h b/trunk/drivers/usb/dwc3/core.h
index 71d958af2393..4dac9828577e 100644
--- a/trunk/drivers/usb/dwc3/core.h
+++ b/trunk/drivers/usb/dwc3/core.h
@@ -684,6 +684,9 @@ struct dwc3 {
 
 	struct dwc3_hwparams	hwparams;
 	struct dentry		*root;
+
+	u8			test_mode;
+	u8			test_mode_nr;
 };
 
 /* -------------------------------------------------------------------------- */
diff --git a/trunk/drivers/usb/dwc3/ep0.c b/trunk/drivers/usb/dwc3/ep0.c
index 5104dbf46680..c20e30c8b695 100644
--- a/trunk/drivers/usb/dwc3/ep0.c
+++ b/trunk/drivers/usb/dwc3/ep0.c
@@ -316,7 +316,6 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 	u32			wValue;
 	u32			wIndex;
 	int			ret;
-	u32			mode;
 
 	wValue = le16_to_cpu(ctrl->wValue);
 	wIndex = le16_to_cpu(ctrl->wIndex);
@@ -355,13 +354,8 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 			if (!set)
 				return -EINVAL;
 
-			mode = wIndex >> 8;
-			ret = dwc3_gadget_set_test_mode(dwc, mode);
-			if (ret < 0) {
-				dev_dbg(dwc->dev, "Invalid Test #%d\n",
-						mode);
-				return ret;
-			}
+			dwc->test_mode_nr = wIndex >> 8;
+			dwc->test_mode = true;
 		}
 		break;
 
@@ -604,6 +598,17 @@ static void dwc3_ep0_complete_req(struct dwc3 *dwc,
 		dwc3_gadget_giveback(dep, r, 0);
 	}
 
+	if (dwc->test_mode) {
+		int ret;
+
+		ret = dwc3_gadget_set_test_mode(dwc, dwc->test_mode_nr);
+		if (ret < 0) {
+			dev_dbg(dwc->dev, "Invalid Test #%d\n",
+					dwc->test_mode_nr);
+			dwc3_ep0_stall_and_restart(dwc);
+		}
+	}
+
 	dwc->ep0state = EP0_SETUP_PHASE;
 	dwc3_ep0_out_start(dwc);
 }
diff --git a/trunk/drivers/usb/dwc3/gadget.c b/trunk/drivers/usb/dwc3/gadget.c
index c30ab6da3d0e..76327005d54c 100644
--- a/trunk/drivers/usb/dwc3/gadget.c
+++ b/trunk/drivers/usb/dwc3/gadget.c
@@ -1962,6 +1962,7 @@ static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	reg &= ~DWC3_DCTL_TSTCTRL_MASK;
 	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+	dwc->test_mode = false;
 
 	dwc3_stop_active_transfers(dwc);
 	dwc3_clear_stall_all_ep(dwc);