[linux.git] / tools / usb / ffs-test.c

/*
 * ffs-test.c -- user mode filesystem api for usb composite function
 *
 * Copyright (C) 2010 Samsung Electronics
 *                    Author: Michal Nazarewicz <[email protected]>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/* $(CROSS_COMPILE)cc -Wall -Wextra -g -o ffs-test ffs-test.c -lpthread */


#define _BSD_SOURCE /* for endian.h */

#include <endian.h>
#include <errno.h>
#include <fcntl.h>
#include <pthread.h>
#include <stdarg.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <tools/le_byteshift.h>

#include "../../include/uapi/linux/usb/functionfs.h"


/******************** Little Endian Handling ********************************/

#define cpu_to_le16(x)  htole16(x)
#define cpu_to_le32(x)  htole32(x)
#define le32_to_cpu(x)  le32toh(x)
#define le16_to_cpu(x)  le16toh(x)


/******************** Messages and Errors ***********************************/

static const char argv0[] = "ffs-test";

static unsigned verbosity = 7;

static void _msg(unsigned level, const char *fmt, ...)
{
	if (level < 2)
		level = 2;
	else if (level > 7)
		level = 7;

	if (level <= verbosity) {
		static const char levels[8][6] = {
			[2] = "crit:",
			[3] = "err: ",
			[4] = "warn:",
			[5] = "note:",
			[6] = "info:",
			[7] = "dbg: "
		};

		int _errno = errno;
		va_list ap;

		fprintf(stderr, "%s: %s ", argv0, levels[level]);
		va_start(ap, fmt);
		vfprintf(stderr, fmt, ap);
		va_end(ap);

		if (fmt[strlen(fmt) - 1] != '\n') {
			char buffer[128];
			strerror_r(_errno, buffer, sizeof buffer);
			fprintf(stderr, ": (-%d) %s\n", _errno, buffer);
		}

		fflush(stderr);
	}
}

#define die(...)  (_msg(2, __VA_ARGS__), exit(1))
#define err(...)   _msg(3, __VA_ARGS__)
#define warn(...)  _msg(4, __VA_ARGS__)
#define note(...)  _msg(5, __VA_ARGS__)
#define info(...)  _msg(6, __VA_ARGS__)
#define debug(...) _msg(7, __VA_ARGS__)

#define die_on(cond, ...) do { \
	if (cond) \
		die(__VA_ARGS__); \
	} while (0)


/******************** Descriptors and Strings *******************************/

static const struct {
	struct usb_functionfs_descs_head_v2 header;
	__le32 fs_count;
	__le32 hs_count;
	struct {
		struct usb_interface_descriptor intf;
		struct usb_endpoint_descriptor_no_audio sink;
		struct usb_endpoint_descriptor_no_audio source;
	} __attribute__((packed)) fs_descs, hs_descs;
} __attribute__((packed)) descriptors = {
	.header = {
		.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2),
		.flags = cpu_to_le32(FUNCTIONFS_HAS_FS_DESC |
				     FUNCTIONFS_HAS_HS_DESC),
		.length = cpu_to_le32(sizeof descriptors),
	},
	.fs_count = cpu_to_le32(3),
	.fs_descs = {
		.intf = {
			.bLength = sizeof descriptors.fs_descs.intf,
			.bDescriptorType = USB_DT_INTERFACE,
			.bNumEndpoints = 2,
			.bInterfaceClass = USB_CLASS_VENDOR_SPEC,
			.iInterface = 1,
		},
		.sink = {
			.bLength = sizeof descriptors.fs_descs.sink,
			.bDescriptorType = USB_DT_ENDPOINT,
			.bEndpointAddress = 1 | USB_DIR_IN,
			.bmAttributes = USB_ENDPOINT_XFER_BULK,
			/* .wMaxPacketSize = autoconfiguration (kernel) */
		},
		.source = {
			.bLength = sizeof descriptors.fs_descs.source,
			.bDescriptorType = USB_DT_ENDPOINT,
			.bEndpointAddress = 2 | USB_DIR_OUT,
			.bmAttributes = USB_ENDPOINT_XFER_BULK,
			/* .wMaxPacketSize = autoconfiguration (kernel) */
		},
	},
	.hs_count = cpu_to_le32(3),
	.hs_descs = {
		.intf = {
			.bLength = sizeof descriptors.fs_descs.intf,
			.bDescriptorType = USB_DT_INTERFACE,
			.bNumEndpoints = 2,
			.bInterfaceClass = USB_CLASS_VENDOR_SPEC,
			.iInterface = 1,
		},
		.sink = {
			.bLength = sizeof descriptors.hs_descs.sink,
			.bDescriptorType = USB_DT_ENDPOINT,
			.bEndpointAddress = 1 | USB_DIR_IN,
			.bmAttributes = USB_ENDPOINT_XFER_BULK,
			.wMaxPacketSize = cpu_to_le16(512),
		},
		.source = {
			.bLength = sizeof descriptors.hs_descs.source,
			.bDescriptorType = USB_DT_ENDPOINT,
			.bEndpointAddress = 2 | USB_DIR_OUT,
			.bmAttributes = USB_ENDPOINT_XFER_BULK,
			.wMaxPacketSize = cpu_to_le16(512),
			.bInterval = 1, /* NAK every 1 uframe */
		},
	},
};

