1 files changed, 107 insertions, 5 deletions

diff --git a/tools/usb/ffs-test.c b/tools/usb/ffs-test.c
index 708d317b0f37..88d5e71be044 100644
--- a/tools/usb/ffs-test.c
+++ b/tools/usb/ffs-test.c
@@ -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,89 @@ static const struct {
         },
 };
 
+static size_t descs_to_legacy(void **legacy, const void *descriptors_v2)
+{
+        const unsigned char *descs_end, *descs_start;
+        __u32 length, fs_count = 0, hs_count = 0, count;
+
+        /* Read v2 header */
+        {
+                const struct {
+                        const struct usb_functionfs_descs_head_v2 header;
+                        const __le32 counts[];
+                } __attribute__((packed)) *const in = descriptors_v2;
+                const __le32 *counts = in->counts;
+                __u32 flags;
+
+                if (le32_to_cpu(in->header.magic) !=
+                    FUNCTIONFS_DESCRIPTORS_MAGIC_V2)
+                        return 0;
+                length = le32_to_cpu(in->header.length);
+                if (length <= sizeof in->header)
+                        return 0;
+                length -= sizeof in->header;
+                flags = le32_to_cpu(in->header.flags);
+                if (flags & ~(FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC |
+                              FUNCTIONFS_HAS_SS_DESC))
+                        return 0;
+
+#define GET_NEXT_COUNT_IF_FLAG(ret, flg) do {           \
+                        if (!(flags & (flg)))           \
+                                break;                  \
+                        if (length < 4)                 \
+                                return 0;               \
+                        ret = le32_to_cpu(*counts);     \
+                        length -= 4;                    \
+                        ++counts;                       \
+                } while (0)
+
+                GET_NEXT_COUNT_IF_FLAG(fs_count, FUNCTIONFS_HAS_FS_DESC);
+                GET_NEXT_COUNT_IF_FLAG(hs_count, FUNCTIONFS_HAS_HS_DESC);
+                GET_NEXT_COUNT_IF_FLAG(count, FUNCTIONFS_HAS_SS_DESC);
+
+                count = fs_count + hs_count;
+                if (!count)
+                        return 0;
+                descs_start = (const void *)counts;
+
+#undef GET_NEXT_COUNT_IF_FLAG
+        }
+
+        /*
+         * Find the end of FS and HS USB descriptors.  SS descriptors
+         * are ignored since legacy format does not support them.
+         */
+        descs_end = descs_start;
+        do {
+                if (length < *descs_end)
+                        return 0;
+                length -= *descs_end;
+                descs_end += *descs_end;
+        } while (--count);
+
+        /* Allocate legacy descriptors and copy the data. */
+        {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+                struct {
+                        struct usb_functionfs_descs_head header;
+                        __u8 descriptors[];
+                } __attribute__((packed)) *out;
+#pragma GCC diagnostic pop
+
+                length = sizeof out->header + (descs_end - descs_start);
+                out = malloc(length);
+                out->header.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC);
+                out->header.length = cpu_to_le32(length);
+                out->header.fs_count = cpu_to_le32(fs_count);
+                out->header.hs_count = cpu_to_le32(hs_count);
+                memcpy(out->descriptors, descs_start, descs_end - descs_start);
+                *legacy = out;
+        }
+
+        return length;
+}
+
 
 #define STR_INTERFACE_ "Source/Sink"
 
@@ -491,12 +575,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 +608,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);