usb: dwc2: gadget: add TEST_MODE feature support

Handle SET_FEATURE TEST_MODE request sent by the host.
Slightly rework FEATURE request handling to allow parsing
other request types than Endpoint.
Also add a debugfs to change test mode value from user space.

Tested-by: Robert Baldyga <r.baldyga@samsung.com>
Acked-by: John Youn <johnyoun@synopsys.com>
Signed-off-by: Gregory Herrero <gregory.herrero@intel.com>
Signed-off-by: Felipe Balbi <balbi@ti.com>

diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index f09b3deffdeee6f976f5c369378b6f523c2000c8..c750fd3d88b330603a842a04e8e0baa08939b842 100644 (file)
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -567,12 +567,14 @@ struct dwc2_hw_params {
  * @num_of_eps:         Number of available EPs (excluding EP0)
  * @debug_root:         Root directrory for debugfs.
  * @debug_file:         Main status file for debugfs.
+ * @debug_testmode:     Testmode status file for debugfs.
  * @debug_fifo:         FIFO status file for debugfs.
  * @ep0_reply:          Request used for ep0 reply.
  * @ep0_buff:           Buffer for EP0 reply data, if needed.
  * @ctrl_buff:          Buffer for EP0 control requests.
  * @ctrl_req:           Request for EP0 control packets.
  * @ep0_state:          EP0 control transfers state
+ * @test_mode:          USB test mode requested by the host
  * @last_rst:           Time of last reset
  * @eps:                The endpoints being supplied to the gadget framework
  * @g_using_dma:          Indicate if dma usage is enabled
@@ -610,6 +612,7 @@ struct dwc2_hsotg {
 
        struct dentry *debug_root;
        struct dentry *debug_file;
+       struct dentry *debug_testmode;
        struct dentry *debug_fifo;
 
        /* DWC OTG HW Release versions */
@@ -706,6 +709,7 @@ struct dwc2_hsotg {
        void *ep0_buff;
        void *ctrl_buff;
        enum dwc2_ep0_state ep0_state;
+       u8 test_mode;
 
        struct usb_gadget gadget;
        unsigned int enabled:1;

diff --git a/drivers/usb/dwc2/gadget.c b/drivers/usb/dwc2/gadget.c
index 02d0e9af9242329ec8b8a38c3cf27b7d99e4169c..67ea258335ad8af378f70ef816476e4c6e5de027 100644 (file)
--- a/drivers/usb/dwc2/gadget.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -35,6 +35,7 @@
 #include <linux/usb/gadget.h>
 #include <linux/usb/phy.h>
 #include <linux/platform_data/s3c-hsotg.h>
+#include <linux/uaccess.h>
 
 #include "core.h"
 #include "hw.h"
@@ -834,6 +835,32 @@ static struct s3c_hsotg_ep *ep_from_windex(struct dwc2_hsotg *hsotg,
        return ep;
 }
 
+/**
+ * s3c_hsotg_set_test_mode - Enable usb Test Modes
+ * @hsotg: The driver state.
+ * @testmode: requested usb test mode
+ * Enable usb Test Mode requested by the Host.
+ */
+static int s3c_hsotg_set_test_mode(struct dwc2_hsotg *hsotg, int testmode)
+{
+       int dctl = readl(hsotg->regs + DCTL);
+
+       dctl &= ~DCTL_TSTCTL_MASK;
+       switch (testmode) {
+       case TEST_J:
+       case TEST_K:
+       case TEST_SE0_NAK:
+       case TEST_PACKET:
+       case TEST_FORCE_EN:
+               dctl |= testmode << DCTL_TSTCTL_SHIFT;
+               break;
+       default:
+               return -EINVAL;
+       }
+       writel(dctl, hsotg->regs + DCTL);
+       return 0;
+}
+
 /**
  * s3c_hsotg_send_reply - send reply to control request
  * @hsotg: The device state
@@ -968,19 +995,48 @@ static int s3c_hsotg_process_req_feature(struct dwc2_hsotg *hsotg,
        struct s3c_hsotg_ep *ep;
        int ret;
        bool halted;
+       u32 recip;
+       u32 wValue;
+       u32 wIndex;
 
        dev_dbg(hsotg->dev, "%s: %s_FEATURE\n",
                __func__, set ? "SET" : "CLEAR");
 
-       if (ctrl->bRequestType == USB_RECIP_ENDPOINT) {
-               ep = ep_from_windex(hsotg, le16_to_cpu(ctrl->wIndex));
+       wValue = le16_to_cpu(ctrl->wValue);
+       wIndex = le16_to_cpu(ctrl->wIndex);
+       recip = ctrl->bRequestType & USB_RECIP_MASK;
+
+       switch (recip) {
+       case USB_RECIP_DEVICE:
+               switch (wValue) {
+               case USB_DEVICE_TEST_MODE:
+                       if ((wIndex & 0xff) != 0)
+                               return -EINVAL;
+                       if (!set)
+                               return -EINVAL;
+
+                       hsotg->test_mode = wIndex >> 8;
+                       ret = s3c_hsotg_send_reply(hsotg, ep0, NULL, 0);
+                       if (ret) {
+                               dev_err(hsotg->dev,
+                                       "%s: failed to send reply\n", __func__);
+                               return ret;
+                       }
+                       break;
+               default:
+                       return -ENOENT;
+               }
+               break;
+
+       case USB_RECIP_ENDPOINT:
+               ep = ep_from_windex(hsotg, wIndex);
                if (!ep) {
                        dev_dbg(hsotg->dev, "%s: no endpoint for 0x%04x\n",
-                               __func__, le16_to_cpu(ctrl->wIndex));
+                               __func__, wIndex);
                        return -ENOENT;
                }
 
-               switch (le16_to_cpu(ctrl->wValue)) {
+               switch (wValue) {
                case USB_ENDPOINT_HALT:
                        halted = ep->halted;
 
@@ -1031,9 +1087,10 @@ static int s3c_hsotg_process_req_feature(struct dwc2_hsotg *hsotg,
                default:
                        return -ENOENT;
                }
-       } else
-               return -ENOENT;  /* currently only deal with endpoint */
-
+               break;
+       default:
+               return -ENOENT;
+       }
        return 1;
 }
 
@@ -1734,6 +1791,17 @@ static void s3c_hsotg_complete_in(struct dwc2_hsotg *hsotg,
        if (hs_ep->index == 0 && hsotg->ep0_state == DWC2_EP0_STATUS_IN) {
                dev_dbg(hsotg->dev, "zlp packet sent\n");
                s3c_hsotg_complete_request(hsotg, hs_ep, hs_req, 0);
+               if (hsotg->test_mode) {
+                       int ret;
+
+                       ret = s3c_hsotg_set_test_mode(hsotg, hsotg->test_mode);
+                       if (ret < 0) {
+                               dev_dbg(hsotg->dev, "Invalid Test #%d\n",
+                                               hsotg->test_mode);
+                               s3c_hsotg_stall_ep0(hsotg);
+                               return;
+                       }
+               }
                s3c_hsotg_enqueue_setup(hsotg);
                return;
        }
@@ -2045,6 +2113,7 @@ void s3c_hsotg_disconnect(struct dwc2_hsotg *hsotg)
                return;
 
        hsotg->connected = 0;
+       hsotg->test_mode = 0;
 
        for (ep = 0; ep < hsotg->num_of_eps; ep++) {
                if (hsotg->eps_in[ep])
@@ -3256,6 +3325,103 @@ static void s3c_hsotg_dump(struct dwc2_hsotg *hsotg)
 #endif
 }
 
+/**
+ * testmode_write - debugfs: change usb test mode
+ * @seq: The seq file to write to.
+ * @v: Unused parameter.
+ *
+ * This debugfs entry modify the current usb test mode.
+ */
+static ssize_t testmode_write(struct file *file, const char __user *ubuf, size_t
+               count, loff_t *ppos)
+{
+       struct seq_file         *s = file->private_data;
+       struct dwc2_hsotg       *hsotg = s->private;
+       unsigned long           flags;
+       u32                     testmode = 0;
+       char                    buf[32];
+
+       if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+               return -EFAULT;
+
+       if (!strncmp(buf, "test_j", 6))
+               testmode = TEST_J;
+       else if (!strncmp(buf, "test_k", 6))
+               testmode = TEST_K;
+       else if (!strncmp(buf, "test_se0_nak", 12))
+               testmode = TEST_SE0_NAK;
+       else if (!strncmp(buf, "test_packet", 11))
+               testmode = TEST_PACKET;
+       else if (!strncmp(buf, "test_force_enable", 17))
+               testmode = TEST_FORCE_EN;
+       else
+               testmode = 0;
+
+       spin_lock_irqsave(&hsotg->lock, flags);
+       s3c_hsotg_set_test_mode(hsotg, testmode);
+       spin_unlock_irqrestore(&hsotg->lock, flags);
+       return count;
+}
+
+/**
+ * testmode_show - debugfs: show usb test mode state
+ * @seq: The seq file to write to.
+ * @v: Unused parameter.
+ *
+ * This debugfs entry shows which usb test mode is currently enabled.
+ */
+static int testmode_show(struct seq_file *s, void *unused)
+{
+       struct dwc2_hsotg *hsotg = s->private;
+       unsigned long flags;
+       int dctl;
+
+       spin_lock_irqsave(&hsotg->lock, flags);
+       dctl = readl(hsotg->regs + DCTL);
+       dctl &= DCTL_TSTCTL_MASK;
+       dctl >>= DCTL_TSTCTL_SHIFT;
+       spin_unlock_irqrestore(&hsotg->lock, flags);
+
+       switch (dctl) {
+       case 0:
+               seq_puts(s, "no test\n");
+               break;
+       case TEST_J:
+               seq_puts(s, "test_j\n");
+               break;
+       case TEST_K:
+               seq_puts(s, "test_k\n");
+               break;
+       case TEST_SE0_NAK:
+               seq_puts(s, "test_se0_nak\n");
+               break;
+       case TEST_PACKET:
+               seq_puts(s, "test_packet\n");
+               break;
+       case TEST_FORCE_EN:
+               seq_puts(s, "test_force_enable\n");
+               break;
+       default:
+               seq_printf(s, "UNKNOWN %d\n", dctl);
+       }
+
+       return 0;
+}
+
+static int testmode_open(struct inode *inode, struct file *file)
+{
+       return single_open(file, testmode_show, inode->i_private);
+}
+
+static const struct file_operations testmode_fops = {
+       .owner          = THIS_MODULE,
+       .open           = testmode_open,
+       .write          = testmode_write,
+       .read           = seq_read,
+       .llseek         = seq_lseek,
+       .release        = single_release,
+};
+
 /**
  * state_show - debugfs: show overall driver and device state.
  * @seq: The seq file to write to.
@@ -3490,6 +3656,14 @@ static void s3c_hsotg_create_debug(struct dwc2_hsotg *hsotg)
        if (IS_ERR(hsotg->debug_file))
                dev_err(hsotg->dev, "%s: failed to create state\n", __func__);
 
+       hsotg->debug_testmode = debugfs_create_file("testmode",
+                                       S_IRUGO | S_IWUSR, root,
+                                       hsotg, &testmode_fops);
+
+       if (IS_ERR(hsotg->debug_testmode))
+               dev_err(hsotg->dev, "%s: failed to create testmode\n",
+                               __func__);
+
        hsotg->debug_fifo = debugfs_create_file("fifo", 0444, root,
                                                hsotg, &fifo_fops);
 
@@ -3544,6 +3718,7 @@ static void s3c_hsotg_delete_debug(struct dwc2_hsotg *hsotg)
        }
 
        debugfs_remove(hsotg->debug_file);
+       debugfs_remove(hsotg->debug_testmode);
        debugfs_remove(hsotg->debug_fifo);
        debugfs_remove(hsotg->debug_root);
 }