static size_t descs_to_legacy(void **legacy, const void *descriptors_v2)
{
	const unsigned char *descs_end, *descs_start;
	__u32 length, fs_count = 0, hs_count = 0, count;

	/* Read v2 header */
	{
		const struct {
			const struct usb_functionfs_descs_head_v2 header;
			const __le32 counts[];
		} __attribute__((packed)) *const in = descriptors_v2;
		const __le32 *counts = in->counts;
		__u32 flags;

		if (le32_to_cpu(in->header.magic) !=
		    FUNCTIONFS_DESCRIPTORS_MAGIC_V2)
			return 0;
		length = le32_to_cpu(in->header.length);
		if (length <= sizeof in->header)
			return 0;
		length -= sizeof in->header;
		flags = le32_to_cpu(in->header.flags);
		if (flags & ~(FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC |
			      FUNCTIONFS_HAS_SS_DESC))
			return 0;

#define GET_NEXT_COUNT_IF_FLAG(ret, flg) do {		\
			if (!(flags & (flg)))		\
				break;			\
			if (length < 4)			\
				return 0;		\
			ret = le32_to_cpu(*counts);	\
			length -= 4;			\
			++counts;			\
		} while (0)

		GET_NEXT_COUNT_IF_FLAG(fs_count, FUNCTIONFS_HAS_FS_DESC);
		GET_NEXT_COUNT_IF_FLAG(hs_count, FUNCTIONFS_HAS_HS_DESC);
		GET_NEXT_COUNT_IF_FLAG(count, FUNCTIONFS_HAS_SS_DESC);

		count = fs_count + hs_count;
		if (!count)
			return 0;
		descs_start = (const void *)counts;

#undef GET_NEXT_COUNT_IF_FLAG
	}

	/*
	 * Find the end of FS and HS USB descriptors.  SS descriptors
	 * are ignored since legacy format does not support them.
	 */
	descs_end = descs_start;
	do {
		if (length < *descs_end)
			return 0;
		length -= *descs_end;
		descs_end += *descs_end;
	} while (--count);

	/* Allocate legacy descriptors and copy the data. */
	{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
		struct {
			struct usb_functionfs_descs_head header;
			__u8 descriptors[];
		} __attribute__((packed)) *out;
#pragma GCC diagnostic pop

		length = sizeof out->header + (descs_end - descs_start);
		out = malloc(length);
		out->header.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC);
		out->header.length = cpu_to_le32(length);
		out->header.fs_count = cpu_to_le32(fs_count);
		out->header.hs_count = cpu_to_le32(hs_count);
		memcpy(out->descriptors, descs_start, descs_end - descs_start);
		*legacy = out;
	}

	return length;
}


#define STR_INTERFACE_ "Source/Sink"

static const struct {
	struct usb_functionfs_strings_head header;
	struct {
		__le16 code;
		const char str1[sizeof STR_INTERFACE_];
	} __attribute__((packed)) lang0;
} __attribute__((packed)) strings = {
	.header = {
		.magic = cpu_to_le32(FUNCTIONFS_STRINGS_MAGIC),
		.length = cpu_to_le32(sizeof strings),
		.str_count = cpu_to_le32(1),
		.lang_count = cpu_to_le32(1),
	},
	.lang0 = {
		cpu_to_le16(0x0409), /* en-us */
		STR_INTERFACE_,
	},
};

