3 files changed, 173 insertions, 11 deletions

diff --git a/tools/include/tools/endian.h b/tools/include/tools/endian.h
new file mode 100644
index 000000000000..8001194008da
--- /dev/null
+++ b/tools/include/tools/endian.h
@@ -0,0 +1,56 @@
+#ifndef _TOOLS_ENDIAN_H
+#define _TOOLS_ENDIAN_H
+
+#include <byteswap.h>
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+
+#ifndef htole16
+#define htole16(x) (x)
+#endif
+#ifndef htole32
+#define htole32(x) (x)
+#endif
+#ifndef htole64
+#define htole64(x) (x)
+#endif
+
+#ifndef le16toh
+#define le16toh(x) (x)
+#endif
+
+#ifndef le32toh
+#define le32toh(x) (x)
+#endif
+
+#ifndef le64toh
+#define le64toh(x) (x)
+#endif
+
+#else /* __BYTE_ORDER */
+
+#ifndef htole16
+#define htole16(x) __bswap_16(x)
+#endif
+#ifndef htole32
+#define htole32(x) __bswap_32(x)
+#endif
+#ifndef htole64
+#define htole64(x) __bswap_64(x)
+#endif
+
+#ifndef le16toh
+#define le16toh(x) __bswap_16(x)
+#endif
+
+#ifndef le32toh
+#define le32toh(x) __bswap_32(x)
+#endif
+
+#ifndef le64toh
+#define le64toh(x) __bswap_64(x)
+#endif
+
+#endif
+
+#endif /* _TOOLS_ENDIAN_H */
diff --git a/tools/usb/ffs-test.c b/tools/usb/ffs-test.c
index a87e99f37c52..88d5e71be044 100644
--- a/tools/usb/ffs-test.c
+++ b/tools/usb/ffs-test.c
@@ -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 <mina86@mina86.com>
@@ -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>
@@ -106,7 +107,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 +117,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 +146,7 @@ static const struct {
                         /* .wMaxPacketSize = autoconfiguration (kernel) */
                 },
         },
+        .hs_count = cpu_to_le32(3),
         .hs_descs = {
                 .intf = {
                         .bLength = sizeof descriptors.fs_descs.intf,
@@ -168,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"
 
@@ -487,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);
@@ -503,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);
diff --git a/tools/usb/usbip/libsrc/usbip_common.h b/tools/usb/usbip/libsrc/usbip_common.h
index 5a0e95edf4df..15fe792e1e96 100644
--- a/tools/usb/usbip/libsrc/usbip_common.h
+++ b/tools/usb/usbip/libsrc/usbip_common.h
@@ -15,7 +15,7 @@
 #include <syslog.h>
 #include <unistd.h>
 #include <linux/usb/ch9.h>
-#include "../../uapi/usbip.h"
+#include <linux/usbip.h>
 
 #ifndef USBIDS_FILE
 #define USBIDS_FILE "/usr/share/hwdata/usb.ids"