usb: dwc3: gadget: re-factor USB2 test mode to a function

There are some situations were we might need to
enable USB Test Modes without having access to a
Host stack. In such situations we cannot rely
solely on USB Control Messages to enable test
features.

For those cases, we will also allow test mode
to be enabled via debugfs and this patch is a
preparation for that.

Signed-off-by: Felipe Balbi <balbi@ti.com>

1 files changed, 33 insertions, 0 deletions

diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 064b6e2cd41..1dee17e7b77 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -56,6 +56,39 @@
 
 #define DMA_ADDR_INVALID        (~(dma_addr_t)0)
 
+/**
+ * dwc3_gadget_set_test_mode - Enables USB2 Test Modes
+ * @dwc: pointer to our context structure
+ * @mode: the mode to set (J, K SE0 NAK, Force Enable)
+ *
+ * Caller should take care of locking. This function will
+ * return 0 on success or -EINVAL if wrong Test Selector
+ * is passed
+ */
+int dwc3_gadget_set_test_mode(struct dwc3 *dwc, int mode)
+{
+        u32             reg;
+
+        reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+        reg &= ~DWC3_DCTL_TSTCTRL_MASK;
+
+        switch (mode) {
+        case TEST_J:
+        case TEST_K:
+        case TEST_SE0_NAK:
+        case TEST_PACKET:
+        case TEST_FORCE_EN:
+                reg |= mode << 1;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+
+        return 0;
+}
+
 void dwc3_map_buffer_to_dma(struct dwc3_request *req)
 {
         struct dwc3                     *dwc = req->dep->dwc;