usb: renesas_usbhs: add test-mode support

Signed-off-by: Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>
Signed-off-by: Felipe Balbi <balbi@ti.com>

3 files changed, 28 insertions, 1 deletions

diff --git a/drivers/usb/renesas_usbhs/common.c b/drivers/usb/renesas_usbhs/common.c
index 016a34f58523..32a879fb0e36 100644
--- a/drivers/usb/renesas_usbhs/common.c
+++ b/drivers/usb/renesas_usbhs/common.c
@@ -132,6 +132,11 @@ void usbhs_sys_function_ctrl(struct usbhs_priv *priv, int enable)
         usbhs_bset(priv, SYSCFG, mask, enable ? val : 0);
 }
 
+void usbhs_sys_set_test_mode(struct usbhs_priv *priv, u16 mode)
+{
+        usbhs_write(priv, TESTMODE, mode);
+}
+
 /*
  *              frame functions
  */
diff --git a/drivers/usb/renesas_usbhs/common.h b/drivers/usb/renesas_usbhs/common.h
index 71c295c76d9e..d79b3e27db95 100644
--- a/drivers/usb/renesas_usbhs/common.h
+++ b/drivers/usb/renesas_usbhs/common.h
@@ -33,6 +33,7 @@ struct usbhs_priv;
 #define SYSCFG          0x0000
 #define BUSWAIT         0x0002
 #define DVSTCTR         0x0008
+#define TESTMODE        0x000C
 #define CFIFO           0x0014
 #define CFIFOSEL        0x0020
 #define CFIFOCTR        0x0022
@@ -283,6 +284,7 @@ void usbhs_bset(struct usbhs_priv *priv, u32 reg, u16 mask, u16 data);
  */
 void usbhs_sys_host_ctrl(struct usbhs_priv *priv, int enable);
 void usbhs_sys_function_ctrl(struct usbhs_priv *priv, int enable);
+void usbhs_sys_set_test_mode(struct usbhs_priv *priv, u16 mode);
 
 /*
  * usb request
diff --git a/drivers/usb/renesas_usbhs/mod_gadget.c b/drivers/usb/renesas_usbhs/mod_gadget.c
index dba15e07fbd2..1951de02957e 100644
--- a/drivers/usb/renesas_usbhs/mod_gadget.c
+++ b/drivers/usb/renesas_usbhs/mod_gadget.c
@@ -14,6 +14,7 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  *
  */
+#include <linux/delay.h>
 #include <linux/dma-mapping.h>
 #include <linux/io.h>
 #include <linux/module.h>
@@ -286,6 +287,24 @@ struct usbhsg_recip_handle req_clear_feature = {
 /*
  *              USB_TYPE_STANDARD / set feature functions
  */
+static int usbhsg_recip_handler_std_set_device(struct usbhs_priv *priv,
+                                                 struct usbhsg_uep *uep,
+                                                 struct usb_ctrlrequest *ctrl)
+{
+        switch (le16_to_cpu(ctrl->wValue)) {
+        case USB_DEVICE_TEST_MODE:
+                usbhsg_recip_handler_std_control_done(priv, uep, ctrl);
+                udelay(100);
+                usbhs_sys_set_test_mode(priv, le16_to_cpu(ctrl->wIndex >> 8));
+                break;
+        default:
+                usbhsg_recip_handler_std_control_done(priv, uep, ctrl);
+                break;
+        }
+
+        return 0;
+}
+
 static int usbhsg_recip_handler_std_set_endpoint(struct usbhs_priv *priv,
                                                  struct usbhsg_uep *uep,
                                                  struct usb_ctrlrequest *ctrl)
@@ -301,7 +320,7 @@ static int usbhsg_recip_handler_std_set_endpoint(struct usbhs_priv *priv,
 
 struct usbhsg_recip_handle req_set_feature = {
         .name           = "set feature",
-        .device         = usbhsg_recip_handler_std_control_done,
+        .device         = usbhsg_recip_handler_std_set_device,
         .interface      = usbhsg_recip_handler_std_control_done,
         .endpoint       = usbhsg_recip_handler_std_set_endpoint,
 };
@@ -849,6 +868,7 @@ static int usbhsg_try_stop(struct usbhs_priv *priv, u32 status)
         gpriv->gadget.speed = USB_SPEED_UNKNOWN;
 
         /* disable sys */
+        usbhs_sys_set_test_mode(priv, 0);
         usbhs_sys_function_ctrl(priv, 0);
 
         usbhsg_pipe_disable(dcp);