0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012 #define _DEFAULT_SOURCE
0013
0014 #include <endian.h>
0015 #include <errno.h>
0016 #include <fcntl.h>
0017 #include <pthread.h>
0018 #include <stdarg.h>
0019 #include <stdbool.h>
0020 #include <stdio.h>
0021 #include <stdlib.h>
0022 #include <string.h>
0023 #include <sys/ioctl.h>
0024 #include <sys/stat.h>
0025 #include <sys/types.h>
0026 #include <unistd.h>
0027 #include <tools/le_byteshift.h>
0028
0029 #include "../../include/uapi/linux/usb/functionfs.h"
0030
0031
0032
0033
0034
0035
0036
0037
0038
0039
0040 #if __BYTE_ORDER == __LITTLE_ENDIAN
0041 #define cpu_to_le16(x) (x)
0042 #define cpu_to_le32(x) (x)
0043 #else
0044 #define cpu_to_le16(x) ((((x) >> 8) & 0xffu) | (((x) & 0xffu) << 8))
0045 #define cpu_to_le32(x) \
0046 ((((x) & 0xff000000u) >> 24) | (((x) & 0x00ff0000u) >> 8) | \
0047 (((x) & 0x0000ff00u) << 8) | (((x) & 0x000000ffu) << 24))
0048 #endif
0049
0050 #define le32_to_cpu(x) le32toh(x)
0051 #define le16_to_cpu(x) le16toh(x)
0052
0053
0054
0055 static const char argv0[] = "ffs-test";
0056
0057 static unsigned verbosity = 7;
0058
0059 static void _msg(unsigned level, const char *fmt, ...)
0060 {
0061 if (level < 2)
0062 level = 2;
0063 else if (level > 7)
0064 level = 7;
0065
0066 if (level <= verbosity) {
0067 static const char levels[8][6] = {
0068 [2] = "crit:",
0069 [3] = "err: ",
0070 [4] = "warn:",
0071 [5] = "note:",
0072 [6] = "info:",
0073 [7] = "dbg: "
0074 };
0075
0076 int _errno = errno;
0077 va_list ap;
0078
0079 fprintf(stderr, "%s: %s ", argv0, levels[level]);
0080 va_start(ap, fmt);
0081 vfprintf(stderr, fmt, ap);
0082 va_end(ap);
0083
0084 if (fmt[strlen(fmt) - 1] != '\n') {
0085 char buffer[128];
0086 strerror_r(_errno, buffer, sizeof buffer);
0087 fprintf(stderr, ": (-%d) %s\n", _errno, buffer);
0088 }
0089
0090 fflush(stderr);
0091 }
0092 }
0093
0094 #define die(...) (_msg(2, __VA_ARGS__), exit(1))
0095 #define err(...) _msg(3, __VA_ARGS__)
0096 #define warn(...) _msg(4, __VA_ARGS__)
0097 #define note(...) _msg(5, __VA_ARGS__)
0098 #define info(...) _msg(6, __VA_ARGS__)
0099 #define debug(...) _msg(7, __VA_ARGS__)
0100
0101 #define die_on(cond, ...) do { \
0102 if (cond) \
0103 die(__VA_ARGS__); \
0104 } while (0)
0105
0106
0107
0108
0109 static const struct {
0110 struct usb_functionfs_descs_head_v2 header;
0111 __le32 fs_count;
0112 __le32 hs_count;
0113 __le32 ss_count;
0114 struct {
0115 struct usb_interface_descriptor intf;
0116 struct usb_endpoint_descriptor_no_audio sink;
0117 struct usb_endpoint_descriptor_no_audio source;
0118 } __attribute__((packed)) fs_descs, hs_descs;
0119 struct {
0120 struct usb_interface_descriptor intf;
0121 struct usb_endpoint_descriptor_no_audio sink;
0122 struct usb_ss_ep_comp_descriptor sink_comp;
0123 struct usb_endpoint_descriptor_no_audio source;
0124 struct usb_ss_ep_comp_descriptor source_comp;
0125 } ss_descs;
0126 } __attribute__((packed)) descriptors = {
0127 .header = {
0128 .magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2),
0129 .flags = cpu_to_le32(FUNCTIONFS_HAS_FS_DESC |
0130 FUNCTIONFS_HAS_HS_DESC |
0131 FUNCTIONFS_HAS_SS_DESC),
0132 .length = cpu_to_le32(sizeof descriptors),
0133 },
0134 .fs_count = cpu_to_le32(3),
0135 .fs_descs = {
0136 .intf = {
0137 .bLength = sizeof descriptors.fs_descs.intf,
0138 .bDescriptorType = USB_DT_INTERFACE,
0139 .bNumEndpoints = 2,
0140 .bInterfaceClass = USB_CLASS_VENDOR_SPEC,
0141 .iInterface = 1,
0142 },
0143 .sink = {
0144 .bLength = sizeof descriptors.fs_descs.sink,
0145 .bDescriptorType = USB_DT_ENDPOINT,
0146 .bEndpointAddress = 1 | USB_DIR_IN,
0147 .bmAttributes = USB_ENDPOINT_XFER_BULK,
0148
0149 },
0150 .source = {
0151 .bLength = sizeof descriptors.fs_descs.source,
0152 .bDescriptorType = USB_DT_ENDPOINT,
0153 .bEndpointAddress = 2 | USB_DIR_OUT,
0154 .bmAttributes = USB_ENDPOINT_XFER_BULK,
0155
0156 },
0157 },
0158 .hs_count = cpu_to_le32(3),
0159 .hs_descs = {
0160 .intf = {
0161 .bLength = sizeof descriptors.fs_descs.intf,
0162 .bDescriptorType = USB_DT_INTERFACE,
0163 .bNumEndpoints = 2,
0164 .bInterfaceClass = USB_CLASS_VENDOR_SPEC,
0165 .iInterface = 1,
0166 },
0167 .sink = {
0168 .bLength = sizeof descriptors.hs_descs.sink,
0169 .bDescriptorType = USB_DT_ENDPOINT,
0170 .bEndpointAddress = 1 | USB_DIR_IN,
0171 .bmAttributes = USB_ENDPOINT_XFER_BULK,
0172 .wMaxPacketSize = cpu_to_le16(512),
0173 },
0174 .source = {
0175 .bLength = sizeof descriptors.hs_descs.source,
0176 .bDescriptorType = USB_DT_ENDPOINT,
0177 .bEndpointAddress = 2 | USB_DIR_OUT,
0178 .bmAttributes = USB_ENDPOINT_XFER_BULK,
0179 .wMaxPacketSize = cpu_to_le16(512),
0180 .bInterval = 1,
0181 },
0182 },
0183 .ss_count = cpu_to_le32(5),
0184 .ss_descs = {
0185 .intf = {
0186 .bLength = sizeof descriptors.fs_descs.intf,
0187 .bDescriptorType = USB_DT_INTERFACE,
0188 .bNumEndpoints = 2,
0189 .bInterfaceClass = USB_CLASS_VENDOR_SPEC,
0190 .iInterface = 1,
0191 },
0192 .sink = {
0193 .bLength = sizeof descriptors.hs_descs.sink,
0194 .bDescriptorType = USB_DT_ENDPOINT,
0195 .bEndpointAddress = 1 | USB_DIR_IN,
0196 .bmAttributes = USB_ENDPOINT_XFER_BULK,
0197 .wMaxPacketSize = cpu_to_le16(1024),
0198 },
0199 .sink_comp = {
0200 .bLength = USB_DT_SS_EP_COMP_SIZE,
0201 .bDescriptorType = USB_DT_SS_ENDPOINT_COMP,
0202 .bMaxBurst = 0,
0203 .bmAttributes = 0,
0204 .wBytesPerInterval = 0,
0205 },
0206 .source = {
0207 .bLength = sizeof descriptors.hs_descs.source,
0208 .bDescriptorType = USB_DT_ENDPOINT,
0209 .bEndpointAddress = 2 | USB_DIR_OUT,
0210 .bmAttributes = USB_ENDPOINT_XFER_BULK,
0211 .wMaxPacketSize = cpu_to_le16(1024),
0212 .bInterval = 1,
0213 },
0214 .source_comp = {
0215 .bLength = USB_DT_SS_EP_COMP_SIZE,
0216 .bDescriptorType = USB_DT_SS_ENDPOINT_COMP,
0217 .bMaxBurst = 0,
0218 .bmAttributes = 0,
0219 .wBytesPerInterval = 0,
0220 },
0221 },
0222 };
0223
0224 static size_t descs_to_legacy(void **legacy, const void *descriptors_v2)
0225 {
0226 const unsigned char *descs_end, *descs_start;
0227 __u32 length, fs_count = 0, hs_count = 0, count;
0228
0229
0230 {
0231 const struct {
0232 const struct usb_functionfs_descs_head_v2 header;
0233 const __le32 counts[];
0234 } __attribute__((packed)) *const in = descriptors_v2;
0235 const __le32 *counts = in->counts;
0236 __u32 flags;
0237
0238 if (le32_to_cpu(in->header.magic) !=
0239 FUNCTIONFS_DESCRIPTORS_MAGIC_V2)
0240 return 0;
0241 length = le32_to_cpu(in->header.length);
0242 if (length <= sizeof in->header)
0243 return 0;
0244 length -= sizeof in->header;
0245 flags = le32_to_cpu(in->header.flags);
0246 if (flags & ~(FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC |
0247 FUNCTIONFS_HAS_SS_DESC))
0248 return 0;
0249
0250 #define GET_NEXT_COUNT_IF_FLAG(ret, flg) do { \
0251 if (!(flags & (flg))) \
0252 break; \
0253 if (length < 4) \
0254 return 0; \
0255 ret = le32_to_cpu(*counts); \
0256 length -= 4; \
0257 ++counts; \
0258 } while (0)
0259
0260 GET_NEXT_COUNT_IF_FLAG(fs_count, FUNCTIONFS_HAS_FS_DESC);
0261 GET_NEXT_COUNT_IF_FLAG(hs_count, FUNCTIONFS_HAS_HS_DESC);
0262 GET_NEXT_COUNT_IF_FLAG(count, FUNCTIONFS_HAS_SS_DESC);
0263
0264 count = fs_count + hs_count;
0265 if (!count)
0266 return 0;
0267 descs_start = (const void *)counts;
0268
0269 #undef GET_NEXT_COUNT_IF_FLAG
0270 }
0271
0272
0273
0274
0275
0276 descs_end = descs_start;
0277 do {
0278 if (length < *descs_end)
0279 return 0;
0280 length -= *descs_end;
0281 descs_end += *descs_end;
0282 } while (--count);
0283
0284
0285 {
0286 #pragma GCC diagnostic push
0287 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
0288 struct {
0289 struct usb_functionfs_descs_head header;
0290 __u8 descriptors[];
0291 } __attribute__((packed)) *out;
0292 #pragma GCC diagnostic pop
0293
0294 length = sizeof out->header + (descs_end - descs_start);
0295 out = malloc(length);
0296 out->header.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC);
0297 out->header.length = cpu_to_le32(length);
0298 out->header.fs_count = cpu_to_le32(fs_count);
0299 out->header.hs_count = cpu_to_le32(hs_count);
0300 memcpy(out->descriptors, descs_start, descs_end - descs_start);
0301 *legacy = out;
0302 }
0303
0304 return length;
0305 }
0306
0307
0308 #define STR_INTERFACE_ "Source/Sink"
0309
0310 static const struct {
0311 struct usb_functionfs_strings_head header;
0312 struct {
0313 __le16 code;
0314 const char str1[sizeof STR_INTERFACE_];
0315 } __attribute__((packed)) lang0;
0316 } __attribute__((packed)) strings = {
0317 .header = {
0318 .magic = cpu_to_le32(FUNCTIONFS_STRINGS_MAGIC),
0319 .length = cpu_to_le32(sizeof strings),
0320 .str_count = cpu_to_le32(1),
0321 .lang_count = cpu_to_le32(1),
0322 },
0323 .lang0 = {
0324 cpu_to_le16(0x0409),
0325 STR_INTERFACE_,
0326 },
0327 };
0328
0329 #define STR_INTERFACE strings.lang0.str1
0330
0331
0332
0333
0334 struct thread;
0335
0336 static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes);
0337 static ssize_t write_wrap(struct thread *t, const void *buf, size_t nbytes);
0338 static ssize_t ep0_consume(struct thread *t, const void *buf, size_t nbytes);
0339 static ssize_t fill_in_buf(struct thread *t, void *buf, size_t nbytes);
0340 static ssize_t empty_out_buf(struct thread *t, const void *buf, size_t nbytes);
0341
0342
0343 static struct thread {
0344 const char *const filename;
0345 size_t buf_size;
0346
0347 ssize_t (*in)(struct thread *, void *, size_t);
0348 const char *const in_name;
0349
0350 ssize_t (*out)(struct thread *, const void *, size_t);
0351 const char *const out_name;
0352
0353 int fd;
0354 pthread_t id;
0355 void *buf;
0356 ssize_t status;
0357 } threads[] = {
0358 {
0359 "ep0", 4 * sizeof(struct usb_functionfs_event),
0360 read_wrap, NULL,
0361 ep0_consume, "<consume>",
0362 0, 0, NULL, 0
0363 },
0364 {
0365 "ep1", 8 * 1024,
0366 fill_in_buf, "<in>",
0367 write_wrap, NULL,
0368 0, 0, NULL, 0
0369 },
0370 {
0371 "ep2", 8 * 1024,
0372 read_wrap, NULL,
0373 empty_out_buf, "<out>",
0374 0, 0, NULL, 0
0375 },
0376 };
0377
0378
0379 static void init_thread(struct thread *t)
0380 {
0381 t->buf = malloc(t->buf_size);
0382 die_on(!t->buf, "malloc");
0383
0384 t->fd = open(t->filename, O_RDWR);
0385 die_on(t->fd < 0, "%s", t->filename);
0386 }
0387
0388 static void cleanup_thread(void *arg)
0389 {
0390 struct thread *t = arg;
0391 int ret, fd;
0392
0393 fd = t->fd;
0394 if (t->fd < 0)
0395 return;
0396 t->fd = -1;
0397
0398
0399 if (t != threads) {
0400 ret = ioctl(fd, FUNCTIONFS_FIFO_STATUS);
0401 if (ret < 0) {
0402
0403 if (errno != ENODEV)
0404 err("%s: get fifo status", t->filename);
0405 } else if (ret) {
0406 warn("%s: unclaimed = %d\n", t->filename, ret);
0407 if (ioctl(fd, FUNCTIONFS_FIFO_FLUSH) < 0)
0408 err("%s: fifo flush", t->filename);
0409 }
0410 }
0411
0412 if (close(fd) < 0)
0413 err("%s: close", t->filename);
0414
0415 free(t->buf);
0416 t->buf = NULL;
0417 }
0418
0419 static void *start_thread_helper(void *arg)
0420 {
0421 const char *name, *op, *in_name, *out_name;
0422 struct thread *t = arg;
0423 ssize_t ret;
0424
0425 info("%s: starts\n", t->filename);
0426 in_name = t->in_name ? t->in_name : t->filename;
0427 out_name = t->out_name ? t->out_name : t->filename;
0428
0429 pthread_cleanup_push(cleanup_thread, arg);
0430
0431 for (;;) {
0432 pthread_testcancel();
0433
0434 ret = t->in(t, t->buf, t->buf_size);
0435 if (ret > 0) {
0436 ret = t->out(t, t->buf, ret);
0437 name = out_name;
0438 op = "write";
0439 } else {
0440 name = in_name;
0441 op = "read";
0442 }
0443
0444 if (ret > 0) {
0445
0446 } else if (!ret) {
0447 debug("%s: %s: EOF", name, op);
0448 break;
0449 } else if (errno == EINTR || errno == EAGAIN) {
0450 debug("%s: %s", name, op);
0451 } else {
0452 warn("%s: %s", name, op);
0453 break;
0454 }
0455 }
0456
0457 pthread_cleanup_pop(1);
0458
0459 t->status = ret;
0460 info("%s: ends\n", t->filename);
0461 return NULL;
0462 }
0463
0464 static void start_thread(struct thread *t)
0465 {
0466 debug("%s: starting\n", t->filename);
0467
0468 die_on(pthread_create(&t->id, NULL, start_thread_helper, t) < 0,
0469 "pthread_create(%s)", t->filename);
0470 }
0471
0472 static void join_thread(struct thread *t)
0473 {
0474 int ret = pthread_join(t->id, NULL);
0475
0476 if (ret < 0)
0477 err("%s: joining thread", t->filename);
0478 else
0479 debug("%s: joined\n", t->filename);
0480 }
0481
0482
0483 static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes)
0484 {
0485 return read(t->fd, buf, nbytes);
0486 }
0487
0488 static ssize_t write_wrap(struct thread *t, const void *buf, size_t nbytes)
0489 {
0490 return write(t->fd, buf, nbytes);
0491 }
0492
0493
0494
0495
0496
0497 enum pattern { PAT_ZERO, PAT_SEQ, PAT_PIPE };
0498 static enum pattern pattern;
0499
0500 static ssize_t
0501 fill_in_buf(struct thread *ignore, void *buf, size_t nbytes)
0502 {
0503 size_t i;
0504 __u8 *p;
0505
0506 (void)ignore;
0507
0508 switch (pattern) {
0509 case PAT_ZERO:
0510 memset(buf, 0, nbytes);
0511 break;
0512
0513 case PAT_SEQ:
0514 for (p = buf, i = 0; i < nbytes; ++i, ++p)
0515 *p = i % 63;
0516 break;
0517
0518 case PAT_PIPE:
0519 return fread(buf, 1, nbytes, stdin);
0520 }
0521
0522 return nbytes;
0523 }
0524
0525 static ssize_t
0526 empty_out_buf(struct thread *ignore, const void *buf, size_t nbytes)
0527 {
0528 const __u8 *p;
0529 __u8 expected;
0530 ssize_t ret;
0531 size_t len;
0532
0533 (void)ignore;
0534
0535 switch (pattern) {
0536 case PAT_ZERO:
0537 expected = 0;
0538 for (p = buf, len = 0; len < nbytes; ++p, ++len)
0539 if (*p)
0540 goto invalid;
0541 break;
0542
0543 case PAT_SEQ:
0544 for (p = buf, len = 0; len < nbytes; ++p, ++len)
0545 if (*p != len % 63) {
0546 expected = len % 63;
0547 goto invalid;
0548 }
0549 break;
0550
0551 case PAT_PIPE:
0552 ret = fwrite(buf, nbytes, 1, stdout);
0553 if (ret > 0)
0554 fflush(stdout);
0555 break;
0556
0557 invalid:
0558 err("bad OUT byte %zd, expected %02x got %02x\n",
0559 len, expected, *p);
0560 for (p = buf, len = 0; len < nbytes; ++p, ++len) {
0561 if (0 == (len % 32))
0562 fprintf(stderr, "%4zd:", len);
0563 fprintf(stderr, " %02x", *p);
0564 if (31 == (len % 32))
0565 fprintf(stderr, "\n");
0566 }
0567 fflush(stderr);
0568 errno = EILSEQ;
0569 return -1;
0570 }
0571
0572 return len;
0573 }
0574
0575
0576
0577
0578 static void handle_setup(const struct usb_ctrlrequest *setup)
0579 {
0580 printf("bRequestType = %d\n", setup->bRequestType);
0581 printf("bRequest = %d\n", setup->bRequest);
0582 printf("wValue = %d\n", le16_to_cpu(setup->wValue));
0583 printf("wIndex = %d\n", le16_to_cpu(setup->wIndex));
0584 printf("wLength = %d\n", le16_to_cpu(setup->wLength));
0585 }
0586
0587 static ssize_t
0588 ep0_consume(struct thread *ignore, const void *buf, size_t nbytes)
0589 {
0590 static const char *const names[] = {
0591 [FUNCTIONFS_BIND] = "BIND",
0592 [FUNCTIONFS_UNBIND] = "UNBIND",
0593 [FUNCTIONFS_ENABLE] = "ENABLE",
0594 [FUNCTIONFS_DISABLE] = "DISABLE",
0595 [FUNCTIONFS_SETUP] = "SETUP",
0596 [FUNCTIONFS_SUSPEND] = "SUSPEND",
0597 [FUNCTIONFS_RESUME] = "RESUME",
0598 };
0599
0600 const struct usb_functionfs_event *event = buf;
0601 size_t n;
0602
0603 (void)ignore;
0604
0605 for (n = nbytes / sizeof *event; n; --n, ++event)
0606 switch (event->type) {
0607 case FUNCTIONFS_BIND:
0608 case FUNCTIONFS_UNBIND:
0609 case FUNCTIONFS_ENABLE:
0610 case FUNCTIONFS_DISABLE:
0611 case FUNCTIONFS_SETUP:
0612 case FUNCTIONFS_SUSPEND:
0613 case FUNCTIONFS_RESUME:
0614 printf("Event %s\n", names[event->type]);
0615 if (event->type == FUNCTIONFS_SETUP)
0616 handle_setup(&event->u.setup);
0617 break;
0618
0619 default:
0620 printf("Event %03u (unknown)\n", event->type);
0621 }
0622
0623 return nbytes;
0624 }
0625
0626 static void ep0_init(struct thread *t, bool legacy_descriptors)
0627 {
0628 void *legacy;
0629 ssize_t ret;
0630 size_t len;
0631
0632 if (legacy_descriptors) {
0633 info("%s: writing descriptors\n", t->filename);
0634 goto legacy;
0635 }
0636
0637 info("%s: writing descriptors (in v2 format)\n", t->filename);
0638 ret = write(t->fd, &descriptors, sizeof descriptors);
0639
0640 if (ret < 0 && errno == EINVAL) {
0641 warn("%s: new format rejected, trying legacy\n", t->filename);
0642 legacy:
0643 len = descs_to_legacy(&legacy, &descriptors);
0644 if (len) {
0645 ret = write(t->fd, legacy, len);
0646 free(legacy);
0647 }
0648 }
0649 die_on(ret < 0, "%s: write: descriptors", t->filename);
0650
0651 info("%s: writing strings\n", t->filename);
0652 ret = write(t->fd, &strings, sizeof strings);
0653 die_on(ret < 0, "%s: write: strings", t->filename);
0654 }
0655
0656
0657
0658
0659 int main(int argc, char **argv)
0660 {
0661 bool legacy_descriptors;
0662 unsigned i;
0663
0664 legacy_descriptors = argc > 2 && !strcmp(argv[1], "-l");
0665
0666 init_thread(threads);
0667 ep0_init(threads, legacy_descriptors);
0668
0669 for (i = 1; i < sizeof threads / sizeof *threads; ++i)
0670 init_thread(threads + i);
0671
0672 for (i = 1; i < sizeof threads / sizeof *threads; ++i)
0673 start_thread(threads + i);
0674
0675 start_thread_helper(threads);
0676
0677 for (i = 1; i < sizeof threads / sizeof *threads; ++i)
0678 join_thread(threads + i);
0679
0680 return 0;
0681 }