1 files changed, 339 insertions, 24 deletions

diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index 5cbb1a41c223..5ef87794fd32 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -27,7 +27,7 @@
 #include <linux/utsname.h>
 
 #include <linux/usb/composite.h>
-
+#include <asm/unaligned.h>
 
 /*
  * The code in this file is utility code, used to build a gadget driver
@@ -74,6 +74,130 @@ MODULE_PARM_DESC(iSerialNumber, "SerialNumber string");
 static char composite_manufacturer[50];
 
 /*-------------------------------------------------------------------------*/
+/**
+ * next_ep_desc() - advance to the next EP descriptor
+ * @t: currect pointer within descriptor array
+ *
+ * Return: next EP descriptor or NULL
+ *
+ * Iterate over @t until either EP descriptor found or
+ * NULL (that indicates end of list) encountered
+ */
+static struct usb_descriptor_header**
+next_ep_desc(struct usb_descriptor_header **t)
+{
+        for (; *t; t++) {
+                if ((*t)->bDescriptorType == USB_DT_ENDPOINT)
+                        return t;
+        }
+        return NULL;
+}
+
+/*
+ * for_each_ep_desc()- iterate over endpoint descriptors in the
+ *              descriptors list
+ * @start:      pointer within descriptor array.
+ * @ep_desc:    endpoint descriptor to use as the loop cursor
+ */
+#define for_each_ep_desc(start, ep_desc) \
+        for (ep_desc = next_ep_desc(start); \
+              ep_desc; ep_desc = next_ep_desc(ep_desc+1))
+
+/**
+ * config_ep_by_speed() - configures the given endpoint
+ * according to gadget speed.
+ * @g: pointer to the gadget
+ * @f: usb function
+ * @_ep: the endpoint to configure
+ *
+ * Return: error code, 0 on success
+ *
+ * This function chooses the right descriptors for a given
+ * endpoint according to gadget speed and saves it in the
+ * endpoint desc field. If the endpoint already has a descriptor
+ * assigned to it - overwrites it with currently corresponding
+ * descriptor. The endpoint maxpacket field is updated according
+ * to the chosen descriptor.
+ * Note: the supplied function should hold all the descriptors
+ * for supported speeds
+ */
+int config_ep_by_speed(struct usb_gadget *g,
+                        struct usb_function *f,
+                        struct usb_ep *_ep)
+{
+        struct usb_endpoint_descriptor *chosen_desc = NULL;
+        struct usb_descriptor_header **speed_desc = NULL;
+
+        struct usb_ss_ep_comp_descriptor *comp_desc = NULL;
+        int want_comp_desc = 0;
+
+        struct usb_descriptor_header **d_spd; /* cursor for speed desc */
+
+        if (!g || !f || !_ep)
+                return -EIO;
+
+        /* select desired speed */
+        switch (g->speed) {
+        case USB_SPEED_SUPER:
+                if (gadget_is_superspeed(g)) {
+                        speed_desc = f->ss_descriptors;
+                        want_comp_desc = 1;
+                        break;
+                }
+                /* else: Fall trough */
+        case USB_SPEED_HIGH:
+                if (gadget_is_dualspeed(g)) {
+                        speed_desc = f->hs_descriptors;
+                        break;
+                }
+                /* else: fall through */
+        default:
+                speed_desc = f->descriptors;
+        }
+        /* find descriptors */
+        for_each_ep_desc(speed_desc, d_spd) {
+                chosen_desc = (struct usb_endpoint_descriptor *)*d_spd;
+                if (chosen_desc->bEndpointAddress == _ep->address)
+                        goto ep_found;
+        }
+        return -EIO;
+
+ep_found:
+        /* commit results */
+        _ep->maxpacket = le16_to_cpu(chosen_desc->wMaxPacketSize);
+        _ep->desc = chosen_desc;
+        _ep->comp_desc = NULL;
+        _ep->maxburst = 0;
+        _ep->mult = 0;
+        if (!want_comp_desc)
+                return 0;
+
+        /*
+         * Companion descriptor should follow EP descriptor
+         * USB 3.0 spec, #9.6.7
+         */
+        comp_desc = (struct usb_ss_ep_comp_descriptor *)*(++d_spd);
+        if (!comp_desc ||
+            (comp_desc->bDescriptorType != USB_DT_SS_ENDPOINT_COMP))
+                return -EIO;
+        _ep->comp_desc = comp_desc;
+        if (g->speed == USB_SPEED_SUPER) {
+                switch (usb_endpoint_type(_ep->desc)) {
+                case USB_ENDPOINT_XFER_BULK:
+                case USB_ENDPOINT_XFER_INT:
+                        _ep->maxburst = comp_desc->bMaxBurst;
+                        break;
+                case USB_ENDPOINT_XFER_ISOC:
+                        /* mult: bits 1:0 of bmAttributes */
+                        _ep->mult = comp_desc->bmAttributes & 0x3;
+                        break;
+                default:
+                        /* Do nothing for control endpoints */
+                        break;
+                }
+        }
+        return 0;
+}
 
 /**
  * usb_add_function() - add a function to a configuration
@@ -123,6 +247,8 @@ int usb_add_function(struct usb_configuration *config,
                 config->fullspeed = true;
         if (!config->highspeed && function->hs_descriptors)
                 config->highspeed = true;
+        if (!config->superspeed && function->ss_descriptors)
+                config->superspeed = true;
 
 done:
         if (value)
@@ -266,10 +392,17 @@ static int config_buf(struct usb_configuration *config,
         list_for_each_entry(f, &config->functions, list) {
                 struct usb_descriptor_header **descriptors;
 
-                if (speed == USB_SPEED_HIGH)
+                switch (speed) {
+                case USB_SPEED_SUPER:
+                        descriptors = f->ss_descriptors;
+                        break;
+                case USB_SPEED_HIGH:
                         descriptors = f->hs_descriptors;
-                else
+                        break;
+                default:
                         descriptors = f->descriptors;
+                }
+
                 if (!descriptors)
                         continue;
                 status = usb_descriptor_fillbuf(next, len,
@@ -292,9 +425,10 @@ static int config_desc(struct usb_composite_dev *cdev, unsigned w_value)
         u8                              type = w_value >> 8;
         enum usb_device_speed           speed = USB_SPEED_UNKNOWN;
 
-        if (gadget_is_dualspeed(gadget)) {
-                int                     hs = 0;
-
+        if (gadget->speed == USB_SPEED_SUPER)
+                speed = gadget->speed;
+        else if (gadget_is_dualspeed(gadget)) {
+                int     hs = 0;
                 if (gadget->speed == USB_SPEED_HIGH)
                         hs = 1;
                 if (type == USB_DT_OTHER_SPEED_CONFIG)
@@ -308,13 +442,20 @@ static int config_desc(struct usb_composite_dev *cdev, unsigned w_value)
         w_value &= 0xff;
         list_for_each_entry(c, &cdev->configs, list) {
                 /* ignore configs that won't work at this speed */
