2 files changed, 33 insertions, 0 deletions

diff --git a/drivers/usb/gadget/langwell_udc.c b/drivers/usb/gadget/langwell_udc.c
index b8ec954c069..777972454e3 100644
--- a/drivers/usb/gadget/langwell_udc.c
+++ b/drivers/usb/gadget/langwell_udc.c
@@ -2225,6 +2225,7 @@ static void handle_setup_packet(struct langwell_udc *dev,
         u16     wValue = le16_to_cpu(setup->wValue);
         u16     wIndex = le16_to_cpu(setup->wIndex);
         u16     wLength = le16_to_cpu(setup->wLength);
+        u32     portsc1;
 
         dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
 
@@ -2313,6 +2314,28 @@ static void handle_setup_packet(struct langwell_udc *dev,
                                         dev->dev_status &= ~(1 << wValue);
                                 }
                                 break;
+                        case USB_DEVICE_TEST_MODE:
+                                dev_dbg(&dev->pdev->dev, "SETUP: TEST MODE\n");
+                                if ((wIndex & 0xff) ||
+                                        (dev->gadget.speed != USB_SPEED_HIGH))
+                                        ep0_stall(dev);
+
+                                switch (wIndex >> 8) {
+                                case TEST_J:
+                                case TEST_K:
+                                case TEST_SE0_NAK:
+                                case TEST_PACKET:
+                                case TEST_FORCE_EN:
+                                        if (prime_status_phase(dev, EP_DIR_IN))
+                                                ep0_stall(dev);
+                                        portsc1 = readl(&dev->op_regs->portsc1);
+                                        portsc1 |= (wIndex & 0xf00) << 8;
+                                        writel(portsc1, &dev->op_regs->portsc1);
+                                        goto end;
+                                default:
+                                        rc = -EOPNOTSUPP;
+                                }
+                                break;
                         default:
                                 rc = -EOPNOTSUPP;
                                 break;
diff --git a/include/linux/usb/ch9.h b/include/linux/usb/ch9.h
index f917bbbc890..ab461948b57 100644
--- a/include/linux/usb/ch9.h
+++ b/include/linux/usb/ch9.h
@@ -124,6 +124,16 @@
 #define USB_DEVICE_DEBUG_MODE           6       /* (special devices only) */
 
 /*
+ * Test Mode Selectors
+ * See USB 2.0 spec Table 9-7
+ */
+#define TEST_J          1
+#define TEST_K          2
+#define TEST_SE0_NAK    3
+#define TEST_PACKET     4
+#define TEST_FORCE_EN   5
+
+/*
  * New Feature Selectors as added by USB 3.0
  * See USB 3.0 spec Table 9-6
  */