[PATCH v2 1/1] usb: gadget: f_fs: Add support for SuperSpeed Mode

[Manu Gautam <mgautam-sgV2jX0FEOL9JmXXK+q4OQ@public.gmane.org>]

Allow userspace to pass SuperSpeed descriptors and
handle them in the driver accordingly.
This also requires changing usb_functionfs_descs_head
to accommodate ss_count i.e. SuperSpeed Descriptors
count.

Signed-off-by: Manu Gautam <mgautam-sgV2jX0FEOL9JmXXK+q4OQ@public.gmane.org>
---
 drivers/usb/gadget/f_fs.c           | 148 +++++++++++++++++++++++++++---------
 include/uapi/linux/usb/functionfs.h |   5 +-
 2 files changed, 115 insertions(+), 38 deletions(-)

diff --git a/drivers/usb/gadget/f_fs.c b/drivers/usb/gadget/f_fs.c
index f394f29..95a5746 100644
--- a/drivers/usb/gadget/f_fs.c
+++ b/drivers/usb/gadget/f_fs.c
@@ -194,16 +194,18 @@ struct ffs_data {
 
 	/* filled by __ffs_data_got_descs() */
 	/*
-	 * Real descriptors are 16 bytes after raw_descs (so you need
-	 * to skip 16 bytes (ie. ffs->raw_descs + 16) to get to the
-	 * first full speed descriptor).  raw_descs_length and
-	 * raw_fs_descs_length do not have those 16 bytes added.
+	 * Real descriptors are 20 bytes after raw_descs (so you need
+	 * to skip 20 bytes (ie. ffs->raw_descs + 20) to get to the
+	 * first full speed descriptor).  raw_(fs|hs|ss)descs_length
+	 * do not have those 20 bytes added.
 	 */
 	const void			*raw_descs;
-	unsigned			raw_descs_length;
+	unsigned			raw_hs_descs_length;
 	unsigned			raw_fs_descs_length;
+	unsigned			raw_ss_descs_length;
 	unsigned			fs_descs_count;
 	unsigned			hs_descs_count;
+	unsigned			ss_descs_count;
 
 	unsigned short			strings_count;
 	unsigned short			interfaces_count;
@@ -301,8 +303,8 @@ struct ffs_ep {
 	struct usb_ep			*ep;	/* P: ffs->eps_lock */
 	struct usb_request		*req;	/* P: epfile->mutex */
 
-	/* [0]: full speed, [1]: high speed */
-	struct usb_endpoint_descriptor	*descs[2];
+	/* [0]: full speed, [1]: high speed, [2]: super speed */
+	struct usb_endpoint_descriptor	*descs[3];
 
 	u8				num;
 
@@ -1358,10 +1360,12 @@ static void ffs_data_reset(struct ffs_data *ffs)
 	ffs->raw_strings = NULL;
 	ffs->stringtabs = NULL;
 
-	ffs->raw_descs_length = 0;
+	ffs->raw_hs_descs_length = 0;
 	ffs->raw_fs_descs_length = 0;
+	ffs->raw_ss_descs_length = 0;
 	ffs->fs_descs_count = 0;
 	ffs->hs_descs_count = 0;
+	ffs->ss_descs_count = 0;
 
 	ffs->strings_count = 0;
 	ffs->interfaces_count = 0;
@@ -1569,7 +1573,20 @@ static int ffs_func_eps_enable(struct ffs_function *func)
 	spin_lock_irqsave(&func->ffs->eps_lock, flags);
 	do {
 		struct usb_endpoint_descriptor *ds;
-		ds = ep->descs[ep->descs[1] ? 1 : 0];
+		int desc_idx;
+
+		if (ffs->gadget->speed == USB_SPEED_SUPER)
+			desc_idx = 2;
+		else if (ffs->gadget->speed == USB_SPEED_HIGH)
+			desc_idx = 1;
+		else
+			desc_idx = 0;
+
+		ds = ep->descs[desc_idx];
+		if (!ds) {
+			ret = -EINVAL;
+			break;
+		}
 
 		ep->ep->driver_data = ep;
 		ep->ep->desc = ds;
@@ -1704,6 +1721,12 @@ static int __must_check ffs_do_desc(char *data, unsigned len,
 	}
 		break;
 
+	case USB_DT_SS_ENDPOINT_COMP:
+		pr_vdebug("EP SS companion descriptor\n");
+		if (length != sizeof(struct usb_ss_ep_comp_descriptor))
+			goto inv_length;
+		break;
+
 	case USB_DT_OTHER_SPEED_CONFIG:
 	case USB_DT_INTERFACE_POWER:
 	case USB_DT_DEBUG:
@@ -1814,8 +1837,8 @@ static int __ffs_data_do_entity(enum ffs_entity_type type,
 static int __ffs_data_got_descs(struct ffs_data *ffs,
 				char *const _data, size_t len)
 {
-	unsigned fs_count, hs_count;
-	int fs_len, ret = -EINVAL;
+	unsigned fs_count, hs_count, ss_count;
+	int fs_len, hs_len, ss_len, ret = -EINVAL;
 	char *data = _data;
 
 	ENTER();
@@ -1825,12 +1848,13 @@ static int __ffs_data_got_descs(struct ffs_data *ffs,
 		goto error;
 	fs_count = get_unaligned_le32(data +  8);
 	hs_count = get_unaligned_le32(data + 12);
+	ss_count = get_unaligned_le32(data + 16);
 
-	if (!fs_count && !hs_count)
+	if (!fs_count && !hs_count && !ss_count)
 		goto einval;
 
-	data += 16;
-	len  -= 16;
+	data += 20;
+	len  -= 20;
 
 	if (likely(fs_count)) {
 		fs_len = ffs_do_descs(fs_count, data, len,
@@ -1847,11 +1871,29 @@ static int __ffs_data_got_descs(struct ffs_data *ffs,
 	}
 
 	if (likely(hs_count)) {
-		ret = ffs_do_descs(hs_count, data, len,
+		hs_len = ffs_do_descs(hs_count, data, len,
 				   __ffs_data_do_entity, ffs);
-		if (unlikely(ret < 0))
+		if (unlikely(hs_len < 0)) {
+			ret = hs_len;
 			goto error;
+		}
+
+		data += hs_len;
+		len  -= hs_len;
+	} else {
+		hs_len = 0;
+	}
+
+	if (likely(ss_count)) {
+		ss_len = ffs_do_descs(ss_count, data, len,
+				   __ffs_data_do_entity, ffs);
+		if (unlikely(ss_len < 0)) {
+			ret = ss_len;
+			goto error;
+		}
+		ret = ss_len;
 	} else {
+		ss_len = 0;
 		ret = 0;
 	}
 
@@ -1859,10 +1901,12 @@ static int __ffs_data_got_descs(struct ffs_data *ffs,
 		goto einval;
 
 	ffs->raw_fs_descs_length = fs_len;
-	ffs->raw_descs_length    = fs_len + ret;
+	ffs->raw_hs_descs_length    = ffs->raw_fs_descs_length + hs_len;
+	ffs->raw_ss_descs_length = ffs->raw_hs_descs_length + ss_len;
 	ffs->raw_descs           = _data;
 	ffs->fs_descs_count      = fs_count;
 	ffs->hs_descs_count      = hs_count;
+	ffs->ss_descs_count      = ss_count;
 
 	return 0;
 
@@ -2086,16 +2130,23 @@ static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
 	 * If hs_descriptors is not NULL then we are reading hs
 	 * descriptors now
 	 */
-	const int isHS = func->function.hs_descriptors != NULL;
-	unsigned idx;
+	const int is_hs = func->function.hs_descriptors != NULL;
+	const int is_ss = func->function.ss_descriptors != NULL;
+	unsigned ep_desc_id, idx;
 
 	if (type != FFS_DESCRIPTOR)
 		return 0;
 
-	if (isHS)
+	if (is_ss) {
+		func->function.ss_descriptors[(long)valuep] = desc;
+		ep_desc_id = 2;
+	} else if (is_hs) {
 		func->function.hs_descriptors[(long)valuep] = desc;
-	else
+		ep_desc_id = 1;
+	} else {
 		func->function.fs_descriptors[(long)valuep]    = desc;
+		ep_desc_id = 0;
+	}
 
 	if (!desc || desc->bDescriptorType != USB_DT_ENDPOINT)
 		return 0;
@@ -2103,13 +2154,13 @@ static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
 	idx = (ds->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK) - 1;
 	ffs_ep = func->eps + idx;
 
-	if (unlikely(ffs_ep->descs[isHS])) {
+	if (unlikely(ffs_ep->descs[ep_desc_id])) {
 		pr_vdebug("two %sspeed descriptors for EP %d\n",
-			  isHS ? "high" : "full",
+			  is_ss ? "super" : "high/full",
 			  ds->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
 		return -EINVAL;
 	}
-	ffs_ep->descs[isHS] = ds;
+	ffs_ep->descs[ep_desc_id] = ds;
 
 	ffs_dump_mem(": Original  ep desc", ds, ds->bLength);
 	if (ffs_ep->ep) {
@@ -2204,8 +2255,10 @@ static int ffs_func_bind(struct usb_configuration *c,
 	const int full = !!func->ffs->fs_descs_count;
 	const int high = gadget_is_dualspeed(func->gadget) &&
 		func->ffs->hs_descs_count;
+	const int super = gadget_is_superspeed(func->gadget) &&
+		func->ffs->ss_descs_count;
 
-	int ret;
+	int fs_len, hs_len, ret;
 
 	/* Make it a single chunk, less management later on */
 	struct {
@@ -2214,9 +2267,10 @@ static int ffs_func_bind(struct usb_configuration *c,
 			*fs_descs[full ? ffs->fs_descs_count + 1 : 0];
 		struct usb_descriptor_header
 			*hs_descs[high ? ffs->hs_descs_count + 1 : 0];
+		struct usb_descriptor_header
+			*ss_descs[super ? ffs->ss_descs_count + 1 : 0];
 		short inums[ffs->interfaces_count];
-		char raw_descs[high ? ffs->raw_descs_length
-				    : ffs->raw_fs_descs_length];
+		char raw_descs[ffs->raw_ss_descs_length];
 	} *data;
 
 	ENTER();
@@ -2232,7 +2286,7 @@ static int ffs_func_bind(struct usb_configuration *c,
 
 	/* Zero */
 	memset(data->eps, 0, sizeof data->eps);
-	memcpy(data->raw_descs, ffs->raw_descs + 16, sizeof data->raw_descs);
+	memcpy(data->raw_descs, ffs->raw_descs + 20, sizeof(data->raw_descs));
 	memset(data->inums, 0xff, sizeof data->inums);
 	for (ret = ffs->eps_count; ret; --ret)
 		data->eps[ret].num = -1;
@@ -2248,32 +2302,52 @@ static int ffs_func_bind(struct usb_configuration *c,
 	 */
 	if (likely(full)) {
 		func->function.fs_descriptors = data->fs_descs;
-		ret = ffs_do_descs(ffs->fs_descs_count,
+		fs_len = ffs_do_descs(ffs->fs_descs_count,
 				   data->raw_descs,
-				   sizeof data->raw_descs,
+				   sizeof(data->raw_descs),
 				   __ffs_func_bind_do_descs, func);
-		if (unlikely(ret < 0))
+		if (unlikely(fs_len < 0)) {
+			ret = fs_len;
 			goto error;
+		}
 	} else {
-		ret = 0;
+		fs_len = 0;
 	}
 
 	if (likely(high)) {
 		func->function.hs_descriptors = data->hs_descs;
-		ret = ffs_do_descs(ffs->hs_descs_count,
-				   data->raw_descs + ret,
-				   (sizeof data->raw_descs) - ret,
+		hs_len = ffs_do_descs(ffs->hs_descs_count,
+				   data->raw_descs + fs_len,
+				   (sizeof(data->raw_descs)) - fs_len,
 				   __ffs_func_bind_do_descs, func);
+		if (unlikely(hs_len < 0)) {
+			ret = hs_len;
+			goto error;
+		}
+	} else {
+		hs_len = 0;
 	}
 
+	if (likely(super)) {
+		func->function.ss_descriptors = data->ss_descs;
+		ret = ffs_do_descs(ffs->ss_descs_count,
+				   data->raw_descs + fs_len + hs_len,
+				   (sizeof(data->raw_descs)) - fs_len - hs_len,
+				   __ffs_func_bind_do_descs, func);
+		if (unlikely(ret < 0))
+			goto error;
+	}
+
+
 	/*
 	 * Now handle interface numbers allocation and interface and
 	 * endpoint numbers rewriting.  We can do that in one go
 	 * now.
 	 */
 	ret = ffs_do_descs(ffs->fs_descs_count +
-			   (high ? ffs->hs_descs_count : 0),
-			   data->raw_descs, sizeof data->raw_descs,
+			   (high ? ffs->hs_descs_count : 0) +
+			   (super ? ffs->ss_descs_count : 0),
+			   data->raw_descs, sizeof(data->raw_descs),
 			   __ffs_func_bind_do_nums, func);
 	if (unlikely(ret < 0))
 		goto error;
diff --git a/include/uapi/linux/usb/functionfs.h b/include/uapi/linux/usb/functionfs.h
index d6b0128..d6940d7 100644
--- a/include/uapi/linux/usb/functionfs.h
+++ b/include/uapi/linux/usb/functionfs.h
@@ -37,6 +37,7 @@ struct usb_functionfs_descs_head {
 	__le32 length;
 	__le32 fs_count;
 	__le32 hs_count;
+	__le32 ss_count;
 } __attribute__((packed));
 
 /*
@@ -48,8 +49,10 @@ struct usb_functionfs_descs_head {
  * |   4 | length    | LE32         | length of the whole data chunk       |
  * |   8 | fs_count  | LE32         | number of full-speed descriptors     |
  * |  12 | hs_count  | LE32         | number of high-speed descriptors     |
- * |  16 | fs_descrs | Descriptor[] | list of full-speed descriptors       |
+ * |  16 | ss_count  | LE32         | number of super-speed descriptors    |
+ * |  20 | fs_descrs | Descriptor[] | list of full-speed descriptors       |
  * |     | hs_descrs | Descriptor[] | list of high-speed descriptors       |
+ * |     | ss_descrs | Descriptor[] | list of super-speed descriptors      |
  *
  * descs are just valid USB descriptors and have the following format:
  *
-- 
The Qualcomm Innovation Center, Inc. is a member of the Code Aurora Forum,
hosted by The Linux Foundation

--
To unsubscribe from this list: send the line "unsubscribe linux-usb" in
the body of a message to majordomo-u79uwXL29TY76Z2rM5mHXA@public.gmane.org
More majordomo info at  http://vger.kernel.org/majordomo-info.html