MLK-17062 usb: cdns3: gadget: add test mode support for USB2

Add USB2 device test mode support for CDNS3 IP

Acked-by: Jun Li <jun.li@nxp.com>
Signed-off-by: Peter Chen <peter.chen@nxp.com>

diff --git a/drivers/usb/cdns3/dev-regs-macro.h b/drivers/usb/cdns3/dev-regs-macro.h
index 477de38..5b30762 100644 (file)
--- a/drivers/usb/cdns3/dev-regs-macro.h
+++ b/drivers/usb/cdns3/dev-regs-macro.h
@@ -62,6 +62,8 @@
 
 /* macros for field SET_ADDR */
 #define USB_CMD__SET_ADDR__MASK                                     0x00000001U
+#define USB_CMD__STMODE                                                0x00000200U
+#define USB_CMD__TMODE_SEL(x)                                    (x << 10)
 #define USB_CMD__FADDR__WRITE(src)       (((uint32_t)(src) << 1) & 0x000000feU)
 #endif /* __USB_CMD_MACRO__ */
 

diff --git a/drivers/usb/cdns3/gadget.c b/drivers/usb/cdns3/gadget.c
index b9d12d2..c2e42f8 100644 (file)
--- a/drivers/usb/cdns3/gadget.c
+++ b/drivers/usb/cdns3/gadget.c
@@ -516,6 +516,7 @@ static int cdns_req_ep0_handle_feature(struct usb_ss_dev *usb_ss,
        u32 recip = ctrl_req->bRequestType & USB_RECIP_MASK;
        struct usb_ss_endpoint *usb_ss_ep;
        u32 reg;
+       u8 tmode = 0;
 
        switch (recip) {
 
@@ -570,6 +571,38 @@ static int cdns_req_ep0_handle_feature(struct usb_ss_dev *usb_ss,
                        usb_ss->wake_up_flag = !!set;
                        break;
 
+               case USB_DEVICE_TEST_MODE:
+                       if (usb_ss->gadget.state != USB_STATE_CONFIGURED)
+                               return -EINVAL;
+                       if (usb_ss->gadget.speed != USB_SPEED_HIGH &&
+                               usb_ss->gadget.speed != USB_SPEED_FULL)
+                               return -EINVAL;
+                       if (ctrl_req->wLength != 0 ||
+                               ctrl_req->bRequestType & USB_DIR_IN) {
+                               dev_err(&usb_ss->dev, "req is error\n");
+                               return -EINVAL;
+                       }
+                       tmode = le16_to_cpu(ctrl_req->wIndex) >> 8;
+                       switch (tmode) {
+                       case TEST_J:
+                       case TEST_K:
+                       case TEST_SE0_NAK:
+                       case TEST_PACKET:
+                               reg = gadget_readl(usb_ss,
+                                       &usb_ss->regs->usb_cmd);
+                               tmode -= 1;
+                               reg |= USB_CMD__STMODE |
+                                       USB_CMD__TMODE_SEL(tmode);
+                               gadget_writel(usb_ss, &usb_ss->regs->usb_cmd,
+                                               reg);
+                               dev_info(&usb_ss->dev,
+                                       "set test mode, val=0x%x", reg);
+                               break;
+                       default:
+                               return -EINVAL;
+                       }
+                       break;
+
                default:
                        return -EINVAL;