-                if (speed == USB_SPEED_HIGH) {
+                switch (speed) {
+                case USB_SPEED_SUPER:
+                        if (!c->superspeed)
+                                continue;
+                        break;
+                case USB_SPEED_HIGH:
                         if (!c->highspeed)
                                 continue;
-                } else {
+                        break;
+                default:
                         if (!c->fullspeed)
                                 continue;
                 }
+
                 if (w_value == 0)
                         return config_buf(c, speed, cdev->req->buf, type);
                 w_value--;
@@ -328,16 +469,22 @@ static int count_configs(struct usb_composite_dev *cdev, unsigned type)
         struct usb_configuration        *c;
         unsigned                        count = 0;
         int                             hs = 0;
+        int                             ss = 0;
 
         if (gadget_is_dualspeed(gadget)) {
                 if (gadget->speed == USB_SPEED_HIGH)
                         hs = 1;
+                if (gadget->speed == USB_SPEED_SUPER)
+                        ss = 1;
                 if (type == USB_DT_DEVICE_QUALIFIER)
                         hs = !hs;
         }
         list_for_each_entry(c, &cdev->configs, list) {
                 /* ignore configs that won't work at this speed */
-                if (hs) {
+                if (ss) {
+                        if (!c->superspeed)
+                                continue;
+                } else if (hs) {
                         if (!c->highspeed)
                                 continue;
                 } else {
@@ -349,6 +496,71 @@ static int count_configs(struct usb_composite_dev *cdev, unsigned type)
         return count;
 }
 
+/**
+ * bos_desc() - prepares the BOS descriptor.
+ * @cdev: pointer to usb_composite device to generate the bos
+ *      descriptor for
+ *
+ * This function generates the BOS (Binary Device Object)
+ * descriptor and its device capabilities descriptors. The BOS
+ * descriptor should be supported by a SuperSpeed device.
+ */
+static int bos_desc(struct usb_composite_dev *cdev)
+{
+        struct usb_ext_cap_descriptor   *usb_ext;
+        struct usb_ss_cap_descriptor    *ss_cap;
+        struct usb_dcd_config_params    dcd_config_params;
+        struct usb_bos_descriptor       *bos = cdev->req->buf;
+
+        bos->bLength = USB_DT_BOS_SIZE;
+        bos->bDescriptorType = USB_DT_BOS;
+
+        bos->wTotalLength = cpu_to_le16(USB_DT_BOS_SIZE);
+        bos->bNumDeviceCaps = 0;
+
+        /*
+         * A SuperSpeed device shall include the USB2.0 extension descriptor
+         * and shall support LPM when operating in USB2.0 HS mode.
+         */
+        usb_ext = cdev->req->buf + le16_to_cpu(bos->wTotalLength);
+        bos->bNumDeviceCaps++;
+        le16_add_cpu(&bos->wTotalLength, USB_DT_USB_EXT_CAP_SIZE);
+        usb_ext->bLength = USB_DT_USB_EXT_CAP_SIZE;
+        usb_ext->bDescriptorType = USB_DT_DEVICE_CAPABILITY;
+        usb_ext->bDevCapabilityType = USB_CAP_TYPE_EXT;
+        usb_ext->bmAttributes = cpu_to_le32(USB_LPM_SUPPORT);
+
+        /*
+         * The Superspeed USB Capability descriptor shall be implemented by all
+         * SuperSpeed devices.
+         */
+        ss_cap = cdev->req->buf + le16_to_cpu(bos->wTotalLength);
+        bos->bNumDeviceCaps++;
+        le16_add_cpu(&bos->wTotalLength, USB_DT_USB_SS_CAP_SIZE);
+        ss_cap->bLength = USB_DT_USB_SS_CAP_SIZE;
+        ss_cap->bDescriptorType = USB_DT_DEVICE_CAPABILITY;
+        ss_cap->bDevCapabilityType = USB_SS_CAP_TYPE;
+        ss_cap->bmAttributes = 0; /* LTM is not supported yet */
+        ss_cap->wSpeedSupported = cpu_to_le16(USB_LOW_SPEED_OPERATION |
+                                USB_FULL_SPEED_OPERATION |
+                                USB_HIGH_SPEED_OPERATION |
+                                USB_5GBPS_OPERATION);
+        ss_cap->bFunctionalitySupport = USB_LOW_SPEED_OPERATION;
+
+        /* Get Controller configuration */
+        if (cdev->gadget->ops->get_config_params)
+                cdev->gadget->ops->get_config_params(&dcd_config_params);
+        else {
+                dcd_config_params.bU1devExitLat = USB_DEFULT_U1_DEV_EXIT_LAT;
+                dcd_config_params.bU2DevExitLat =
+                        cpu_to_le16(USB_DEFULT_U2_DEV_EXIT_LAT);
+        }
+        ss_cap->bU1devExitLat = dcd_config_params.bU1devExitLat;
+        ss_cap->bU2DevExitLat = dcd_config_params.bU2DevExitLat;
+
+        return le16_to_cpu(bos->wTotalLength);
+}
+
 static void device_qual(struct usb_composite_dev *cdev)
 {
         struct usb_qualifier_descriptor *qual = cdev->req->buf;
@@ -361,7 +573,7 @@ static void device_qual(struct usb_composite_dev *cdev)
         qual->bDeviceSubClass = cdev->desc.bDeviceSubClass;
         qual->bDeviceProtocol = cdev->desc.bDeviceProtocol;
         /* ASSUME same EP0 fifo size at both speeds */
-        qual->bMaxPacketSize0 = cdev->desc.bMaxPacketSize0;
+        qual->bMaxPacketSize0 = cdev->gadget->ep0->maxpacket;
         qual->bNumConfigurations = count_configs(cdev, USB_DT_DEVICE_QUALIFIER);
         qual->bRESERVED = 0;
 }
@@ -392,28 +604,46 @@ static int set_config(struct usb_composite_dev *cdev,
         unsigned                power = gadget_is_otg(gadget) ? 8 : 100;
         int                     tmp;
 
-        if (cdev->config)
-                reset_config(cdev);
-
         if (number) {
                 list_for_each_entry(c, &cdev->configs, list) {
                         if (c->bConfigurationValue == number) {
+                                /*
+                                 * We disable the FDs of the previous
+                                 * configuration only if the new configuration
+                                 * is a valid one
+                                 */
+                                if (cdev->config)
+                                        reset_config(cdev);
                                 result = 0;
                                 break;
                         }
                 }
                 if (result < 0)
                         goto done;
-        } else
+        } else { /* Zero configuration value - need to reset the config */
+                if (cdev->config)
+                        reset_config(cdev);
                 result = 0;
+        }
 
         INFO(cdev, "%s speed config #%d: %s\n",
                 ({ char *speed;
                 switch (gadget->speed) {
-                case USB_SPEED_LOW:     speed = "low"; break;
-                case USB_SPEED_FULL:    speed = "full"; break;
-                case USB_SPEED_HIGH:    speed = "high"; break;
-                default:                speed = "?"; break;
+                case USB_SPEED_LOW:
+                        speed = "low";
+                        break;
+                case USB_SPEED_FULL:
+                        speed = "full";
+                        break;
+                case USB_SPEED_HIGH:
+                        speed = "high";
+                        break;
+                case USB_SPEED_SUPER:
+                        speed = "super";
+                        break;
+                default:
+                        speed = "?";
+                        break;
                 } ; speed; }), number, c ? c->label : "unconfigured");
 
         if (!c)
@@ -435,10 +665,16 @@ static int set_config(struct usb_composite_dev *cdev,
                  * function's setup callback instead of the current
                  * configuration's setup callback.
                  */
-                if (gadget->speed == USB_SPEED_HIGH)
+                switch (gadget->speed) {
+                case USB_SPEED_SUPER:
+                        descriptors = f->ss_descriptors;
+                        break;
+                case USB_SPEED_HIGH:
                         descriptors = f->hs_descriptors;
-                else
+                        break;
+                default:
                         descriptors = f->descriptors;
+                }
 
                 for (; *descriptors; ++descriptors) {
                         struct usb_endpoint_descriptor *ep;
@@ -531,8 +767,9 @@ int usb_add_config(struct usb_composite_dev *cdev,
         } else {
                 unsigned        i;
 
-                DBG(cdev, "cfg %d/%p speeds:%s%s\n",
+                DBG(cdev, "cfg %d/%p speeds:%s%s%s\n",
                         config->bConfigurationValue, config,
+                        config->superspeed ? " super" : "",
                         config->highspeed ? " high" : "",
                         config->fullspeed
                                 ? (gadget_is_dualspeed(cdev->gadget)
@@ -811,6 +1048,7 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
         struct usb_composite_dev        *cdev = get_gadget_data(gadget);
         struct usb_request              *req = cdev->req;
         int                             value = -EOPNOTSUPP;
+        int                             status = 0;
         u16                             w_index = le16_to_cpu(ctrl->wIndex);
         u8                              intf = w_index & 0xFF;
         u16                             w_value = le16_to_cpu(ctrl->wValue);
@@ -838,18 +1076,29 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
                 case USB_DT_DEVICE:
                         cdev->desc.bNumConfigurations =
                                 count_configs(cdev, USB_DT_DEVICE);
+                        cdev->desc.bMaxPacketSize0 =
+                                cdev->gadget->ep0->maxpacket;
+                        if (gadget_is_superspeed(gadget)) {
+                                if (gadget->speed >= USB_SPEED_SUPER)
+                                        cdev->desc.bcdUSB = cpu_to_le16(0x0300);
+                                else
+                                        cdev->desc.bcdUSB = cpu_to_le16(0x0210);
+                        }
+
                         value = min(w_length, (u16) sizeof cdev->desc);
                         memcpy(req->buf, &cdev->desc, value);
                         break;
                 case USB_DT_DEVICE_QUALIFIER:
-                        if (!gadget_is_dualspeed(gadget))
+                        if (!gadget_is_dualspeed(gadget) ||
+                            gadget->speed >= USB_SPEED_SUPER)
                                 break;
                         device_qual(cdev);
                         value = min_t(int, w_length,
                                 sizeof(struct usb_qualifier_descriptor));
                         break;
                 case USB_DT_OTHER_SPEED_CONFIG:
-                        if (!gadget_is_dualspeed(gadget))
+                        if (!gadget_is_dualspeed(gadget) ||
+                            gadget->speed >= USB_SPEED_SUPER)
                                 break;
                         /* FALLTHROUGH */
                 case USB_DT_CONFIG:
@@ -863,6 +1112,12 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
                         if (value >= 0)
                                 value = min(w_length, (u16) value);
                         break;
+                case USB_DT_BOS:
+                        if (gadget_is_superspeed(gadget)) {
+                                value = bos_desc(cdev);
+                                value = min(w_length, (u16) value);
+                        }
+                        break;
                 }
                 break;
 
@@ -930,6 +1185,61 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
                 *((u8 *)req->buf) = value;
                 value = min(w_length, (u16) 1);
                 break;
+
+        /*
+         * USB 3.0 additions:
+         * Function driver should handle get_status request. If such cb
+         * wasn't supplied we respond with default value = 0
+         * Note: function driver should supply such cb only for the first
+         * interface of the function
+         */
+        case USB_REQ_GET_STATUS:
+                if (!gadget_is_superspeed(gadget))
+                        goto unknown;
+                if (ctrl->bRequestType != (USB_DIR_IN | USB_RECIP_INTERFACE))
+                        goto unknown;
+                value = 2;      /* This is the length of the get_status reply */
+                put_unaligned_le16(0, req->buf);
+                if (!cdev->config || intf >= MAX_CONFIG_INTERFACES)
+                        break;
+                f = cdev->config->interface[intf];
+                if (!f)
+                        break;
+                status = f->get_status ? f->get_status(f) : 0;
+                if (status < 0)
+                        break;
+                put_unaligned_le16(status & 0x0000ffff, req->buf);
+                break;
+        /*
+         * Function drivers should handle SetFeature/ClearFeature
+         * (FUNCTION_SUSPEND) request. function_suspend cb should be supplied
+         * only for the first interface of the function
+         */
+        case USB_REQ_CLEAR_FEATURE:
+        case USB_REQ_SET_FEATURE:
+                if (!gadget_is_superspeed(gadget))
+                        goto unknown;
+                if (ctrl->bRequestType != (USB_DIR_OUT | USB_RECIP_INTERFACE))
+                        goto unknown;
+                switch (w_value) {
+                case USB_INTRF_FUNC_SUSPEND:
+                        if (!cdev->config || intf >= MAX_CONFIG_INTERFACES)
+                                break;
+                        f = cdev->config->interface[intf];
+                        if (!f)
+                                break;
+                        value = 0;
+                        if (f->func_suspend)
+                                value = f->func_suspend(f, w_index >> 8);
+                        if (value < 0) {
+                                ERROR(cdev,
+                                      "func_suspend() returned error %d\n",
+                                      value);
+                                value = 0;
+                        }
+                        break;
+                }
+                break;
         default:
 unknown:
                 VDBG(cdev,
@@ -1140,7 +1450,6 @@ static int composite_bind(struct usb_gadget *gadget)
                 goto fail;
 
         cdev->desc = *composite->dev;
-        cdev->desc.bMaxPacketSize0 = gadget->ep0->maxpacket;
 
         /* standardized runtime overrides for device ID data */
         if (idVendor)
@@ -1247,7 +1556,11 @@ composite_resume(struct usb_gadget *gadget)
 /*-------------------------------------------------------------------------*/
 
 static struct usb_gadget_driver composite_driver = {
+#ifdef CONFIG_USB_GADGET_SUPERSPEED
+        .speed          = USB_SPEED_SUPER,
+#else
         .speed          = USB_SPEED_HIGH,
+#endif
 
         .unbind         = composite_unbind,
 
@@ -1293,6 +1606,8 @@ int usb_composite_probe(struct usb_composite_driver *driver,
                 driver->iProduct = driver->name;
         composite_driver.function =  (char *) driver->name;
         composite_driver.driver.name = driver->name;
+        composite_driver.speed = min((u8)composite_driver.speed,
+                                     (u8)driver->max_speed);
         composite = driver;
         composite_gadget_bind = bind;