3 files changed, 263 insertions, 2 deletions

diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 269a2e8ad0ab..d0d4cc151e71 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -712,6 +712,142 @@ ehci_hub_descriptor (
 }
 
 /*-------------------------------------------------------------------------*/
+#define EHSET_TEST_SINGLE_STEP_SET_FEATURE 0x06
+
+static void usb_ehset_completion(struct urb *urb)
+{
+        struct completion  *done = urb->context;
+
+        complete(done);
+}
+static int submit_single_step_set_feature(
+        struct usb_hcd  *hcd,
+        struct urb      *urb,
+        int             is_setup
+);
+
+/*
+ * Allocate and initialize a control URB. This request will be used by the
+ * EHSET SINGLE_STEP_SET_FEATURE test in which the DATA and STATUS stages
+ * of the GetDescriptor request are sent 15 seconds after the SETUP stage.
+ * Return NULL if failed.
+ */
+static struct urb *request_single_step_set_feature_urb(
+        struct usb_device       *udev,
+        void                    *dr,
+        void                    *buf,
+        struct completion       *done
+) {
+        struct urb *urb;
+        struct usb_hcd *hcd = bus_to_hcd(udev->bus);
+        struct usb_host_endpoint *ep;
+
+        urb = usb_alloc_urb(0, GFP_KERNEL);
+        if (!urb)
+                return NULL;
+
+        urb->pipe = usb_rcvctrlpipe(udev, 0);
+        ep = (usb_pipein(urb->pipe) ? udev->ep_in : udev->ep_out)
+                                [usb_pipeendpoint(urb->pipe)];
+        if (!ep) {
+                usb_free_urb(urb);
+                return NULL;
+        }
+
+        urb->ep = ep;
+        urb->dev = udev;
+        urb->setup_packet = (void *)dr;
+        urb->transfer_buffer = buf;
+        urb->transfer_buffer_length = USB_DT_DEVICE_SIZE;
+        urb->complete = usb_ehset_completion;
+        urb->status = -EINPROGRESS;
+        urb->actual_length = 0;
+        urb->transfer_flags = URB_DIR_IN;
+        usb_get_urb(urb);
+        atomic_inc(&urb->use_count);
+        atomic_inc(&urb->dev->urbnum);
+        urb->setup_dma = dma_map_single(
+                        hcd->self.controller,
+                        urb->setup_packet,
+                        sizeof(struct usb_ctrlrequest),
+                        DMA_TO_DEVICE);
+        urb->transfer_dma = dma_map_single(
+                        hcd->self.controller,
+                        urb->transfer_buffer,
+                        urb->transfer_buffer_length,
+                        DMA_FROM_DEVICE);
+        urb->context = done;
+        return urb;
+}
+
+static int ehset_single_step_set_feature(struct usb_hcd *hcd, int port)
+{
+        int retval = -ENOMEM;
+        struct usb_ctrlrequest *dr;
+        struct urb *urb;
+        struct usb_device *udev;
+        struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+        struct usb_device_descriptor *buf;
+        DECLARE_COMPLETION_ONSTACK(done);
+
+        /* Obtain udev of the rhub's child port */
+        udev = usb_hub_find_child(hcd->self.root_hub, port);
+        if (!udev) {
+                ehci_err(ehci, "No device attached to the RootHub\n");
+                return -ENODEV;
+        }
+        buf = kmalloc(USB_DT_DEVICE_SIZE, GFP_KERNEL);
+        if (!buf)
+                return -ENOMEM;
+
+        dr = kmalloc(sizeof(struct usb_ctrlrequest), GFP_KERNEL);
+        if (!dr) {
+                kfree(buf);
+                return -ENOMEM;
+        }
+
+        /* Fill Setup packet for GetDescriptor */
+        dr->bRequestType = USB_DIR_IN;
+        dr->bRequest = USB_REQ_GET_DESCRIPTOR;
+        dr->wValue = cpu_to_le16(USB_DT_DEVICE << 8);
+        dr->wIndex = 0;
+        dr->wLength = cpu_to_le16(USB_DT_DEVICE_SIZE);
+        urb = request_single_step_set_feature_urb(udev, dr, buf, &done);
+        if (!urb)
+                goto cleanup;
+
+        /* Submit just the SETUP stage */
+        retval = submit_single_step_set_feature(hcd, urb, 1);
+        if (retval)
+                goto out1;
+        if (!wait_for_completion_timeout(&done, msecs_to_jiffies(2000))) {
+                usb_kill_urb(urb);
+                retval = -ETIMEDOUT;
+                ehci_err(ehci, "%s SETUP stage timed out on ep0\n", __func__);
+                goto out1;
+        }
+        msleep(15 * 1000);
+
+        /* Complete remaining DATA and STATUS stages using the same URB */
+        urb->status = -EINPROGRESS;
+        usb_get_urb(urb);
+        atomic_inc(&urb->use_count);
+        atomic_inc(&urb->dev->urbnum);
+        retval = submit_single_step_set_feature(hcd, urb, 0);
+        if (!retval && !wait_for_completion_timeout(&done,
+                                                msecs_to_jiffies(2000))) {
+                usb_kill_urb(urb);
+                retval = -ETIMEDOUT;
+                ehci_err(ehci, "%s IN stage timed out on ep0\n", __func__);
+        }
+out1:
+        usb_free_urb(urb);
+cleanup:
+        kfree(dr);
+        kfree(buf);
+        return retval;
+}
+/*-------------------------------------------------------------------------*/
 
 static int ehci_hub_control (
         struct usb_hcd  *hcd,
@@ -1086,7 +1222,13 @@ static int ehci_hub_control (
                  * about the EHCI-specific stuff.
                  */
                 case USB_PORT_FEAT_TEST:
-                        if (!selector || selector > 5)
+                        if (selector == EHSET_TEST_SINGLE_STEP_SET_FEATURE) {
+                                spin_unlock_irqrestore(&ehci->lock, flags);
+                                retval = ehset_single_step_set_feature(hcd,
+                                                                        wIndex);
+                                spin_lock_irqsave(&ehci->lock, flags);
+                                break;
+                        } else if (!selector || selector > 5)
                                 goto error;
                         spin_unlock_irqrestore(&ehci->lock, flags);
                         ehci_quiesce(ehci);
diff --git a/drivers/usb/host/ehci-q.c b/drivers/usb/host/ehci-q.c
index b637a65e1e52..903d00dd6683 100644
--- a/drivers/usb/host/ehci-q.c
+++ b/drivers/usb/host/ehci-q.c
@@ -1139,6 +1139,107 @@ submit_async (
 }
 
 /*-------------------------------------------------------------------------*/
+/*
+ * This function creates the qtds and submits them for the
+ * SINGLE_STEP_SET_FEATURE Test.
+ * This is done in two parts: first SETUP req for GetDesc is sent then
+ * 15 seconds later, the IN stage for GetDesc starts to req data from dev
+ *
+ * is_setup : i/p arguement decides which of the two stage needs to be
+ * performed; TRUE - SETUP and FALSE - IN+STATUS
+ * Returns 0 if success
+ */
+static int submit_single_step_set_feature(
+        struct usb_hcd  *hcd,
+        struct urb      *urb,
+        int             is_setup
+) {
+        struct ehci_hcd         *ehci = hcd_to_ehci(hcd);
+        struct list_head        qtd_list;
+        struct list_head        *head;
+
+        struct ehci_qtd         *qtd, *qtd_prev;
+        dma_addr_t              buf;
+        int                     len, maxpacket;
+        u32                     token;
+
+        INIT_LIST_HEAD(&qtd_list);
+        head = &qtd_list;
+
+        /* URBs map to sequences of QTDs:  one logical transaction */
+        qtd = ehci_qtd_alloc(ehci, GFP_KERNEL);
+        if (unlikely(!qtd))
+                return -1;
+        list_add_tail(&qtd->qtd_list, head);
+        qtd->urb = urb;
+
+        token = QTD_STS_ACTIVE;
+        token |= (EHCI_TUNE_CERR << 10);
+
+        len = urb->transfer_buffer_length;
+        /*
+         * Check if the request is to perform just the SETUP stage (getDesc)
+         * as in SINGLE_STEP_SET_FEATURE test, DATA stage (IN) happens
+         * 15 secs after the setup
+         */
+        if (is_setup) {
+                /* SETUP pid */
+                qtd_fill(ehci, qtd, urb->setup_dma,
+                                sizeof(struct usb_ctrlrequest),
+                                token | (2 /* "setup" */ << 8), 8);
+
+                submit_async(ehci, urb, &qtd_list, GFP_ATOMIC);
+                return 0; /*Return now; we shall come back after 15 seconds*/
+        }
+
+        /*
+         * IN: data transfer stage:  buffer setup : start the IN txn phase for
+         * the get_Desc SETUP which was sent 15seconds back
+         */
+        token ^= QTD_TOGGLE;   /*We need to start IN with DATA-1 Pid-sequence*/
+        buf = urb->transfer_dma;
+
+        token |= (1 /* "in" */ << 8);  /*This is IN stage*/
+
+        maxpacket = max_packet(usb_maxpacket(urb->dev, urb->pipe, 0));
+
+        qtd_fill(ehci, qtd, buf, len, token, maxpacket);
+
+        /*
+         * Our IN phase shall always be a short read; so keep the queue running
+         * and let it advance to the next qtd which zero length OUT status
+         */
+        qtd->hw_alt_next = EHCI_LIST_END(ehci);
+
+        /* STATUS stage for GetDesc control request */
+        token ^= 0x0100;        /* "in" <--> "out"  */
+        token |= QTD_TOGGLE;    /* force DATA1 */
+
+        qtd_prev = qtd;
+        qtd = ehci_qtd_alloc(ehci, GFP_ATOMIC);
+        if (unlikely(!qtd))
+                goto cleanup;
+        qtd->urb = urb;
+        qtd_prev->hw_next = QTD_NEXT(ehci, qtd->qtd_dma);
+        list_add_tail(&qtd->qtd_list, head);
+
+        /* dont fill any data in such packets */
+        qtd_fill(ehci, qtd, 0, 0, token, 0);
+
+        /* by default, enable interrupt on urb completion */
+        if (likely(!(urb->transfer_flags & URB_NO_INTERRUPT)))
+                qtd->hw_token |= cpu_to_hc32(ehci, QTD_IOC);
+
+        submit_async(ehci, urb, &qtd_list, GFP_KERNEL);
+
+        return 0;
+
+cleanup:
+        qtd_list_free(ehci, urb, head);
+        return -1;
+}
+
+/*-------------------------------------------------------------------------*/
 
 static void single_unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
diff --git a/drivers/usb/misc/ehset.c b/drivers/usb/misc/ehset.c
index d2864b3f41fe..c31b4a33e6bb 100644
--- a/drivers/usb/misc/ehset.c
+++ b/drivers/usb/misc/ehset.c
@@ -96,7 +96,25 @@ static int ehset_probe(struct usb_interface *intf,
                 kfree(buf);
                 break;
         case TEST_SINGLE_STEP_SET_FEATURE:
-                /* unsupported for now */
+                /*
+                 * GetDescriptor SETUP request -> 15secs delay -> IN & STATUS
+                 *
+                 * Note, this test is only supported on root hubs since the
+                 * SetPortFeature handling can only be done inside the HCD's
+                 * hub_control callback function.
+                 */
+                if (hub_udev != dev->bus->root_hub) {
+                        dev_err(&intf->dev, "SINGLE_STEP_SET_FEATURE test only supported on root hub\n");
+                        break;
+                }
+
+                ret = usb_control_msg(hub_udev, usb_sndctrlpipe(hub_udev, 0),
+                                        USB_REQ_SET_FEATURE, USB_RT_PORT,
+                                        USB_PORT_FEAT_TEST,
+                                        (6 << 8) | portnum,
+                                        NULL, 0, 60 * 1000);
+
+                break;
         default:
                 dev_err(&intf->dev, "%s: unsupported PID: 0x%x\n",
                         __func__, test_pid);