usb: gadget: f_fs: resurect usb_functionfs_descs_head structure

index 2a4b4a7..24b68c5 100644
@@ -33,6 +33,13 @@ struct usb_endpoint_descriptor_no_audio {
 	__u8  bInterval;
 } __attribute__((packed));
 
+/* Legacy format, deprecated as of 3.14. */
+struct usb_functionfs_descs_head {
+	__le32 magic;
+	__le32 length;
+	__le32 fs_count;
+	__le32 hs_count;
+} __attribute__((packed, deprecated));
 
 /*
  * Descriptors format:

index fe1e66b..a87e99f 100644
@@ -116,8 +116,8 @@ static const struct {
 	.header = {
 		.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC),
 		.length = cpu_to_le32(sizeof descriptors),
-		.fs_count = 3,
-		.hs_count = 3,
+		.fs_count = cpu_to_le32(3),
+		.hs_count = cpu_to_le32(3),
 	},
 	.fs_descs = {
 		.intf = {

index 24b68c5..2592d86 100644
@@ -33,6 +33,16 @@ struct usb_endpoint_descriptor_no_audio {
 	__u8  bInterval;
 } __attribute__((packed));
 
+struct usb_functionfs_descs_head_v2 {
+	__le32 magic;
+	__le32 length;
+	__le32 flags;
+	/*
+	 * __le32 fs_count, hs_count, fs_count; must be included manually in
+	 * the structure taking flags into consideration.
+	 */
+} __attribute__((packed));
+
 /* Legacy format, deprecated as of 3.14. */
 struct usb_functionfs_descs_head {
 	__le32 magic;

index 2592d86..1dc473a 100644
@@ -70,7 +70,7 @@ struct usb_functionfs_descs_head {
  * structure.  Any flags that are not recognised cause the whole block to be
  * rejected with -ENOSYS.
  *
- * Legacy descriptors format:
+ * Legacy descriptors format (deprecated as of 3.14):
  *
  * | off | name      | type         | description                          |
  * |-----+-----------+--------------+--------------------------------------|

index a87e99f..708d317 100644
@@ -1,5 +1,5 @@
 /*
- * ffs-test.c.c -- user mode filesystem api for usb composite function
+ * ffs-test.c -- user mode filesystem api for usb composite function
  *
  * Copyright (C) 2010 Samsung Electronics
  *                    Author: Michal Nazarewicz <[email protected]>

@@ -106,7 +106,9 @@ static void _msg(unsigned level, const char *fmt, ...)
 /******************** Descriptors and Strings *******************************/
 
 static const struct {
-	struct usb_functionfs_descs_head header;
+	struct usb_functionfs_descs_head_v2 header;
+	__le32 fs_count;
+	__le32 hs_count;
 	struct {
 		struct usb_interface_descriptor intf;
 		struct usb_endpoint_descriptor_no_audio sink;

@@ -114,11 +116,12 @@ static const struct {
 	} __attribute__((packed)) fs_descs, hs_descs;
 } __attribute__((packed)) descriptors = {
 	.header = {
-		.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC),
+		.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2),
+		.flags = cpu_to_le32(FUNCTIONFS_HAS_FS_DESC |
+				     FUNCTIONFS_HAS_HS_DESC),
 		.length = cpu_to_le32(sizeof descriptors),
-		.fs_count = cpu_to_le32(3),
-		.hs_count = cpu_to_le32(3),
 	},
+	.fs_count = cpu_to_le32(3),
 	.fs_descs = {
 		.intf = {
 			.bLength = sizeof descriptors.fs_descs.intf,

@@ -142,6 +145,7 @@ static const struct {
 			/* .wMaxPacketSize = autoconfiguration (kernel) */
 		},
 	},
+	.hs_count = cpu_to_le32(3),
 	.hs_descs = {
 		.intf = {
 			.bLength = sizeof descriptors.fs_descs.intf,

index 708d317..407c828 100644
@@ -29,6 +29,7 @@
 #include <fcntl.h>
 #include <pthread.h>
 #include <stdarg.h>
+#include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>

@@ -172,6 +173,61 @@ static const struct {
 	},
 };
 
+static size_t descs_to_legacy(void **legacy, const void *descriptors) {
+	__u32 length, flags, fs_count = 0, hs_count = 0, count;
+	const unsigned char *descs = descriptors, *usb_descs;
+	unsigned char *out;
+
+	if (get_unaligned_le32(descs) != FUNCTIONFS_DESCRIPTORS_MAGIC_V2)
+		return 0;
+
+	length = get_unaligned_le32(descs + 4);
+	if (length < 8)
+		return 0;
+	descs += 8;
+	length -= 8;
+
+#define GET_LE32(ret) do {				\
+		if (length < 4)				\
+			return 0;			\
+		ret = get_unaligned_le32(descs);	\
+		descs += 4;				\
+		length -= 4;				\
+	} while (0)
+
+	GET_LE32(flags);
+	if (flags & FUNCTIONFS_HAS_FS_DESC)
+		GET_LE32(fs_count);
+	if (flags & FUNCTIONFS_HAS_HS_DESC)
+		GET_LE32(hs_count);
+	if (!fs_count && !hs_count)
+		return 0;
+	if (flags & FUNCTIONFS_HAS_SS_DESC)
+		GET_LE32(count);  /* The value is ignored later on anyway. */
+	if (flags)
+		return 0;
+
+#undef GET_LE32
+
+	usb_descs = descs;
+	for (count = fs_count + hs_count; count; --count) {
+		if (length < *descs)
+			return 0;
+		length -= *descs;
+		descs += *descs;
+	}
+
+	length = 16 + (descs - usb_descs);
+	out = malloc(length);
+	put_unaligned_le32(FUNCTIONFS_DESCRIPTORS_MAGIC, out);
+	put_unaligned_le32(length, out + 4);
+	put_unaligned_le32(fs_count, out + 8);
+	put_unaligned_le32(hs_count, out + 12);
+	memcpy(out + 16, usb_descs, length - 16);
+	*legacy = out;
+	return length;
+}
+
 
 #define STR_INTERFACE_ "Source/Sink"
 

@@ -491,12 +547,29 @@ ep0_consume(struct thread *ignore, const void *buf, size_t nbytes)
 	return nbytes;
 }
 
-static void ep0_init(struct thread *t)
+static void ep0_init(struct thread *t, bool legacy_descriptors)
 {
+	void *legacy;
 	ssize_t ret;
+	size_t len;
+
+	if (legacy_descriptors) {
+		info("%s: writing descriptors\n", t->filename);
+		goto legacy;
+	}
 
-	info("%s: writing descriptors\n", t->filename);
+	info("%s: writing descriptors (in v2 format)\n", t->filename);
 	ret = write(t->fd, &descriptors, sizeof descriptors);
+
+	if (ret < 0 && errno == EINVAL) {
+		warn("%s: new format rejected, trying legacy\n", t->filename);
+legacy:
+		len = descs_to_legacy(&legacy, &descriptors);
+		if (len) {
+			ret = write(t->fd, legacy, len);
+			free(legacy);
+		}
+	}
 	die_on(ret < 0, "%s: write: descriptors", t->filename);
 
 	info("%s: writing strings\n", t->filename);

@@ -507,14 +580,15 @@ static void ep0_init(struct thread *t)
 
 /******************** Main **************************************************/
 
-int main(void)
+int main(int argc, char **argv)
 {
+	bool legacy_descriptors;
 	unsigned i;
 
-	/* XXX TODO: Argument parsing missing */
+	legacy_descriptors = argc > 2 && !strcmp(argv[1], "-l");
 
 	init_thread(threads);
-	ep0_init(threads);
+	ep0_init(threads, legacy_descriptors);
 
 	for (i = 1; i < sizeof threads / sizeof *threads; ++i)
 		init_thread(threads + i);

index 708d317..afd69ea 100644
@@ -29,6 +29,7 @@
 #include <fcntl.h>
 #include <pthread.h>
 #include <stdarg.h>
+#include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>

@@ -172,6 +173,82 @@ static const struct {
 	},
 };
 
+static size_t descs_to_legacy(void **legacy, const void *descriptors) {
+	__u32 length, flags, fs_count = 0, hs_count = 0, count;
+	const unsigned char *descs, *usb_descs;
+
+	/* Read v2 header */
+	{
+		const struct usb_functionfs_descs_head_v2 *head = descriptors;
+
+		if (le32_to_cpu(head->magic) !=
+		    FUNCTIONFS_DESCRIPTORS_MAGIC_V2)
+			return 0;
+		length = le32_to_cpu(head->length);
+		if (length <= sizeof *head)
+			return 0;
+		flags = le32_to_cpu(head->flags);
+		descs = (const void *)(head + 1);
+		length -= sizeof *head;
+	}
+
+	/* Read v2 counts */
+#define GET_LE32(ret) do {					\
+		if (length < 4)					\
+			return 0;				\
+		ret = le32_to_cpu(*(const __le32 *)descs);	\
+		descs += 4;					\
+		length -= 4;					\
+	} while (0)
+
+	if (flags & FUNCTIONFS_HAS_FS_DESC)
+		GET_LE32(fs_count);
+	if (flags & FUNCTIONFS_HAS_HS_DESC)
+		GET_LE32(hs_count);
+	if (!fs_count && !hs_count)
+		return 0;
+	if (flags & FUNCTIONFS_HAS_SS_DESC)
+		/* Legacy does not support SS so this is ignored. */
+		GET_LE32(count);
+	if (flags & ~(FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC |
+		      FUNCTIONFS_HAS_SS_DESC))
+		return 0;
+
+#undef GET_LE32
+
+	/*
+	 * Find the end of FS and HS USB descriptors.  SS descriptors
+	 * are ignored since legacy format does not support them.
+	 */
+	count = fs_count + hs_count;
+	usb_descs = descs;
+	do {
+		if (length < *descs)
+			return 0;
+		length -= *descs;
+		descs += *descs;
+	} while (--count);
+
+	/* Allocate legacy descriptors and copy the data. */
+	{
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+		struct usb_functionfs_descs_head *head;
+#pragma GCC diagnostic pop
+
+		length = sizeof *head + (descs - usb_descs);
+		head = malloc(length);
+		head->magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC);
+		head->length = cpu_to_le32(length);
+		head->fs_count = cpu_to_le32(fs_count);
+		head->hs_count = cpu_to_le32(hs_count);
+		memcpy(head + 1, usb_descs, descs - usb_descs);
+		*legacy = head;
+	}
+
+	return length;
+}
+
 
 #define STR_INTERFACE_ "Source/Sink"
 

@@ -491,12 +568,29 @@ ep0_consume(struct thread *ignore, const void *buf, size_t nbytes)
 	return nbytes;
 }
 
