usb: gadget: f_fs: Add flags to descriptors block

This reworks the way SuperSpeed descriptors are added and instead of
having a magic after full and high speed descriptors, it reworks the
whole descriptors block to include a flags field which lists which
descriptors are present and makes future extensions possible.

Signed-off-by: Michal Nazarewicz <mina86@mina86.com>
Signed-off-by: Felipe Balbi <balbi@ti.com>
[backport from upstream commit 410b1f0e1c86443c57dc70d78db7aec193c79787]
Signed-off-by: Marek Szyprowski <m.szyprowski@samsung.com>
Change-id: I410b1f0e1c86443c57dc70d78db7aec193c79787

3 files changed, 91 insertions, 102 deletions

diff --git a/drivers/usb/gadget/f_fs.c b/drivers/usb/gadget/f_fs.c
index 68bb857f7df..16b8b7a4c1f 100644
--- a/drivers/usb/gadget/f_fs.c
+++ b/drivers/usb/gadget/f_fs.c
@@ -1439,7 +1439,7 @@ static void ffs_data_clear(struct ffs_data *ffs)
 	if (ffs->epfiles)
 		ffs_epfiles_destroy(ffs->epfiles, ffs->eps_count);
 
-	kfree(ffs->raw_descs);
+	kfree(ffs->raw_descs_data);
 	kfree(ffs->raw_strings);
 	kfree(ffs->stringtabs);
 }
@@ -1451,12 +1451,12 @@ static void ffs_data_reset(struct ffs_data *ffs)
 	ffs_data_clear(ffs);
 
 	ffs->epfiles = NULL;
+	ffs->raw_descs_data = NULL;
 	ffs->raw_descs = NULL;
 	ffs->raw_strings = NULL;
 	ffs->stringtabs = NULL;
 
-	ffs->raw_fs_hs_descs_length = 0;
-	ffs->raw_ss_descs_length = 0;
+	ffs->raw_descs_length = 0;
 	ffs->fs_descs_count = 0;
 	ffs->hs_descs_count = 0;
 	ffs->ss_descs_count = 0;
@@ -1870,89 +1870,76 @@ 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, ss_count = 0;
-	int fs_len, hs_len, ss_len, ret = -EINVAL;
-	char *data = _data;
+	char *data = _data, *raw_descs;
+	unsigned counts[3], flags;
+	int ret = -EINVAL, i;
 
 	ENTER();
 
-	if (unlikely(get_unaligned_le32(data) != FUNCTIONFS_DESCRIPTORS_MAGIC ||
-		     get_unaligned_le32(data + 4) != len))
+	if (get_unaligned_le32(data + 4) != len)
 		goto error;
-	fs_count = get_unaligned_le32(data +  8);
-	hs_count = get_unaligned_le32(data + 12);
-
-	data += 16;
-	len  -= 16;
 
