usb: gadget: mv_udc: add test mode support

Add test mode support for marvell udc driver.

Signed-off-by: Neil Zhang <zhangwm@marvell.com>
Signed-off-by: Felipe Balbi <balbi@ti.com>

2 files changed, 54 insertions, 19 deletions

diff --git a/drivers/usb/gadget/mv_udc.h b/drivers/usb/gadget/mv_udc.h
index d3d26454b8bb..3e5e6ea7b0fb 100644
--- a/drivers/usb/gadget/mv_udc.h
+++ b/drivers/usb/gadget/mv_udc.h
@@ -202,6 +202,7 @@ struct mv_udc {
         unsigned int            ep0_dir;
 
         unsigned int            dev_addr;
+        unsigned int            test_mode;
 
         int                     errors;
         unsigned                softconnect:1,
@@ -238,6 +239,7 @@ struct mv_req {
         struct mv_dtd           *dtd, *head, *tail;
         struct mv_ep            *ep;
         struct list_head        queue;
+        unsigned int            test_mode;
         unsigned                dtd_count;
         unsigned                mapped:1;
 };
diff --git a/drivers/usb/gadget/mv_udc_core.c b/drivers/usb/gadget/mv_udc_core.c
index 1f47d03d6871..333b85b96292 100644
--- a/drivers/usb/gadget/mv_udc_core.c
+++ b/drivers/usb/gadget/mv_udc_core.c
@@ -1204,11 +1204,6 @@ static const struct usb_gadget_ops mv_ops = {
         .stop           = mv_udc_stop,
 };
 
-static void mv_udc_testmode(struct mv_udc *udc, u16 index, bool enter)
-{
-        dev_info(&udc->dev->dev, "Test Mode is not support yet\n");
-}
-
 static int eps_init(struct mv_udc *udc)
 {
         struct mv_ep    *ep;
@@ -1359,6 +1354,31 @@ static int mv_udc_stop(struct usb_gadget_driver *driver)
         return 0;
 }
 
+static void mv_set_ptc(struct mv_udc *udc, u32 mode)
+{
+        u32 portsc;
+
+        portsc = readl(&udc->op_regs->portsc[0]);
+        portsc |= mode << 16;
+        writel(portsc, &udc->op_regs->portsc[0]);
+}
+
+static void prime_status_complete(struct usb_ep *ep, struct usb_request *_req)
+{
+        struct mv_udc *udc = the_controller;
+        struct mv_req *req = container_of(_req, struct mv_req, req);
+        unsigned long flags;
+
+        dev_info(&udc->dev->dev, "switch to test mode %d\n", req->test_mode);
+
+        spin_lock_irqsave(&udc->lock, flags);
+        if (req->test_mode) {
+                mv_set_ptc(udc, req->test_mode);
+                req->test_mode = 0;
+        }
+        spin_unlock_irqrestore(&udc->lock, flags);
+}
+
 static int
 udc_prime_status(struct mv_udc *udc, u8 direction, u16 status, bool empty)
 {
@@ -1382,7 +1402,12 @@ udc_prime_status(struct mv_udc *udc, u8 direction, u16 status, bool empty)
         req->ep = ep;
         req->req.status = -EINPROGRESS;
         req->req.actual = 0;
-        req->req.complete = NULL;
+        if (udc->test_mode) {
+                req->req.complete = prime_status_complete;
+                req->test_mode = udc->test_mode;
+                udc->test_mode = 0;
+        } else
+                req->req.complete = NULL;
         req->dtd_count = 0;
 
         if (req->req.dma == DMA_ADDR_INVALID) {
@@ -1412,6 +1437,17 @@ out:
         return retval;
 }
 
+static void mv_udc_testmode(struct mv_udc *udc, u16 index)
+{
+        if (index <= TEST_FORCE_EN) {
+                udc->test_mode = index;
+                if (udc_prime_status(udc, EP_DIR_IN, 0, true))
+                        ep0_stall(udc);
+        } else
+                dev_err(&udc->dev->dev,
+                        "This test mode(%d) is not supported\n", index);
+}
+
 static void ch9setaddress(struct mv_udc *udc, struct usb_ctrlrequest *setup)
 {
         udc->dev_addr = (u8)setup->wValue;
@@ -1470,9 +1506,6 @@ static void ch9clearfeature(struct mv_udc *udc, struct usb_ctrlrequest *setup)
                 case USB_DEVICE_REMOTE_WAKEUP:
                         udc->remote_wakeup = 0;
                         break;
-                case USB_DEVICE_TEST_MODE:
-                        mv_udc_testmode(udc, 0, false);
-                        break;
                 default:
                         goto out;
                 }
@@ -1518,16 +1551,16 @@ static void ch9setfeature(struct mv_udc *udc, struct usb_ctrlrequest *setup)
                         break;
                 case USB_DEVICE_TEST_MODE:
                         if (setup->wIndex & 0xFF
-                                && udc->gadget.speed != USB_SPEED_HIGH)
-                                goto out;
-                        if (udc->usb_state == USB_STATE_CONFIGURED
-                                || udc->usb_state == USB_STATE_ADDRESS
-                                || udc->usb_state == USB_STATE_DEFAULT)
-                                mv_udc_testmode(udc,
-                                        setup->wIndex & 0xFF00, true);
-                        else
-                                goto out;
-                        break;
+                                ||  udc->gadget.speed != USB_SPEED_HIGH)
+                                ep0_stall(udc);
+
+                        if (udc->usb_state != USB_STATE_CONFIGURED
+                                && udc->usb_state != USB_STATE_ADDRESS
+                                && udc->usb_state != USB_STATE_DEFAULT)
+                                ep0_stall(udc);
+
+                        mv_udc_testmode(udc, (setup->wIndex >> 8));
+                        goto out;
                 default:
                         goto out;
                 }