1 /* 2 * ffs-test.c -- user mode filesystem api for usb composite function 3 * 4 * Copyright (C) 2010 Samsung Electronics 5 * Author: Michal Nazarewicz <mina86@mina86.com> 6 * 7 * This program is free software; you can redistribute it and/or modify 8 * it under the terms of the GNU General Public License as published by 9 * the Free Software Foundation; either version 2 of the License, or 10 * (at your option) any later version. 11 * 12 * This program is distributed in the hope that it will be useful, 13 * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 * GNU General Public License for more details. 16 * 17 * You should have received a copy of the GNU General Public License 18 * along with this program; if not, write to the Free Software 19 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 20 */ 21 22 /* $(CROSS_COMPILE)cc -Wall -Wextra -g -o ffs-test ffs-test.c -lpthread */ 23 24 25 #define _BSD_SOURCE /* for endian.h */ 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 /******************** Little Endian Handling ********************************/ 46 47 #define cpu_to_le16(x) htole16(x) 48 #define cpu_to_le32(x) htole32(x) 49 #define le32_to_cpu(x) le32toh(x) 50 #define le16_to_cpu(x) le16toh(x) 51 52 53 /******************** Messages and Errors ***********************************/ 54 55 static const char argv0[] = "ffs-test"; 56 57 static unsigned verbosity = 7; 58 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 struct { 114 struct usb_interface_descriptor intf; 115 struct usb_endpoint_descriptor_no_audio sink; 116 struct usb_endpoint_descriptor_no_audio source; 117 } __attribute__((packed)) fs_descs, hs_descs; 118 } __attribute__((packed)) descriptors = { 119 .header = { 120 .magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2), 121 .flags = cpu_to_le32(FUNCTIONFS_HAS_FS_DESC | 122 FUNCTIONFS_HAS_HS_DESC), 123 .length = cpu_to_le32(sizeof descriptors), 124 }, 125 .fs_count = cpu_to_le32(3), 126 .fs_descs = { 127 .intf = { 128 .bLength = sizeof descriptors.fs_descs.intf, 129 .bDescriptorType = USB_DT_INTERFACE, 130 .bNumEndpoints = 2, 131 .bInterfaceClass = USB_CLASS_VENDOR_SPEC, 132 .iInterface = 1, 133 }, 134 .sink = { 135 .bLength = sizeof descriptors.fs_descs.sink, 136 .bDescriptorType = USB_DT_ENDPOINT, 137 .bEndpointAddress = 1 | USB_DIR_IN, 138 .bmAttributes = USB_ENDPOINT_XFER_BULK, 139 /* .wMaxPacketSize = autoconfiguration (kernel) */ 140 }, 141 .source = { 142 .bLength = sizeof descriptors.fs_descs.source, 143 .bDescriptorType = USB_DT_ENDPOINT, 144 .bEndpointAddress = 2 | USB_DIR_OUT, 145 .bmAttributes = USB_ENDPOINT_XFER_BULK, 146 /* .wMaxPacketSize = autoconfiguration (kernel) */ 147 }, 148 }, 149 .hs_count = cpu_to_le32(3), 150 .hs_descs = { 151 .intf = { 152 .bLength = sizeof descriptors.fs_descs.intf, 153 .bDescriptorType = USB_DT_INTERFACE, 154 .bNumEndpoints = 2, 155 .bInterfaceClass = USB_CLASS_VENDOR_SPEC, 156 .iInterface = 1, 157 }, 158 .sink = { 159 .bLength = sizeof descriptors.hs_descs.sink, 160 .bDescriptorType = USB_DT_ENDPOINT, 161 .bEndpointAddress = 1 | USB_DIR_IN, 162 .bmAttributes = USB_ENDPOINT_XFER_BULK, 163 .wMaxPacketSize = cpu_to_le16(512), 164 }, 165 .source = { 166 .bLength = sizeof descriptors.hs_descs.source, 167 .bDescriptorType = USB_DT_ENDPOINT, 168 .bEndpointAddress = 2 | USB_DIR_OUT, 169 .bmAttributes = USB_ENDPOINT_XFER_BULK, 170 .wMaxPacketSize = cpu_to_le16(512), 171 .bInterval = 1, /* NAK every 1 uframe */ 172 }, 173 }, 174 }; 175 176 static size_t descs_to_legacy(void **legacy, const void *descriptors_v2) 177 { 178 const unsigned char *descs_end, *descs_start; 179 __u32 length, fs_count = 0, hs_count = 0, count; 180 181 /* Read v2 header */ 182 { 183 const struct { 184 const struct usb_functionfs_descs_head_v2 header; 185 const __le32 counts[]; 186 } __attribute__((packed)) *const in = descriptors_v2; 187 const __le32 *counts = in->counts; 188 __u32 flags; 189 190 if (le32_to_cpu(in->header.magic) != 191 FUNCTIONFS_DESCRIPTORS_MAGIC_V2) 192 return 0; 193 length = le32_to_cpu(in->header.length); 194 if (length <= sizeof in->header) 195 return 0; 196 length -= sizeof in->header; 197 flags = le32_to_cpu(in->header.flags); 198 if (flags & ~(FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC | 199 FUNCTIONFS_HAS_SS_DESC)) 200 return 0; 201 202 #define GET_NEXT_COUNT_IF_FLAG(ret, flg) do { \ 203 if (!(flags & (flg))) \ 204 break; \ 205 if (length < 4) \ 206 return 0; \ 207 ret = le32_to_cpu(*counts); \ 208 length -= 4; \ 209 ++counts; \ 210 } while (0) 211 212 GET_NEXT_COUNT_IF_FLAG(fs_count, FUNCTIONFS_HAS_FS_DESC); 213 GET_NEXT_COUNT_IF_FLAG(hs_count, FUNCTIONFS_HAS_HS_DESC); 214 GET_NEXT_COUNT_IF_FLAG(count, FUNCTIONFS_HAS_SS_DESC); 215 216 count = fs_count + hs_count; 217 if (!count) 218 return 0; 219 descs_start = (const void *)counts; 220 221 #undef GET_NEXT_COUNT_IF_FLAG 222 } 223 224 /* 225 * Find the end of FS and HS USB descriptors. SS descriptors 226 * are ignored since legacy format does not support them. 227 */ 228 descs_end = descs_start; 229 do { 230 if (length < *descs_end) 231 return 0; 232 length -= *descs_end; 233 descs_end += *descs_end; 234 } while (--count); 235 236 /* Allocate legacy descriptors and copy the data. */ 237 { 238 #pragma GCC diagnostic push 239 #pragma GCC diagnostic ignored "-Wdeprecated-declarations" 240 struct { 241 struct usb_functionfs_descs_head header; 242 __u8 descriptors[]; 243 } __attribute__((packed)) *out; 244 #pragma GCC diagnostic pop 245 246 length = sizeof out->header + (descs_end - descs_start); 247 out = malloc(length); 248 out->header.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC); 249 out->header.length = cpu_to_le32(length); 250 out->header.fs_count = cpu_to_le32(fs_count); 251 out->header.hs_count = cpu_to_le32(hs_count); 252 memcpy(out->descriptors, descs_start, descs_end - descs_start); 253 *legacy = out; 254 } 255 256 return length; 257 } 258 259 260 #define STR_INTERFACE_ "Source/Sink" 261 262 static const struct { 263 struct usb_functionfs_strings_head header; 264 struct { 265 __le16 code; 266 const char str1[sizeof STR_INTERFACE_]; 267 } __attribute__((packed)) lang0; 268 } __attribute__((packed)) strings = { 269 .header = { 270 .magic = cpu_to_le32(FUNCTIONFS_STRINGS_MAGIC), 271 .length = cpu_to_le32(sizeof strings), 272 .str_count = cpu_to_le32(1), 273 .lang_count = cpu_to_le32(1), 274 }, 275 .lang0 = { 276 cpu_to_le16(0x0409), /* en-us */ 277 STR_INTERFACE_, 278 }, 279 }; 280 281 #define STR_INTERFACE strings.lang0.str1 282 283 284 /******************** Files and Threads Handling ****************************/ 285 286 struct thread; 287 288 static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes); 289 static ssize_t write_wrap(struct thread *t, const void *buf, size_t nbytes); 290 static ssize_t ep0_consume(struct thread *t, const void *buf, size_t nbytes); 291 static ssize_t fill_in_buf(struct thread *t, void *buf, size_t nbytes); 292 static ssize_t empty_out_buf(struct thread *t, const void *buf, size_t nbytes); 293 294 295 static struct thread { 296 const char *const filename; 297 size_t buf_size; 298 299 ssize_t (*in)(struct thread *, void *, size_t); 300 const char *const in_name; 301 302 ssize_t (*out)(struct thread *, const void *, size_t); 303 const char *const out_name; 304 305 int fd; 306 pthread_t id; 307 void *buf; 308 ssize_t status; 309 } threads[] = { 310 { 311 "ep0", 4 * sizeof(struct usb_functionfs_event), 312 read_wrap, NULL, 313 ep0_consume, "<consume>", 314 0, 0, NULL, 0 315 }, 316 { 317 "ep1", 8 * 1024, 318 fill_in_buf, "<in>", 319 write_wrap, NULL, 320 0, 0, NULL, 0 321 }, 322 { 323 "ep2", 8 * 1024, 324 read_wrap, NULL, 325 empty_out_buf, "<out>", 326 0, 0, NULL, 0 327 }, 328 }; 329 330 331 static void init_thread(struct thread *t) 332 { 333 t->buf = malloc(t->buf_size); 334 die_on(!t->buf, "malloc"); 335 336 t->fd = open(t->filename, O_RDWR); 337 die_on(t->fd < 0, "%s", t->filename); 338 } 339 340 static void cleanup_thread(void *arg) 341 { 342 struct thread *t = arg; 343 int ret, fd; 344 345 fd = t->fd; 346 if (t->fd < 0) 347 return; 348 t->fd = -1; 349 350 /* test the FIFO ioctls (non-ep0 code paths) */ 351 if (t != threads) { 352 ret = ioctl(fd, FUNCTIONFS_FIFO_STATUS); 353 if (ret < 0) { 354 /* ENODEV reported after disconnect */ 355 if (errno != ENODEV) 356 err("%s: get fifo status", t->filename); 357 } else if (ret) { 358 warn("%s: unclaimed = %d\n", t->filename, ret); 359 if (ioctl(fd, FUNCTIONFS_FIFO_FLUSH) < 0) 360 err("%s: fifo flush", t->filename); 361 } 362 } 363 364 if (close(fd) < 0) 365 err("%s: close", t->filename); 366 367 free(t->buf); 368 t->buf = NULL; 369 } 370 371 static void *start_thread_helper(void *arg) 372 { 373 const char *name, *op, *in_name, *out_name; 374 struct thread *t = arg; 375 ssize_t ret; 376 377 info("%s: starts\n", t->filename); 378 in_name = t->in_name ? t->in_name : t->filename; 379 out_name = t->out_name ? t->out_name : t->filename; 380 381 pthread_cleanup_push(cleanup_thread, arg); 382 383 for (;;) { 384 pthread_testcancel(); 385 386 ret = t->in(t, t->buf, t->buf_size); 387 if (ret > 0) { 388 ret = t->out(t, t->buf, ret); 389 name = out_name; 390 op = "write"; 391 } else { 392 name = in_name; 393 op = "read"; 394 } 395 396 if (ret > 0) { 397 /* nop */ 398 } else if (!ret) { 399 debug("%s: %s: EOF", name, op); 400 break; 401 } else if (errno == EINTR || errno == EAGAIN) { 402 debug("%s: %s", name, op); 403 } else { 404 warn("%s: %s", name, op); 405 break; 406 } 407 } 408 409 pthread_cleanup_pop(1); 410 411 t->status = ret; 412 info("%s: ends\n", t->filename); 413 return NULL; 414 } 415 416 static void start_thread(struct thread *t) 417 { 418 debug("%s: starting\n", t->filename); 419 420 die_on(pthread_create(&t->id, NULL, start_thread_helper, t) < 0, 421 "pthread_create(%s)", t->filename); 422 } 423 424 static void join_thread(struct thread *t) 425 { 426 int ret = pthread_join(t->id, NULL); 427 428 if (ret < 0) 429 err("%s: joining thread", t->filename); 430 else 431 debug("%s: joined\n", t->filename); 432 } 433 434 435 static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes) 436 { 437 return read(t->fd, buf, nbytes); 438 } 439 440 static ssize_t write_wrap(struct thread *t, const void *buf, size_t nbytes) 441 { 442 return write(t->fd, buf, nbytes); 443 } 444 445 446 /******************** Empty/Fill buffer routines ****************************/ 447 448 /* 0 -- stream of zeros, 1 -- i % 63, 2 -- pipe */ 449 enum pattern { PAT_ZERO, PAT_SEQ, PAT_PIPE }; 450 static enum pattern pattern; 451 452 static ssize_t 453 fill_in_buf(struct thread *ignore, void *buf, size_t nbytes) 454 { 455 size_t i; 456 __u8 *p; 457 458 (void)ignore; 459 460 switch (pattern) { 461 case PAT_ZERO: 462 memset(buf, 0, nbytes); 463 break; 464 465 case PAT_SEQ: 466 for (p = buf, i = 0; i < nbytes; ++i, ++p) 467 *p = i % 63; 468 break; 469 470 case PAT_PIPE: 471 return fread(buf, 1, nbytes, stdin); 472 } 473 474 return nbytes; 475 } 476 477 static ssize_t 478 empty_out_buf(struct thread *ignore, const void *buf, size_t nbytes) 479 { 480 const __u8 *p; 481 __u8 expected; 482 ssize_t ret; 483 size_t len; 484 485 (void)ignore; 486 487 switch (pattern) { 488 case PAT_ZERO: 489 expected = 0; 490 for (p = buf, len = 0; len < nbytes; ++p, ++len) 491 if (*p) 492 goto invalid; 493 break; 494 495 case PAT_SEQ: 496 for (p = buf, len = 0; len < nbytes; ++p, ++len) 497 if (*p != len % 63) { 498 expected = len % 63; 499 goto invalid; 500 } 501 break; 502 503 case PAT_PIPE: 504 ret = fwrite(buf, nbytes, 1, stdout); 505 if (ret > 0) 506 fflush(stdout); 507 break; 508 509 invalid: 510 err("bad OUT byte %zd, expected %02x got %02x\n", 511 len, expected, *p); 512 for (p = buf, len = 0; len < nbytes; ++p, ++len) { 513 if (0 == (len % 32)) 514 fprintf(stderr, "%4zd:", len); 515 fprintf(stderr, " %02x", *p); 516 if (31 == (len % 32)) 517 fprintf(stderr, "\n"); 518 } 519 fflush(stderr); 520 errno = EILSEQ; 521 return -1; 522 } 523 524 return len; 525 } 526 527 528 /******************** Endpoints routines ************************************/ 529 530 static void handle_setup(const struct usb_ctrlrequest *setup) 531 { 532 printf("bRequestType = %d\n", setup->bRequestType); 533 printf("bRequest = %d\n", setup->bRequest); 534 printf("wValue = %d\n", le16_to_cpu(setup->wValue)); 535 printf("wIndex = %d\n", le16_to_cpu(setup->wIndex)); 536 printf("wLength = %d\n", le16_to_cpu(setup->wLength)); 537 } 538 539 static ssize_t 540 ep0_consume(struct thread *ignore, const void *buf, size_t nbytes) 541 { 542 static const char *const names[] = { 543 [FUNCTIONFS_BIND] = "BIND", 544 [FUNCTIONFS_UNBIND] = "UNBIND", 545 [FUNCTIONFS_ENABLE] = "ENABLE", 546 [FUNCTIONFS_DISABLE] = "DISABLE", 547 [FUNCTIONFS_SETUP] = "SETUP", 548 [FUNCTIONFS_SUSPEND] = "SUSPEND", 549 [FUNCTIONFS_RESUME] = "RESUME", 550 }; 551 552 const struct usb_functionfs_event *event = buf; 553 size_t n; 554 555 (void)ignore; 556 557 for (n = nbytes / sizeof *event; n; --n, ++event) 558 switch (event->type) { 559 case FUNCTIONFS_BIND: 560 case FUNCTIONFS_UNBIND: 561 case FUNCTIONFS_ENABLE: 562 case FUNCTIONFS_DISABLE: 563 case FUNCTIONFS_SETUP: 564 case FUNCTIONFS_SUSPEND: 565 case FUNCTIONFS_RESUME: 566 printf("Event %s\n", names[event->type]); 567 if (event->type == FUNCTIONFS_SETUP) 568 handle_setup(&event->u.setup); 569 break; 570 571 default: 572 printf("Event %03u (unknown)\n", event->type); 573 } 574 575 return nbytes; 576 } 577 578 static void ep0_init(struct thread *t, bool legacy_descriptors) 579 { 580 void *legacy; 581 ssize_t ret; 582 size_t len; 583 584 if (legacy_descriptors) { 585 info("%s: writing descriptors\n", t->filename); 586 goto legacy; 587 } 588 589 info("%s: writing descriptors (in v2 format)\n", t->filename); 590 ret = write(t->fd, &descriptors, sizeof descriptors); 591 592 if (ret < 0 && errno == EINVAL) { 593 warn("%s: new format rejected, trying legacy\n", t->filename); 594 legacy: 595 len = descs_to_legacy(&legacy, &descriptors); 596 if (len) { 597 ret = write(t->fd, legacy, len); 598 free(legacy); 599 } 600 } 601 die_on(ret < 0, "%s: write: descriptors", t->filename); 602 603 info("%s: writing strings\n", t->filename); 604 ret = write(t->fd, &strings, sizeof strings); 605 die_on(ret < 0, "%s: write: strings", t->filename); 606 } 607 608 609 /******************** Main **************************************************/ 610 611 int main(int argc, char **argv) 612 { 613 bool legacy_descriptors; 614 unsigned i; 615 616 legacy_descriptors = argc > 2 && !strcmp(argv[1], "-l"); 617 618 init_thread(threads); 619 ep0_init(threads, legacy_descriptors); 620 621 for (i = 1; i < sizeof threads / sizeof *threads; ++i) 622 init_thread(threads + i); 623 624 for (i = 1; i < sizeof threads / sizeof *threads; ++i) 625 start_thread(threads + i); 626 627 start_thread_helper(threads); 628 629 for (i = 1; i < sizeof threads / sizeof *threads; ++i) 630 join_thread(threads + i); 631 632 return 0; 633 } 634