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);
 }