Initial commit of my USB test code which can exercise connected USB

devices and the FreeBSD USB stack itself. This program can be used to
test compliance against well established usb.org standards, also
called chapter-9 tests. The host platform can act as either USB device
or USB host depending on the available hardware. The basic USB
communication happens through FreeBSD's own libusb v2, and some
sysctls are also used to invoke specific error conditions. This test
program can be used to verify correct operation of external USB
harddisks under heavy load and various other conditions. The software
is driven via a simple command line interface. Main supported USB host
classes are "USB mass storage" and "USB modems".

7 files changed, 3579 insertions, 0 deletions

diff --git a/tools/tools/usbtest/Makefile b/tools/tools/usbtest/Makefile
new file mode 100644
index 0000000..1cc0c83
--- /dev/null
+++ b/tools/tools/usbtest/Makefile
@@ -0,0 +1,40 @@
+#
+# $FreeBSD$
+#
+# Copyright (c) 2013 Hans Petter Selasky. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+# OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+# OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+# SUCH DAMAGE.
+#
+PROG=	usbtest
+MAN=
+SRCS+=	usbtest.c
+SRCS+=	usb_msc_test.c
+SRCS+=	usb_modem_test.c
+SRCS+=	usb_control_ep_test.c
+
+LDADD+=	-lusb
+
+WARNS=	3
+
+CFLAGS+= -I ${.CURDIR}/../../../sys/dev/usb/gadget
+
+.include <bsd.prog.mk>
diff --git a/tools/tools/usbtest/usb_control_ep_test.c b/tools/tools/usbtest/usb_control_ep_test.c
new file mode 100644
index 0000000..d2153c2
--- /dev/null
+++ b/tools/tools/usbtest/usb_control_ep_test.c
@@ -0,0 +1,673 @@
+/* $FreeBSD$ */
+/*-
+ * Copyright (c) 2007-2010 Hans Petter Selasky. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <err.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <sys/sysctl.h>
+#include <sys/time.h>
+
+#include <libusb20.h>
+#include <libusb20_desc.h>
+
+#include <dev/usb/usb_endian.h>
+#include <dev/usb/usb.h>
+#include <dev/usb/usb_cdc.h>
+
+#include "usbtest.h"
+
+static void
+set_ctrl_ep_fail(int bus, int dev, int ds_fail, int ss_fail)
+{
+	int error;
+
+	error = sysctlbyname("hw.usb.ctrl_bus_fail", NULL, NULL,
+	    &bus, sizeof(bus));
+	if (error != 0)
+		goto emissing;
+
+	error = sysctlbyname("hw.usb.ctrl_dev_fail", NULL, NULL,
+	    &dev, sizeof(dev));
+	if (error != 0)
+		goto emissing;
+
+	error = sysctlbyname("hw.usb.ctrl_ds_fail", NULL, NULL,
+	    &ds_fail, sizeof(ds_fail));
+	if (error != 0)
+		goto emissing;
+
+	error = sysctlbyname("hw.usb.ctrl_ss_fail", NULL, NULL,
+	    &ss_fail, sizeof(ss_fail));
+	if (error != 0)
+		goto emissing;
+	return;
+
+emissing:
+	printf("Cannot set USB sysctl, missing USB_REQ_DEBUG option?\n");
+}
+
+void
+usb_control_ep_error_test(uint16_t vid, uint16_t pid)
+{
+	struct LIBUSB20_CONTROL_SETUP_DECODED req;
+	struct libusb20_device *pdev;
+	uint8_t buffer[256];
+	int error;
+	int fail = 0;
+	int bus;
+	int dev;
+	int cfg;
+
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+	error = libusb20_dev_open(pdev, 0);
+	if (error) {
+		printf("Could not open USB device\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+
+	bus = libusb20_dev_get_bus_number(pdev);
+	dev = libusb20_dev_get_address(pdev);
+
+	for (cfg = 0; cfg != 255; cfg++) {
+
+		LIBUSB20_INIT(LIBUSB20_CONTROL_SETUP, &req);
+		req.bmRequestType = 0x80; /* read */
+		req.bRequest = 0x06; /* descriptor */
+		req.wValue = 0x0200 | cfg; /* config descriptor */
+		req.wIndex = 0;
+		req.wLength = 255;
+
+		printf("Test #%d.1/3 ...\n", cfg);
+
+		set_ctrl_ep_fail(-1,-1,0,0);
+
+		error = libusb20_dev_request_sync(pdev, &req, buffer,
+		    NULL, 1000, 0);
+		if (error != 0) {
+			printf("Last configuration index is: %d\n", cfg - 1);
+			break;
+		}
+
+		printf("Test #%d.2/3 ...\n", cfg);
+
+		set_ctrl_ep_fail(bus,dev,1,1);
+
+		error = libusb20_dev_request_sync(pdev, &req, buffer,
+		    NULL, 1000, 0);
+
+		set_ctrl_ep_fail(-1,-1,0,0);
+
+		error = libusb20_dev_request_sync(pdev, &req, buffer,
+		    NULL, 1000, 0);
+		if (error != 0) {
+			printf("Cannot fetch descriptor (unexpected)\n");
+			fail++;
+		}
+
+		printf("Test #%d.3/3 ...\n", cfg);
+
+		set_ctrl_ep_fail(bus,dev,0,1);
+
+		error = libusb20_dev_request_sync(pdev, &req, buffer,
+		    NULL, 1000, 0);
+
+		set_ctrl_ep_fail(-1,-1,0,0);
+
+		error = libusb20_dev_request_sync(pdev, &req, buffer,
+		    NULL, 1000, 0);
+		if (error != 0) {
+			printf("Cannot fetch descriptor (unexpected)\n");
+			fail++;
+		}
+	}
+
+	libusb20_dev_close(pdev);
+	libusb20_dev_free(pdev);
+
+	printf("Test completed detecting %d failures\nDone\n\n", fail);
+}
+
+void
+usb_get_string_desc_test(uint16_t vid, uint16_t pid)
+{
+	struct libusb20_device *pdev;
+	uint32_t x;
+	uint32_t y;
+	uint32_t valid;
+	uint8_t *buf;
+	int error;
+
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+	error = libusb20_dev_open(pdev, 0);
+	if (error) {
+		printf("Could not open USB device\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	buf = malloc(256);
+	if (buf == NULL) {
+		printf("Cannot allocate memory\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	valid = 0;
+
+	printf("Starting string descriptor test for "
+	    "VID=0x%04x PID=0x%04x\n", vid, pid);
+
+	for (x = 0; x != 256; x++) {
+
+		if (libusb20_dev_check_connected(pdev) != 0) {
+			printf("Device disconnected\n");
+			break;
+		}
+		printf("%d .. ", (int)x);
+
+		fflush(stdout);
+
+		error = libusb20_dev_req_string_simple_sync(pdev, x, buf, 255);
+
+		if (error == 0) {
+			printf("\nINDEX=%d, STRING='%s' (Default language)\n", (int)x, buf);
+			fflush(stdout);
+		} else {
+			continue;
+		}
+
+		valid = 0;
+
+		for (y = 0; y != 65536; y++) {
+
+			if (libusb20_dev_check_connected(pdev) != 0) {
+				printf("Device disconnected\n");
+				break;
+			}
+			error = libusb20_dev_req_string_sync(pdev, x, y, buf, 256);
+			if (error == 0)
+				valid++;
+		}
+
+		printf("String at INDEX=%d responds to %d "
+		    "languages\n", (int)x, (int)valid);
+	}
+
+	printf("\nDone\n");
+
+	free(buf);
+
+	libusb20_dev_free(pdev);
+}
+
+void
+usb_port_reset_test(uint16_t vid, uint16_t pid, uint32_t duration)
+{
+	struct timeval sub_tv;
+	struct timeval ref_tv;
+	struct timeval res_tv;
+
+	struct libusb20_device *pdev;
+
+	int error;
+	int iter;
+	int errcnt;
+
+	time_t last_sec;
+
+	/* sysctl() - no set config */
+
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+	error = libusb20_dev_open(pdev, 0);
+	if (error) {
+		libusb20_dev_free(pdev);
+		printf("Could not open USB device\n");
+		return;
+	}
+	iter = 0;
+
+	errcnt = 0;
+
+	gettimeofday(&ref_tv, 0);
+
+	last_sec = ref_tv.tv_sec;
+
+	while (1) {
+
+		gettimeofday(&sub_tv, 0);
+
+		if (last_sec != sub_tv.tv_sec) {
+
+			printf("STATUS: ID=%u, ERR=%u\n",
+			    (int)iter, (int)errcnt);
+
+			fflush(stdout);
+
+			last_sec = sub_tv.tv_sec;
+		}
+		timersub(&sub_tv, &ref_tv, &res_tv);
+
+		if ((res_tv.tv_sec < 0) || (res_tv.tv_sec >= duration))
+			break;
+
+		if (libusb20_dev_reset(pdev)) {
+			errcnt++;
+			usleep(50000);
+		}
+		if (libusb20_dev_check_connected(pdev) != 0) {
+			printf("Device disconnected\n");
+			break;
+		}
+		iter++;
+	}
+
+	libusb20_dev_reset(pdev);
+
+	libusb20_dev_free(pdev);
+}
+
+void
+usb_set_config_test(uint16_t vid, uint16_t pid, uint32_t duration)
+{
+	struct libusb20_device *pdev;
+	struct LIBUSB20_DEVICE_DESC_DECODED *ddesc;
+	int x;
+	int error;
+	int failed;
+	int exp;
+
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+	error = libusb20_dev_open(pdev, 0);
+	if (error) {
+		printf("Could not open USB device\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	failed = 0;
+
+	printf("Starting set config test for "
+	    "VID=0x%04x PID=0x%04x\n", vid, pid);
+
+	for (x = 255; x > -1; x--) {
+
+		error = libusb20_dev_set_config_index(pdev, x);
+		if (error == 0) {
+			if (x == 255) {
+				printf("Unconfiguring USB device "
+				    "was successful\n");
+			} else {
+				printf("Setting configuration %d "
+				    "was successful\n", x);
+			}
+		} else {
+			failed++;
+		}
+	}
+
+	ddesc = libusb20_dev_get_device_desc(pdev);
+	if (ddesc != NULL)
+		exp = ddesc->bNumConfigurations + 1;
+	else
+		exp = 1;
+
+	printf("\n\n"
+	    "Set configuration summary\n"
+	    "Valid count:  %d/%d %s\n"
+	    "Failed count: %d\n",
+	    256 - failed, exp,
+	    (exp == (256 - failed)) ? "(expected)" : "(unexpected)",
+	    failed);
+
+	libusb20_dev_free(pdev);
+}
+
+void
+usb_get_descriptor_test(uint16_t vid, uint16_t pid, uint32_t duration)
+{
+	struct libusb20_device *pdev;
+
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+	libusb20_dev_free(pdev);
+}
+
+void
+usb_suspend_resume_test(uint16_t vid, uint16_t pid, uint32_t duration)
+{
+	struct timeval sub_tv;
+	struct timeval ref_tv;
+	struct timeval res_tv;
+
+	struct libusb20_device *pdev;
+
+	time_t last_sec;
+
+	int iter;
+	int error;
+	int ptimo;
+	int errcnt;
+	int power_old;
+
+	ptimo = 1;			/* second(s) */
+
+	error = sysctlbyname("hw.usb.power_timeout", NULL, NULL,
+	    &ptimo, sizeof(ptimo));
+
+	if (error != 0) {
+		printf("WARNING: Could not set power "
+		    "timeout to 1 (error=%d) \n", errno);
+	}
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+	error = libusb20_dev_open(pdev, 0);
+	if (error) {
+		printf("Could not open USB device\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	power_old = libusb20_dev_get_power_mode(pdev);
+
+	printf("Starting suspend and resume "
+	    "test for VID=0x%04x PID=0x%04x\n", vid, pid);
+
+	iter = 0;
+	errcnt = 0;
+
+	gettimeofday(&ref_tv, 0);
+
+	last_sec = ref_tv.tv_sec;
+
+	while (1) {
+
+		if (libusb20_dev_check_connected(pdev) != 0) {
+			printf("Device disconnected\n");
+			break;
+		}
+		gettimeofday(&sub_tv, 0);
+
+		if (last_sec != sub_tv.tv_sec) {
+
+			printf("STATUS: ID=%u, ERR=%u\n",
+			    (int)iter, (int)errcnt);
+
+			fflush(stdout);
+
+			last_sec = sub_tv.tv_sec;
+		}
+		timersub(&sub_tv, &ref_tv, &res_tv);
+
+		if ((res_tv.tv_sec < 0) || (res_tv.tv_sec >= duration))
+			break;
+
+		error = libusb20_dev_set_power_mode(pdev, (iter & 1) ?
+		    LIBUSB20_POWER_ON : LIBUSB20_POWER_SAVE);
+
+		if (error)
+			errcnt++;
+
+		/* wait before switching power mode */
+		usleep(4100000 +
+		    (((uint32_t)usb_ts_rand_noise()) % 2000000U));
+
+		iter++;
+	}
+
+	/* restore default power mode */
+	libusb20_dev_set_power_mode(pdev, power_old);
+
+	libusb20_dev_free(pdev);
+}
+
+void
+usb_set_and_clear_stall_test(uint16_t vid, uint16_t pid)
+{
+	struct libusb20_device *pdev;
+	struct libusb20_transfer *pxfer;
+
+	int iter;
+	int error;
+	int errcnt;
+	int ep;
+
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+	error = libusb20_dev_open(pdev, 1);
+	if (error) {
+		printf("Could not open USB device\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	printf("Starting set and clear stall test "
+	    "for VID=0x%04x PID=0x%04x\n", vid, pid);
+
+	iter = 0;
+	errcnt = 0;
+
+	for (ep = 2; ep != 32; ep++) {
+
+		struct LIBUSB20_CONTROL_SETUP_DECODED setup_set_stall;
+		struct LIBUSB20_CONTROL_SETUP_DECODED setup_get_status;
+
+		uint8_t epno = ((ep / 2) | ((ep & 1) << 7));
+		uint8_t buf[1];
+
+		LIBUSB20_INIT(LIBUSB20_CONTROL_SETUP, &setup_set_stall);
+		setup_set_stall.bmRequestType = 0x02;	/* write endpoint */
+		setup_set_stall.bRequest = 0x03;	/* set feature */
+		setup_set_stall.wValue = 0x00;	/* UF_ENDPOINT_HALT */
+		setup_set_stall.wIndex = epno;
+		setup_set_stall.wLength = 0;
+
+		LIBUSB20_INIT(LIBUSB20_CONTROL_SETUP, &setup_get_status);
+		setup_get_status.bmRequestType = 0x82;	/* read endpoint */
+		setup_get_status.bRequest = 0x00;	/* get status */
+		setup_get_status.wValue = 0x00;
+		setup_get_status.wIndex = epno;
+		setup_get_status.wLength = 1;
+
+		if (libusb20_dev_check_connected(pdev) != 0) {
+			printf("Device disconnected\n");
+			break;
+		}
+		pxfer = libusb20_tr_get_pointer(pdev, 0);
+
+		error = libusb20_tr_open(pxfer, 1, 1, epno);
+
+		if (error != 0) {
+			printf("Endpoint 0x%02x does not exist "
+			    "in current setting. (%s, ignored)\n",
+			    epno, libusb20_strerror(error));
+			continue;
+		}
+		printf("Stalling endpoint 0x%02x\n", epno);
+
+		/* set stall */
+		error = libusb20_dev_request_sync(pdev,
+		    &setup_set_stall, NULL, NULL, 250, 0);
+
+		if (error != 0) {
+			printf("Endpoint 0x%02x does not allow "
+			    "setting of stall. (%s)\n",
+			    epno, libusb20_strerror(error));
+			errcnt++;
+		}
+		/* get EP status */
+		buf[0] = 0;
+		error = libusb20_dev_request_sync(pdev,
+		    &setup_get_status, buf, NULL, 250, 0);
+
+		if (error != 0) {
+			printf("Endpoint 0x%02x does not allow "
+			    "reading status. (%s)\n",
+			    epno, libusb20_strerror(error));
+			errcnt++;
+		} else {
+			if (!(buf[0] & 1)) {
+				printf("Endpoint 0x%02x status is "
+				    "not set to stalled\n", epno);
+				errcnt++;
+			}
+		}
+
+		buf[0] = 0;
+		error = libusb20_tr_bulk_intr_sync(pxfer, buf, 1, NULL, 250);
+		if (error != LIBUSB20_TRANSFER_STALL) {
+			printf("Endpoint 0x%02x does not appear to "
+			    "have stalled. Missing stall PID!\n", epno);
+			errcnt++;
+		}
+		printf("Unstalling endpoint 0x%02x\n", epno);
+
+		libusb20_tr_clear_stall_sync(pxfer);
+
+		/* get EP status */
+		buf[0] = 0;
+		error = libusb20_dev_request_sync(pdev,
+		    &setup_get_status, buf, NULL, 250, 0);
+
+		if (error != 0) {
+			printf("Endpoint 0x%02x does not allow "
+			    "reading status. (%s)\n",
+			    epno, libusb20_strerror(error));
+			errcnt++;
+		} else {
+			if (buf[0] & 1) {
+				printf("Endpoint 0x%02x status is "
+				    "still stalled\n", epno);
+				errcnt++;
+			}
+		}
+
+		libusb20_tr_close(pxfer);
+		iter++;
+	}
+
+	libusb20_dev_free(pdev);
+
+	printf("\n"
+	    "Test summary\n"
+	    "============\n"
+	    "Endpoints tested: %d\n"
+	    "Errors: %d\n", iter, errcnt);
+}
+
+void
+usb_set_alt_interface_test(uint16_t vid, uint16_t pid)
+{
+	struct libusb20_device *pdev;
+	struct libusb20_config *config;
+
+	int iter;
+	int error;
+	int errcnt;
+	int n;
+	int m;
+
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+	printf("Starting set alternate setting test "
+	    "for VID=0x%04x PID=0x%04x\n", vid, pid);
+
+	config = libusb20_dev_alloc_config(pdev,
+	    libusb20_dev_get_config_index(pdev));
+	if (config == NULL) {
+		printf("Could not get configuration descriptor\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	iter = 0;
+	errcnt = 0;
+
+	for (n = 0; n != config->num_interface; n++) {
+		/* detach kernel driver */
+		libusb20_dev_detach_kernel_driver(pdev, n);
+
+		error = libusb20_dev_open(pdev, 0);
+		if (error)
+			printf("ERROR could not open device\n");
+
+		/* Try the alternate settings */
+		for (m = 0; m != config->interface[n].num_altsetting; m++) {
+
+			iter++;
+
+			if (libusb20_dev_set_alt_index(pdev, n, m + 1)) {
+				printf("ERROR on interface %d alt %d\n", n, m + 1);
+				errcnt++;
+			}
+		}
+
+		/* Restore to default */
+
+		iter++;
+
+		if (libusb20_dev_set_alt_index(pdev, n, 0)) {
+			printf("ERROR on interface %d alt %d\n", n, 0);
+			errcnt++;
+		}
+		libusb20_dev_close(pdev);
+	}
+
+	libusb20_dev_free(pdev);
+
+	printf("\n"
+	    "Test summary\n"
+	    "============\n"
+	    "Interfaces tested: %d\n"
+	    "Errors: %d\n", iter, errcnt);
+}
diff --git a/tools/tools/usbtest/usb_modem_test.c b/tools/tools/usbtest/usb_modem_test.c
new file mode 100644
index 0000000..5aec956
--- /dev/null
+++ b/tools/tools/usbtest/usb_modem_test.c
@@ -0,0 +1,585 @@
+/* $FreeBSD$ */
+/*-
+ * Copyright (c) 2007-2010 Hans Petter Selasky. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <err.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <sys/sysctl.h>
+#include <sys/time.h>
+
+#include <libusb20.h>
+#include <libusb20_desc.h>
+
+#include <dev/usb/usb_endian.h>
+#include <dev/usb/usb.h>
+#include <dev/usb/usb_cdc.h>
+
+#include "usbtest.h"
+
+static struct modem {
+	struct libusb20_transfer *xfer_in;
+	struct libusb20_transfer *xfer_out;
+	struct libusb20_device *usb_dev;
+
+	struct bps rx_bytes;
+	struct bps tx_bytes;
+	uint32_t c0;
+	uint32_t c1;
+	uint32_t out_state;
+	uint32_t in_last;
+	uint32_t in_synced;
+	uint32_t duration;
+	uint32_t errors;
+
+	uint8_t use_vendor_specific;
+	uint8_t	loop_data;
+	uint8_t	modem_at_mode;
+	uint8_t	data_stress_test;
+	uint8_t	control_ep_test;
+	uint8_t	usb_iface;
+	uint8_t	random_tx_length;
+	uint8_t	random_tx_delay;
+
+}	modem;
+
+static void
+set_defaults(struct modem *p)
+{
+	memset(p, 0, sizeof(*p));
+
+	p->data_stress_test = 1;
+	p->control_ep_test = 1;
+	p->duration = 60;		/* seconds */
+}
+
+void
+do_bps(const char *desc, struct bps *bps, uint32_t len)
+{
+	bps->bytes += len;
+}
+
+static void
+modem_out_state(uint8_t *buf)
+{
+	if (modem.modem_at_mode) {
+		switch (modem.out_state & 3) {
+		case 0:
+			*buf = 'A';
+			break;
+		case 1:
+			*buf = 'T';
+			break;
+		case 2:
+			*buf = '\r';
+			break;
+		default:
+			*buf = '\n';
+			modem.c0++;
+			break;
+		}
+		modem.out_state++;
+	} else {
+		*buf = modem.out_state;
+		modem.out_state++;
+		modem.out_state %= 255;
+	}
+}
+
+static void
+modem_in_state(uint8_t buf, uint32_t counter)
+{
+	if ((modem.in_last == 'O') && (buf == 'K')) {
+		modem.c1++;
+		modem.in_last = buf;
+	} else if (buf == modem.in_last) {
+		modem.c1++;
+		modem.in_last++;
+		modem.in_last %= 255;
+		if (modem.in_synced == 0) {
+			if (modem.errors < 64) {
+				printf("Got sync\n");
+			}
+			modem.in_synced = 1;
+		}
+	} else {
+		if (modem.in_synced) {
+			if (modem.errors < 64) {
+				printf("Lost sync @ %d, 0x%02x != 0x%02x\n",
+				    counter % 512, buf, modem.in_last);
+			}
+			modem.in_synced = 0;
+			modem.errors++;
+		}
+		modem.in_last = buf;
+		modem.in_last++;
+		modem.in_last %= 255;
+	}
+}
+
+static void
+modem_write(uint8_t *buf, uint32_t len)
+{
+	uint32_t n;
+
+	for (n = 0; n != len; n++) {
+		modem_out_state(buf + n);
+	}
+
+	do_bps("transmitted", &modem.tx_bytes, len);
+}
+
+static void
+modem_read(uint8_t *buf, uint32_t len)
+{
+	uint32_t n;
+
+	for (n = 0; n != len; n++) {
+		modem_in_state(buf[n], n);
+	}
+
+	do_bps("received", &modem.rx_bytes, len);
+}
+
+static void
+usb_modem_control_ep_test(struct modem *p, uint32_t duration, uint8_t flag)
+{
+	struct timeval sub_tv;
+	struct timeval ref_tv;
+	struct timeval res_tv;
+	struct LIBUSB20_CONTROL_SETUP_DECODED setup;
+	struct usb_cdc_abstract_state ast;
+	struct usb_cdc_line_state ls;
+	uint16_t feature = UCDC_ABSTRACT_STATE;
+	uint16_t state = UCDC_DATA_MULTIPLEXED;
+	uint8_t iface_no;
+	uint8_t buf[4];
+	int id = 0;
+	int iter = 0;
+
+	time_t last_sec;
+
+	iface_no = p->usb_iface - 1;
+
+	gettimeofday(&ref_tv, 0);
+
+	last_sec = ref_tv.tv_sec;
+
+	printf("\nTest=%d\n", (int)flag);
+
+	while (1) {
+
+		gettimeofday(&sub_tv, 0);
+
+		if (last_sec != sub_tv.tv_sec) {
+
+			printf("STATUS: ID=%u, COUNT=%u tests/sec ERR=%u\n",
+			    (int)id,
+			    (int)iter,
+			    (int)p->errors);
+
+			fflush(stdout);
+
+			last_sec = sub_tv.tv_sec;
+
+			id++;
+
+			iter = 0;
+		}
+		timersub(&sub_tv, &ref_tv, &res_tv);
+
+		if ((res_tv.tv_sec < 0) || (res_tv.tv_sec >= duration))
+			break;
+
+		LIBUSB20_INIT(LIBUSB20_CONTROL_SETUP, &setup);
+
+		if (flag & 1) {
+			setup.bmRequestType = UT_READ_CLASS_INTERFACE;
+			setup.bRequest = 0x03;
+			setup.wValue = 0x0001;
+			setup.wIndex = iface_no;
+			setup.wLength = 0x0002;
+
+			if (libusb20_dev_request_sync(p->usb_dev, &setup, buf, NULL, 250, 0)) {
+				p->errors++;
+			}
+		}
+		if (flag & 2) {
+			setup.bmRequestType = UT_WRITE_CLASS_INTERFACE;
+			setup.bRequest = UCDC_SET_COMM_FEATURE;
+			setup.wValue = feature;
+			setup.wIndex = iface_no;
+			setup.wLength = UCDC_ABSTRACT_STATE_LENGTH;
+			USETW(ast.wState, state);
+
+			if (libusb20_dev_request_sync(p->usb_dev, &setup, &ast, NULL, 250, 0)) {
+				p->errors++;
+			}
+		}
+		if (flag & 4) {
+			USETDW(ls.dwDTERate, 115200);
+			ls.bCharFormat = UCDC_STOP_BIT_1;
+			ls.bParityType = UCDC_PARITY_NONE;
+			ls.bDataBits = 8;
+
+			setup.bmRequestType = UT_WRITE_CLASS_INTERFACE;
+			setup.bRequest = UCDC_SET_LINE_CODING;
+			setup.wValue = 0;
+			setup.wIndex = iface_no;
+			setup.wLength = sizeof(ls);
+
+			if (libusb20_dev_request_sync(p->usb_dev, &setup, &ls, NULL, 250, 0)) {
+				p->errors++;
+			}
+		}
+		iter++;
+	}
+
+	printf("\nModem control endpoint test done!\n");
+}
+
+static void
+usb_modem_data_stress_test(struct modem *p, uint32_t duration)
+{
+	struct timeval sub_tv;
+	struct timeval ref_tv;
+	struct timeval res_tv;
+
+	time_t last_sec;
+
+	uint8_t in_pending = 0;
+	uint8_t in_ready = 0;
+	uint8_t out_pending = 0;
+
+	uint32_t id = 0;
+
+	uint32_t in_max;
+	uint32_t out_max;
+	uint32_t io_max;
+
+	uint8_t *in_buffer = 0;
+	uint8_t *out_buffer = 0;
+
+	gettimeofday(&ref_tv, 0);
+
+	last_sec = ref_tv.tv_sec;
+
+	printf("\n");
+
+	in_max = libusb20_tr_get_max_total_length(p->xfer_in);
+	out_max = libusb20_tr_get_max_total_length(p->xfer_out);
+
+	/* get the smallest buffer size and use that */
+	io_max = (in_max < out_max) ? in_max : out_max;
+
+	if (in_max != out_max)
+		printf("WARNING: Buffer sizes are un-equal: %u vs %u\n", in_max, out_max);
+
+	in_buffer = malloc(io_max);
+	if (in_buffer == NULL)
+		goto fail;
+
+	out_buffer = malloc(io_max);
+	if (out_buffer == NULL)
+		goto fail;
+
+	while (1) {
+
+		gettimeofday(&sub_tv, 0);
+
+		if (last_sec != sub_tv.tv_sec) {
+
+			printf("STATUS: ID=%u, RX=%u bytes/sec, TX=%u bytes/sec, ERR=%d\n",
+			    (int)id,
+			    (int)p->rx_bytes.bytes,
+			    (int)p->tx_bytes.bytes,
+			    (int)p->errors);
+
+			p->rx_bytes.bytes = 0;
+			p->tx_bytes.bytes = 0;
+
+			fflush(stdout);
+
+			last_sec = sub_tv.tv_sec;
+
+			id++;
+		}
+		timersub(&sub_tv, &ref_tv, &res_tv);
+
+		if ((res_tv.tv_sec < 0) || (res_tv.tv_sec >= duration))
+			break;
+
+		libusb20_dev_process(p->usb_dev);
+
+		if (!libusb20_tr_pending(p->xfer_in)) {
+			if (in_pending) {
+				if (libusb20_tr_get_status(p->xfer_in) == 0) {
+					modem_read(in_buffer, libusb20_tr_get_length(p->xfer_in, 0));
+				} else {
+					p->errors++;
+					usleep(10000);
+				}
+				in_pending = 0;
+				in_ready = 1;
+			}
+			if (p->loop_data == 0) {
+				libusb20_tr_setup_bulk(p->xfer_in, in_buffer, io_max, 0);
+				libusb20_tr_start(p->xfer_in);
+				in_pending = 1;
+				in_ready = 0;
+			}
+		}
+		if (!libusb20_tr_pending(p->xfer_out)) {
+
+			uint32_t len;
+			uint32_t dly;
+
+			if (out_pending) {
+				if (libusb20_tr_get_status(p->xfer_out) != 0) {
+					p->errors++;
+					usleep(10000);
+				}
+			}
+			if (p->random_tx_length) {
+				len = ((uint32_t)usb_ts_rand_noise()) % ((uint32_t)io_max);
+			} else {
+				len = io_max;
+			}
+
+			if (p->random_tx_delay) {
+				dly = ((uint32_t)usb_ts_rand_noise()) % 16000U;
+			} else {
+				dly = 0;
+			}
+
+			if (p->loop_data != 0) {
+				if (in_ready != 0) {
+					len = libusb20_tr_get_length(p->xfer_in, 0);
+					memcpy(out_buffer, in_buffer, len);
+					in_ready = 0;
+				} else {
+					len = io_max + 1;
+				}
+				if (!libusb20_tr_pending(p->xfer_in)) {
+					libusb20_tr_setup_bulk(p->xfer_in, in_buffer, io_max, 0);
+					libusb20_tr_start(p->xfer_in);
+					in_pending = 1;
+				}
+			} else {
+				modem_write(out_buffer, len);
+			}
+
+			if (len <= io_max) {
+				libusb20_tr_setup_bulk(p->xfer_out, out_buffer, len, 0);
+
+				if (dly != 0)
+					usleep(dly);
+
+				libusb20_tr_start(p->xfer_out);
+
+				out_pending = 1;
+			}
+		}
+		libusb20_dev_wait_process(p->usb_dev, 500);
+
+		if (libusb20_dev_check_connected(p->usb_dev) != 0) {
+			printf("Device disconnected\n");
+			break;
+		}
+	}
+
+	libusb20_tr_stop(p->xfer_in);
+	libusb20_tr_stop(p->xfer_out);
+
+	printf("\nData stress test done!\n");
+
+fail:
+	if (in_buffer)
+		free(in_buffer);
+	if (out_buffer)
+		free(out_buffer);
+}
+
+static void
+exec_host_modem_test(struct modem *p, uint16_t vid, uint16_t pid)
+{
+	struct libusb20_device *pdev;
+
+	uint8_t ntest = 0;
+	uint8_t x;
+	uint8_t in_ep;
+	uint8_t out_ep;
+	uint8_t iface;
+
+	int error;
+
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+
+	if (p->use_vendor_specific)
+		find_usb_endpoints(pdev, 255, 255, 255, 0, &iface, &in_ep, &out_ep, 0);
+	else
+		find_usb_endpoints(pdev, 2, 2, 1, 0, &iface, &in_ep, &out_ep, 1);
+
+	if ((in_ep == 0) || (out_ep == 0)) {
+		printf("Could not find USB endpoints\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	printf("Attaching to: %s @ iface %d\n",
+	    libusb20_dev_get_desc(pdev), iface);
+
+	if (libusb20_dev_open(pdev, 2)) {
+		printf("Could not open USB device\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	if (libusb20_dev_detach_kernel_driver(pdev, iface)) {
+		printf("WARNING: Could not detach kernel driver\n");
+	}
+	p->xfer_in = libusb20_tr_get_pointer(pdev, 0);
+	error = libusb20_tr_open(p->xfer_in, 65536 / 4, 1, in_ep);
+	if (error) {
+		printf("Could not open USB endpoint %d\n", in_ep);
+		libusb20_dev_free(pdev);
+		return;
+	}
+	p->xfer_out = libusb20_tr_get_pointer(pdev, 1);
+	error = libusb20_tr_open(p->xfer_out, 65536 / 4, 1, out_ep);
+	if (error) {
+		printf("Could not open USB endpoint %d\n", out_ep);
+		libusb20_dev_free(pdev);
+		return;
+	}
+	p->usb_dev = pdev;
+	p->usb_iface = iface;
+	p->errors = 0;
+
+	if (p->control_ep_test)
+		ntest += 7;
+
+	if (p->data_stress_test)
+		ntest += 1;
+
+	if (ntest == 0) {
+		printf("No tests selected\n");
+	} else {
+
+		if (p->control_ep_test) {
+			for (x = 1; x != 8; x++) {
+				usb_modem_control_ep_test(p,
+				    (p->duration + ntest - 1) / ntest, x);
+			}
+		}
+		if (p->data_stress_test) {
+			usb_modem_data_stress_test(p,
+			    (p->duration + ntest - 1) / ntest);
+		}
+	}
+
+	printf("\nDone\n");
+
+	libusb20_dev_free(pdev);
+}
+
+void
+show_host_modem_test(uint8_t level, uint16_t vid, uint16_t pid, uint32_t duration)
+{
+	uint8_t retval;
+
+	set_defaults(&modem);
+
+	modem.duration = duration;
+
+	while (1) {
+
+		retval = usb_ts_show_menu(level, "Modem Test Parameters",
+		    " 1) Execute Data Stress Test: <%s>\n"
+		    " 2) Execute Modem Control Endpoint Test: <%s>\n"
+		    " 3) Use random transmit length: <%s>\n"
+		    " 4) Use random transmit delay: <%s> ms\n"
+		    " 5) Use vendor specific interface: <%s>\n"
+		    "10) Loop data: <%s>\n"
+		    "13) Set test duration: <%d> seconds\n"
+		    "20) Reset parameters\n"
+		    "30) Start test (VID=0x%04x, PID=0x%04x)\n"
+		    "40) Select another device\n"
+		    " x) Return to previous menu \n",
+		    (modem.data_stress_test ? "YES" : "NO"),
+		    (modem.control_ep_test ? "YES" : "NO"),
+		    (modem.random_tx_length ? "YES" : "NO"),
+		    (modem.random_tx_delay ? "16" : "0"),
+		    (modem.use_vendor_specific ? "YES" : "NO"),
+		    (modem.loop_data ? "YES" : "NO"),
+		    (int)(modem.duration),
+		    (int)vid, (int)pid);
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			modem.data_stress_test ^= 1;
+			break;
+		case 2:
+			modem.control_ep_test ^= 1;
+			break;
+		case 3:
+			modem.random_tx_length ^= 1;
+			break;
+		case 4:
+			modem.random_tx_delay ^= 1;
+			break;
+		case 5:
+			modem.use_vendor_specific ^= 1;
+			modem.control_ep_test = 0;
+			break;
+		case 10:
+			modem.loop_data ^= 1;
+			break;
+		case 13:
+			modem.duration = get_integer();
+			break;
+		case 20:
+			set_defaults(&modem);
+			break;
+		case 30:
+			exec_host_modem_test(&modem, vid, pid);
+			break;
+		case 40:
+			show_host_device_selection(level + 1, &vid, &pid);
+			break;
+		default:
+			return;
+		}
+	}
+}
diff --git a/tools/tools/usbtest/usb_msc_test.c b/tools/tools/usbtest/usb_msc_test.c
new file mode 100644
index 0000000..5ece858
--- /dev/null
+++ b/tools/tools/usbtest/usb_msc_test.c
@@ -0,0 +1,1290 @@
+/* $FreeBSD$ */
+/*-
+ * Copyright (c) 2007-2012 Hans Petter Selasky. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <err.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <sys/sysctl.h>
+#include <sys/time.h>
+
+#include <libusb20.h>
+#include <libusb20_desc.h>
+
+#include <dev/usb/usb_endian.h>
+
+#include "usbtest.h"
+
+#include "usb_msc_test.h"
+
+/* Command Block Wrapper */
+typedef struct {
+	uDWord	dCBWSignature;
+#define	CBWSIGNATURE	0x43425355
+	uDWord	dCBWTag;
+	uDWord	dCBWDataTransferLength;
+	uByte	bCBWFlags;
+#define	CBWFLAGS_OUT	0x00
+#define	CBWFLAGS_IN	0x80
+	uByte	bCBWLUN;
+	uByte	bCDBLength;
+#define	CBWCDBLENGTH	16
+	uByte	CBWCDB[CBWCDBLENGTH];
+} umass_bbb_cbw_t;
+
+#define	UMASS_BBB_CBW_SIZE	31
+
+/* Command Status Wrapper */
+typedef struct {
+	uDWord	dCSWSignature;
+#define	CSWSIGNATURE	0x53425355
+#define	CSWSIGNATURE_IMAGINATION_DBX1	0x43425355
+#define	CSWSIGNATURE_OLYMPUS_C1	0x55425355
+	uDWord	dCSWTag;
+	uDWord	dCSWDataResidue;
+	uByte	bCSWStatus;
+#define	CSWSTATUS_GOOD	0x0
+#define	CSWSTATUS_FAILED	0x1
+#define	CSWSTATUS_PHASE	0x2
+} umass_bbb_csw_t;
+
+#define	UMASS_BBB_CSW_SIZE	13
+
+#define	SC_READ_6			0x08
+#define	SC_READ_10			0x28
+#define	SC_READ_12			0xa8
+#define	SC_WRITE_6			0x0a
+#define	SC_WRITE_10			0x2a
+#define	SC_WRITE_12			0xaa
+
+static struct stats {
+	uint64_t xfer_error;
+	uint64_t xfer_success;
+	uint64_t xfer_reset;
+	uint64_t xfer_rx_bytes;
+	uint64_t xfer_tx_bytes;
+	uint64_t data_error;
+}	stats;
+
+static uint32_t xfer_current_id;
+static uint32_t xfer_wrapper_sig;
+static uint32_t block_size = 512;
+
+static struct libusb20_transfer *xfer_in;
+static struct libusb20_transfer *xfer_out;
+static struct libusb20_device *usb_pdev;
+static uint8_t usb_iface;
+static int sense_recurse;
+
+/*
+ * SCSI commands sniffed off the wire - LUN maybe needs to be
+ * adjusted!  Refer to "dev/usb/storage/ustorage_fs.c" for more
+ * information.
+ */
+static uint8_t mode_sense_6[0x6] = {0x1a, 0, 0x3f, 0, 0x0c};
+static uint8_t read_capacity[0xA] = {0x25,};
+static uint8_t request_sense[0xC] = {0x03, 0, 0, 0, 0x12};
+static uint8_t test_unit_ready[0x6] = {0};
+static uint8_t mode_page_inquiry[0x6] = {0x12, 1, 0x80, 0, 0xff, 0};
+static uint8_t request_invalid[0xC] = {0xEA, 0, 0, 0, 0};
+static uint8_t prevent_removal[0x6] = {0x1E, 0, 0, 0, 1};
+static uint8_t read_toc[0xA] = {0x43, 0x02, 0, 0, 0, 0xAA, 0, 0x0C};
+
+#define	TIMEOUT_FILTER(x) (x)
+
+static void usb_request_sense(uint8_t lun);
+
+static void
+do_msc_reset(uint8_t lun)
+{
+	struct LIBUSB20_CONTROL_SETUP_DECODED setup;
+
+	LIBUSB20_INIT(LIBUSB20_CONTROL_SETUP, &setup);
+
+	setup.bmRequestType = LIBUSB20_REQUEST_TYPE_CLASS |
+	    LIBUSB20_RECIPIENT_INTERFACE;
+	setup.bRequest = 0xFF;		/* BBB reset */
+	setup.wValue = 0;
+	setup.wIndex = usb_iface;
+	setup.wLength = 0;
+
+	if (libusb20_dev_request_sync(usb_pdev, &setup, NULL, NULL, 5000, 0)) {
+		printf("ERROR: %s\n", __FUNCTION__);
+		stats.xfer_error++;
+	}
+	libusb20_tr_clear_stall_sync(xfer_in);
+	libusb20_tr_clear_stall_sync(xfer_out);
+
+	stats.xfer_reset++;
+
+	usb_request_sense(lun);
+}
+
+static uint8_t
+do_msc_cmd(uint8_t *pcmd, uint8_t cmdlen, void *pdata, uint32_t datalen,
+    uint8_t isread, uint8_t isshort, uint8_t lun, uint8_t flags)
+{
+	umass_bbb_cbw_t cbw;
+	umass_bbb_csw_t csw;
+	struct libusb20_transfer *xfer_io;
+	uint32_t actlen;
+	uint32_t timeout;
+	int error;
+
+	memset(&cbw, 0, sizeof(cbw));
+
+	USETDW(cbw.dCBWSignature, xfer_wrapper_sig);
+	USETDW(cbw.dCBWTag, xfer_current_id);
+	xfer_current_id++;
+	USETDW(cbw.dCBWDataTransferLength, datalen);
+	cbw.bCBWFlags = (isread ? CBWFLAGS_IN : CBWFLAGS_OUT);
+	cbw.bCBWLUN = lun;
+	cbw.bCDBLength = cmdlen;
+	bcopy(pcmd, cbw.CBWCDB, cmdlen);
+
+	actlen = 0;
+
+	timeout = ((datalen + 299999) / 300000) * 1000;
+	timeout += 5000;
+
+	if ((error = libusb20_tr_bulk_intr_sync(xfer_out,
+	    &cbw, sizeof(cbw), &actlen, TIMEOUT_FILTER(1000)))) {
+		printf("ERROR: CBW reception: %d\n", error);
+		do_msc_reset(lun);
+		return (1);
+	}
+	if (actlen != sizeof(cbw)) {
+		printf("ERROR: CBW length: %d != %d\n",
+		    actlen, (int)sizeof(cbw));
+		do_msc_reset(lun);
+		return (1);
+	}
+	if (flags & 1)
+		datalen /= 2;
+
+	if (datalen != 0) {
+		xfer_io = isread ? xfer_in : xfer_out;
+
+		if ((error = libusb20_tr_bulk_intr_sync(xfer_io,
+		    pdata, datalen, &actlen, TIMEOUT_FILTER(timeout)))) {
+			printf("ERROR: Data transfer: %d\n", error);
+			do_msc_reset(lun);
+			return (1);
+		}
+		if ((actlen != datalen) && (!isshort)) {
+			printf("ERROR: Short data: %d of %d bytes\n",
+			    actlen, datalen);
+			do_msc_reset(lun);
+			return (1);
+		}
+	}
+	actlen = 0;
+	timeout = 8;
+
+	do {
+		error = libusb20_tr_bulk_intr_sync(xfer_in, &csw,
+		    sizeof(csw), &actlen, TIMEOUT_FILTER(1000));
+		if (error) {
+			if (error == LIBUSB20_TRANSFER_TIMED_OUT) {
+				printf("TIMEOUT: Trying to get CSW again. "
+				    "%d tries left.\n", timeout);
+			} else {
+				break;
+			}
+		} else {
+			break;
+		}
+	} while (--timeout);
+
+	if (error) {
+		libusb20_tr_clear_stall_sync(xfer_in);
+		error = libusb20_tr_bulk_intr_sync(xfer_in, &csw,
+		    sizeof(csw), &actlen, TIMEOUT_FILTER(1000));
+		if (error) {
+			libusb20_tr_clear_stall_sync(xfer_in);
+			printf("ERROR: Could not read CSW: Stalled or "
+			    "timeout (%d).\n", error);
+			do_msc_reset(lun);
+			return (1);
+		}
+	}
+	if (UGETDW(csw.dCSWSignature) != CSWSIGNATURE) {
+		printf("ERROR: Wrong CSW signature\n");
+		do_msc_reset(lun);
+		return (1);
+	}
+	if (actlen != sizeof(csw)) {
+		printf("ERROR: Wrong CSW length: %d != %d\n",
+		    actlen, (int)sizeof(csw));
+		do_msc_reset(lun);
+		return (1);
+	}
+	if (csw.bCSWStatus != 0) {
+		printf("ERROR: CSW status: %d\n", (int)csw.bCSWStatus);
+		return (1);
+	} else {
+		stats.xfer_success++;
+		return (0);
+	}
+}
+
+static uint8_t
+do_read_10(uint32_t lba, uint32_t len, void *buf, uint8_t lun)
+{
+	static uint8_t cmd[10];
+	uint8_t retval;
+
+	cmd[0] = SC_READ_10;
+
+	len /= block_size;
+
+	cmd[2] = lba >> 24;
+	cmd[3] = lba >> 16;
+	cmd[4] = lba >> 8;
+	cmd[5] = lba >> 0;
+
+	cmd[7] = len >> 8;
+	cmd[8] = len;
+
+	retval = do_msc_cmd(cmd, 10, buf, len * block_size, 1, 0, lun, 0);
+
+	if (retval) {
+		printf("ERROR: %s\n", __FUNCTION__);
+		stats.xfer_error++;
+	}
+	return (retval);
+}
+
+static uint8_t
+do_write_10(uint32_t lba, uint32_t len, void *buf, uint8_t lun)
+{
+	static uint8_t cmd[10];
+	uint8_t retval;
+	uint8_t abort;
+
+	cmd[0] = SC_WRITE_10;
+
+	abort = len & 1;
+
+	len /= block_size;
+
+	cmd[2] = lba >> 24;
+	cmd[3] = lba >> 16;
+	cmd[4] = lba >> 8;
+	cmd[5] = lba >> 0;
+
+	cmd[7] = len >> 8;
+	cmd[8] = len;
+
+	retval = do_msc_cmd(cmd, 10, buf, (len * block_size), 0, 0, lun, abort);
+
+	if (retval) {
+		printf("ERROR: %s\n", __FUNCTION__);
+		stats.xfer_error++;
+	}
+	return (retval);
+}
+
+static void
+do_io_test(struct usb_msc_params *p, uint8_t lun, uint32_t lba_max,
+    uint8_t *buffer, uint8_t *reference)
+{
+	uint32_t io_offset;
+	uint32_t io_size;
+	uint32_t temp;
+	uint8_t do_read;
+	uint8_t retval;
+
+	switch (p->io_mode) {
+	case USB_MSC_IO_MODE_WRITE_ONLY:
+		do_read = 0;
+		break;
+	case USB_MSC_IO_MODE_READ_WRITE:
+		do_read = (usb_ts_rand_noise() & 1);
+		break;
+	default:
+		do_read = 1;
+		break;
+	}
+
+	switch (p->io_offset) {
+	case USB_MSC_IO_OFF_RANDOM:
+		io_offset = usb_ts_rand_noise();
+		break;
+	default:
+		io_offset = 0;
+		break;
+	}
+
+	switch (p->io_delay) {
+	case USB_MSC_IO_DELAY_RANDOM_10MS:
+		usleep(((uint32_t)usb_ts_rand_noise()) % 10000U);
+		break;
+	case USB_MSC_IO_DELAY_RANDOM_100MS:
+		usleep(((uint32_t)usb_ts_rand_noise()) % 100000U);
+		break;
+	case USB_MSC_IO_DELAY_FIXED_10MS:
+		usleep(10000);
+		break;
+	case USB_MSC_IO_DELAY_FIXED_100MS:
+		usleep(100000);
+		break;
+	default:
+		break;
+	}
+
+	switch (p->io_size) {
+	case USB_MSC_IO_SIZE_RANDOM:
+		io_size = ((uint32_t)usb_ts_rand_noise()) & 65535U;
+		break;
+	case USB_MSC_IO_SIZE_INCREASING:
+		io_size = (xfer_current_id & 65535U);
+		break;
+	case USB_MSC_IO_SIZE_FIXED_1BLK:
+		io_size = 1;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_2BLK:
+		io_size = 2;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_4BLK:
+		io_size = 4;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_8BLK:
+		io_size = 8;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_16BLK:
+		io_size = 16;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_32BLK:
+		io_size = 32;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_64BLK:
+		io_size = 64;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_128BLK:
+		io_size = 128;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_256BLK:
+		io_size = 256;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_512BLK:
+		io_size = 512;
+		break;
+	case USB_MSC_IO_SIZE_FIXED_1024BLK:
+		io_size = 1024;
+		break;
+	default:
+		io_size = 1;
+		break;
+	}
+
+	if (io_size == 0)
+		io_size = 1;
+
+	io_offset %= lba_max;
+
+	temp = (lba_max - io_offset);
+
+	if (io_size > temp)
+		io_size = temp;
+
+	if (do_read) {
+		retval = do_read_10(io_offset, io_size * block_size,
+		    buffer + (io_offset * block_size), lun);
+
+		if (retval == 0) {
+			if (bcmp(buffer + (io_offset * block_size),
+			    reference + (io_offset * block_size),
+			    io_size * block_size)) {
+				printf("ERROR: Data comparison failure\n");
+				stats.data_error++;
+				retval = 1;
+			}
+		}
+		stats.xfer_rx_bytes += (io_size * block_size);
+
+	} else {
+
+		retval = do_write_10(io_offset, io_size * block_size,
+		    reference + (io_offset * block_size), lun);
+
+		stats.xfer_tx_bytes += (io_size * block_size);
+	}
+
+	if ((stats.xfer_error + stats.data_error +
+	    stats.xfer_reset) >= p->max_errors) {
+		printf("Maximum number of errors exceeded\n");
+		p->done = 1;
+	}
+}
+
+static void
+usb_request_sense(uint8_t lun)
+{
+	uint8_t dummy_buf[255];
+
+	if (sense_recurse)
+		return;
+
+	sense_recurse++;
+
+	do_msc_cmd(request_sense, sizeof(request_sense),
+	    dummy_buf, 255, 1, 1, lun, 0);
+
+	sense_recurse--;
+}
+
+static void
+usb_msc_test(struct usb_msc_params *p)
+{
+	struct stats last_stat;
+	struct timeval sub_tv;
+	struct timeval ref_tv;
+	struct timeval res_tv;
+	uint8_t *buffer = NULL;
+	uint8_t *reference = NULL;
+	uint32_t dummy_buf[65536 / 4];
+	uint32_t lba_max;
+	uint32_t x;
+	uint32_t y;
+	uint32_t capacity_lba;
+	uint32_t capacity_bs;
+	time_t last_sec;
+	uint8_t lun;
+	int tries;
+
+	memset(&last_stat, 0, sizeof(last_stat));
+
+	switch (p->io_lun) {
+	case USB_MSC_IO_LUN_0:
+		lun = 0;
+		break;
+	case USB_MSC_IO_LUN_1:
+		lun = 1;
+		break;
+	case USB_MSC_IO_LUN_2:
+		lun = 2;
+		break;
+	case USB_MSC_IO_LUN_3:
+		lun = 3;
+		break;
+	default:
+		lun = 0;
+		break;
+	}
+
+	p->done = 0;
+
+	sense_recurse = p->try_sense_on_error ? 0 : 1;
+
+	printf("Resetting device ...\n");
+
+	do_msc_reset(lun);
+
+	printf("Testing SCSI commands ...\n");
+
+	if (p->try_all_lun) {
+		printf("Requesting sense from LUN 0..255 ... ");
+		for (x = y = 0; x != 256; x++) {
+			if (do_msc_cmd(mode_sense_6, sizeof(mode_sense_6),
+			    dummy_buf, 255, 1, 1, x, 0))
+				y++;
+
+			if (libusb20_dev_check_connected(usb_pdev) != 0) {
+				printf(" disconnect ");
+				break;
+			}
+		}
+		printf("Passed=%d, Failed=%d\n", 256 - y, y);
+	}
+	do_msc_cmd(mode_sense_6, sizeof(mode_sense_6),
+	    dummy_buf, 255, 1, 1, lun, 0);
+	do_msc_cmd(request_sense, sizeof(request_sense),
+	    dummy_buf, 255, 1, 1, lun, 0);
+
+	for (tries = 0; tries != 4; tries++) {
+
+		memset(dummy_buf, 0, sizeof(dummy_buf));
+
+		if (do_msc_cmd(read_capacity, sizeof(read_capacity),
+		    dummy_buf, 255, 1, 1, lun, 0) != 0) {
+			printf("Cannot read disk capacity (%u / 4)\n", tries);
+			if (tries == 3)
+				return;
+			usleep(50000);
+			continue;
+		} else {
+			break;
+		}
+	}
+
+	capacity_lba = be32toh(dummy_buf[0]);
+	capacity_bs = be32toh(dummy_buf[1]);
+
+	printf("Disk reports a capacity of LBA=%u and BS=%u\n",
+	    capacity_lba, capacity_bs);
+
+	block_size = capacity_bs;
+
+	if (capacity_bs > 65535) {
+		printf("Blocksize is too big\n");
+		return;
+	}
+	if (capacity_bs < 1) {
+		printf("Blocksize is too small\n");
+		return;
+	}
+	if (capacity_bs != 512)
+		printf("INFO: Blocksize is not 512 bytes\n");
+
+	if (p->try_invalid_scsi_command) {
+		int status;
+
+		for (tries = 0; tries != 4; tries++) {
+
+			printf("Trying invalid SCSI command: ");
+
+			status = do_msc_cmd(request_invalid,
+			    sizeof(request_invalid), dummy_buf,
+			    255, 1, 1, lun, 0);
+
+			printf("Result%s as expected\n", status ? "" : " NOT");
+
+			usleep(50000);
+		}
+	}
+	if (p->try_invalid_wrapper_block) {
+		int status;
+
+		for (tries = 0; tries != 4; tries++) {
+
+			printf("Trying invalid USB wrapper block signature: ");
+
+			xfer_wrapper_sig = 0x55663322;
+
+			status = do_msc_cmd(read_capacity,
+			    sizeof(read_capacity), dummy_buf,
+			    255, 1, 1, lun, 0);
+
+			printf("Result%s as expected\n", status ? "" : " NOT");
+
+			xfer_wrapper_sig = CBWSIGNATURE;
+
+			usleep(50000);
+		}
+	}
+	do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
+	do_msc_cmd(read_capacity, sizeof(read_capacity), dummy_buf, 255, 1, 1, lun, 0);
+	do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
+	do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
+	do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
+	do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
+	do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
+	do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
+	do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
+	do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
+	do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
+	do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
+	do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
+	do_msc_cmd(read_capacity, sizeof(read_capacity), dummy_buf, 255, 1, 1, lun, 0);
+	do_msc_cmd(mode_page_inquiry, sizeof(mode_page_inquiry), dummy_buf, 255, 1, 1, lun, 0);
+	do_msc_cmd(mode_page_inquiry, sizeof(mode_page_inquiry), dummy_buf, 255, 1, 1, lun, 0);
+
+	if (do_msc_cmd(prevent_removal, sizeof(prevent_removal),
+	    0, 0, 1, 1, lun, 0)) {
+		printf("INFO: Prevent medium removal failed\n");
+	}
+	if (do_msc_cmd(read_toc, sizeof(read_toc),
+	    dummy_buf, 255, 1, 1, lun, 0)) {
+		printf("INFO: Read Table Of Content failed\n");
+	}
+	if (p->try_last_lba) {
+
+		for (y = 0, x = (1UL << 31); x; x >>= 1) {
+			if (do_read_10(x | y, block_size, dummy_buf, lun) == 0)
+				y |= x;
+		}
+
+		printf("Highest readable LBA: %u (%s), "
+		    "Capacity is %u MBytes\n", y,
+		    (capacity_lba != y) ? "WRONG" : "OK",
+		    (int)((((uint64_t)(y) * (uint64_t)block_size) +
+		    (uint64_t)block_size) / 1000000ULL));
+	} else {
+
+		y = capacity_lba;
+
+		printf("Highest readable LBA: %u (not "
+		    "verified), Capacity is %u MBytes\n", y,
+		    (int)((((uint64_t)(y) * (uint64_t)block_size) +
+		    (uint64_t)block_size) / 1000000ULL));
+	}
+
+	if (y != 0xFFFFFFFFU)
+		y++;
+
+	lba_max = y;
+
+	switch (p->io_area) {
+	case USB_MSC_IO_AREA_1MB:
+		lba_max = 1024;
+		break;
+	case USB_MSC_IO_AREA_16MB:
+		lba_max = 1024 * 16;
+		break;
+	case USB_MSC_IO_AREA_256MB:
+		lba_max = 1024 * 256;
+		break;
+	case USB_MSC_IO_AREA_COMPLETE:
+	default:
+		break;
+	}
+
+	if (lba_max > 65535)
+		lba_max = 65535;
+
+	printf("Highest testable LBA: %u\n", (int)lba_max);
+
+	buffer = malloc(block_size * lba_max);
+	if (buffer == NULL) {
+		printf("ERROR: Could not allocate memory\n");
+		goto fail;
+	}
+	reference = malloc(block_size * lba_max);
+	if (reference == NULL) {
+		printf("ERROR: Could not allocate memory\n");
+		goto fail;
+	}
+retry_read_init:
+
+	printf("Setting up initial data pattern, "
+	    "LBA limit = %u ... ", lba_max);
+
+	switch (p->io_mode) {
+	case USB_MSC_IO_MODE_WRITE_ONCE_READ_ONLY:
+	case USB_MSC_IO_MODE_WRITE_ONLY:
+	case USB_MSC_IO_MODE_READ_WRITE:
+
+		switch (p->io_pattern) {
+		case USB_MSC_IO_PATTERN_FIXED:
+			for (x = 0; x != (block_size * lba_max); x += 8) {
+				reference[x + 0] = x >> 24;
+				reference[x + 1] = x >> 16;
+				reference[x + 2] = x >> 8;
+				reference[x + 3] = x >> 0;
+				reference[x + 4] = 0xFF;
+				reference[x + 5] = 0x00;
+				reference[x + 6] = 0xFF;
+				reference[x + 7] = 0x00;
+			}
+			if (do_write_10(0, lba_max * block_size,
+			    reference, lun)) {
+				printf("FAILED\n");
+				lba_max /= 2;
+				if (lba_max)
+					goto retry_read_init;
+				goto fail;
+			}
+			printf("SUCCESS\n");
+			break;
+		case USB_MSC_IO_PATTERN_RANDOM:
+			for (x = 0; x != (block_size * lba_max); x++) {
+				reference[x] = usb_ts_rand_noise() % 255U;
+			}
+			if (do_write_10(0, lba_max * block_size,
+			    reference, lun)) {
+				printf("FAILED\n");
+				lba_max /= 2;
+				if (lba_max)
+					goto retry_read_init;
+				goto fail;
+			}
+			printf("SUCCESS\n");
+			break;
+		default:
+			if (do_read_10(0, lba_max * block_size,
+			    reference, lun)) {
+				printf("FAILED\n");
+				lba_max /= 2;
+				if (lba_max)
+					goto retry_read_init;
+				goto fail;
+			}
+			printf("SUCCESS\n");
+			break;
+		}
+		break;
+
+	default:
+		if (do_read_10(0, lba_max * block_size, reference, lun)) {
+			printf("FAILED\n");
+			lba_max /= 2;
+			if (lba_max)
+				goto retry_read_init;
+			goto fail;
+		}
+		printf("SUCCESS\n");
+		break;
+	}
+
+
+	if (p->try_abort_data_write) {
+		if (do_write_10(0, (2 * block_size) | 1, reference, lun))
+			printf("Aborted data write failed (OK)!\n");
+		else
+			printf("Aborted data write did not fail (ERROR)!\n");
+
+		if (do_read_10(0, (2 * block_size), reference, lun))
+			printf("Post-aborted data read failed (ERROR)\n");
+		else
+			printf("Post-aborted data read success (OK)!\n");
+	}
+	printf("Starting test ...\n");
+
+	gettimeofday(&ref_tv, 0);
+
+	last_sec = ref_tv.tv_sec;
+
+	printf("\n");
+
+	while (1) {
+
+		gettimeofday(&sub_tv, 0);
+
+		if (last_sec != sub_tv.tv_sec) {
+
+			printf("STATUS: ID=%u, RX=%u bytes/sec, "
+			    "TX=%u bytes/sec, ERR=%u, RST=%u, DERR=%u\n",
+			    (int)xfer_current_id,
+			    (int)(stats.xfer_rx_bytes -
+			    last_stat.xfer_rx_bytes),
+			    (int)(stats.xfer_tx_bytes -
+			    last_stat.xfer_tx_bytes),
+			    (int)(stats.xfer_error),
+			    (int)(stats.xfer_reset),
+			    (int)(stats.data_error));
+
+			fflush(stdout);
+
+			last_sec = sub_tv.tv_sec;
+			last_stat = stats;
+		}
+		timersub(&sub_tv, &ref_tv, &res_tv);
+
+		if ((res_tv.tv_sec < 0) || (res_tv.tv_sec >= p->duration))
+			break;
+
+		do_io_test(p, lun, lba_max, buffer, reference);
+
+		if (libusb20_dev_check_connected(usb_pdev) != 0) {
+			printf("Device disconnected\n");
+			break;
+		}
+		if (p->done) {
+			printf("Maximum number of errors exceeded\n");
+			break;
+		}
+	}
+
+	printf("\nTest done!\n");
+
+fail:
+	if (buffer)
+		free(buffer);
+	if (reference)
+		free(reference);
+}
+
+void
+show_host_device_selection(uint8_t level, uint16_t *pvid, uint16_t *ppid)
+{
+	struct libusb20_backend *pbe;
+	struct libusb20_device *pdev;
+	struct LIBUSB20_DEVICE_DESC_DECODED *ddesc;
+
+	uint16_t vid[USB_DEVICES_MAX];
+	uint16_t pid[USB_DEVICES_MAX];
+
+	int index;
+	int sel;
+
+	const char *ptr;
+
+top:
+	pbe = libusb20_be_alloc_default();
+	pdev = NULL;
+	index = 0;
+
+	printf("\n[] Select USB device:\n");
+
+	while ((pdev = libusb20_be_device_foreach(pbe, pdev))) {
+
+		if (libusb20_dev_get_mode(pdev) != LIBUSB20_MODE_HOST)
+			continue;
+
+		if (index < USB_DEVICES_MAX) {
+			ddesc = libusb20_dev_get_device_desc(pdev);
+			ptr = libusb20_dev_get_desc(pdev);
+			printf("%s%d) %s\n", indent[level], index, ptr);
+			vid[index] = ddesc->idVendor;
+			pid[index] = ddesc->idProduct;
+			index++;
+		} else {
+			break;
+		}
+	}
+
+	printf("%sr) Refresh device list\n", indent[level]);
+	printf("%sx) Return to previous menu\n", indent[level]);
+
+	/* release data */
+	libusb20_be_free(pbe);
+
+	sel = get_integer();
+
+	if (sel == -2)
+		goto top;
+
+	if ((sel < 0) || (sel >= index)) {
+		*pvid = 0;
+		*ppid = 0;
+		return;
+	}
+	*pvid = vid[sel];
+	*ppid = pid[sel];
+}
+
+struct libusb20_device *
+find_usb_device(uint16_t vid, uint16_t pid)
+{
+	struct libusb20_backend *pbe = libusb20_be_alloc_default();
+	struct libusb20_device *pdev = NULL;
+	struct LIBUSB20_DEVICE_DESC_DECODED *ddesc;
+
+	while ((pdev = libusb20_be_device_foreach(pbe, pdev))) {
+
+		if (libusb20_dev_get_mode(pdev) != LIBUSB20_MODE_HOST)
+			continue;
+
+		ddesc = libusb20_dev_get_device_desc(pdev);
+
+		if ((vid == ddesc->idVendor) &&
+		    (pid == ddesc->idProduct)) {
+			libusb20_be_dequeue_device(pbe, pdev);
+			break;
+		}
+	}
+
+	/* release data */
+	libusb20_be_free(pbe);
+
+	return (pdev);
+}
+
+void
+find_usb_endpoints(struct libusb20_device *pdev, uint8_t class,
+    uint8_t subclass, uint8_t protocol, uint8_t alt_setting,
+    uint8_t *pif, uint8_t *in_ep, uint8_t *out_ep, uint8_t next_if)
+{
+	struct libusb20_config *pcfg;
+	struct libusb20_interface *iface;
+	struct libusb20_endpoint *ep;
+	uint8_t x;
+	uint8_t y;
+	uint8_t z;
+
+	*in_ep = 0;
+	*out_ep = 0;
+	*pif = 0;
+
+	pcfg = libusb20_dev_alloc_config(pdev,
+	    libusb20_dev_get_config_index(pdev));
+
+	if (pcfg == NULL)
+		return;
+
+	for (x = 0; x != pcfg->num_interface; x++) {
+
+		y = alt_setting;
+
+		iface = (pcfg->interface + x);
+
+		if ((iface->desc.bInterfaceClass == class) &&
+		    (iface->desc.bInterfaceSubClass == subclass ||
+		    subclass == 255) &&
+		    (iface->desc.bInterfaceProtocol == protocol ||
+		    protocol == 255)) {
+
+			if (next_if) {
+				x++;
+				if (x == pcfg->num_interface)
+					break;
+				iface = (pcfg->interface + x);
+			}
+			*pif = x;
+
+			for (z = 0; z != iface->num_endpoints; z++) {
+				ep = iface->endpoints + z;
+
+				/* BULK only */
+				if ((ep->desc.bmAttributes & 3) != 2)
+					continue;
+
+				if (ep->desc.bEndpointAddress & 0x80)
+					*in_ep = ep->desc.bEndpointAddress;
+				else
+					*out_ep = ep->desc.bEndpointAddress;
+			}
+			break;
+		}
+	}
+
+	free(pcfg);
+}
+
+static void
+exec_host_msc_test(struct usb_msc_params *p, uint16_t vid, uint16_t pid)
+{
+	struct libusb20_device *pdev;
+
+	uint8_t in_ep;
+	uint8_t out_ep;
+	uint8_t iface;
+
+	int error;
+
+	memset(&stats, 0, sizeof(stats));
+
+	xfer_current_id = 0;
+	xfer_wrapper_sig = CBWSIGNATURE;
+
+	pdev = find_usb_device(vid, pid);
+	if (pdev == NULL) {
+		printf("USB device not found\n");
+		return;
+	}
+	find_usb_endpoints(pdev, 8, 6, 0x50, 0, &iface, &in_ep, &out_ep, 0);
+
+	if ((in_ep == 0) || (out_ep == 0)) {
+		printf("Could not find USB endpoints\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	printf("Attaching to: %s @ iface %d\n",
+	    libusb20_dev_get_desc(pdev), iface);
+
+	if (libusb20_dev_open(pdev, 2)) {
+		printf("Could not open USB device\n");
+		libusb20_dev_free(pdev);
+		return;
+	}
+	if (libusb20_dev_detach_kernel_driver(pdev, iface)) {
+		printf("WARNING: Could not detach kernel driver\n");
+	}
+	xfer_in = libusb20_tr_get_pointer(pdev, 0);
+	error = libusb20_tr_open(xfer_in, 65536, 1, in_ep);
+	if (error) {
+		printf("Could not open USB endpoint %d\n", in_ep);
+		libusb20_dev_free(pdev);
+		return;
+	}
+	xfer_out = libusb20_tr_get_pointer(pdev, 1);
+	error = libusb20_tr_open(xfer_out, 65536, 1, out_ep);
+	if (error) {
+		printf("Could not open USB endpoint %d\n", out_ep);
+		libusb20_dev_free(pdev);
+		return;
+	}
+	usb_pdev = pdev;
+	usb_iface = iface;
+
+	usb_msc_test(p);
+
+	libusb20_dev_free(pdev);
+}
+
+static void
+set_defaults(struct usb_msc_params *p)
+{
+	memset(p, 0, sizeof(*p));
+
+	p->duration = 60;		/* seconds */
+	p->try_invalid_scsi_command = 1;
+	p->try_invalid_wrapper_block = 1;
+	p->try_last_lba = 1;
+	p->max_errors = -1;
+}
+
+static const char *
+get_io_mode(const struct usb_msc_params *p)
+{
+	;				/* indent fix */
+	switch (p->io_mode) {
+	case USB_MSC_IO_MODE_READ_ONLY:
+		return ("Read Only");
+	case USB_MSC_IO_MODE_WRITE_ONCE_READ_ONLY:
+		return ("Write Once, Read Only");
+	case USB_MSC_IO_MODE_WRITE_ONLY:
+		return ("Write Only");
+	case USB_MSC_IO_MODE_READ_WRITE:
+		return ("Read and Write");
+	default:
+		return ("Unknown");
+	}
+}
+
+static const char *
+get_io_pattern(const struct usb_msc_params *p)
+{
+	;				/* indent fix */
+	switch (p->io_pattern) {
+	case USB_MSC_IO_PATTERN_FIXED:
+		return ("Fixed");
+	case USB_MSC_IO_PATTERN_RANDOM:
+		return ("Random");
+	case USB_MSC_IO_PATTERN_PRESERVE:
+		return ("Preserve");
+	default:
+		return ("Unknown");
+	}
+}
+
+static const char *
+get_io_size(const struct usb_msc_params *p)
+{
+	;				/* indent fix */
+	switch (p->io_size) {
+	case USB_MSC_IO_SIZE_RANDOM:
+		return ("Random");
+	case USB_MSC_IO_SIZE_INCREASING:
+		return ("Increasing");
+	case USB_MSC_IO_SIZE_FIXED_1BLK:
+		return ("Single block");
+	case USB_MSC_IO_SIZE_FIXED_2BLK:
+		return ("2 blocks");
+	case USB_MSC_IO_SIZE_FIXED_4BLK:
+		return ("4 blocks");
+	case USB_MSC_IO_SIZE_FIXED_8BLK:
+		return ("8 blocks");
+	case USB_MSC_IO_SIZE_FIXED_16BLK:
+		return ("16 blocks");
+	case USB_MSC_IO_SIZE_FIXED_32BLK:
+		return ("32 blocks");
+	case USB_MSC_IO_SIZE_FIXED_64BLK:
+		return ("64 blocks");
+	case USB_MSC_IO_SIZE_FIXED_128BLK:
+		return ("128 blocks");
+	case USB_MSC_IO_SIZE_FIXED_256BLK:
+		return ("256 blocks");
+	case USB_MSC_IO_SIZE_FIXED_512BLK:
+		return ("512 blocks");
+	case USB_MSC_IO_SIZE_FIXED_1024BLK:
+		return ("1024 blocks");
+	default:
+		return ("Unknown");
+	}
+}
+
+static const char *
+get_io_delay(const struct usb_msc_params *p)
+{
+	;				/* indent fix */
+	switch (p->io_delay) {
+	case USB_MSC_IO_DELAY_NONE:
+		return ("None");
+	case USB_MSC_IO_DELAY_RANDOM_10MS:
+		return ("Random 10ms");
+	case USB_MSC_IO_DELAY_RANDOM_100MS:
+		return ("Random 100ms");
+	case USB_MSC_IO_DELAY_FIXED_10MS:
+		return ("Fixed 10ms");
+	case USB_MSC_IO_DELAY_FIXED_100MS:
+		return ("Fixed 100ms");
+	default:
+		return ("Unknown");
+	}
+}
+
+static const char *
+get_io_offset(const struct usb_msc_params *p)
+{
+	;				/* indent fix */
+	switch (p->io_offset) {
+	case USB_MSC_IO_OFF_START_OF_DISK:
+		return ("Start Of Disk");
+	case USB_MSC_IO_OFF_RANDOM:
+		return ("Random Offset");
+	default:
+		return ("Unknown");
+	}
+}
+
+static const char *
+get_io_area(const struct usb_msc_params *p)
+{
+	;				/* indent fix */
+	switch (p->io_area) {
+	case USB_MSC_IO_AREA_COMPLETE:
+		return ("Complete Disk");
+	case USB_MSC_IO_AREA_1MB:
+		return ("First MegaByte");
+	case USB_MSC_IO_AREA_16MB:
+		return ("First 16 MegaBytes");
+	case USB_MSC_IO_AREA_256MB:
+		return ("First 256 MegaBytes");
+	default:
+		return ("Unknown");
+	}
+}
+
+void
+show_host_msc_test(uint8_t level, uint16_t vid,
+    uint16_t pid, uint32_t duration)
+{
+	struct usb_msc_params params;
+	uint8_t retval;
+
+	set_defaults(&params);
+
+	params.duration = duration;
+
+	while (1) {
+
+		retval = usb_ts_show_menu(level,
+		    "Mass Storage Test Parameters",
+		    " 1) Toggle I/O mode: <%s>\n"
+		    " 2) Toggle I/O size: <%s>\n"
+		    " 3) Toggle I/O delay: <%s>\n"
+		    " 4) Toggle I/O offset: <%s>\n"
+		    " 5) Toggle I/O area: <%s>\n"
+		    " 6) Toggle I/O pattern: <%s>\n"
+		    " 7) Toggle try invalid SCSI command: <%s>\n"
+		    " 8) Toggle try invalid wrapper block: <%s>\n"
+		    " 9) Toggle try invalid MaxPacketSize: <%s>\n"
+		    "10) Toggle try last Logical Block Address: <%s>\n"
+		    "11) Toggle I/O lun: <%d>\n"
+		    "12) Set maximum number of errors: <%d>\n"
+		    "13) Set test duration: <%d> seconds\n"
+		    "14) Toggle try aborted write transfer: <%s>\n"
+		    "15) Toggle request sense on error: <%s>\n"
+		    "16) Toggle try all LUN: <%s>\n"
+		    "20) Reset parameters\n"
+		    "30) Start test (VID=0x%04x, PID=0x%04x)\n"
+		    "40) Select another device\n"
+		    " x) Return to previous menu \n",
+		    get_io_mode(&params),
+		    get_io_size(&params),
+		    get_io_delay(&params),
+		    get_io_offset(&params),
+		    get_io_area(&params),
+		    get_io_pattern(&params),
+		    (params.try_invalid_scsi_command ? "YES" : "NO"),
+		    (params.try_invalid_wrapper_block ? "YES" : "NO"),
+		    (params.try_invalid_max_packet_size ? "YES" : "NO"),
+		    (params.try_last_lba ? "YES" : "NO"),
+		    params.io_lun,
+		    (int)params.max_errors,
+		    (int)params.duration,
+		    (params.try_abort_data_write ? "YES" : "NO"),
+		    (params.try_sense_on_error ? "YES" : "NO"),
+		    (params.try_all_lun ? "YES" : "NO"),
+		    vid, pid);
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			params.io_mode++;
+			params.io_mode %= USB_MSC_IO_MODE_MAX;
+			break;
+		case 2:
+			params.io_size++;
+			params.io_size %= USB_MSC_IO_SIZE_MAX;
+			break;
+		case 3:
+			params.io_delay++;
+			params.io_delay %= USB_MSC_IO_DELAY_MAX;
+			break;
+		case 4:
+			params.io_offset++;
+			params.io_offset %= USB_MSC_IO_OFF_MAX;
+			break;
+		case 5:
+			params.io_area++;
+			params.io_area %= USB_MSC_IO_AREA_MAX;
+			break;
+		case 6:
+			params.io_pattern++;
+			params.io_pattern %= USB_MSC_IO_PATTERN_MAX;
+			break;
+		case 7:
+			params.try_invalid_scsi_command ^= 1;
+			break;
+		case 8:
+			params.try_invalid_wrapper_block ^= 1;
+			break;
+		case 9:
+			params.try_invalid_max_packet_size ^= 1;
+			break;
+		case 10:
+			params.try_last_lba ^= 1;
+			break;
+		case 11:
+			params.io_lun++;
+			params.io_lun %= USB_MSC_IO_LUN_MAX;
+			break;
+		case 12:
+			params.max_errors = get_integer();
+			break;
+		case 13:
+			params.duration = get_integer();
+			break;
+		case 14:
+			params.try_abort_data_write ^= 1;
+			break;
+		case 15:
+			params.try_sense_on_error ^= 1;
+			break;
+		case 16:
+			params.try_all_lun ^= 1;
+			break;
+		case 20:
+			set_defaults(&params);
+			break;
+		case 30:
+			exec_host_msc_test(&params, vid, pid);
+			break;
+		case 40:
+			show_host_device_selection(level + 1, &vid, &pid);
+			break;
+		default:
+			return;
+		}
+	}
+}
diff --git a/tools/tools/usbtest/usb_msc_test.h b/tools/tools/usbtest/usb_msc_test.h
new file mode 100644
index 0000000..3af7b08
--- /dev/null
+++ b/tools/tools/usbtest/usb_msc_test.h
@@ -0,0 +1,120 @@
+/* $FreeBSD$ */
+/*-
+ * Copyright (c) 2010 Hans Petter Selasky. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#ifndef _USB_MSC_TEST_H_
+#define	_USB_MSC_TEST_H_
+
+enum {
+	USB_MSC_IO_MODE_READ_ONLY,
+	USB_MSC_IO_MODE_WRITE_ONCE_READ_ONLY,
+	USB_MSC_IO_MODE_WRITE_ONLY,
+	USB_MSC_IO_MODE_READ_WRITE,
+	USB_MSC_IO_MODE_MAX,
+};
+
+enum {
+	USB_MSC_IO_PATTERN_FIXED,
+	USB_MSC_IO_PATTERN_RANDOM,
+	USB_MSC_IO_PATTERN_PRESERVE,
+	USB_MSC_IO_PATTERN_MAX,
+};
+
+enum {
+	USB_MSC_IO_SIZE_RANDOM,
+	USB_MSC_IO_SIZE_INCREASING,
+	USB_MSC_IO_SIZE_FIXED_1BLK,
+	USB_MSC_IO_SIZE_FIXED_2BLK,
+	USB_MSC_IO_SIZE_FIXED_4BLK,
+	USB_MSC_IO_SIZE_FIXED_8BLK,
+	USB_MSC_IO_SIZE_FIXED_16BLK,
+	USB_MSC_IO_SIZE_FIXED_32BLK,
+	USB_MSC_IO_SIZE_FIXED_64BLK,
+	USB_MSC_IO_SIZE_FIXED_128BLK,
+	USB_MSC_IO_SIZE_FIXED_256BLK,
+	USB_MSC_IO_SIZE_FIXED_512BLK,
+	USB_MSC_IO_SIZE_FIXED_1024BLK,
+	USB_MSC_IO_SIZE_MAX,
+};
+
+enum {
+	USB_MSC_IO_DELAY_NONE,
+	USB_MSC_IO_DELAY_RANDOM_10MS,
+	USB_MSC_IO_DELAY_RANDOM_100MS,
+	USB_MSC_IO_DELAY_FIXED_10MS,
+	USB_MSC_IO_DELAY_FIXED_100MS,
+	USB_MSC_IO_DELAY_MAX,
+};
+
+enum {
+	USB_MSC_IO_OFF_START_OF_DISK,
+	USB_MSC_IO_OFF_RANDOM,
+	USB_MSC_IO_OFF_MAX,
+};
+
+enum {
+	USB_MSC_IO_AREA_COMPLETE,
+	USB_MSC_IO_AREA_1MB,
+	USB_MSC_IO_AREA_16MB,
+	USB_MSC_IO_AREA_256MB,
+	USB_MSC_IO_AREA_MAX,
+};
+
+enum {
+	USB_MSC_IO_LUN_0,
+	USB_MSC_IO_LUN_1,
+	USB_MSC_IO_LUN_2,
+	USB_MSC_IO_LUN_3,
+	USB_MSC_IO_LUN_MAX,
+};
+
+struct usb_msc_params {
+
+	uint32_t duration;
+	uint32_t max_errors;
+
+	/* See "USB_MSC_XXX" enums */
+
+	uint8_t	io_mode;
+	uint8_t	io_size;
+	uint8_t	io_delay;
+	uint8_t	io_offset;
+	uint8_t	io_area;
+	uint8_t	io_pattern;
+	uint8_t	io_lun;
+
+	/* booleans */
+	uint8_t	try_invalid_scsi_command;
+	uint8_t	try_invalid_wrapper_block;
+	uint8_t	try_invalid_max_packet_size;
+	uint8_t	try_last_lba;
+	uint8_t	try_abort_data_write;
+	uint8_t try_sense_on_error;
+	uint8_t try_all_lun;
+
+	uint8_t	done;
+};
+
+#endif					/* _USB_MSC_TEST_H_ */
diff --git a/tools/tools/usbtest/usbtest.c b/tools/tools/usbtest/usbtest.c
new file mode 100644
index 0000000..725b9ea
--- /dev/null
+++ b/tools/tools/usbtest/usbtest.c
@@ -0,0 +1,809 @@
+/* $FreeBSD$ */
+/*-
+ * Copyright (c) 2010 Hans Petter Selasky. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <err.h>
+#include <string.h>
+#include <errno.h>
+#include <stdarg.h>
+#include <stdlib.h>
+
+#include <sys/types.h>
+#include <sys/sysctl.h>
+
+#include <dev/usb/usb_ioctl.h>
+
+#include "usbtest.h"
+
+#include <g_keyboard.h>
+#include <g_mouse.h>
+#include <g_modem.h>
+#include <g_audio.h>
+
+static uint8_t usb_ts_select[USB_TS_MAX_LEVELS];
+
+const char *indent[USB_TS_MAX_LEVELS] = {
+	" ",
+	"   ",
+	"     ",
+	"       ",
+	"         ",
+	"           ",
+	"             ",
+	"               ",
+};
+
+/* a perceptual white noise generator (after HPS' invention) */
+
+int32_t
+usb_ts_rand_noise(void)
+{
+	uint32_t temp;
+	const uint32_t prime = 0xFFFF1D;
+	static uint32_t noise_rem = 1;
+
+	if (noise_rem & 1) {
+		noise_rem += prime;
+	}
+	noise_rem /= 2;
+
+	temp = noise_rem;
+
+	/* unsigned to signed conversion */
+
+	temp ^= 0x800000;
+	if (temp & 0x800000) {
+		temp |= (-0x800000);
+	}
+	return temp;
+}
+
+uint8_t
+usb_ts_show_menu(uint8_t level, const char *title, const char *fmt,...)
+{
+	va_list args;
+	uint8_t x;
+	uint8_t retval;
+	char *pstr;
+	char buf[16];
+	char menu[80 * 20];
+
+	va_start(args, fmt);
+	vsnprintf(menu, sizeof(menu), fmt, args);
+	va_end(args);
+
+	printf("[");
+
+	for (x = 0; x != level; x++) {
+		if ((x + 1) == level)
+			printf("%d", usb_ts_select[x]);
+		else
+			printf("%d.", usb_ts_select[x]);
+	}
+
+	printf("] - %s:\n\n", title);
+
+	x = 1;
+	for (pstr = menu; *pstr; pstr++) {
+		if (x != 0) {
+			printf("%s", indent[level]);
+			x = 0;
+		}
+		printf("%c", *pstr);
+
+		if (*pstr == '\n')
+			x = 1;
+	}
+
+	printf("\n>");
+
+	if (fgets(buf, sizeof(buf), stdin) == NULL)
+		err(1, "Cannot read input");
+
+	if (buf[0] == 'x')
+		retval = 255;
+	else
+		retval = atoi(buf);
+
+	usb_ts_select[level] = retval;
+
+	return (retval);
+}
+
+void
+get_string(char *ptr, int size)
+{
+	printf("\nEnter string>");
+
+	if (fgets(ptr, size, stdin) == NULL)
+		err(1, "Cannot read input");
+
+	ptr[size - 1] = 0;
+
+	size = strlen(ptr);
+
+	/* strip trailing newline, if any */
+	if (size == 0)
+		return;
+	else if (ptr[size - 1] == '\n')
+		ptr[size - 1] = 0;
+}
+
+int
+get_integer(void)
+{
+	char buf[32];
+
+	printf("\nEnter integer value>");
+
+	if (fgets(buf, sizeof(buf), stdin) == NULL)
+		err(1, "Cannot read input");
+
+	if (strcmp(buf, "x\n") == 0)
+		return (-1);
+	if (strcmp(buf, "r\n") == 0)
+		return (-2);
+
+	return ((int)strtol(buf, 0, 0));
+}
+
+static void
+set_template(int template)
+{
+	int error;
+
+	error = sysctlbyname("hw.usb.template", NULL, NULL,
+	    &template, sizeof(template));
+
+	if (error != 0) {
+		printf("WARNING: Could not set USB template "
+		    "to %d (error=%d)\n", template, errno);
+	}
+}
+
+static void
+show_default_audio_select(uint8_t level)
+{
+	int error;
+	int retval;
+	int mode = 0;
+	int pattern_interval = 128;
+	int throughput = 0;
+	size_t len;
+	char pattern[G_AUDIO_MAX_STRLEN] = {"0123456789abcdef"};
+
+	set_template(USB_TEMP_AUDIO);
+
+	while (1) {
+
+		error = sysctlbyname("hw.usb.g_audio.mode", NULL, NULL,
+		    &mode, sizeof(mode));
+
+		if (error != 0) {
+			printf("WARNING: Could not set audio mode "
+			    "to %d (error=%d)\n", mode, errno);
+		}
+		error = sysctlbyname("hw.usb.g_audio.pattern_interval", NULL, NULL,
+		    &pattern_interval, sizeof(pattern_interval));
+
+		if (error != 0) {
+			printf("WARNING: Could not set pattern interval "
+			    "to %d (error=%d)\n", pattern_interval, errno);
+		}
+		len = sizeof(throughput);
+
+		error = sysctlbyname("hw.usb.g_audio.throughput",
+		    &throughput, &len, 0, 0);
+
+		if (error != 0) {
+			printf("WARNING: Could not get throughput "
+			    "(error=%d)\n", errno);
+		}
+		error = sysctlbyname("hw.usb.g_audio.pattern", NULL, NULL,
+		    &pattern, strlen(pattern));
+
+		if (error != 0) {
+			printf("WARNING: Could not set audio pattern "
+			    "to '%s' (error=%d)\n", pattern, errno);
+		}
+		retval = usb_ts_show_menu(level, "Default Audio Settings",
+		    "1) Set Silent mode %s\n"
+		    "2) Set Dump mode %s\n"
+		    "3) Set Loop mode %s\n"
+		    "4) Set Pattern mode %s\n"
+		    "5) Change DTMF pattern: '%s'\n"
+		    "6) Change pattern advance interval: %d ms\n"
+		    "x) Return to previous menu\n"
+		    "s: Ready for enumeration\n"
+		    "t: Throughput: %d bytes/second\n",
+		    (mode == G_AUDIO_MODE_SILENT) ? "(selected)" : "",
+		    (mode == G_AUDIO_MODE_DUMP) ? "(selected)" : "",
+		    (mode == G_AUDIO_MODE_LOOP) ? "(selected)" : "",
+		    (mode == G_AUDIO_MODE_PATTERN) ? "(selected)" : "",
+		    pattern, pattern_interval, throughput);
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			mode = G_AUDIO_MODE_SILENT;
+			break;
+		case 2:
+			mode = G_AUDIO_MODE_DUMP;
+			break;
+		case 3:
+			mode = G_AUDIO_MODE_LOOP;
+			break;
+		case 4:
+			mode = G_AUDIO_MODE_PATTERN;
+			break;
+		case 5:
+			get_string(pattern, sizeof(pattern));
+			break;
+		case 6:
+			pattern_interval = get_integer();
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_device_audio_select(uint8_t level)
+{
+	uint8_t retval;
+
+	while (1) {
+
+		retval = usb_ts_show_menu(level, "Select Audio Device Model",
+		    "1) Generic Audio Device\n"
+		    "x) Return to previous menu\n");
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			show_default_audio_select(level + 1);
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_device_msc_select(uint8_t level)
+{
+	set_template(USB_TEMP_MSC);
+}
+
+static void
+show_device_ethernet_select(uint8_t level)
+{
+	set_template(USB_TEMP_CDCE);
+}
+
+static void
+show_default_keyboard_select(uint8_t level)
+{
+	int error;
+	int retval;
+	int mode = 0;
+	int interval = 1023;
+	char pattern[G_KEYBOARD_MAX_STRLEN] = {"abcdefpattern"};
+
+	set_template(USB_TEMP_KBD);
+
+	while (1) {
+
+		error = sysctlbyname("hw.usb.g_keyboard.mode", NULL, NULL,
+		    &mode, sizeof(mode));
+
+		if (error != 0) {
+			printf("WARNING: Could not set keyboard mode "
+			    " to %d (error=%d) \n", mode, errno);
+		}
+		error = sysctlbyname("hw.usb.g_keyboard.key_press_interval", NULL, NULL,
+		    &interval, sizeof(interval));
+
+		if (error != 0) {
+			printf("WARNING: Could not set key press interval "
+			    "to %d (error=%d)\n", interval, errno);
+		}
+		error = sysctlbyname("hw.usb.g_keyboard.key_press_pattern", NULL, NULL,
+		    &pattern, strlen(pattern));
+
+		if (error != 0) {
+			printf("WARNING: Could not set key pattern "
+			    "to '%s' (error=%d)\n", pattern, errno);
+		}
+		retval = usb_ts_show_menu(level, "Default Keyboard Settings",
+		    "1) Set silent mode %s\n"
+		    "2) Set pattern mode %s\n"
+		    "3) Change pattern: '%s'\n"
+		    "4) Change key press interval: %d ms\n"
+		    "x) Return to previous menu\n"
+		    "s: Ready for enumeration\n",
+		    (mode == G_KEYBOARD_MODE_SILENT) ? "(selected)" : "",
+		    (mode == G_KEYBOARD_MODE_PATTERN) ? "(selected)" : "",
+		    pattern, interval);
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			mode = G_KEYBOARD_MODE_SILENT;
+			break;
+		case 2:
+			mode = G_KEYBOARD_MODE_PATTERN;
+			break;
+		case 3:
+			get_string(pattern, sizeof(pattern));
+			break;
+		case 4:
+			interval = get_integer();
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_device_keyboard_select(uint8_t level)
+{
+	uint8_t retval;
+
+	while (1) {
+
+		retval = usb_ts_show_menu(level, "Select Keyboard Model",
+		    "1) Generic Keyboard \n"
+		    "x) Return to previous menu \n");
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			show_default_keyboard_select(level + 1);
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_default_mouse_select(uint8_t level)
+{
+	int error;
+	int retval;
+	int mode = 0;
+	int cursor_interval = 128;
+	int cursor_radius = 75;
+	int button_interval = 0;
+
+	set_template(USB_TEMP_MOUSE);
+
+	while (1) {
+
+		error = sysctlbyname("hw.usb.g_mouse.mode", NULL, NULL,
+		    &mode, sizeof(mode));
+
+		if (error != 0) {
+			printf("WARNING: Could not set mouse mode "
+			    "to %d (error=%d)\n", mode, errno);
+		}
+		error = sysctlbyname("hw.usb.g_mouse.cursor_update_interval", NULL, NULL,
+		    &cursor_interval, sizeof(cursor_interval));
+
+		if (error != 0) {
+			printf("WARNING: Could not set cursor update interval "
+			    "to %d (error=%d)\n", cursor_interval, errno);
+		}
+		error = sysctlbyname("hw.usb.g_mouse.button_press_interval", NULL, NULL,
+		    &button_interval, sizeof(button_interval));
+
+		if (error != 0) {
+			printf("WARNING: Could not set button press interval "
+			    "to %d (error=%d)\n", button_interval, errno);
+		}
+		error = sysctlbyname("hw.usb.g_mouse.cursor_radius", NULL, NULL,
+		    &cursor_radius, sizeof(cursor_radius));
+
+		if (error != 0) {
+			printf("WARNING: Could not set cursor radius "
+			    "to %d (error=%d)\n", cursor_radius, errno);
+		}
+		retval = usb_ts_show_menu(level, "Default Mouse Settings",
+		    "1) Set Silent mode %s\n"
+		    "2) Set Circle mode %s\n"
+		    "3) Set Square mode %s\n"
+		    "4) Set Spiral mode %s\n"
+		    "5) Change cursor radius: %d pixels\n"
+		    "6) Change cursor update interval: %d ms\n"
+		    "7) Change button[0] press interval: %d ms\n"
+		    "x) Return to previous menu\n"
+		    "s: Ready for enumeration\n",
+		    (mode == G_MOUSE_MODE_SILENT) ? "(selected)" : "",
+		    (mode == G_MOUSE_MODE_CIRCLE) ? "(selected)" : "",
+		    (mode == G_MOUSE_MODE_BOX) ? "(selected)" : "",
+		    (mode == G_MOUSE_MODE_SPIRAL) ? "(selected)" : "",
+		    cursor_radius, cursor_interval, button_interval);
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			mode = G_MOUSE_MODE_SILENT;
+			break;
+		case 2:
+			mode = G_MOUSE_MODE_CIRCLE;
+			break;
+		case 3:
+			mode = G_MOUSE_MODE_BOX;
+			break;
+		case 4:
+			mode = G_MOUSE_MODE_SPIRAL;
+			break;
+		case 5:
+			cursor_radius = get_integer();
+			break;
+		case 6:
+			cursor_interval = get_integer();
+			break;
+		case 7:
+			button_interval = get_integer();
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_device_mouse_select(uint8_t level)
+{
+	uint8_t retval;
+
+	while (1) {
+
+		retval = usb_ts_show_menu(level, "Select Mouse Model",
+		    "1) Generic Mouse\n"
+		    "x) Return to previous menu\n");
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			show_default_mouse_select(level + 1);
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_device_mtp_select(uint8_t level)
+{
+	set_template(USB_TEMP_MTP);
+}
+
+static void
+show_default_modem_select(uint8_t level)
+{
+	int error;
+	int retval;
+	int mode = 0;
+	int pattern_interval = 128;
+	int throughput = 0;
+	size_t len;
+	char pattern[G_MODEM_MAX_STRLEN] = {"abcdefpattern"};
+
+	set_template(USB_TEMP_MODEM);
+
+	while (1) {
+
+		error = sysctlbyname("hw.usb.g_modem.mode", NULL, NULL,
+		    &mode, sizeof(mode));
+
+		if (error != 0) {
+			printf("WARNING: Could not set modem mode "
+			    "to %d (error=%d)\n", mode, errno);
+		}
+		error = sysctlbyname("hw.usb.g_modem.pattern_interval", NULL, NULL,
+		    &pattern_interval, sizeof(pattern_interval));
+
+		if (error != 0) {
+			printf("WARNING: Could not set pattern interval "
+			    "to %d (error=%d)\n", pattern_interval, errno);
+		}
+		len = sizeof(throughput);
+
+		error = sysctlbyname("hw.usb.g_modem.throughput",
+		    &throughput, &len, 0, 0);
+
+		if (error != 0) {
+			printf("WARNING: Could not get throughput "
+			    "(error=%d)\n", errno);
+		}
+		error = sysctlbyname("hw.usb.g_modem.pattern", NULL, NULL,
+		    &pattern, strlen(pattern));
+
+		if (error != 0) {
+			printf("WARNING: Could not set modem pattern "
+			    "to '%s' (error=%d)\n", pattern, errno);
+		}
+		retval = usb_ts_show_menu(level, "Default Modem Settings",
+		    "1) Set Silent mode %s\n"
+		    "2) Set Dump mode %s\n"
+		    "3) Set Loop mode %s\n"
+		    "4) Set Pattern mode %s\n"
+		    "5) Change test pattern: '%s'\n"
+		    "6) Change data transmit interval: %d ms\n"
+		    "x) Return to previous menu\n"
+		    "s: Ready for enumeration\n"
+		    "t: Throughput: %d bytes/second\n",
+		    (mode == G_MODEM_MODE_SILENT) ? "(selected)" : "",
+		    (mode == G_MODEM_MODE_DUMP) ? "(selected)" : "",
+		    (mode == G_MODEM_MODE_LOOP) ? "(selected)" : "",
+		    (mode == G_MODEM_MODE_PATTERN) ? "(selected)" : "",
+		    pattern, pattern_interval, throughput);
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			mode = G_MODEM_MODE_SILENT;
+			break;
+		case 2:
+			mode = G_MODEM_MODE_DUMP;
+			break;
+		case 3:
+			mode = G_MODEM_MODE_LOOP;
+			break;
+		case 4:
+			mode = G_MODEM_MODE_PATTERN;
+			break;
+		case 5:
+			get_string(pattern, sizeof(pattern));
+			break;
+		case 6:
+			pattern_interval = get_integer();
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_device_modem_select(uint8_t level)
+{
+	uint8_t retval;
+
+	while (1) {
+
+		retval = usb_ts_show_menu(level, "Select Modem Model",
+		    "1) Generic Modem\n"
+		    "x) Return to previous menu\n");
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			show_default_modem_select(level + 1);
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_device_generic_select(uint8_t level)
+{
+}
+
+static void
+show_device_select(uint8_t level)
+{
+	uint8_t retval;
+
+	while (1) {
+
+		retval = usb_ts_show_menu(level, "Select Device Mode Test Group",
+		    "1) Audio (UAUDIO)\n"
+		    "2) Mass Storage (MSC)\n"
+		    "3) Ethernet (CDCE)\n"
+		    "4) Keyboard Input Device (UKBD)\n"
+		    "5) Mouse Input Device (UMS)\n"
+		    "6) Message Transfer Protocol (MTP)\n"
+		    "7) Modem (CDC)\n"
+		    "8) Generic Endpoint Loopback (GENERIC)\n"
+		    "x) Return to previous menu\n");
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			show_device_audio_select(level + 1);
+			break;
+		case 2:
+			show_device_msc_select(level + 1);
+			break;
+		case 3:
+			show_device_ethernet_select(level + 1);
+			break;
+		case 4:
+			show_device_keyboard_select(level + 1);
+			break;
+		case 5:
+			show_device_mouse_select(level + 1);
+			break;
+		case 6:
+			show_device_mtp_select(level + 1);
+			break;
+		case 7:
+			show_device_modem_select(level + 1);
+			break;
+		case 8:
+			show_device_generic_select(level + 1);
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_host_select(uint8_t level)
+{
+	int force_fs = 0;
+	int error;
+	uint32_t duration = 60;
+
+	uint16_t dev_vid = 0;
+	uint16_t dev_pid = 0;
+	uint8_t retval;
+
+	while (1) {
+
+		error = sysctlbyname("hw.usb.ehci.no_hs", NULL, NULL,
+		    &force_fs, sizeof(force_fs));
+
+		if (error != 0) {
+			printf("WARNING: Could not set non-FS mode "
+			    "to %d (error=%d)\n", force_fs, errno);
+		}
+		retval = usb_ts_show_menu(level, "Select Host Mode Test (via LibUSB)",
+		    " 1) Select USB device (VID=0x%04x, PID=0x%04x)\n"
+		    " 2) Manually enter USB vendor and product ID\n"
+		    " 3) Force FULL speed operation: <%s>\n"
+		    " 4) Mass Storage (UMASS)\n"
+		    " 5) Modem (UMODEM)\n"
+		    "10) Start String Descriptor Test\n"
+		    "11) Start Port Reset Test\n"
+		    "12) Start Set Config Test\n"
+		    "13) Start Get Descriptor Test\n"
+		    "14) Start Suspend and Resume Test\n"
+		    "15) Start Set and Clear Endpoint Stall Test\n"
+		    "16) Start Set Alternate Interface Setting Test\n"
+		    "17) Start Invalid Control Request Test\n"
+		    "30) Duration: <%d> seconds\n"
+		    "x) Return to previous menu\n",
+		    dev_vid, dev_pid,
+		    force_fs ? "YES" : "NO",
+		    (int)duration);
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			show_host_device_selection(level + 1, &dev_vid, &dev_pid);
+			break;
+		case 2:
+			dev_vid = get_integer() & 0xFFFF;
+			dev_pid = get_integer() & 0xFFFF;
+			break;
+		case 3:
+			force_fs ^= 1;
+			break;
+		case 4:
+			show_host_msc_test(level + 1, dev_vid, dev_pid, duration);
+			break;
+		case 5:
+			show_host_modem_test(level + 1, dev_vid, dev_pid, duration);
+			break;
+		case 10:
+			usb_get_string_desc_test(dev_vid, dev_pid);
+			break;
+		case 11:
+			usb_port_reset_test(dev_vid, dev_pid, duration);
+			break;
+		case 12:
+			usb_set_config_test(dev_vid, dev_pid, duration);
+			break;
+		case 13:
+			usb_get_descriptor_test(dev_vid, dev_pid, duration);
+			break;
+		case 14:
+			usb_suspend_resume_test(dev_vid, dev_pid, duration);
+			break;
+		case 15:
+			usb_set_and_clear_stall_test(dev_vid, dev_pid);
+			break;
+		case 16:
+			usb_set_alt_interface_test(dev_vid, dev_pid);
+			break;
+		case 17:
+			usb_control_ep_error_test(dev_vid, dev_pid);
+			break;
+		case 30:
+			duration = get_integer();
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+static void
+show_mode_select(uint8_t level)
+{
+	uint8_t retval;
+
+	while (1) {
+
+		retval = usb_ts_show_menu(level, "Select Computer Mode",
+		    "1) This computer is Running the Device Side\n"
+		    "2) This computer is Running the Host Side\n"
+		    "x) Return to previous menu\n");
+
+		switch (retval) {
+		case 0:
+			break;
+		case 1:
+			show_device_select(level + 1);
+			break;
+		case 2:
+			show_host_select(level + 1);
+			break;
+		default:
+			return;
+		}
+	}
+}
+
+int
+main(int argc, char **argv)
+{
+	show_mode_select(1);
+
+	return (0);
+}
diff --git a/tools/tools/usbtest/usbtest.h b/tools/tools/usbtest/usbtest.h
new file mode 100644
index 0000000..3d6643c
--- /dev/null
+++ b/tools/tools/usbtest/usbtest.h
@@ -0,0 +1,62 @@
+/* $FreeBSD$ */
+/*-
+ * Copyright (c) 2010 Hans Petter Selasky. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#ifndef _USBTEST_H_
+#define	_USBTEST_H_
+
+#define	USB_DEVICES_MAX		128
+#define	USB_TS_MAX_LEVELS	8
+
+struct libusb20_device;
+
+struct bps {
+	uint32_t bytes;
+	time_t	time;
+};
+
+extern void usb_get_string_desc_test(uint16_t, uint16_t);
+extern void usb_port_reset_test(uint16_t, uint16_t, uint32_t);
+extern void usb_set_config_test(uint16_t, uint16_t, uint32_t);
+extern void usb_get_descriptor_test(uint16_t, uint16_t, uint32_t);
+extern void usb_control_ep_error_test(uint16_t, uint16_t);
+extern void usb_set_and_clear_stall_test(uint16_t, uint16_t);
+extern void usb_set_alt_interface_test(uint16_t, uint16_t);
+
+extern void usb_suspend_resume_test(uint16_t, uint16_t, uint32_t);
+extern void do_bps(const char *, struct bps *, uint32_t len);
+extern const char *indent[USB_TS_MAX_LEVELS];
+extern void show_host_msc_test(uint8_t, uint16_t, uint16_t, uint32_t);
+extern void show_host_modem_test(uint8_t, uint16_t, uint16_t, uint32_t);
+extern void show_host_device_selection(uint8_t, uint16_t *, uint16_t *);
+extern struct libusb20_device *find_usb_device(uint16_t, uint16_t);
+extern void find_usb_endpoints(struct libusb20_device *, uint8_t, uint8_t,
+    uint8_t, uint8_t, uint8_t *, uint8_t *, uint8_t *, uint8_t);
+extern void get_string(char *, int);
+extern int get_integer(void);
+extern uint8_t usb_ts_show_menu(uint8_t, const char *, const char *,...);
+extern int32_t usb_ts_rand_noise(void);
+
+#endif				/* _USBTEST_H_ */