-	if (likely(fs_count)) {
-		fs_len = ffs_do_descs(fs_count, data, len,
-				      __ffs_data_do_entity, ffs);
-		if (unlikely(fs_len < 0)) {
-			ret = fs_len;
+	switch (get_unaligned_le32(data)) {
+	case FUNCTIONFS_DESCRIPTORS_MAGIC:
+		flags = FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC;
+		data += 8;
+		len  -= 8;
+		break;
+	case FUNCTIONFS_DESCRIPTORS_MAGIC_V2:
+		flags = get_unaligned_le32(data + 8);
+		if (flags & ~(FUNCTIONFS_HAS_FS_DESC |
+			      FUNCTIONFS_HAS_HS_DESC |
+			      FUNCTIONFS_HAS_SS_DESC)) {
+			ret = -ENOSYS;
 			goto error;
 		}
-
-		data += fs_len;
-		len  -= fs_len;
-	} else {
-		fs_len = 0;
+		data += 12;
+		len  -= 12;
+		break;
+	default:
+		goto error;
 	}
 
-	if (likely(hs_count)) {
-		hs_len = ffs_do_descs(hs_count, data, len,
-				   __ffs_data_do_entity, ffs);
-		if (unlikely(hs_len < 0)) {
-			ret = hs_len;
+	/* Read fs_count, hs_count and ss_count (if present) */
+	for (i = 0; i < 3; ++i) {
+		if (!(flags & (1 << i))) {
+			counts[i] = 0;
+		} else if (len < 4) {
 			goto error;
+		} else {
+			counts[i] = get_unaligned_le32(data);
+			data += 4;
+			len  -= 4;
 		}
-
-		data += hs_len;
-		len  -= hs_len;
-	} else {
-		hs_len = 0;
-	}
-
-	if (len >= 8) {
-		/* Check SS_MAGIC for presence of ss_descs and get SS_COUNT */
-		if (get_unaligned_le32(data) != FUNCTIONFS_SS_DESC_MAGIC)
-			goto einval;
-
-		ss_count = get_unaligned_le32(data + 4);
-		data += 8;
-		len  -= 8;
 	}
 
-	if (!fs_count && !hs_count && !ss_count)
-		goto einval;
-
-	if (ss_count) {
-		ss_len = ffs_do_descs(ss_count, data, len,
+	/* Read descriptors */
+	raw_descs = data;
+	for (i = 0; i < 3; ++i) {
+		if (!counts[i])
+			continue;
+		ret = ffs_do_descs(counts[i], data, len,
 				   __ffs_data_do_entity, ffs);
-		if (unlikely(ss_len < 0)) {
-			ret = ss_len;
+		if (ret < 0)
 			goto error;
-		}
-		ret = ss_len;
-	} else {
-		ss_len = 0;
-		ret = 0;
+		data += ret;
+		len  -= ret;
 	}
 
-	if (unlikely(len != ret))
-		goto einval;
+	if (raw_descs == data || len) {
+		ret = -EINVAL;
+		goto error;
+	}
 
-	ffs->raw_fs_hs_descs_length	 = fs_len + hs_len;
-	ffs->raw_ss_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;
+	ffs->raw_descs_data	= _data;
+	ffs->raw_descs		= raw_descs;
+	ffs->raw_descs_length	= data - raw_descs;
+	ffs->fs_descs_count	= counts[0];
+	ffs->hs_descs_count	= counts[1];
+	ffs->ss_descs_count	= counts[2];
 
 	return 0;
 
-einval:
-	ret = -EINVAL;
 error:
 	kfree(_data);
 	return ret;
@@ -2364,8 +2351,7 @@ static int _ffs_func_bind(struct usb_configuration *c,
 	vla_item_with_sz(d, struct usb_descriptor_header *, ss_descs,
 		super ? ffs->ss_descs_count + 1 : 0);
 	vla_item_with_sz(d, short, inums, ffs->interfaces_count);
-	vla_item_with_sz(d, char, raw_descs,
-			ffs->raw_fs_hs_descs_length + ffs->raw_ss_descs_length);
+	vla_item_with_sz(d, char, raw_descs, ffs->raw_descs_length);
 	char *vlabuf;
 
 	ENTER();
@@ -2381,15 +2367,9 @@ static int _ffs_func_bind(struct usb_configuration *c,
 
 	/* Zero */
 	memset(vla_ptr(vlabuf, d, eps), 0, d_eps__sz);
-	/* Copy only raw (hs,fs) descriptors (until ss_magic and ss_count) */
-	memcpy(vla_ptr(vlabuf, d, raw_descs), ffs->raw_descs + 16,
-		ffs->raw_fs_hs_descs_length);
-	/* Copy SS descs present @ header + hs_fs_descs + ss_magic + ss_count */
-	if (func->ffs->ss_descs_count)
-		memcpy(vla_ptr(vlabuf, d, raw_descs) +
-				ffs->raw_fs_hs_descs_length,
-		       ffs->raw_descs + 16 + ffs->raw_fs_hs_descs_length + 8,
-		       ffs->raw_ss_descs_length);
+	/* Copy descriptors  */
+	memcpy(vla_ptr(vlabuf, d, raw_descs), ffs->raw_descs,
+	       ffs->raw_descs_length);
 
 	memset(vla_ptr(vlabuf, d, inums), 0xff, d_inums__sz);
 	for (ret = ffs->eps_count; ret; --ret) {
diff --git a/drivers/usb/gadget/u_fs.h b/drivers/usb/gadget/u_fs.h
index 0deb6d5f7c3..bf0ba375d45 100644
--- a/drivers/usb/gadget/u_fs.h
+++ b/drivers/usb/gadget/u_fs.h
@@ -206,15 +206,13 @@ 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_fs_hs_descs_length does not have those 16 bytes added.
-	 * ss_descs are 8 bytes (ss_magic + count) pass the hs_descs
+	 * raw_descs is what you kfree, real_descs points inside of raw_descs,
+	 * where full speed, high speed and super speed descriptors start.
+	 * real_descs_length is the length of all those descriptors.
 	 */
+	const void			*raw_descs_data;
 	const void			*raw_descs;
-	unsigned			raw_fs_hs_descs_length;
-	unsigned			raw_ss_descs_length;
+	unsigned			raw_descs_length;
 	unsigned			fs_descs_count;
 	unsigned			hs_descs_count;
 	unsigned			ss_descs_count;
diff --git a/include/uapi/linux/usb/functionfs.h b/include/uapi/linux/usb/functionfs.h
index 0f8f7be5b0d..2a4b4a72a4f 100644
--- a/include/uapi/linux/usb/functionfs.h
+++ b/include/uapi/linux/usb/functionfs.h
@@ -10,10 +10,15 @@
 
 enum {
 	FUNCTIONFS_DESCRIPTORS_MAGIC = 1,
-	FUNCTIONFS_STRINGS_MAGIC     = 2
+	FUNCTIONFS_STRINGS_MAGIC = 2,
+	FUNCTIONFS_DESCRIPTORS_MAGIC_V2 = 3,
 };
 
-#define FUNCTIONFS_SS_DESC_MAGIC 0x0055DE5C
+enum functionfs_flags {
+	FUNCTIONFS_HAS_FS_DESC = 1,
+	FUNCTIONFS_HAS_HS_DESC = 2,
+	FUNCTIONFS_HAS_SS_DESC = 4,
+};
 
 #ifndef __KERNEL__
 
@@ -30,33 +35,39 @@ struct usb_endpoint_descriptor_no_audio {
 
 
 /*
- * All numbers must be in little endian order.
- */
-
-struct usb_functionfs_descs_head {
-	__le32 magic;
-	__le32 length;
-	__le32 fs_count;
-	__le32 hs_count;
-} __attribute__((packed));
-
-/*
  * Descriptors format:
  *
  * | off | name      | type         | description                          |
  * |-----+-----------+--------------+--------------------------------------|
- * |   0 | magic     | LE32         | FUNCTIONFS_{FS,HS}_DESCRIPTORS_MAGIC |
+ * |   0 | magic     | LE32         | FUNCTIONFS_DESCRIPTORS_MAGIC_V2      |
+ * |   4 | length    | LE32         | length of the whole data chunk       |
+ * |   8 | flags     | LE32         | combination of functionfs_flags      |
+ * |     | fs_count  | LE32         | number of full-speed descriptors     |
+ * |     | hs_count  | LE32         | number of high-speed descriptors     |
+ * |     | ss_count  | LE32         | number of super-speed descriptors    |
+ * |     | fs_descrs | Descriptor[] | list of full-speed descriptors       |
+ * |     | hs_descrs | Descriptor[] | list of high-speed descriptors       |
+ * |     | ss_descrs | Descriptor[] | list of super-speed descriptors      |
+ *
+ * Depending on which flags are set, various fields may be missing in the
+ * structure.  Any flags that are not recognised cause the whole block to be
+ * rejected with -ENOSYS.
+ *
+ * Legacy descriptors format:
+ *
+ * | off | name      | type         | description                          |
+ * |-----+-----------+--------------+--------------------------------------|
+ * |   0 | magic     | LE32         | FUNCTIONFS_DESCRIPTORS_MAGIC         |
  * |   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       |
  * |     | hs_descrs | Descriptor[] | list of high-speed descriptors       |
- * |     | ss_magic  | LE32         | FUNCTIONFS_SS_DESC_MAGIC             |
- * |     | ss_count  | LE32         | number of super-speed descriptors    |
- * |     | ss_descrs | Descriptor[] | list of super-speed descriptors      |
  *
- * ss_magic: if present then it implies that SS_DESCs are also present
- * descs are just valid USB descriptors and have the following format:
+ * All numbers must be in little endian order.
+ *
+ * Descriptor[] is an array of valid USB descriptors which have the following
+ * format:
  *
  * | off | name            | type | description              |
  * |-----+-----------------+------+--------------------------|