682 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * ffs-test.c -- user mode filesystem api for usb composite function4 *5 * Copyright (C) 2010 Samsung Electronics6 * 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 a36 * function call is not allowed. To solve this, we code cpu_to_le16/32 in a way37 * that allows them to be used when initializing structures.38 */39 40#if __BYTE_ORDER == __LITTLE_ENDIAN41#define cpu_to_le16(x) (x)42#define cpu_to_le32(x) (x)43#else44#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#endif49 50#define le32_to_cpu(x) le32toh(x)51#define le16_to_cpu(x) le16toh(x)52 53/******************** Messages and Errors ***********************************/54 55static const char argv0[] = "ffs-test";56 57static unsigned verbosity = 7;58 59static 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 109static 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 224static 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_FLAG270 }271 272 /*273 * Find the end of FS and HS USB descriptors. SS descriptors274 * 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 push287#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 pop293 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 310static 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.str1330 331 332/******************** Files and Threads Handling ****************************/333 334struct thread;335 336static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes);337static ssize_t write_wrap(struct thread *t, const void *buf, size_t nbytes);338static ssize_t ep0_consume(struct thread *t, const void *buf, size_t nbytes);339static ssize_t fill_in_buf(struct thread *t, void *buf, size_t nbytes);340static ssize_t empty_out_buf(struct thread *t, const void *buf, size_t nbytes);341 342 343static 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, 0363 },364 {365 "ep1", 8 * 1024,366 fill_in_buf, "<in>",367 write_wrap, NULL,368 0, 0, NULL, 0369 },370 {371 "ep2", 8 * 1024,372 read_wrap, NULL,373 empty_out_buf, "<out>",374 0, 0, NULL, 0375 },376};377 378 379static 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 388static 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 419static 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 464static 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 472static 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 else479 debug("%s: joined\n", t->filename);480}481 482 483static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes)484{485 return read(t->fd, buf, nbytes);486}487 488static 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 */497enum pattern { PAT_ZERO, PAT_SEQ, PAT_PIPE };498static enum pattern pattern;499 500static ssize_t501fill_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 525static ssize_t526empty_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 557invalid: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 578static 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 587static ssize_t588ep0_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 626static 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);642legacy: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 659int 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