2 files changed, 47 insertions, 8 deletions

diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index d05397ec8a18..8498f1a114d5 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -373,6 +373,8 @@ static void reset_config(struct usb_composite_dev *cdev)
         list_for_each_entry(f, &cdev->config->functions, list) {
                 if (f->disable)
                         f->disable(f);
+
+                bitmap_zero(f->endpoints, 32);
         }
         cdev->config = NULL;
 }
@@ -418,10 +420,35 @@ static int set_config(struct usb_composite_dev *cdev,
         /* Initialize all interfaces by setting them to altsetting zero. */
         for (tmp = 0; tmp < MAX_CONFIG_INTERFACES; tmp++) {
                 struct usb_function     *f = c->interface[tmp];
+                struct usb_descriptor_header **descriptors;
 
                 if (!f)
                         break;
 
+                /*
+                 * Record which endpoints are used by the function. This is used
+                 * to dispatch control requests targeted at that endpoint to the
+                 * function's setup callback instead of the current
+                 * configuration's setup callback.
+                 */
+                if (gadget->speed == USB_SPEED_HIGH)
+                        descriptors = f->hs_descriptors;
+                else
+                        descriptors = f->descriptors;
+
+                for (; *descriptors; ++descriptors) {
+                        struct usb_endpoint_descriptor *ep;
+                        int addr;
+
+                        if ((*descriptors)->bDescriptorType != USB_DT_ENDPOINT)
+                                continue;
+
+                        ep = (struct usb_endpoint_descriptor *)*descriptors;
+                        addr = ((ep->bEndpointAddress & 0x80) >> 3)
+                             |  (ep->bEndpointAddress & 0x0f);
+                        set_bit(addr, f->endpoints);
+                }
+
                 result = f->set_alt(f, tmp, 0);
                 if (result < 0) {
                         DBG(cdev, "interface %d (%s/%p) alt 0 --> %d\n",
@@ -688,6 +715,7 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
         u16                             w_value = le16_to_cpu(ctrl->wValue);
         u16                             w_length = le16_to_cpu(ctrl->wLength);
         struct usb_function             *f = NULL;
+        u8                              endp;
 
         /* partial re-init of the response message; the function or the
          * gadget might need to intercept e.g. a control-OUT completion
@@ -800,23 +828,33 @@ unknown:
                         ctrl->bRequestType, ctrl->bRequest,
                         w_value, w_index, w_length);
 
-                /* functions always handle their interfaces ... punt other
-                 * recipients (endpoint, other, WUSB, ...) to the current
+                /* functions always handle their interfaces and endpoints...
+                 * punt other recipients (other, WUSB, ...) to the current
                  * configuration code.
                  *
                  * REVISIT it could make sense to let the composite device
                  * take such requests too, if that's ever needed:  to work
                  * in config 0, etc.
                  */
-                if ((ctrl->bRequestType & USB_RECIP_MASK)
-                                == USB_RECIP_INTERFACE) {
+                switch (ctrl->bRequestType & USB_RECIP_MASK) {
+                case USB_RECIP_INTERFACE:
                         f = cdev->config->interface[intf];
-                        if (f && f->setup)
-                                value = f->setup(f, ctrl);
-                        else
+                        break;
+
+                case USB_RECIP_ENDPOINT:
+                        endp = ((w_index & 0x80) >> 3) | (w_index & 0x0f);
+                        list_for_each_entry(f, &cdev->config->functions, list) {
+                                if (test_bit(endp, f->endpoints))
+                                        break;
+                        }
+                        if (&f->list == &cdev->config->functions)
                                 f = NULL;
+                        break;
                 }
-                if (value < 0 && !f) {
+
+                if (f && f->setup)
+                        value = f->setup(f, ctrl);
+                else {
                         struct usb_configuration        *c;
 
                         c = cdev->config;
diff --git a/include/linux/usb/composite.h b/include/linux/usb/composite.h
index 4f6bb3d2160e..738ea1a691cb 100644
--- a/include/linux/usb/composite.h
+++ b/include/linux/usb/composite.h
@@ -127,6 +127,7 @@ struct usb_function {
         /* private: */
         /* internals */
         struct list_head                list;
+        DECLARE_BITMAP(endpoints, 32);
 };
 
 int usb_add_function(struct usb_configuration *, struct usb_function *);