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