-static void ep0_init(struct thread *t)
+static void ep0_init(struct thread *t, bool legacy_descriptors)
 {
+	void *legacy;
 	ssize_t ret;
+	size_t len;
+
+	if (legacy_descriptors) {
+		info("%s: writing descriptors\n", t->filename);
+		goto legacy;
+	}
 
-	info("%s: writing descriptors\n", t->filename);
+	info("%s: writing descriptors (in v2 format)\n", t->filename);
 	ret = write(t->fd, &descriptors, sizeof descriptors);
+
+	if (ret < 0 && errno == EINVAL) {
+		warn("%s: new format rejected, trying legacy\n", t->filename);
+legacy:
+		len = descs_to_legacy(&legacy, &descriptors);
+		if (len) {
+			ret = write(t->fd, legacy, len);
+			free(legacy);
+		}
+	}
 	die_on(ret < 0, "%s: write: descriptors", t->filename);
 
 	info("%s: writing strings\n", t->filename);

@@ -507,14 +601,15 @@ static void ep0_init(struct thread *t)
 
 /******************** Main **************************************************/
 
-int main(void)
+int main(int argc, char **argv)
 {
+	bool legacy_descriptors;
 	unsigned i;
 
-	/* XXX TODO: Argument parsing missing */
+	legacy_descriptors = argc > 2 && !strcmp(argv[1], "-l");
 
 	init_thread(threads);
-	ep0_init(threads);
+	ep0_init(threads, legacy_descriptors);
 
 	for (i = 1; i < sizeof threads / sizeof *threads; ++i)
 		init_thread(threads + i);