diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 71d958a..4dac982 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -684,6 +684,9 @@
 
 	struct dwc3_hwparams	hwparams;
 	struct dentry		*root;
+
+	u8			test_mode;
+	u8			test_mode_nr;
 };
 
 /* -------------------------------------------------------------------------- */
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 5104dbf..c20e30c 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -316,7 +316,6 @@
 	u32			wValue;
 	u32			wIndex;
 	int			ret;
-	u32			mode;
 
 	wValue = le16_to_cpu(ctrl->wValue);
 	wIndex = le16_to_cpu(ctrl->wIndex);
@@ -355,13 +354,8 @@
 			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 @@
 		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/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index c30ab6d..7632700 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -1962,6 +1962,7 @@
 	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);