#define STR_INTERFACE strings.lang0.str1


/******************** Files and Threads Handling ****************************/

struct thread;

static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes);
static ssize_t write_wrap(struct thread *t, const void *buf, size_t nbytes);
static ssize_t ep0_consume(struct thread *t, const void *buf, size_t nbytes);
static ssize_t fill_in_buf(struct thread *t, void *buf, size_t nbytes);
static ssize_t empty_out_buf(struct thread *t, const void *buf, size_t nbytes);


static struct thread {
	const char *const filename;
	size_t buf_size;

	ssize_t (*in)(struct thread *, void *, size_t);
	const char *const in_name;

	ssize_t (*out)(struct thread *, const void *, size_t);
	const char *const out_name;

	int fd;
	pthread_t id;
	void *buf;
	ssize_t status;
} threads[] = {
	{
		"ep0", 4 * sizeof(struct usb_functionfs_event),
		read_wrap, NULL,
		ep0_consume, "<consume>",
		0, 0, NULL, 0
	},
	{
		"ep1", 8 * 1024,
		fill_in_buf, "<in>",
		write_wrap, NULL,
		0, 0, NULL, 0
	},
	{
		"ep2", 8 * 1024,
		read_wrap, NULL,
		empty_out_buf, "<out>",
		0, 0, NULL, 0
	},
};


static void init_thread(struct thread *t)
{
	t->buf = malloc(t->buf_size);
	die_on(!t->buf, "malloc");

	t->fd = open(t->filename, O_RDWR);
	die_on(t->fd < 0, "%s", t->filename);
}

static void cleanup_thread(void *arg)
{
	struct thread *t = arg;
	int ret, fd;

	fd = t->fd;
	if (t->fd < 0)
		return;
	t->fd = -1;

	/* test the FIFO ioctls (non-ep0 code paths) */
	if (t != threads) {
		ret = ioctl(fd, FUNCTIONFS_FIFO_STATUS);
		if (ret < 0) {
			/* ENODEV reported after disconnect */
			if (errno != ENODEV)
				err("%s: get fifo status", t->filename);
		} else if (ret) {
			warn("%s: unclaimed = %d\n", t->filename, ret);
			if (ioctl(fd, FUNCTIONFS_FIFO_FLUSH) < 0)
				err("%s: fifo flush", t->filename);
		}
	}

	if (close(fd) < 0)
		err("%s: close", t->filename);

	free(t->buf);
	t->buf = NULL;
}

static void *start_thread_helper(void *arg)
{
	const char *name, *op, *in_name, *out_name;
	struct thread *t = arg;
	ssize_t ret;

	info("%s: starts\n", t->filename);
	in_name = t->in_name ? t->in_name : t->filename;
	out_name = t->out_name ? t->out_name : t->filename;

	pthread_cleanup_push(cleanup_thread, arg);

	for (;;) {
		pthread_testcancel();

		ret = t->in(t, t->buf, t->buf_size);
		if (ret > 0) {
			ret = t->out(t, t->buf, ret);
			name = out_name;
			op = "write";
		} else {
			name = in_name;
			op = "read";
		}

		if (ret > 0) {
			/* nop */
		} else if (!ret) {
			debug("%s: %s: EOF", name, op);
			break;
		} else if (errno == EINTR || errno == EAGAIN) {
			debug("%s: %s", name, op);
		} else {
			warn("%s: %s", name, op);
			break;
		}
	}

	pthread_cleanup_pop(1);

	t->status = ret;
	info("%s: ends\n", t->filename);
	return NULL;
}

static void start_thread(struct thread *t)
{
	debug("%s: starting\n", t->filename);

	die_on(pthread_create(&t->id, NULL, start_thread_helper, t) < 0,
	       "pthread_create(%s)", t->filename);
}

static void join_thread(struct thread *t)
{
	int ret = pthread_join(t->id, NULL);

	if (ret < 0)
		err("%s: joining thread", t->filename);
	else
		debug("%s: joined\n", t->filename);
}


