3 files changed, 17 insertions, 8 deletions

diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 71d958af2393..4dac9828577e 100644
--- a/drivers/usb/dwc3/core.h
+++ b/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/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 5104dbf46680..c20e30c8b695 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/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/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index c30ab6da3d0e..76327005d54c 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/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);