1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25#define _DEFAULT_SOURCE
26
27#include <endian.h>
28#include <errno.h>
29#include <fcntl.h>
30#include <pthread.h>
31#include <stdarg.h>
32#include <stdbool.h>
33#include <stdio.h>
34#include <stdlib.h>
35#include <string.h>
36#include <sys/ioctl.h>
37#include <sys/stat.h>
38#include <sys/types.h>
39#include <unistd.h>
40#include <tools/le_byteshift.h>
41
42#include "../../include/uapi/linux/usb/functionfs.h"
43
44
45
46
47
48
49
50
51
52
53#if __BYTE_ORDER == __LITTLE_ENDIAN
54#define cpu_to_le16(x) (x)
55#define cpu_to_le32(x) (x)
56#else
57#define cpu_to_le16(x) ((((x) >> 8) & 0xffu) | (((x) & 0xffu) << 8))
58#define cpu_to_le32(x) \
59 ((((x) & 0xff000000u) >> 24) | (((x) & 0x00ff0000u) >> 8) | \
60 (((x) & 0x0000ff00u) << 8) | (((x) & 0x000000ffu) << 24))
61#endif
62
63#define le32_to_cpu(x) le32toh(x)
64#define le16_to_cpu(x) le16toh(x)
65
66
67
68static const char argv0[] = "ffs-test";
69
70static unsigned verbosity = 7;
71
72static void _msg(unsigned level, const char *fmt, ...)
73{
74 if (level < 2)
75 level = 2;
76 else if (level > 7)
77 level = 7;
78
79 if (level <= verbosity) {
80 static const char levels[8][6] = {
81 [2] = "crit:",
82 [3] = "err: ",
83 [4] = "warn:",
84 [5] = "note:",
85 [6] = "info:",
86 [7] = "dbg: "
87 };
88
89 int _errno = errno;
90 va_list ap;
91
92 fprintf(stderr, "%s: %s ", argv0, levels[level]);
93 va_start(ap, fmt);
94 vfprintf(stderr, fmt, ap);
95 va_end(ap);
96
97 if (fmt[strlen(fmt) - 1] != '\n') {
98 char buffer[128];
99 strerror_r(_errno, buffer, sizeof buffer);
100 fprintf(stderr, ": (-%d) %s\n", _errno, buffer);
101 }
102
103 fflush(stderr);
104 }
105}
106
107#define die(...) (_msg(2, __VA_ARGS__), exit(1))
108#define err(...) _msg(3, __VA_ARGS__)
109#define warn(...) _msg(4, __VA_ARGS__)
110#define note(...) _msg(5, __VA_ARGS__)
111#define info(...) _msg(6, __VA_ARGS__)
112#define debug(...) _msg(7, __VA_ARGS__)
113
114#define die_on(cond, ...) do { \
115 if (cond) \
116 die(__VA_ARGS__); \
117 } while (0)
118
119
120
121
122static const struct {
123 struct usb_functionfs_descs_head_v2 header;
124 __le32 fs_count;
125 __le32 hs_count;
126 __le32 ss_count;
127 struct {
128 struct usb_interface_descriptor intf;
129 struct usb_endpoint_descriptor_no_audio sink;
130 struct usb_endpoint_descriptor_no_audio source;
131 } __attribute__((packed)) fs_descs, hs_descs;
132 struct {
133 struct usb_interface_descriptor intf;
134 struct usb_endpoint_descriptor_no_audio sink;
135 struct usb_ss_ep_comp_descriptor sink_comp;
136 struct usb_endpoint_descriptor_no_audio source;
137 struct usb_ss_ep_comp_descriptor source_comp;
138 } ss_descs;
139} __attribute__((packed)) descriptors = {
140 .header = {
141 .magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2),
142 .flags = cpu_to_le32(FUNCTIONFS_HAS_FS_DESC |
143 FUNCTIONFS_HAS_HS_DESC |
144 FUNCTIONFS_HAS_SS_DESC),
145 .length = cpu_to_le32(sizeof descriptors),
146 },
147 .fs_count = cpu_to_le32(3),
148 .fs_descs = {
149 .intf = {
150 .bLength = sizeof descriptors.fs_descs.intf,
151 .bDescriptorType = USB_DT_INTERFACE,
152 .bNumEndpoints = 2,
153 .bInterfaceClass = USB_CLASS_VENDOR_SPEC,
154 .iInterface = 1,
155 },
156 .sink = {
157 .bLength = sizeof descriptors.fs_descs.sink,
158 .bDescriptorType = USB_DT_ENDPOINT,
159 .bEndpointAddress = 1 | USB_DIR_IN,
160 .bmAttributes = USB_ENDPOINT_XFER_BULK,
161
162 },
163 .source = {
164 .bLength = sizeof descriptors.fs_descs.source,
165 .bDescriptorType = USB_DT_ENDPOINT,
166 .bEndpointAddress = 2 | USB_DIR_OUT,
167 .bmAttributes = USB_ENDPOINT_XFER_BULK,
168
169 },
170 },
171 .hs_count = cpu_to_le32(3),
172 .hs_descs = {
173 .intf = {
174 .bLength = sizeof descriptors.fs_descs.intf,
175 .bDescriptorType = USB_DT_INTERFACE,
176 .bNumEndpoints = 2,
177 .bInterfaceClass = USB_CLASS_VENDOR_SPEC,
178 .iInterface = 1,
179 },
180 .sink = {
181 .bLength = sizeof descriptors.hs_descs.sink,
182 .bDescriptorType = USB_DT_ENDPOINT,
183 .bEndpointAddress = 1 | USB_DIR_IN,
184 .bmAttributes = USB_ENDPOINT_XFER_BULK,
185 .wMaxPacketSize = cpu_to_le16(512),
186 },
187 .source = {
188 .bLength = sizeof descriptors.hs_descs.source,
189 .bDescriptorType = USB_DT_ENDPOINT,
190 .bEndpointAddress = 2 | USB_DIR_OUT,
191 .bmAttributes = USB_ENDPOINT_XFER_BULK,
192 .wMaxPacketSize = cpu_to_le16(512),
193 .bInterval = 1,
194 },
195 },
196 .ss_count = cpu_to_le32(5),
197 .ss_descs = {
198 .intf = {
199 .bLength = sizeof descriptors.fs_descs.intf,
200 .bDescriptorType = USB_DT_INTERFACE,
201 .bNumEndpoints = 2,
202 .bInterfaceClass = USB_CLASS_VENDOR_SPEC,
203 .iInterface = 1,
204 },
205 .sink = {
206 .bLength = sizeof descriptors.hs_descs.sink,
207 .bDescriptorType = USB_DT_ENDPOINT,
208 .bEndpointAddress = 1 | USB_DIR_IN,
209 .bmAttributes = USB_ENDPOINT_XFER_BULK,
210 .wMaxPacketSize = cpu_to_le16(1024),
211 },
212 .sink_comp = {
213 .bLength = USB_DT_SS_EP_COMP_SIZE,
214 .bDescriptorType = USB_DT_SS_ENDPOINT_COMP,
215 .bMaxBurst = 0,
216 .bmAttributes = 0,
217 .wBytesPerInterval = 0,
218 },
219 .source = {
220 .bLength = sizeof descriptors.hs_descs.source,
221 .bDescriptorType = USB_DT_ENDPOINT,
222 .bEndpointAddress = 2 | USB_DIR_OUT,
223 .bmAttributes = USB_ENDPOINT_XFER_BULK,
224 .wMaxPacketSize = cpu_to_le16(1024),
225 .bInterval = 1,
226 },
227 .source_comp = {
228 .bLength = USB_DT_SS_EP_COMP_SIZE,
229 .bDescriptorType = USB_DT_SS_ENDPOINT_COMP,
230 .bMaxBurst = 0,
231 .bmAttributes = 0,
232 .wBytesPerInterval = 0,
233 },
234 },
235};
236
237static size_t descs_to_legacy(void **legacy, const void *descriptors_v2)
238{
239 const unsigned char *descs_end, *descs_start;
240 __u32 length, fs_count = 0, hs_count = 0, count;
241
242
243 {
244 const struct {
245 const struct usb_functionfs_descs_head_v2 header;
246 const __le32 counts[];
247 } __attribute__((packed)) *const in = descriptors_v2;
248 const __le32 *counts = in->counts;
249 __u32 flags;
250
251 if (le32_to_cpu(in->header.magic) !=
252 FUNCTIONFS_DESCRIPTORS_MAGIC_V2)
253 return 0;
254 length = le32_to_cpu(in->header.length);
255 if (length <= sizeof in->header)
256 return 0;
257 length -= sizeof in->header;
258 flags = le32_to_cpu(in->header.flags);
259 if (flags & ~(FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC |
260 FUNCTIONFS_HAS_SS_DESC))
261 return 0;
262
263#define GET_NEXT_COUNT_IF_FLAG(ret, flg) do { \
264 if (!(flags & (flg))) \
265 break; \
266 if (length < 4) \
267 return 0; \
268 ret = le32_to_cpu(*counts); \
269 length -= 4; \
270 ++counts; \
271 } while (0)
272
273 GET_NEXT_COUNT_IF_FLAG(fs_count, FUNCTIONFS_HAS_FS_DESC);
274 GET_NEXT_COUNT_IF_FLAG(hs_count, FUNCTIONFS_HAS_HS_DESC);
275 GET_NEXT_COUNT_IF_FLAG(count, FUNCTIONFS_HAS_SS_DESC);
276
277 count = fs_count + hs_count;
278 if (!count)
279 return 0;
280 descs_start = (const void *)counts;
281
282#undef GET_NEXT_COUNT_IF_FLAG
283 }
284
285
286
287
288
289 descs_end = descs_start;
290 do {
291 if (length < *descs_end)
292 return 0;
293 length -= *descs_end;
294 descs_end += *descs_end;
295 } while (--count);
296
297
298 {
299#pragma GCC diagnostic push
300#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
301 struct {
302 struct usb_functionfs_descs_head header;
303 __u8 descriptors[];
304 } __attribute__((packed)) *out;
305#pragma GCC diagnostic pop
306
307 length = sizeof out->header + (descs_end - descs_start);
308 out = malloc(length);
309 out->header.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC);
310 out->header.length = cpu_to_le32(length);
311 out->header.fs_count = cpu_to_le32(fs_count);
312 out->header.hs_count = cpu_to_le32(hs_count);
313 memcpy(out->descriptors, descs_start, descs_end - descs_start);
314 *legacy = out;
315 }
316
317 return length;
318}
319
320
321#define STR_INTERFACE_ "Source/Sink"
322
323static const struct {
324 struct usb_functionfs_strings_head header;
325 struct {
326 __le16 code;
327 const char str1[sizeof STR_INTERFACE_];
328 } __attribute__((packed)) lang0;
329} __attribute__((packed)) strings = {
330 .header = {
331 .magic = cpu_to_le32(FUNCTIONFS_STRINGS_MAGIC),
332 .length = cpu_to_le32(sizeof strings),
333 .str_count = cpu_to_le32(1),
334 .lang_count = cpu_to_le32(1),
335 },
336 .lang0 = {
337 cpu_to_le16(0x0409),
338 STR_INTERFACE_,
339 },
340};
341
342#define STR_INTERFACE strings.lang0.str1
343
344
345
346
347struct thread;
348
349static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes);
350static ssize_t write_wrap(struct thread *t, const void *buf, size_t nbytes);
351static ssize_t ep0_consume(struct thread *t, const void *buf, size_t nbytes);
352static ssize_t fill_in_buf(struct thread *t, void *buf, size_t nbytes);
353static ssize_t empty_out_buf(struct thread *t, const void *buf, size_t nbytes);
354
355
356static struct thread {
357 const char *const filename;
358 size_t buf_size;
359
360 ssize_t (*in)(struct thread *, void *, size_t);
361 const char *const in_name;
362
363 ssize_t (*out)(struct thread *, const void *, size_t);
364 const char *const out_name;
365
366 int fd;
367 pthread_t id;
368 void *buf;
369 ssize_t status;
370} threads[] = {
371 {
372 "ep0", 4 * sizeof(struct usb_functionfs_event),
373 read_wrap, NULL,
374 ep0_consume, "<consume>",
375 0, 0, NULL, 0
376 },
377 {
378 "ep1", 8 * 1024,
379 fill_in_buf, "<in>",
380 write_wrap, NULL,
381 0, 0, NULL, 0
382 },
383 {
384 "ep2", 8 * 1024,
385 read_wrap, NULL,
386 empty_out_buf, "<out>",
387 0, 0, NULL, 0
388 },
389};
390
391
392static void init_thread(struct thread *t)
393{
394 t->buf = malloc(t->buf_size);
395 die_on(!t->buf, "malloc");
396
397 t->fd = open(t->filename, O_RDWR);
398 die_on(t->fd < 0, "%s", t->filename);
399}
400
401static void cleanup_thread(void *arg)
402{
403 struct thread *t = arg;
404 int ret, fd;
405
406 fd = t->fd;
407 if (t->fd < 0)
408 return;
409 t->fd = -1;
410
411
412 if (t != threads) {
413 ret = ioctl(fd, FUNCTIONFS_FIFO_STATUS);
414 if (ret < 0) {
415
416 if (errno != ENODEV)
417 err("%s: get fifo status", t->filename);
418 } else if (ret) {
419 warn("%s: unclaimed = %d\n", t->filename, ret);
420 if (ioctl(fd, FUNCTIONFS_FIFO_FLUSH) < 0)
421 err("%s: fifo flush", t->filename);
422 }
423 }
424
425 if (close(fd) < 0)
426 err("%s: close", t->filename);
427
428 free(t->buf);
429 t->buf = NULL;
430}
431
432static void *start_thread_helper(void *arg)
433{
434 const char *name, *op, *in_name, *out_name;
435 struct thread *t = arg;
436 ssize_t ret;
437
438 info("%s: starts\n", t->filename);
439 in_name = t->in_name ? t->in_name : t->filename;
440 out_name = t->out_name ? t->out_name : t->filename;
441
442 pthread_cleanup_push(cleanup_thread, arg);
443
444 for (;;) {
445 pthread_testcancel();
446
447 ret = t->in(t, t->buf, t->buf_size);
448 if (ret > 0) {
449 ret = t->out(t, t->buf, ret);
450 name = out_name;
451 op = "write";
452 } else {
453 name = in_name;
454 op = "read";
455 }
456
457 if (ret > 0) {
458
459 } else if (!ret) {
460 debug("%s: %s: EOF", name, op);
461 break;
462 } else if (errno == EINTR || errno == EAGAIN) {
463 debug("%s: %s", name, op);
464 } else {
465 warn("%s: %s", name, op);
466 break;
467 }
468 }
469
470 pthread_cleanup_pop(1);
471
472 t->status = ret;
473 info("%s: ends\n", t->filename);
474 return NULL;
475}
476
477static void start_thread(struct thread *t)
478{
479 debug("%s: starting\n", t->filename);
480
481 die_on(pthread_create(&t->id, NULL, start_thread_helper, t) < 0,
482 "pthread_create(%s)", t->filename);
483}
484
485static void join_thread(struct thread *t)
486{
487 int ret = pthread_join(t->id, NULL);
488
489 if (ret < 0)
490 err("%s: joining thread", t->filename);
491 else
492 debug("%s: joined\n", t->filename);
493}
494
495
496static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes)
497{
498 return read(t->fd, buf, nbytes);
499}
500
501static ssize_t write_wrap(struct thread *t, const void *buf, size_t nbytes)
502{
503 return write(t->fd, buf, nbytes);
504}
505
506
507
508
509
510enum pattern { PAT_ZERO, PAT_SEQ, PAT_PIPE };
511static enum pattern pattern;
512
513static ssize_t
514fill_in_buf(struct thread *ignore, void *buf, size_t nbytes)
515{
516 size_t i;
517 __u8 *p;
518
519 (void)ignore;
520
521 switch (pattern) {
522 case PAT_ZERO:
523 memset(buf, 0, nbytes);
524 break;
525
526 case PAT_SEQ:
527 for (p = buf, i = 0; i < nbytes; ++i, ++p)
528 *p = i % 63;
529 break;
530
531 case PAT_PIPE:
532 return fread(buf, 1, nbytes, stdin);
533 }
534
535 return nbytes;
536}
537
538static ssize_t
539empty_out_buf(struct thread *ignore, const void *buf, size_t nbytes)
540{
541 const __u8 *p;
542 __u8 expected;
543 ssize_t ret;
544 size_t len;
545
546 (void)ignore;
547
548 switch (pattern) {
549 case PAT_ZERO:
550 expected = 0;
551 for (p = buf, len = 0; len < nbytes; ++p, ++len)
552 if (*p)
553 goto invalid;
554 break;
555
556 case PAT_SEQ:
557 for (p = buf, len = 0; len < nbytes; ++p, ++len)
558 if (*p != len % 63) {
559 expected = len % 63;
560 goto invalid;
561 }
562 break;
563
564 case PAT_PIPE:
565 ret = fwrite(buf, nbytes, 1, stdout);
566 if (ret > 0)
567 fflush(stdout);
568 break;
569
570invalid:
571 err("bad OUT byte %zd, expected %02x got %02x\n",
572 len, expected, *p);
573 for (p = buf, len = 0; len < nbytes; ++p, ++len) {
574 if (0 == (len % 32))
575 fprintf(stderr, "%4zd:", len);
576 fprintf(stderr, " %02x", *p);
577 if (31 == (len % 32))
578 fprintf(stderr, "\n");
579 }
580 fflush(stderr);
581 errno = EILSEQ;
582 return -1;
583 }
584
585 return len;
586}
587
588
589
590
591static void handle_setup(const struct usb_ctrlrequest *setup)
592{
593 printf("bRequestType = %d\n", setup->bRequestType);
594 printf("bRequest = %d\n", setup->bRequest);
595 printf("wValue = %d\n", le16_to_cpu(setup->wValue));
596 printf("wIndex = %d\n", le16_to_cpu(setup->wIndex));
597 printf("wLength = %d\n", le16_to_cpu(setup->wLength));
598}
599
600static ssize_t
601ep0_consume(struct thread *ignore, const void *buf, size_t nbytes)
602{
603 static const char *const names[] = {
604 [FUNCTIONFS_BIND] = "BIND",
605 [FUNCTIONFS_UNBIND] = "UNBIND",
606 [FUNCTIONFS_ENABLE] = "ENABLE",
607 [FUNCTIONFS_DISABLE] = "DISABLE",
608 [FUNCTIONFS_SETUP] = "SETUP",
609 [FUNCTIONFS_SUSPEND] = "SUSPEND",
610 [FUNCTIONFS_RESUME] = "RESUME",
611 };
612
613 const struct usb_functionfs_event *event = buf;
614 size_t n;
615
616 (void)ignore;
617
618 for (n = nbytes / sizeof *event; n; --n, ++event)
619 switch (event->type) {
620 case FUNCTIONFS_BIND:
621 case FUNCTIONFS_UNBIND:
622 case FUNCTIONFS_ENABLE:
623 case FUNCTIONFS_DISABLE:
624 case FUNCTIONFS_SETUP:
625 case FUNCTIONFS_SUSPEND:
626 case FUNCTIONFS_RESUME:
627 printf("Event %s\n", names[event->type]);
628 if (event->type == FUNCTIONFS_SETUP)
629 handle_setup(&event->u.setup);
630 break;
631
632 default:
633 printf("Event %03u (unknown)\n", event->type);
634 }
635
636 return nbytes;
637}
638
639static void ep0_init(struct thread *t, bool legacy_descriptors)
640{
641 void *legacy;
642 ssize_t ret;
643 size_t len;
644
645 if (legacy_descriptors) {
646 info("%s: writing descriptors\n", t->filename);
647 goto legacy;
648 }
649
650 info("%s: writing descriptors (in v2 format)\n", t->filename);
651 ret = write(t->fd, &descriptors, sizeof descriptors);
652
653 if (ret < 0 && errno == EINVAL) {
654 warn("%s: new format rejected, trying legacy\n", t->filename);
655legacy:
656 len = descs_to_legacy(&legacy, &descriptors);
657 if (len) {
658 ret = write(t->fd, legacy, len);
659 free(legacy);
660 }
661 }
662 die_on(ret < 0, "%s: write: descriptors", t->filename);
663
664 info("%s: writing strings\n", t->filename);
665 ret = write(t->fd, &strings, sizeof strings);
666 die_on(ret < 0, "%s: write: strings", t->filename);
667}
668
669
670
671
672int main(int argc, char **argv)
673{
674 bool legacy_descriptors;
675 unsigned i;
676
677 legacy_descriptors = argc > 2 && !strcmp(argv[1], "-l");
678
679 init_thread(threads);
680 ep0_init(threads, legacy_descriptors);
681
682 for (i = 1; i < sizeof threads / sizeof *threads; ++i)
683 init_thread(threads + i);
684
685 for (i = 1; i < sizeof threads / sizeof *threads; ++i)
686 start_thread(threads + i);
687
688 start_thread_helper(threads);
689
690 for (i = 1; i < sizeof threads / sizeof *threads; ++i)
691 join_thread(threads + i);
692
693 return 0;
694}
695