static ssize_t read_wrap(struct thread *t, void *buf, size_t nbytes)
{
	return read(t->fd, buf, nbytes);
}

static ssize_t write_wrap(struct thread *t, const void *buf, size_t nbytes)
{
	return write(t->fd, buf, nbytes);
}


/******************** Empty/Fill buffer routines ****************************/

/* 0 -- stream of zeros, 1 -- i % 63, 2 -- pipe */
enum pattern { PAT_ZERO, PAT_SEQ, PAT_PIPE };
static enum pattern pattern;

static ssize_t
fill_in_buf(struct thread *ignore, void *buf, size_t nbytes)
{
	size_t i;
	__u8 *p;

	(void)ignore;

	switch (pattern) {
	case PAT_ZERO:
		memset(buf, 0, nbytes);
		break;

	case PAT_SEQ:
		for (p = buf, i = 0; i < nbytes; ++i, ++p)
			*p = i % 63;
		break;

	case PAT_PIPE:
		return fread(buf, 1, nbytes, stdin);
	}

	return nbytes;
}

static ssize_t
empty_out_buf(struct thread *ignore, const void *buf, size_t nbytes)
{
	const __u8 *p;
	__u8 expected;
	ssize_t ret;
	size_t len;

	(void)ignore;

	switch (pattern) {
	case PAT_ZERO:
		expected = 0;
		for (p = buf, len = 0; len < nbytes; ++p, ++len)
			if (*p)
				goto invalid;
		break;

	case PAT_SEQ:
		for (p = buf, len = 0; len < nbytes; ++p, ++len)
			if (*p != len % 63) {
				expected = len % 63;
				goto invalid;
			}
		break;

	case PAT_PIPE:
		ret = fwrite(buf, nbytes, 1, stdout);
		if (ret > 0)
			fflush(stdout);
		break;

invalid:
		err("bad OUT byte %zd, expected %02x got %02x\n",
		    len, expected, *p);
		for (p = buf, len = 0; len < nbytes; ++p, ++len) {
			if (0 == (len % 32))
				fprintf(stderr, "%4zd:", len);
			fprintf(stderr, " %02x", *p);
			if (31 == (len % 32))
				fprintf(stderr, "\n");
		}
		fflush(stderr);
		errno = EILSEQ;
		return -1;
	}

	return len;
}


/******************** Endpoints routines ************************************/

static void handle_setup(const struct usb_ctrlrequest *setup)
{
	printf("bRequestType = %d\n", setup->bRequestType);
	printf("bRequest     = %d\n", setup->bRequest);
	printf("wValue       = %d\n", le16_to_cpu(setup->wValue));
	printf("wIndex       = %d\n", le16_to_cpu(setup->wIndex));
	printf("wLength      = %d\n", le16_to_cpu(setup->wLength));
}

static ssize_t
ep0_consume(struct thread *ignore, const void *buf, size_t nbytes)
{
	static const char *const names[] = {
		[FUNCTIONFS_BIND] = "BIND",
		[FUNCTIONFS_UNBIND] = "UNBIND",
		[FUNCTIONFS_ENABLE] = "ENABLE",
		[FUNCTIONFS_DISABLE] = "DISABLE",
		[FUNCTIONFS_SETUP] = "SETUP",
		[FUNCTIONFS_SUSPEND] = "SUSPEND",
		[FUNCTIONFS_RESUME] = "RESUME",
	};

	const struct usb_functionfs_event *event = buf;
	size_t n;

	(void)ignore;

	for (n = nbytes / sizeof *event; n; --n, ++event)
		switch (event->type) {
		case FUNCTIONFS_BIND:
		case FUNCTIONFS_UNBIND:
		case FUNCTIONFS_ENABLE:
		case FUNCTIONFS_DISABLE:
		case FUNCTIONFS_SETUP:
		case FUNCTIONFS_SUSPEND:
		case FUNCTIONFS_RESUME:
			printf("Event %s\n", names[event->type]);
			if (event->type == FUNCTIONFS_SETUP)
				handle_setup(&event->u.setup);
			break;

		default:
			printf("Event %03u (unknown)\n", event->type);
		}

	return nbytes;
}

static void ep0_init(struct thread *t, bool legacy_descriptors)
{
	void *legacy;
	ssize_t ret;
	size_t len;

	if (legacy_descriptors) {
		info("%s: writing descriptors\n", t->filename);
		goto legacy;
	}

	info("%s: writing descriptors (in v2 format)\n", t->filename);
	ret = write(t->fd, &descriptors, sizeof descriptors);

	if (ret < 0 && errno == EINVAL) {
		warn("%s: new format rejected, trying legacy\n", t->filename);
legacy:
		len = descs_to_legacy(&legacy, &descriptors);
		if (len) {
			ret = write(t->fd, legacy, len);
			free(legacy);
		}
	}
	die_on(ret < 0, "%s: write: descriptors", t->filename);

	info("%s: writing strings\n", t->filename);
	ret = write(t->fd, &strings, sizeof strings);
	die_on(ret < 0, "%s: write: strings", t->filename);
}


/******************** Main **************************************************/

int main(int argc, char **argv)
{
	bool legacy_descriptors;
	unsigned i;

	legacy_descriptors = argc > 2 && !strcmp(argv[1], "-l");

	init_thread(threads);
	ep0_init(threads, legacy_descriptors);

	for (i = 1; i < sizeof threads / sizeof *threads; ++i)
		init_thread(threads + i);

	for (i = 1; i < sizeof threads / sizeof *threads; ++i)
		start_thread(threads + i);

	start_thread_helper(threads);

	for (i = 1; i < sizeof threads / sizeof *threads; ++i)
		join_thread(threads + i);

	return 0;
}
Commit	Line	Data
93f2aa4d	1	/*
51c208c7	2	* ffs-test.c -- user mode filesystem api for usb composite function
93f2aa4d MN	3	*
93f2aa4d MN	4	* Copyright (C) 2010 Samsung Electronics
54b8360f	5	* Author: Michal Nazarewicz <[email protected]>
93f2aa4d MN	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>
b9a42746	32	#include <stdbool.h>
93f2aa4d MN	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>
24fa9a9d	40	#include <tools/le_byteshift.h>
93f2aa4d	41
a0f11ace	42	#include "../../include/uapi/linux/usb/functionfs.h"
93f2aa4d MN	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
93f2aa4d MN	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 {
51c208c7 MN	110	struct usb_functionfs_descs_head_v2 header;
	111	__le32 fs_count;
	112	__le32 hs_count;
93f2aa4d MN	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 = {
51c208c7 MN	120	.magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2),
	121	.flags = cpu_to_le32(FUNCTIONFS_HAS_FS_DESC \|
	122	FUNCTIONFS_HAS_HS_DESC),
93f2aa4d	123	.length = cpu_to_le32(sizeof descriptors),
93f2aa4d	124	},
51c208c7	125	.fs_count = cpu_to_le32(3),
93f2aa4d MN	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	},
51c208c7	149	.hs_count = cpu_to_le32(3),
93f2aa4d MN	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
b9a42746 MN	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
93f2aa4d MN	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) {
eb9c5836	388	ret = t->out(t, t->buf, ret);
93f2aa4d MN	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))
d105e74e	514	fprintf(stderr, "%4zd:", len);
93f2aa4d MN	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
b9a42746	578	static void ep0_init(struct thread *t, bool legacy_descriptors)
93f2aa4d	579	{
b9a42746	580	void *legacy;
93f2aa4d	581	ssize_t ret;
b9a42746 MN	582	size_t len;
	583
	584	if (legacy_descriptors) {
	585	info("%s: writing descriptors\n", t->filename);
	586	goto legacy;
	587	}
93f2aa4d	588
b9a42746	589	info("%s: writing descriptors (in v2 format)\n", t->filename);
93f2aa4d	590	ret = write(t->fd, &descriptors, sizeof descriptors);
b9a42746 MN	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	}
93f2aa4d MN	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
b9a42746	611	int main(int argc, char **argv)
93f2aa4d	612	{
b9a42746	613	bool legacy_descriptors;
93f2aa4d MN	614	unsigned i;
93f2aa4d MN	615
b9a42746	616	legacy_descriptors = argc > 2 && !strcmp(argv[1], "-l");
93f2aa4d MN	617
93f2aa4d MN	618	init_thread(threads);
b9a42746	619	ep0_init(threads, legacy_descriptors);
93f2aa4d MN	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	}