Merge branch 'for-next/usb-gadget'

27 files changed, 3621 insertions, 148 deletions

diff --git a/Documentation/devel/background-execution.rst b/Documentation/devel/background-execution.rst
index fa4d23e6d2..d379593efb 100644
--- a/Documentation/devel/background-execution.rst
+++ b/Documentation/devel/background-execution.rst
@@ -10,9 +10,9 @@ Pollers
 -------
 
 Pollers are a way in barebox to frequently execute code in the background.
-barebox is single-threaded, so a poller is not executed as a separate thread,
-but instead pollers are executed whenever ``is_timeout()`` is called.  This has
-a few implications. First of all, pollers are not executed when
+They don't run within their own threads, but instead they are executed
+whenever ``is_timeout()`` is called.
+This has a few implications. First of all, pollers are not executed when
 ``is_timeout()`` is not called. For this and other reasons, loops polling for
 hardware events should always use a timeout, which is best implemented with
 ``is_timeout()``. Another thing to remember is that pollers can be executed
@@ -74,31 +74,19 @@ code. Usually a work item is allocated by the poller and then freed either in
 bthreads
 --------
 
-barebox threads are co-operative green threads, which are scheduled whenever
-``is_timeout()`` is called.  This has a few implications. First of all,
-bthreads are not scheduled when ``is_timeout()`` is not called.
-For this and other reasons, loops polling for hardware events should always
-use a timeout, which is best implemented with ``is_timeout()``.
-Another thing to remember is that bthreads can be scheduled anywhere
-in the middle of other device accesses whenever ``is_timeout()`` is
-called. Care must be taken that a green thread doesn't access the very same device
-again itself. See "slices" below on how devices can safely be accessed from
-bthreads.
+barebox threads are co-operative green threads, which are scheduled for the
+same context as workqueues: Before the shell executes the next command.
+This means that bthreads can be used to implement workqueues, but not pollers.
 
 The bthread interface is declared in ``include/bthread.h``.
 ``bthread_create()`` is used to allocate a bthread control block along with
 its stack. ``bthread_wake()`` can be used to add it into the run queue.
 From this moment on and until the thread terminates, the thread will be
-switched to regularly as long as someone calls ``is_timeout()``.
-bthreads are allowed to call ``is_timeout()``, which will arrange for
-other threads to execute.
-
-barebox threads are planned to replace previous infrastructure, pollers
-and workqueues. Poller like behavior can be easily achieved by looping
-and yielding on every iteration. There's ``bthread_should_stop()``, which
-can be used as condition for continuing the loop. Workqueues could be
-replaced along the same line, but with mutexes protecting underlying device
-access.
+switched to regularly as long as the shell processes commands.
+
+bthreads are allowed to call ``is_timeout()``, which will eventually
+arrange for other threads to execute. This allowed implementing a Linux-like
+completion API on top, which can be useful for porting threaded kernel code.
 
 Slices
 ------
diff --git a/Documentation/user/usb.rst b/Documentation/user/usb.rst
index ca5f8138de..62f6796c95 100644
--- a/Documentation/user/usb.rst
+++ b/Documentation/user/usb.rst
@@ -264,6 +264,11 @@ USB Gadget autostart Options
 ``global.usbgadget.acm``
   Boolean flag. If set to 1, CDC ACM function will be created.
   See :ref:`command_usbgadget` -a. (Default 0).
+``global.system.partitions``
+  Common function description for all of DFU, fastboot and USB mass storage
+  gadgets. Both Fastboot and DFU partitions also have dedicated override
+  variables for backwards-compatibility::
+
 ``global.usbgadget.dfu_function``
   Function description for DFU. See :ref:`command_usbgadget` -D [desc].
 ``global.fastboot.partitions``
diff --git a/commands/bthread.c b/commands/bthread.c
index 964a1044c5..aaade46e92 100644
--- a/commands/bthread.c
+++ b/commands/bthread.c
@@ -10,12 +10,15 @@
 #include <command.h>
 #include <getopt.h>
 #include <clock.h>
+#include <slice.h>
 
 static int bthread_time(void)
 {
 	uint64_t start = get_time_ns();
 	int i = 0;
 
+	slice_release(&command_slice);
+
 	/*
 	 * How many background tasks can we have in one second?
 	 *
@@ -25,15 +28,15 @@ static int bthread_time(void)
 	while (!is_timeout(start, SECOND))
 		i++;
 
+	slice_acquire(&command_slice);
+
 	return i;
 }
 
-static int bthread_infinite(void *data)
+static void bthread_infinite(void *data)
 {
 	while (!bthread_should_stop())
 		;
-
-	return 0;
 }
 
 static int bthread_isolated_time(void)
@@ -57,14 +60,19 @@ static int bthread_isolated_time(void)
 		i += 2;
 	}
 
-	bthread_stop(bthread);
-	bthread_free(bthread);
+	__bthread_stop(bthread);
 
 	return i;
 }
 
-static int bthread_printer(void *arg)
+struct arg {
+	unsigned long in;
+	long out;
+};
+
+static void bthread_printer(void *_arg)
 {
+	struct arg *arg = _arg;
 	volatile u64 start;
 	volatile unsigned long i = 0;
 	start = get_time_ns();
@@ -73,28 +81,30 @@ static int bthread_printer(void *arg)
 		if (!is_timeout_non_interruptible(start, 225 * MSECOND))
 			continue;
 
-		if ((unsigned long)arg == i++)
+		if (arg->in == i++)
 			printf("%s yield #%lu\n", bthread_name(current), i);
 		start = get_time_ns();
 	}
 
-	return i;
+	arg->out = i;
 }
 
 static int yields;
 
-static int bthread_spawner(void *arg)
+static void bthread_spawner(void *_spawner_arg)
 {
+	struct arg *arg, *spawner_arg = _spawner_arg;
 	struct bthread *bthread[4];
 	volatile u64 start;
 	volatile unsigned long i = 0;
 	int ret = 0;
-	int ecode;
 
 	start = get_time_ns();
 
 	for (i = 0; i < ARRAY_SIZE(bthread); i++) {
-		bthread[i] = bthread_run(bthread_printer, (void *)(long)i,
+		arg = malloc(sizeof(*arg));
+		arg->in = i;
+		bthread[i] = bthread_run(bthread_printer, arg,
 					 "%s-bthread%u", bthread_name(current), i+1);
 		if (!bthread[i]) {
 			ret = -ENOMEM;
@@ -107,14 +117,15 @@ static int bthread_spawner(void *arg)
 
 cleanup:
 	while (i--) {
-		ecode = bthread_stop(bthread[i]);
-		bthread_free(bthread[i]);
+		arg = bthread_data(bthread[i]);
+		__bthread_stop(bthread[i]);
 
-		if (!ret && (ecode != 4 || yields < ecode))
+		if (!ret && (arg->out != 4 || yields < arg->out))
 			ret = -EIO;
+		free(arg);
 	}
 
-	return ret;
+	spawner_arg->out = ret;
 }
 
 struct spawn {
@@ -124,14 +135,38 @@ struct spawn {
 
 static int do_bthread(int argc, char *argv[])
 {
+	static int dummynr;
+	static LIST_HEAD(dummies);
 	LIST_HEAD(spawners);
 	struct spawn *spawner, *tmp;
 	int ret = 0;
-	int ecode, opt, i = 0;
+	int opt, i = 0;
 	bool time = false;
+	struct arg *arg;
 
-	while ((opt = getopt(argc, argv, "itcv")) > 0) {
+	while ((opt = getopt(argc, argv, "aritcv")) > 0) {
 		switch (opt) {
+		case 'a':
+			spawner = xzalloc(sizeof(*spawner));
+			spawner->bthread = bthread_run(bthread_infinite, NULL,
+						       "dummy%u", dummynr++);
+			if (!spawner->bthread) {
+				free(spawner);
+				ret = -ENOMEM;
+				goto cleanup;
+			}
+
+			list_add(&spawner->list, &dummies);
+			break;
+		case 'r':
+			if (dummynr == 0)
+				return -EINVAL;
+			spawner = list_first_entry(&dummies, struct spawn, list);
+			bthread_cancel(spawner->bthread);
+			list_del(&spawner->list);
+			free(spawner);
+			dummynr--;
+			break;
 		case 'i':
 			bthread_info();
 			break;
@@ -141,7 +176,8 @@ static int do_bthread(int argc, char *argv[])
 			break;
 		case 'v':
 			spawner = xzalloc(sizeof(*spawner));
-			spawner->bthread = bthread_run(bthread_spawner, NULL,
+			arg = malloc(sizeof(*arg));
+			spawner->bthread = bthread_run(bthread_spawner, arg,
 						       "spawner%u", ++i);
 			if (!spawner->bthread) {
 				free(spawner);
@@ -167,10 +203,11 @@ static int do_bthread(int argc, char *argv[])
 
 cleanup:
 	list_for_each_entry_safe(spawner, tmp, &spawners, list) {
-		ecode = bthread_stop(spawner->bthread);
-		bthread_free(spawner->bthread);
-		if (!ret && ecode)
-			ret = ecode;
+		arg = bthread_data(spawner->bthread);
+		__bthread_stop(spawner->bthread);
+		if (!ret && arg->out)
+			ret = arg->out;
+		free(arg);
 		free(spawner);
 	}
 
@@ -184,6 +221,8 @@ BAREBOX_CMD_HELP_START(bthread)
 	BAREBOX_CMD_HELP_OPT ("-i", "Print information about registered bthreads")
 	BAREBOX_CMD_HELP_OPT ("-t", "measure how many bthreads we currently run in 1s")
 	BAREBOX_CMD_HELP_OPT ("-c", "count maximum context switches in 1s")
+	BAREBOX_CMD_HELP_OPT ("-a", "add a dummy bthread")
+	BAREBOX_CMD_HELP_OPT ("-r", "remove a dummy bthread")
 	BAREBOX_CMD_HELP_OPT ("-v", "verify correct bthread operation")
 BAREBOX_CMD_HELP_END
 
diff --git a/commands/usbgadget.c b/commands/usbgadget.c
index 3b115f147d..290f33eff3 100644
--- a/commands/usbgadget.c
+++ b/commands/usbgadget.c
@@ -18,26 +18,29 @@
 
 static int do_usbgadget(int argc, char *argv[])
 {
+	struct usbgadget_funcs funcs = {};
 	int opt;
-	bool acm = false, dfu = false, fastboot = false, export_bbu = false;
-	const char *fastboot_opts = NULL, *dfu_opts = NULL;
 
-	while ((opt = getopt(argc, argv, "asdA::D::b")) > 0) {
+	while ((opt = getopt(argc, argv, "asdA::D::S::b")) > 0) {
 		switch (opt) {
 		case 'a':
 		case 's':
-			acm = true;
+			funcs.flags |= USBGADGET_ACM;
 			break;
 		case 'D':
-			dfu = true;
-			dfu_opts = optarg;
+			funcs.flags |= USBGADGET_DFU;
+			funcs.dfu_opts = optarg;
 			break;
 		case 'A':
-			fastboot = true;
-			fastboot_opts = optarg;
+			funcs.flags |= USBGADGET_FASTBOOT;
+			funcs.fastboot_opts = optarg;
+			break;
+		case 'S':
+			funcs.flags |= USBGADGET_MASS_STORAGE;
+			funcs.ums_opts = optarg;
 			break;
 		case 'b':
-			export_bbu = true;
+			funcs.flags |= USBGADGET_EXPORT_BBU;
 			break;
 		case 'd':
 			usb_multi_unregister();
@@ -47,8 +50,8 @@ static int do_usbgadget(int argc, char *argv[])
 		}
 	}
 
-	return usbgadget_register(dfu, dfu_opts, fastboot, fastboot_opts, acm,
-				  export_bbu);
+
+	return usbgadget_register(&funcs);
 }
 
 BAREBOX_CMD_HELP_START(usbgadget)
@@ -61,13 +64,15 @@ BAREBOX_CMD_HELP_OPT ("-A <desc>", "Create Android Fastboot function. If 'desc'
 BAREBOX_CMD_HELP_OPT ("-b\t", "include registered barebox update handlers (fastboot specific)")
 BAREBOX_CMD_HELP_OPT ("-D <desc>", "Create DFU function. If 'desc' is not provided, "
 				   "try to use 'global.usbgadget.dfu_function' variable.")
+BAREBOX_CMD_HELP_OPT ("-S <desc>", "Create USB Mass Storage function. If 'desc' is not provided,"
+				   "fallback directly to 'global.system.partitions' variable.")
 BAREBOX_CMD_HELP_OPT ("-d\t", "Disable the currently running gadget")
 BAREBOX_CMD_HELP_END
 
 BAREBOX_CMD_START(usbgadget)
 	.cmd		= do_usbgadget,
 	BAREBOX_CMD_DESC("Create USB Gadget multifunction device")
-	BAREBOX_CMD_OPTS("[-adAD]")
+	BAREBOX_CMD_OPTS("[-adADS]")
 	BAREBOX_CMD_GROUP(CMD_GRP_HWMANIP)
 	BAREBOX_CMD_HELP(cmd_usbgadget_help)
 BAREBOX_CMD_END
diff --git a/common/Kconfig b/common/Kconfig
index 9c9e0525e2..a9feae2ae8 100644
--- a/common/Kconfig
+++ b/common/Kconfig
@@ -997,11 +997,16 @@ config BAREBOXCRC32_TARGET
 	  'bareboxcrc32' is a userspacetool to generate the crc32 checksums the same way
 	  barebox does. Say yes here to build it for the target.
 
+config HAS_SCHED
+	bool
+
 config POLLER
 	bool "generic polling infrastructure"
+	select HAS_SCHED
 
 config BTHREAD
 	bool "barebox co-operative (green) thread infrastructure"
+	select HAS_SCHED
 	depends on HAS_ARCH_SJLJ
 	help
 	 barebox threads are lightweight cooperative (green) threads that are
diff --git a/common/Makefile b/common/Makefile
index daa022ff54..4b45f678c7 100644
--- a/common/Makefile
+++ b/common/Makefile
@@ -51,6 +51,7 @@ extra-$(CONFIG_MODULES)		+= module.lds
 obj-$(CONFIG_OFTREE)		+= oftree.o
 obj-$(CONFIG_PARTITION_DISK)	+= partitions.o partitions/
 obj-$(CONFIG_PASSWORD)		+= password.o
+obj-$(CONFIG_HAS_SCHED)		+= sched.o
 obj-$(CONFIG_POLLER)		+= poller.o
 obj-$(CONFIG_BTHREAD)		+= bthread.o
 obj-$(CONFIG_RESET_SOURCE)	+= reset_source.o
diff --git a/common/bthread.c b/common/bthread.c
index c811797130..46e6987149 100644
--- a/common/bthread.c
+++ b/common/bthread.c
@@ -20,11 +20,8 @@
 #endif
 
 static struct bthread {
-	int (*threadfn)(void *);
-	union {
-		void *data;
-		int ret;
-	};
+	void (*threadfn)(void *);
+	void *data;
 	char *name;
 	jmp_buf jmp_buf;
 	void *stack;
@@ -35,10 +32,12 @@ static struct bthread {
 #endif
 	u8 awake :1;
 	u8 should_stop :1;
+	u8 should_clean :1;
 	u8 has_stopped :1;
 } main_thread = {
 	.list = LIST_HEAD_INIT(main_thread.list),
 	.name = "main",
+	.awake = true,
 };
 
 struct bthread *current = &main_thread;
@@ -54,7 +53,7 @@ static void __noreturn bthread_trampoline(void)
 	finish_switch_fiber(current);
 	bthread_reschedule();
 
-	current->ret = current->threadfn(current->data);
+	current->threadfn(current->data);
 
 	bthread_suspend(current);
 	current->has_stopped = true;
@@ -69,7 +68,7 @@ bool bthread_is_main(struct bthread *bthread)
 	return bthread == &main_thread;
 }
 
-void bthread_free(struct bthread *bthread)
+static void bthread_free(struct bthread *bthread)
 {
 	free(bthread->stack);
 	free(bthread->name);
@@ -81,7 +80,7 @@ const char *bthread_name(struct bthread *bthread)
 	return bthread->name;
 }
 
-struct bthread *bthread_create(int (*threadfn)(void *), void *data,
+struct bthread *bthread_create(void (*threadfn)(void *), void *data,
 			       const char *namefmt, ...)
 {
 	struct bthread *bthread;
@@ -107,6 +106,8 @@ struct bthread *bthread_create(int (*threadfn)(void *), void *data,
 	if (len < 0)
 		goto err;
 
+	list_add_tail(&bthread->list, &current->list);
+
 	/* set up bthread context with the new stack */
 	initjmp(bthread->jmp_buf, bthread_trampoline,
 		bthread->stack + CONFIG_STACK_SIZE);
@@ -117,30 +118,38 @@ err:
 	return NULL;
 }
 
+void *bthread_data(struct bthread *bthread)
+{
+	return bthread->data;
+}
+
 void bthread_wake(struct bthread *bthread)
 {
-	if (bthread->awake)
-		return;
-	list_add_tail(&bthread->list, &current->list);
 	bthread->awake = true;
 }
 
 void bthread_suspend(struct bthread *bthread)
 {
-	if (!bthread->awake)
-		return;
 	bthread->awake = false;
-	list_del(&bthread->list);
 }
 
-int bthread_stop(struct bthread *bthread)
+void bthread_cancel(struct bthread *bthread)
+{
+	bthread->should_stop = true;
+	bthread->should_clean = true;
+}
+
+void __bthread_stop(struct bthread *bthread)
 {
 	bthread->should_stop = true;
 
+	pr_debug("waiting for %s to stop\n", bthread->name);
+
 	while (!bthread->has_stopped)
 		bthread_reschedule();
 
-	return bthread->ret;
+	list_del(&bthread->list);
+	bthread_free(bthread);
 }
 
 int bthread_should_stop(void)
@@ -163,7 +172,23 @@ void bthread_info(void)
 
 void bthread_reschedule(void)
 {
-	bthread_schedule(list_next_entry(current, list));
+	struct bthread *next, *tmp;
+
+	if (current == list_next_entry(current, list))
+		return;
+
+	list_for_each_entry_safe(next, tmp, &current->list, list) {
+		if (next->awake) {
+			pr_debug("switch %s -> %s\n", current->name, next->name);
+			bthread_schedule(next);
+			return;
+		}
+		if (next->has_stopped && next->should_clean) {
+			pr_debug("deleting %s\n", next->name);
+			list_del(&next->list);
+			bthread_free(next);
+		}
+	}
 }
 
 void bthread_schedule(struct bthread *to)
diff --git a/common/poller.c b/common/poller.c
index 61da5698d2..0409d3cf81 100644
--- a/common/poller.c
+++ b/common/poller.c
@@ -10,11 +10,14 @@
 #include <param.h>
 #include <poller.h>
 #include <clock.h>
-#include <work.h>
-#include <slice.h>
 
 static LIST_HEAD(poller_list);
-int poller_active;
+static int __poller_active;
+
+bool poller_active(void)
+{
+	return __poller_active;
+}
 
 int poller_register(struct poller_struct *poller, const char *name)
 {
@@ -110,23 +113,13 @@ int poller_async_unregister(struct poller_async *pa)
 void poller_call(void)
 {
 	struct poller_struct *poller, *tmp;
-	bool run_workqueues = !slice_acquired(&command_slice);
-
-	if (poller_active)
-		return;
-
-	command_slice_acquire();
-
-	if (run_workqueues)
-		wq_do_all_works();
 
-	poller_active = 1;
+	__poller_active = 1;
 
 	list_for_each_entry_safe(poller, tmp, &poller_list, list)
 		poller->func(poller);
 
-	command_slice_release();
-	poller_active = 0;
+	__poller_active = 0;
 }
 
 #if defined CONFIG_CMD_POLLER
diff --git a/common/sched.c b/common/sched.c
new file mode 100644
index 0000000000..02582b2c5e
--- /dev/null
+++ b/common/sched.c
@@ -0,0 +1,26 @@
+/* SPDX License Identifier: GPL-2.0 */
+
+#include <bthread.h>
+#include <poller.h>
+#include <work.h>
+#include <slice.h>
+#include <sched.h>
+
+void resched(void)
+{
+	bool run_workqueues = !slice_acquired(&command_slice);
+
+	if (poller_active())
+		return;
+
+	command_slice_acquire();
+
+	if (run_workqueues) {
+		wq_do_all_works();
+		bthread_reschedule();
+	}
+
+	poller_call();
+
+	command_slice_release();
+}
diff --git a/common/usbgadget.c b/common/usbgadget.c
index d4437b5169..34a685234b 100644
--- a/common/usbgadget.c
+++ b/common/usbgadget.c
@@ -48,36 +48,43 @@ static inline struct file_list *get_dfu_function(void)
 	return NULL;
 }
 
-int usbgadget_register(bool dfu, const char *dfu_opts,
-		       bool fastboot, const char *fastboot_opts,
-		       bool acm, bool export_bbu)
+int usbgadget_register(const struct usbgadget_funcs *funcs)
 {
 	int ret;
+	int flags = funcs->flags;
 	struct device_d *dev;
 	struct f_multi_opts *opts;
 
 	opts = xzalloc(sizeof(*opts));
 	opts->release = usb_multi_opts_release;
 
-	if (dfu) {
-		opts->dfu_opts.files = parse(dfu_opts);
+	if (flags & USBGADGET_DFU) {
+		opts->dfu_opts.files = parse(funcs->dfu_opts);
 		if (IS_ENABLED(CONFIG_USB_GADGET_DFU) && file_list_empty(opts->dfu_opts.files)) {
 			file_list_free(opts->dfu_opts.files);
 			opts->dfu_opts.files = get_dfu_function();
 		}
 	}
 
-	if (fastboot) {
-		opts->fastboot_opts.files = parse(fastboot_opts);
+	if (flags & USBGADGET_MASS_STORAGE) {
+		opts->ums_opts.files = parse(funcs->ums_opts);
+		if (IS_ENABLED(CONFIG_USB_GADGET_MASS_STORAGE) && file_list_empty(opts->ums_opts.files)) {
+			file_list_free(opts->ums_opts.files);
+			opts->ums_opts.files = system_partitions_get();
+		}
+	}
+
+	if (flags & USBGADGET_FASTBOOT) {
+		opts->fastboot_opts.files = parse(funcs->fastboot_opts);
 		if (IS_ENABLED(CONFIG_FASTBOOT_BASE) && file_list_empty(opts->fastboot_opts.files)) {
 			file_list_free(opts->fastboot_opts.files);
 			opts->fastboot_opts.files = get_fastboot_partitions();
 		}
 
-		opts->fastboot_opts.export_bbu = export_bbu;
+		opts->fastboot_opts.export_bbu = flags & USBGADGET_EXPORT_BBU;
 	}
 
-	opts->create_acm = acm;
+	opts->create_acm = flags & USBGADGET_ACM;
 
 	if (usb_multi_count_functions(opts) == 0) {
 		pr_warn("No functions to register\n");
@@ -111,14 +118,21 @@ err:
 
 static int usbgadget_autostart_set(struct param_d *param, void *ctx)
 {
+	struct usbgadget_funcs funcs = {};
 	static bool started;
-	bool fastboot_bbu = get_fastboot_bbu();
 	int err;
 
 	if (!autostart || started)
 		return 0;
 
-	err = usbgadget_register(true, NULL, true, NULL, acm, fastboot_bbu);
+	if (get_fastboot_bbu())
+		funcs.flags |= USBGADGET_EXPORT_BBU;
+	if (acm)
+		funcs.flags |= USBGADGET_ACM;
+
+	funcs.flags |= USBGADGET_DFU | USBGADGET_FASTBOOT | USBGADGET_MASS_STORAGE;
+
+	err = usbgadget_register(&funcs);
 	if (!err)
 		started = true;
 
diff --git a/drivers/input/Kconfig b/drivers/input/Kconfig
index ff3e9d33f6..ba9fa25a3d 100644
--- a/drivers/input/Kconfig
+++ b/drivers/input/Kconfig
@@ -73,7 +73,8 @@ config INPUT_SPECIALKEYS
 
 config VIRTIO_INPUT
 	bool "Virtio input driver"
-	depends on VIRTIO && BTHREAD
+	depends on VIRTIO
+	select POLLER
 	select INPUT
 	help
 	 This driver supports virtio keyboard input devices.
diff --git a/drivers/input/virtio_input.c b/drivers/input/virtio_input.c
index 9c2e4d923f..b354933209 100644
--- a/drivers/input/virtio_input.c
+++ b/drivers/input/virtio_input.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0-only
 
 #include <common.h>
-#include <bthread.h>
+#include <poller.h>
 #include <linux/virtio.h>
 #include <linux/virtio_config.h>
 #include <linux/virtio_ring.h>
@@ -17,7 +17,7 @@ struct virtio_input {
 	struct virtio_device       *vdev;
 	struct virtqueue           *evt, *sts;
 	struct virtio_input_event  evts[64];
-	struct bthread             *bthread;
+	struct poller_struct       poller;
 	struct sound_card          beeper;
 	unsigned long              sndbit[BITS_TO_LONGS(SND_CNT)];
 };
@@ -100,21 +100,17 @@ static int virtinput_recv_status(struct virtio_input *vi)
 	return i;
 }
 
-static int virtinput_poll_vqs(void *_vi)
+static void virtinput_poll_vqs(struct poller_struct *poller)
 {
-	struct virtio_input *vi = _vi;
+	struct virtio_input *vi = container_of(poller, struct virtio_input, poller);
 
-	while (!bthread_should_stop()) {
-		int bufs = 0;
+	int bufs = 0;
 
-		bufs += virtinput_recv_events(vi);
-		bufs += virtinput_recv_status(vi);
+	bufs += virtinput_recv_events(vi);
+	bufs += virtinput_recv_status(vi);
 
-		if (bufs)
-			virtqueue_kick(vi->evt);
-	}
-
-	return 0;
+	if (bufs)
+		virtqueue_kick(vi->evt);
 }
 
 static u8 virtinput_cfg_select(struct virtio_input *vi,
@@ -213,6 +209,8 @@ static int virtinput_probe(struct virtio_device *vdev)
 			   name, min(size, sizeof(name)));
 	name[size] = '\0';
 
+	dev_info(&vdev->dev, "'%s' detected\n", name);
+
 	virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_SND,
 			   vi->sndbit, SND_CNT);
 
@@ -224,12 +222,12 @@ static int virtinput_probe(struct virtio_device *vdev)
 
 	virtinput_fill_evt(vi);
 
-	vi->bthread = bthread_run(virtinput_poll_vqs, vi,
-				  "%s/input0", dev_name(&vdev->dev));
-	if (!vi->bthread) {
-		err = -ENOMEM;
-		goto err_bthread_run;
-	}
+	vi->poller.func = virtinput_poll_vqs;
+	snprintf(name, sizeof(name), "%s/input0", dev_name(&vdev->dev));
+
+	err = poller_register(&vi->poller, name);
+	if (err)
+		goto err_poller_register;
 
 	if (IS_ENABLED(CONFIG_SOUND) &&
 	    (vi->sndbit[0] & (BIT_MASK(SND_BELL) | BIT_MASK(SND_TONE)))) {
@@ -246,12 +244,10 @@ static int virtinput_probe(struct virtio_device *vdev)
 			dev_info(&vdev->dev, "bell registered\n");
 	}
 
-	dev_info(&vdev->dev, "'%s' probed\n", name);
-
 	return 0;
 
-err_bthread_run:
-	bthread_free(vi->bthread);
+err_poller_register:
+	input_device_unregister(&vi->idev);
 err_input_register:
 	vdev->config->del_vqs(vdev);
 err_init_vq:
@@ -263,8 +259,7 @@ static void virtinput_remove(struct virtio_device *vdev)
 {
 	struct virtio_input *vi = vdev->priv;
 
-	bthread_stop(vi->bthread);
-	bthread_free(vi->bthread);
+	poller_unregister(&vi->poller);
 
 	vdev->config->reset(vdev);
 	vdev->config->del_vqs(vdev);
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 4ed6cbbee1..574a6821ad 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -36,10 +36,12 @@ config USB_GADGET_AUTOSTART
 	bool
 	default y
 	prompt "Automatically start usbgadget on boot"
+	select SYSTEM_PARTITIONS if USB_GADGET_MASS_STORAGE
 	help
 	  Enabling this option allows to automatically start a dfu or
 	  fastboot gadget during boot. This behaviour is controlled with
-	  the global.usbgadget.dfu_function and global.fastboot.* variables.
+	  the global.usbgadget.dfu_function, global.system.partitions
+	  and global.fastboot.* variables.
 
 comment "USB Gadget drivers"
 
@@ -57,4 +59,15 @@ config USB_GADGET_FASTBOOT
 	select BANNER
 	select FASTBOOT_BASE
 	prompt "Android Fastboot USB Gadget"
+
+config USB_GADGET_MASS_STORAGE
+	bool
+	select BTHREAD
+	prompt "USB Mass Storage Gadget"
+	help
+	  The Mass Storage Gadget acts as a USB Mass Storage disk drive.
+	  As its storage repository it can use a regular file or a block
+	  device. Multiple storages can be specified at once on
+	  instantiation time.
+
 endif
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 27673fcf0e..5ba4920c08 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_USB_GADGET) += composite.o config.o usbstring.o epautoconf.o udc-co
 obj-$(CONFIG_USB_GADGET_SERIAL) += u_serial.o serial.o f_serial.o f_acm.o
 obj-$(CONFIG_USB_GADGET_DFU) += dfu.o
 obj-$(CONFIG_USB_GADGET_FASTBOOT) += f_fastboot.o
+obj-$(CONFIG_USB_GADGET_MASS_STORAGE) += f_mass_storage.o storage_common.o
 obj-$(CONFIG_USB_GADGET_DRIVER_ARC) += fsl_udc.o
 pbl-$(CONFIG_USB_GADGET_DRIVER_ARC_PBL) += fsl_udc_pbl.o
 obj-$(CONFIG_USB_GADGET_DRIVER_AT91) += at91_udc.o
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
new file mode 100644
index 0000000000..753042125d
--- /dev/null
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -0,0 +1,2714 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * f_mass_storage.c -- Mass Storage USB Composite Function
+ *
+ * Copyright (C) 2003-2008 Alan Stern
+ * Copyright (C) 2009 Samsung Electronics
+ *                    Author: Michal Nazarewicz <m.nazarewicz@samsung.com>
+ * All rights reserved.
+ */
+
+/*
+ * The Mass Storage Function acts as a USB Mass Storage device,
+ * appearing to the host as a disk drive or as a CD-ROM drive.  In
+ * addition to providing an example of a genuinely useful composite
+ * function for a USB device, it also illustrates a technique of
+ * double-buffering for increased throughput.
+ *
+ * Function supports multiple logical units (LUNs).  Backing storage
+ * for each LUN is provided by a regular file or a block device.
+ * Access for each LUN can be limited to read-only.  Moreover, the
+ * function can indicate that LUN is removable and/or CD-ROM.  (The
+ * later implies read-only access.)
+ *
+ * MSF is configured by specifying a fsg_config structure.  It has the
+ * following fields:
+ *
+ *	nluns		Number of LUNs function have (anywhere from 1
+ *				to FSG_MAX_LUNS which is 8).
+ *	luns		An array of LUN configuration values.  This
+ *				should be filled for each LUN that
+ *				function will include (ie. for "nluns"
+ *				LUNs).  Each element of the array has
+ *				the following fields:
+ *	->filename	The path to the backing file for the LUN.
+ *				Required if LUN is not marked as
+ *				removable.
+ *	->ro		Flag specifying access to the LUN shall be
+ *				read-only.  This is implied if CD-ROM
+ *				emulation is enabled as well as when
+ *				it was impossible to open "filename"
+ *				in R/W mode.
+ *	->removable	Flag specifying that LUN shall be indicated as
+ *				being removable.
+ *	->cdrom		Flag specifying that LUN shall be reported as
+ *				being a CD-ROM.
+ *
+ *	vendor_name
+ *	product_name
+ *	release		Information used as a reply to INQUIRY
+ *				request.  To use default set to NULL,
+ *				NULL, 0xffff respectively.  The first
+ *				field should be 8 and the second 16
+ *				characters or less.
+ *
+ *	can_stall	Set to permit function to halt bulk endpoints.
+ *				Disabled on some USB devices known not
+ *				to work correctly.  You should set it
+ *				to true.
+ *
+ * If "removable" is not set for a LUN then a backing file must be
+ * specified.  If it is set, then NULL filename means the LUN's medium
+ * is not loaded (an empty string as "filename" in the fsg_config
+ * structure causes error).  The CD-ROM emulation includes a single
+ * data track and no audio tracks; hence there need be only one
+ * backing file per LUN.  Note also that the CD-ROM block length is
+ * set to 512 rather than the more common value 2048.
+ *
+ *
+ * MSF includes support for module parameters.  If gadget using it
+ * decides to use it, the following module parameters will be
+ * available:
+ *
+ *	file=filename[,filename...]
+ *			Names of the files or block devices used for
+ *				backing storage.
+ *	ro=b[,b...]	Default false, boolean for read-only access.
+ *	removable=b[,b...]
+ *			Default true, boolean for removable media.
+ *	cdrom=b[,b...]	Default false, boolean for whether to emulate
+ *				a CD-ROM drive.
+ *	luns=N		Default N = number of filenames, number of
+ *				LUNs to support.
+ *	stall		Default determined according to the type of
+ *				USB device controller (usually true),
+ *				boolean to permit the driver to halt
+ *				bulk endpoints.
+ *
+ * The module parameters may be prefixed with some string.  You need
+ * to consult gadget's documentation or source to verify whether it is
+ * using those module parameters and if it does what are the prefixes
+ * (look for FSG_MODULE_PARAMETERS() macro usage, what's inside it is
+ * the prefix).
+ *
+ *
+ * Requirements are modest; only a bulk-in and a bulk-out endpoint are
+ * needed.  The memory requirement amounts to two 16K buffers, size
+ * configurable by a parameter.  Support is included for both
+ * full-speed and high-speed operation.
+ *
+ * Note that the driver is slightly non-portable in that it assumes a
+ * single memory/DMA buffer will be useable for bulk-in, bulk-out, and
+ * interrupt-in endpoints.  With most device controllers this isn't an
+ * issue, but there may be some with hardware restrictions that prevent
+ * a buffer from being used by more than one endpoint.
+ *
+ * When a LUN receive an "eject" SCSI request (Start/Stop Unit),
+ * if the LUN is removable, the backing file is released to simulate
+ * ejection.
+ *
+ *
+ * This function is heavily based on "File-backed Storage Gadget" by
+ * Alan Stern which in turn is heavily based on "Gadget Zero" by David
+ * Brownell.  The driver's SCSI command interface was based on the
+ * "Information technology - Small Computer System Interface - 2"
+ * document from X3T9.2 Project 375D, Revision 10L, 7-SEP-93,
+ * available at <http://www.t10.org/ftp/t10/drafts/s2/s2-r10l.pdf>.
+ * The single exception is opcode 0x23 (READ FORMAT CAPACITIES), which
+ * was based on the "Universal Serial Bus Mass Storage Class UFI
+ * Command Specification" document, Revision 1.0, December 14, 1998,
+ * available at
+ * <http://www.usb.org/developers/devclass_docs/usbmass-ufi10.pdf>.
+ */
+
+/*
+ *				Driver Design
+ *
+ * The MSF is fairly straightforward.  There is a main kernel
+ * thread that handles most of the work.  Interrupt routines field
+ * callbacks from the controller driver: bulk- and interrupt-request
+ * completion notifications, endpoint-0 events, and disconnect events.
+ * Completion events are passed to the main thread by wakeup calls.  Many
+ * ep0 requests are handled at interrupt time, but SetInterface,
+ * SetConfiguration, and device reset requests are forwarded to the
+ * thread in the form of "exceptions" using SIGUSR1 signals (since they
+ * should interrupt any ongoing file I/O operations).
+ *
+ * The thread's main routine implements the standard command/data/status
+ * parts of a SCSI interaction.  It and its subroutines are full of tests
+ * for pending signals/exceptions -- all this polling is necessary since
+ * the kernel has no setjmp/longjmp equivalents.  (Maybe this is an
+ * indication that the driver really wants to be running in userspace.)
+ * An important point is that so long as the thread is alive it keeps an
+ * open reference to the backing file.  This will prevent unmounting
+ * the backing file's underlying filesystem and could cause problems
+ * during system shutdown, for example.  To prevent such problems, the
+ * thread catches INT, TERM, and KILL signals and converts them into
+ * an EXIT exception.
+ *
+ * In normal operation the main thread is started during the gadget's
+ * fsg_bind() callback and stopped during fsg_unbind().  But it can
+ * also exit when it receives a signal, and there's no point leaving
+ * the gadget running when the thread is dead.  At of this moment, MSF
+ * provides no way to deregister the gadget when thread dies -- maybe
+ * a callback functions is needed.
+ *
+ * To provide maximum throughput, the driver uses a circular pipeline of
+ * buffer heads (struct fsg_buffhd).  In principle the pipeline can be
+ * arbitrarily long; in practice the benefits don't justify having more
+ * than 2 stages (i.e., double buffering).  But it helps to think of the
+ * pipeline as being a long one.  Each buffer head contains a bulk-in and
+ * a bulk-out request pointer (since the buffer can be used for both
+ * output and input -- directions always are given from the host's
+ * point of view) as well as a pointer to the buffer and various state
+ * variables.
+ *
+ * Use of the pipeline follows a simple protocol.  There is a variable
+ * (fsg->next_buffhd_to_fill) that points to the next buffer head to use.
+ * At any time that buffer head may still be in use from an earlier
+ * request, so each buffer head has a state variable indicating whether
+ * it is EMPTY, FULL, or BUSY.  Typical use involves waiting for the
+ * buffer head to be EMPTY, filling the buffer either by file I/O or by
+ * USB I/O (during which the buffer head is BUSY), and marking the buffer
+ * head FULL when the I/O is complete.  Then the buffer will be emptied
+ * (again possibly by USB I/O, during which it is marked BUSY) and
+ * finally marked EMPTY again (possibly by a completion routine).
+ *
+ * A module parameter tells the driver to avoid stalling the bulk
+ * endpoints wherever the transport specification allows.  This is
+ * necessary for some UDCs like the SuperH, which cannot reliably clear a
+ * halt on a bulk endpoint.  However, under certain circumstances the
+ * Bulk-only specification requires a stall.  In such cases the driver
+ * will halt the endpoint and set a flag indicating that it should clear
+ * the halt in software during the next device reset.  Hopefully this
+ * will permit everything to work correctly.  Furthermore, although the
+ * specification allows the bulk-out endpoint to halt when the host sends
+ * too much data, implementing this would cause an unavoidable race.
+ * The driver will always use the "no-stall" approach for OUT transfers.
+ *
+ * One subtle point concerns sending status-stage responses for ep0
+ * requests.  Some of these requests, such as device reset, can involve
+ * interrupting an ongoing file I/O operation, which might take an
+ * arbitrarily long time.  During that delay the host might give up on
+ * the original ep0 request and issue a new one.  When that happens the
+ * driver should not notify the host about completion of the original
+ * request, as the host will no longer be waiting for it.  So the driver
+ * assigns to each ep0 request a unique tag, and it keeps track of the
+ * tag value of the request associated with a long-running exception
+ * (device-reset, interface-change, or configuration-change).  When the
+ * exception handler is finished, the status-stage response is submitted
+ * only if the current ep0 request tag is equal to the exception request
+ * tag.  Thus only the most recently received ep0 request will get a
+ * status-stage response.
+ *
+ * Warning: This driver source file is too long.  It ought to be split up
+ * into a header file plus about 3 separate .c files, to handle the details
+ * of the Gadget, USB Mass Storage, and SCSI protocols.
+ */
+
+/* #define VERBOSE_DEBUG */
+/* #define DUMP_MSGS */
+
+#define pr_fmt(fmt) "f_ums: " fmt
+
+#include <common.h>
+#include <unistd.h>
+#include <linux/stat.h>
+#include <linux/wait.h>
+#include <fcntl.h>
+#include <file-list.h>
+#include <dma.h>
+#include <linux/bug.h>
+#include <linux/rwsem.h>
+#include <linux/pagemap.h>
+#include <disks.h>
+#include <scsi.h>
+
+#include <linux/err.h>
+#include <usb/mass_storage.h>
+
+#include <asm/unaligned.h>
+#include <linux/bitops.h>
+#include <usb/gadget.h>
+#include <usb/composite.h>
+#include <linux/bitmap.h>
+#include <linux/completion.h>
+#include <bthread.h>
+#include <sched.h>
+
+
+/*------------------------------------------------------------------------*/
+
+#define FSG_DRIVER_DESC	"ums"
+#define UMS_NAME_LEN 16
+
+#define FSG_DRIVER_VERSION	"2012/06/5"
+
+static const char fsg_string_interface[] = "Mass Storage";
+
+#include "storage_common.h"
+
+/* Static strings, in UTF-8 (for simplicity we use only ASCII characters) */
+struct usb_string		fsg_strings[] = {
+	{FSG_STRING_INTERFACE,		fsg_string_interface},
+	{}
+};
+
+static struct usb_gadget_strings	fsg_stringtab = {
+	.language	= 0x0409,		/* en-us */
+	.strings	= fsg_strings,
+};
+
+/*-------------------------------------------------------------------------*/
+
+struct bthread *thread_task;
+
+struct kref {int x; };
+
+struct fsg_dev;
+
+static struct file_list *ums_files;
+
+/* Data shared by all the FSG instances. */
+struct fsg_common {
+	struct usb_gadget	*gadget;
+	struct fsg_dev		*fsg, *new_fsg;
+
+	struct usb_ep		*ep0;		/* Copy of gadget->ep0 */
+	struct usb_request	*ep0req;	/* Copy of cdev->req */
+	unsigned int		ep0_req_tag;
+
+	struct fsg_buffhd	*next_buffhd_to_fill;
+	struct fsg_buffhd	*next_buffhd_to_drain;
+	struct fsg_buffhd	buffhds[FSG_NUM_BUFFERS];
+
+	int			cmnd_size;
+	u8			cmnd[MAX_COMMAND_SIZE];
+
+	unsigned int		nluns;
+	unsigned int		lun;
+	struct fsg_lun          luns[FSG_MAX_LUNS];
+
+	unsigned int		bulk_out_maxpacket;
+	enum fsg_state		state;		/* For exception handling */
+	unsigned int		exception_req_tag;
+
+	enum data_direction	data_dir;
+	u32			data_size;
+	u32			data_size_from_cmnd;
+	u32			tag;
+	u32			residue;
+	u32			usb_amount_left;
+
+	unsigned int		can_stall:1;
+	unsigned int		phase_error:1;
+	unsigned int		short_packet_received:1;
+	unsigned int		bad_lun_okay:1;
+	unsigned int		running:1;
+
+	struct completion	thread_wakeup_needed;
+
+	/* Callback functions. */
+	const struct fsg_operations	*ops;
+	/* Gadget's private data. */
+	void			*private_data;
+
+	const char *vendor_name;		/*  8 characters or less */
+	const char *product_name;		/* 16 characters or less */
+	u16 release;
+
+	/* Vendor (8 chars), product (16 chars), release (4
+	 * hexadecimal digits) and NUL byte */
+	char inquiry_string[8 + 16 + 4 + 1];
+};
+
+struct fsg_config {
+	unsigned nluns;
+	struct fsg_lun_config {
+		const char *filename;
+		char ro;
+		char removable;
+		char cdrom;
+		char nofua;
+	} luns[FSG_MAX_LUNS];
+
+	/* Callback functions. */
+	const struct fsg_operations     *ops;
+	/* Gadget's private data. */
+	void			*private_data;
+
+	const char *vendor_name;		/*  8 characters or less */
+	const char *product_name;		/* 16 characters or less */
+
+	char			can_stall;
+};
+
+struct fsg_dev {
+	struct usb_function	function;
+	struct usb_gadget	*gadget;	/* Copy of cdev->gadget */
+	struct fsg_common	*common;
+
+	u16			interface_number;
+
+	unsigned int		bulk_in_enabled:1;
+	unsigned int		bulk_out_enabled:1;
+
+	unsigned long		atomic_bitflags;
+#define IGNORE_BULK_OUT		0
+
+	struct usb_ep		*bulk_in;
+	struct usb_ep		*bulk_out;
+};
+
+
+static inline int __fsg_is_set(struct fsg_common *common,
+			       const char *func, unsigned line)
+{
+	if (common->fsg)
+		return 1;
+	ERROR(common, "common->fsg is NULL in %s at %u\n", func, line);
+	WARN_ON(1);
+
+	return 0;
+}
+
+#define fsg_is_set(common) likely(__fsg_is_set(common, __func__, __LINE__))
+
+
+static inline struct fsg_dev *fsg_from_func(struct usb_function *f)
+{
+	return container_of(f, struct fsg_dev, function);
+}
+
+static inline struct f_ums_opts *
+fsg_opts_from_func_inst(const struct usb_function_instance *fi)
+{
+	return container_of(fi, struct f_ums_opts, func_inst);
+}
+
+typedef void (*fsg_routine_t)(struct fsg_dev *);
+
+static int exception_in_progress(struct fsg_common *common)
+{
+	return common->state > FSG_STATE_IDLE;
+}
+
+/* Make bulk-out requests be divisible by the maxpacket size */
+static void set_bulk_out_req_length(struct fsg_common *common,
+		struct fsg_buffhd *bh, unsigned int length)
+{
+	unsigned int	rem;
+
+	bh->bulk_out_intended_length = length;
+	rem = length % common->bulk_out_maxpacket;
+	if (rem > 0)
+		length += common->bulk_out_maxpacket - rem;
+	bh->outreq->length = length;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static struct f_ums_opts ums[14]; // FIXME
+static int ums_count;
+
+static int fsg_set_halt(struct fsg_dev *fsg, struct usb_ep *ep)
+{
+	const char	*name;
+
+	if (ep == fsg->bulk_in)
+		name = "bulk-in";
+	else if (ep == fsg->bulk_out)
+		name = "bulk-out";
+	else
+		name = ep->name;
+	DBG(fsg, "%s set halt\n", name);
+	return usb_ep_set_halt(ep);
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* These routines may be called in process context or in_irq */
+
+/* Caller must hold fsg->lock */
+static void wakeup_thread(struct fsg_common *common)
+{
+	complete(&common->thread_wakeup_needed);
+}
+
+static void report_exception(const char *prefix, enum fsg_state state)
+{
+	const char *msg = "<unknown>";
+	switch (state) {
+	/* This one isn't used anywhere */
+	case FSG_STATE_COMMAND_PHASE:
+		msg = "Command Phase";
+		break;
+	case FSG_STATE_DATA_PHASE:
+		msg = "Data Phase";
+		break;
+	case FSG_STATE_STATUS_PHASE:
+		msg = "Status Phase";
+		break;
+
+	case FSG_STATE_IDLE:
+		msg = "Idle";
+		break;
+	case FSG_STATE_ABORT_BULK_OUT:
+		msg = "abort bulk out";
+		break;
+	case FSG_STATE_RESET:
+		msg = "reset";
+		break;
+	case FSG_STATE_INTERFACE_CHANGE:
+		msg = "interface change";
+		break;
+	case FSG_STATE_CONFIG_CHANGE:
+		msg = "config change";
+		break;
+	case FSG_STATE_DISCONNECT:
+		msg = "disconnect";
+		break;
+	case FSG_STATE_EXIT:
+		msg = "exit";
+		break;
+	case FSG_STATE_TERMINATED:
+		msg = "terminated";
+		break;
+	}
+
+	pr_debug("%s: %s\n", prefix, msg);
+}
+
+static void raise_exception(struct fsg_common *common, enum fsg_state new_state)
+{
+	/* Do nothing if a higher-priority exception is already in progress.
+	 * If a lower-or-equal priority exception is in progress, preempt it
+	 * and notify the main thread by sending it a signal. */
+	if (common->state <= new_state) {
+		report_exception("raising", new_state);
+		common->exception_req_tag = common->ep0_req_tag;
+		common->state = new_state;
+		wakeup_thread(common);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int ep0_queue(struct fsg_common *common)
+{
+	int	rc;
+
+	rc = usb_ep_queue(common->ep0, common->ep0req);
+	common->ep0->driver_data = common;
+	if (rc != 0 && rc != -ESHUTDOWN) {
+		/* We can't do much more than wait for a reset */
+		WARNING(common, "error in submission: %s --> %d\n",
+			common->ep0->name, rc);
+	}
+	return rc;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* Bulk and interrupt endpoint completion handlers.
+ * These always run in_irq. */
+
+static void bulk_in_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct fsg_common	*common = ep->driver_data;
+	struct fsg_buffhd	*bh = req->context;
+
+	if (req->status || req->actual != req->length)
+		DBG(common, "%s --> %d, %u/%u\n", __func__,
+				req->status, req->actual, req->length);
+	if (req->status == -ECONNRESET)		/* Request was cancelled */
+		usb_ep_fifo_flush(ep);
+
+	/* Hold the lock while we update the request and buffer states */
+	bh->inreq_busy = 0;
+	bh->state = BUF_STATE_EMPTY;
+	wakeup_thread(common);
+}
+
+static void bulk_out_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct fsg_common	*common = ep->driver_data;
+	struct fsg_buffhd	*bh = req->context;
+
+	dump_msg(common, "bulk-out", req->buf, req->actual);
+	if (req->status || req->actual != bh->bulk_out_intended_length)
+		DBG(common, "%s --> %d, %u/%u\n", __func__,
+				req->status, req->actual,
+				bh->bulk_out_intended_length);
+	if (req->status == -ECONNRESET)		/* Request was cancelled */
+		usb_ep_fifo_flush(ep);
+
+	/* Hold the lock while we update the request and buffer states */
+	bh->outreq_busy = 0;
+	bh->state = BUF_STATE_FULL;
+	wakeup_thread(common);
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* Ep0 class-specific handlers.  These always run in_irq. */
+
+static int fsg_setup(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct fsg_dev		*fsg = fsg_from_func(f);
+	struct usb_request	*req = fsg->common->ep0req;
+	u16			w_index = get_unaligned_le16(&ctrl->wIndex);
+	u16			w_value = get_unaligned_le16(&ctrl->wValue);
+	u16			w_length = get_unaligned_le16(&ctrl->wLength);
+
+	if (!fsg_is_set(fsg->common))
+		return -EOPNOTSUPP;
+
+	switch (ctrl->bRequest) {
+
+	case US_BULK_RESET_REQUEST:
+		if (ctrl->bRequestType !=
+		    (USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE))
+			break;
+		if (w_index != fsg->interface_number || w_value != 0)
+			return -EDOM;
+
+		/* Raise an exception to stop the current operation
+		 * and reinitialize our state. */
+		DBG(fsg, "bulk reset request\n");
+		raise_exception(fsg->common, FSG_STATE_RESET);
+		return DELAYED_STATUS;
+
+	case US_BULK_GET_MAX_LUN:
+		if (ctrl->bRequestType !=
+		    (USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE))
+			break;
+		if (w_index != fsg->interface_number || w_value != 0)
+			return -EDOM;
+		VDBG(fsg, "get max LUN\n");
+		*(u8 *) req->buf = fsg->common->nluns - 1;
+
+		/* Respond with data/status */
+		req->length = min((u16)1, w_length);
+		return ep0_queue(fsg->common);
+	}
+
+	VDBG(fsg,
+	     "unknown class-specific control req "
+	     "%02x.%02x v%04x i%04x l%u\n",
+	     ctrl->bRequestType, ctrl->bRequest,
+	     get_unaligned_le16(&ctrl->wValue), w_index, w_length);
+	return -EOPNOTSUPP;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* All the following routines run in process context */
+
+/* Use this for bulk or interrupt transfers, not ep0 */
+static void start_transfer(struct fsg_dev *fsg, struct usb_ep *ep,
+		struct usb_request *req, int *pbusy,
+		enum fsg_buffer_state *state)
+{
+	int	rc;
+
+	if (ep == fsg->bulk_in)
+		dump_msg(fsg, "bulk-in", req->buf, req->length);
+
+	*pbusy = 1;
+	*state = BUF_STATE_BUSY;
+	rc = usb_ep_queue(ep, req);
+	if (rc != 0) {
+		*pbusy = 0;
+		*state = BUF_STATE_EMPTY;
+
+		/* We can't do much more than wait for a reset */
+
+		/* Note: currently the net2280 driver fails zero-length
+		 * submissions if DMA is enabled. */
+		if (rc != -ESHUTDOWN && !(rc == -EOPNOTSUPP &&
+						req->length == 0))
+			WARNING(fsg, "error in submission: %s --> %d\n",
+					ep->name, rc);
+	}
+}
+
+#define START_TRANSFER_OR(common, ep_name, req, pbusy, state)		\
+	if (fsg_is_set(common))						\
+		start_transfer((common)->fsg, (common)->fsg->ep_name,	\
+			       req, pbusy, state);			\
+	else
+
+#define START_TRANSFER(common, ep_name, req, pbusy, state)		\
+	START_TRANSFER_OR(common, ep_name, req, pbusy, state) (void)0
+
+static int sleep_thread(struct fsg_common *common)
+{
+	int ret;
+
+	/* Wait until a signal arrives or we are woken up */
+	ret = wait_for_completion_interruptible(&common->thread_wakeup_needed);
+	if (ret)
+		return ret;
+
+	reinit_completion(&common->thread_wakeup_needed);
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int do_read(struct fsg_common *common)
+{
+	struct fsg_lun		*curlun = &common->luns[common->lun];
+	u32			lba;
+	struct fsg_buffhd	*bh;
+	int			rc;
+	u32			amount_left;
+	loff_t			file_offset;
+	unsigned int		amount;
+	unsigned int		partial_page;
+	ssize_t			nread;
+
+	/* Get the starting Logical Block Address and check that it's
+	 * not too big */
+	if (common->cmnd[0] == SCSI_READ6)
+		lba = get_unaligned_be24(&common->cmnd[1]);
+	else {
+		lba = get_unaligned_be32(&common->cmnd[2]);
+
+		/* We allow DPO (Disable Page Out = don't save data in the
+		 * cache) and FUA (Force Unit Access = don't read from the
+		 * cache), but we don't implement them. */
+		if ((common->cmnd[1] & ~0x18) != 0) {
+			curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+			return -EINVAL;
+		}
+	}
+	if (lba >= curlun->num_sectors) {
+		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+		return -EINVAL;
+	}
+	file_offset = ((loff_t) lba) << 9;
+
+	/* Carry out the file reads */
+	amount_left = common->data_size_from_cmnd;
+	if (unlikely(amount_left == 0))
+		return -EIO;		/* No default reply */
+
+	for (;;) {
+		/* Wait for the next buffer to become available */
+		bh = common->next_buffhd_to_fill;
+		while (bh->state != BUF_STATE_EMPTY) {
+			rc = sleep_thread(common);
+			if (rc)
+				return rc;
+		}
+
+		/* Figure out how much we need to read:
+		 * Try to read the remaining amount.
+		 * But don't read more than the buffer size.
+		 * And don't try to read past the end of the file.
+		 * Finally, if we're not at a page boundary, don't read past
+		 *	the next page.
+		 * If this means reading 0 then we were asked to read past
+		 *	the end of file. */
+		amount = min(amount_left, FSG_BUFLEN);
+		partial_page = file_offset & (PAGE_CACHE_SIZE - 1);
+		if (partial_page > 0)
+			amount = min(amount, (unsigned int) PAGE_CACHE_SIZE -
+					partial_page);
+
+
+		/* If we were asked to read past the end of file,
+		 * end with an empty buffer. */
+		if (amount == 0) {
+			curlun->sense_data =
+					SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+			curlun->info_valid = 1;
+			bh->inreq->length = 0;
+			bh->state = BUF_STATE_FULL;
+			break;
+		}
+
+		/* Perform the read */
+		nread = pread(ums[common->lun].fd, bh->buf, amount, file_offset);
+
+		VLDBG(curlun, "file read %u @ %llu -> %zd\n", amount,
+				(unsigned long long) file_offset,
+				nread);
+		if (nread <= 0) {
+			const char *err = nread ? strerror(-nread) : "EOF";
+			LDBG(curlun, "error in file read: %s\n", err);
+			nread = 0;
+		} else if (nread < amount) {
+			LDBG(curlun, "partial file read: %d/%u\n",
+					(int) nread, amount);
+			nread -= (nread & 511);	/* Round down to a block */
+		}
+		file_offset  += nread;
+		amount_left  -= nread;
+		common->residue -= nread;
+		bh->inreq->length = nread;
+		bh->state = BUF_STATE_FULL;
+
+		/* If an error occurred, report it and its position */
+		if (nread < amount) {
+			curlun->sense_data = SS_UNRECOVERED_READ_ERROR;
+			curlun->info_valid = 1;
+			break;
+		}
+
+		if (amount_left == 0)
+			break;		/* No more left to read */
+
+		/* Send this buffer and go read some more */
+		bh->inreq->zero = 0;
+		START_TRANSFER_OR(common, bulk_in, bh->inreq,
+			       &bh->inreq_busy, &bh->state)
+			/* Don't know what to do if
+			 * common->fsg is NULL */
+			return -EIO;
+		common->next_buffhd_to_fill = bh->next;
+	}
+
+	return -EIO;		/* No default reply */
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int do_write(struct fsg_common *common)
+{
+	struct fsg_lun		*curlun = &common->luns[common->lun];
+	u32			lba;
+	struct fsg_buffhd	*bh;
+	int			get_some_more;
+	u32			amount_left_to_req, amount_left_to_write;
+	loff_t			usb_offset, file_offset;
+	unsigned int		amount;
+	unsigned int		partial_page;
+	ssize_t			nwritten;
+	int			rc;
+
+	if (curlun->ro) {
+		curlun->sense_data = SS_WRITE_PROTECTED;
+		return -EINVAL;
+	}
+
+	/* Get the starting Logical Block Address and check that it's
+	 * not too big */
+	if (common->cmnd[0] == SCSI_WRITE6)
+		lba = get_unaligned_be24(&common->cmnd[1]);
+	else {
+		lba = get_unaligned_be32(&common->cmnd[2]);
+
+		/* We allow DPO (Disable Page Out = don't save data in the
+		 * cache) and FUA (Force Unit Access = write directly to the
+		 * medium).  We don't implement DPO; we implement FUA by
+		 * performing synchronous output. */
+		if (common->cmnd[1] & ~0x18) {
+			curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+			return -EINVAL;
+		}
+	}
+	if (lba >= curlun->num_sectors) {
+		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+		return -EINVAL;
+	}
+
+	/* Carry out the file writes */
+	get_some_more = 1;
+	file_offset = usb_offset = ((loff_t) lba) << 9;
+	amount_left_to_req = common->data_size_from_cmnd;
+	amount_left_to_write = common->data_size_from_cmnd;
+
+	while (amount_left_to_write > 0) {
+
+		/* Queue a request for more data from the host */
+		bh = common->next_buffhd_to_fill;
+		if (bh->state == BUF_STATE_EMPTY && get_some_more) {
+
+			/* Figure out how much we want to get:
+			 * Try to get the remaining amount.
+			 * But don't get more than the buffer size.
+			 * And don't try to go past the end of the file.
+			 * If we're not at a page boundary,
+			 *	don't go past the next page.
+			 * If this means getting 0, then we were asked
+			 *	to write past the end of file.
+			 * Finally, round down to a block boundary. */
+			amount = min(amount_left_to_req, FSG_BUFLEN);
+			partial_page = usb_offset & (PAGE_CACHE_SIZE - 1);
+			if (partial_page > 0)
+				amount = min(amount,
+	(unsigned int) PAGE_CACHE_SIZE - partial_page);
+
+			if (amount == 0) {
+				get_some_more = 0;
+				curlun->sense_data =
+					SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+				curlun->info_valid = 1;
+				continue;
+			}
+			amount -= (amount & 511);
+			if (amount == 0) {
+
+				/* Why were we were asked to transfer a
+				 * partial block? */
+				get_some_more = 0;
+				continue;
+			}
+
+			/* Get the next buffer */
+			usb_offset += amount;
+			common->usb_amount_left -= amount;
+			amount_left_to_req -= amount;
+			if (amount_left_to_req == 0)
+				get_some_more = 0;
+
+			/* amount is always divisible by 512, hence by
+			 * the bulk-out maxpacket size */
+			bh->outreq->length = amount;
+			bh->bulk_out_intended_length = amount;
+			bh->outreq->short_not_ok = 1;
+			START_TRANSFER_OR(common, bulk_out, bh->outreq,
+					  &bh->outreq_busy, &bh->state)
+				/* Don't know what to do if
+				 * common->fsg is NULL */
+				return -EIO;
+			common->next_buffhd_to_fill = bh->next;
+			continue;
+		}
+
+		/* Write the received data to the backing file */
+		bh = common->next_buffhd_to_drain;
+		if (bh->state == BUF_STATE_EMPTY && !get_some_more)
+			break;			/* We stopped early */
+		if (bh->state == BUF_STATE_FULL) {
+			common->next_buffhd_to_drain = bh->next;
+			bh->state = BUF_STATE_EMPTY;
+
+			/* Did something go wrong with the transfer? */
+			if (bh->outreq->status != 0) {
+				curlun->sense_data = SS_COMMUNICATION_FAILURE;
+				curlun->info_valid = 1;
+				break;
+			}
+
+			amount = bh->outreq->actual;
+
+			/* Perform the write */
+			nwritten = pwrite(ums[common->lun].fd, bh->buf, amount, file_offset);
+
+			VLDBG(curlun, "file write %u @ %llu -> %zd\n", amount,
+					(unsigned long long) file_offset,
+					nwritten);
+
+			if (nwritten < 0) {
+				LDBG(curlun, "error in file write: %pe\n", ERR_PTR(nwritten));
+				nwritten = 0;
+			} else if (nwritten < amount) {
+				LDBG(curlun, "partial file write: %d/%u\n",
+						(int) nwritten, amount);
+				nwritten -= (nwritten & 511);
+				/* Round down to a block */
+			}
+			file_offset += nwritten;
+			amount_left_to_write -= nwritten;
+			common->residue -= nwritten;
+
+			/* If an error occurred, report it and its position */
+			if (nwritten < amount) {
+				pr_warn("nwritten:%zd amount:%u\n", nwritten,
+				       amount);
+				curlun->sense_data = SS_WRITE_ERROR;
+				curlun->info_valid = 1;
+				break;
+			}
+
+			/* Did the host decide to stop early? */
+			if (bh->outreq->actual != bh->outreq->length) {
+				common->short_packet_received = 1;
+				break;
+			}
+			continue;
+		}
+
+		/* Wait for something to happen */
+		rc = sleep_thread(common);
+		if (rc)
+			return rc;
+	}
+
+	return -EIO;		/* No default reply */
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int do_synchronize_cache(struct fsg_common *common)
+{
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int do_verify(struct fsg_common *common)
+{
+	struct fsg_lun		*curlun = &common->luns[common->lun];
+	u32			lba;
+	u32			verification_length;
+	struct fsg_buffhd	*bh = common->next_buffhd_to_fill;
+	loff_t			file_offset;
+	u32			amount_left;
+	unsigned int		amount;
+	ssize_t			nread;
+
+	/* Get the starting Logical Block Address and check that it's
+	 * not too big */
+	lba = get_unaligned_be32(&common->cmnd[2]);
+	if (lba >= curlun->num_sectors) {
+		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+		return -EINVAL;
+	}
+
+	/* We allow DPO (Disable Page Out = don't save data in the
+	 * cache) but we don't implement it. */
+	if (common->cmnd[1] & ~0x10) {
+		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+		return -EINVAL;
+	}
+
+	verification_length = get_unaligned_be16(&common->cmnd[7]);
+	if (unlikely(verification_length == 0))
+		return -EIO;		/* No default reply */
+
+	/* Prepare to carry out the file verify */
+	amount_left = verification_length << 9;
+	file_offset = ((loff_t) lba) << 9;
+
+	/* Write out all the dirty buffers before invalidating them */
+
+	/* Just try to read the requested blocks */
+	while (amount_left > 0) {
+
+		/* Figure out how much we need to read:
+		 * Try to read the remaining amount, but not more than
+		 * the buffer size.
+		 * And don't try to read past the end of the file.
+		 * If this means reading 0 then we were asked to read
+		 * past the end of file. */
+		amount = min(amount_left, FSG_BUFLEN);
+		if (amount == 0) {
+			curlun->sense_data =
+					SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+			curlun->info_valid = 1;
+			break;
+		}
+
+		/* Perform the read */
+		nread = pread(ums[common->lun].fd, bh->buf, amount, file_offset);
+
+		VLDBG(curlun, "file read %u @ %llu -> %zd\n", amount,
+				(unsigned long long) file_offset,
+				nread);
+		if (nread <= 0) {
+			const char *err = nread ? strerror(-nread) : "EOF";
+			LDBG(curlun, "error in file read: %s\n", err);
+			nread = 0;
+		} else if (nread < amount) {
+			LDBG(curlun, "partial file verify: %d/%u\n",
+					(int) nread, amount);
+			nread -= (nread & 511);	/* Round down to a sector */
+		}
+		if (nread == 0) {
+			curlun->sense_data = SS_UNRECOVERED_READ_ERROR;
+			curlun->info_valid = 1;
+			break;
+		}
+		file_offset += nread;
+		amount_left -= nread;
+	}
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int do_inquiry(struct fsg_common *common, struct fsg_buffhd *bh)
+{
+	struct fsg_lun *curlun = &common->luns[common->lun];
+	static const char vendor_id[] = "Linux   ";
+	u8	*buf = (u8 *) bh->buf;
+
+	if (!curlun) {		/* Unsupported LUNs are okay */
+		common->bad_lun_okay = 1;
+		memset(buf, 0, 36);
+		buf[0] = 0x7f;		/* Unsupported, no device-type */
+		buf[4] = 31;		/* Additional length */
+		return 36;
+	}
+
+	memset(buf, 0, 8);
+	buf[0] = TYPE_DISK;
+	buf[1] = curlun->removable ? 0x80 : 0;
+	buf[2] = 2;		/* ANSI SCSI level 2 */
+	buf[3] = 2;		/* SCSI-2 INQUIRY data format */
+	buf[4] = 31;		/* Additional length */
+				/* No special options */
+	sprintf((char *) (buf + 8), "%-8s%-16s%04x", (char*) vendor_id ,
+			ums[common->lun].name, (u16) 0xffff);
+
+	return 36;
+}
+
+
+static int do_request_sense(struct fsg_common *common, struct fsg_buffhd *bh)
+{
+	struct fsg_lun	*curlun = &common->luns[common->lun];
+	u8		*buf = (u8 *) bh->buf;
+	u32		sd, sdinfo = 0;
+	int		valid;
+
+	/*
+	 * From the SCSI-2 spec., section 7.9 (Unit attention condition):
+	 *
+	 * If a REQUEST SENSE command is received from an initiator
+	 * with a pending unit attention condition (before the target
+	 * generates the contingent allegiance condition), then the
+	 * target shall either:
+	 *   a) report any pending sense data and preserve the unit
+	 *	attention condition on the logical unit, or,
+	 *   b) report the unit attention condition, may discard any
+	 *	pending sense data, and clear the unit attention
+	 *	condition on the logical unit for that initiator.
+	 *
+	 * FSG normally uses option a); enable this code to use option b).
+	 */
+#if 0
+	if (curlun && curlun->unit_attention_data != SS_NO_SENSE) {
+		curlun->sense_data = curlun->unit_attention_data;
+		curlun->unit_attention_data = SS_NO_SENSE;
+	}
+#endif
+
+	if (!curlun) {		/* Unsupported LUNs are okay */
+		common->bad_lun_okay = 1;
+		sd = SS_LOGICAL_UNIT_NOT_SUPPORTED;
+		valid = 0;
+	} else {
+		sd = curlun->sense_data;
+		valid = curlun->info_valid << 7;
+		curlun->sense_data = SS_NO_SENSE;
+		curlun->info_valid = 0;
+	}
+
+	memset(buf, 0, 18);
+	buf[0] = valid | 0x70;			/* Valid, current error */
+	buf[2] = SK(sd);
+	put_unaligned_be32(sdinfo, &buf[3]);	/* Sense information */
+	buf[7] = 18 - 8;			/* Additional sense length */
+	buf[12] = ASC(sd);
+	buf[13] = ASCQ(sd);
+	return 18;
+}
+
+static int do_read_capacity(struct fsg_common *common, struct fsg_buffhd *bh)
+{
+	struct fsg_lun	*curlun = &common->luns[common->lun];
+	u32		lba = get_unaligned_be32(&common->cmnd[2]);
+	int		pmi = common->cmnd[8];
+	u8		*buf = (u8 *) bh->buf;
+
+	/* Check the PMI and LBA fields */
+	if (pmi > 1 || (pmi == 0 && lba != 0)) {
+		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+		return -EINVAL;
+	}
+
+	put_unaligned_be32(curlun->num_sectors - 1, &buf[0]);
+						/* Max logical block */
+	put_unaligned_be32(512, &buf[4]);	/* Block length */
+	return 8;
+}
+
+static int do_read_header(struct fsg_common *common, struct fsg_buffhd *bh)
+{
+	struct fsg_lun	*curlun = &common->luns[common->lun];
+	int		msf = common->cmnd[1] & 0x02;
+	u32		lba = get_unaligned_be32(&common->cmnd[2]);
+	u8		*buf = (u8 *) bh->buf;
+
+	if (common->cmnd[1] & ~0x02) {		/* Mask away MSF */
+		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+		return -EINVAL;
+	}
+	if (lba >= curlun->num_sectors) {
+		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+		return -EINVAL;
+	}
+
+	memset(buf, 0, 8);
+	buf[0] = 0x01;		/* 2048 bytes of user data, rest is EC */
+	store_cdrom_address(&buf[4], msf, lba);
+	return 8;
+}
+
+
+static int do_read_toc(struct fsg_common *common, struct fsg_buffhd *bh)
+{
+	struct fsg_lun	*curlun = &common->luns[common->lun];
+	int		msf = common->cmnd[1] & 0x02;
+	int		start_track = common->cmnd[6];
+	u8		*buf = (u8 *) bh->buf;
+
+	if ((common->cmnd[1] & ~0x02) != 0 ||	/* Mask away MSF */
+			start_track > 1) {
+		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+		return -EINVAL;
+	}
+
+	memset(buf, 0, 20);
+	buf[1] = (20-2);		/* TOC data length */
+	buf[2] = 1;			/* First track number */
+	buf[3] = 1;			/* Last track number */
+	buf[5] = 0x16;			/* Data track, copying allowed */
+	buf[6] = 0x01;			/* Only track is number 1 */
+	store_cdrom_address(&buf[8], msf, 0);
+
+	buf[13] = 0x16;			/* Lead-out track is data */
+	buf[14] = 0xAA;			/* Lead-out track number */
+	store_cdrom_address(&buf[16], msf, curlun->num_sectors);
+
+	return 20;
+}
+
+static int do_mode_sense(struct fsg_common *common, struct fsg_buffhd *bh)
+{
+	struct fsg_lun	*curlun = &common->luns[common->lun];
+	int		mscmnd = common->cmnd[0];
+	u8		*buf = (u8 *) bh->buf;
+	u8		*buf0 = buf;
+	int		pc, page_code;
+	int		changeable_values, all_pages;
+	int		valid_page = 0;
+	int		len, limit;
+
+	if ((common->cmnd[1] & ~0x08) != 0) {	/* Mask away DBD */
+		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+		return -EINVAL;
+	}
+	pc = common->cmnd[2] >> 6;
+	page_code = common->cmnd[2] & 0x3f;
+	if (pc == 3) {
+		curlun->sense_data = SS_SAVING_PARAMETERS_NOT_SUPPORTED;
+		return -EINVAL;
+	}
+	changeable_values = (pc == 1);
+	all_pages = (page_code == 0x3f);
+
+	/* Write the mode parameter header.  Fixed values are: default
+	 * medium type, no cache control (DPOFUA), and no block descriptors.
+	 * The only variable value is the WriteProtect bit.  We will fill in
+	 * the mode data length later. */
+	memset(buf, 0, 8);
+	if (mscmnd == SCSI_MODE_SEN6) {
+		buf[2] = (curlun->ro ? 0x80 : 0x00);		/* WP, DPOFUA */
+		buf += 4;
+		limit = 255;
+	} else {			/* SCSI_MODE_SEN10 */
+		buf[3] = (curlun->ro ? 0x80 : 0x00);		/* WP, DPOFUA */
+		buf += 8;
+		limit = 65535;		/* Should really be FSG_BUFLEN */
+	}
+
+	/* No block descriptors */
+
+	/* The mode pages, in numerical order.  The only page we support
+	 * is the Caching page. */
+	if (page_code == 0x08 || all_pages) {
+		valid_page = 1;
+		buf[0] = 0x08;		/* Page code */
+		buf[1] = 10;		/* Page length */
+		memset(buf+2, 0, 10);	/* None of the fields are changeable */
+
+		if (!changeable_values) {
+			buf[2] = 0x04;	/* Write cache enable, */
+					/* Read cache not disabled */
+					/* No cache retention priorities */
+			put_unaligned_be16(0xffff, &buf[4]);
+					/* Don't disable prefetch */
+					/* Minimum prefetch = 0 */
+			put_unaligned_be16(0xffff, &buf[8]);
+					/* Maximum prefetch */
+			put_unaligned_be16(0xffff, &buf[10]);
+					/* Maximum prefetch ceiling */
+		}
+		buf += 12;
+	}
+
+	/* Check that a valid page was requested and the mode data length
+	 * isn't too long. */
+	len = buf - buf0;
+	if (!valid_page || len > limit) {
+		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+		return -EINVAL;
+	}
+
+	/*  Store the mode data length */
+	if (mscmnd == SCSI_MODE_SEN6)
+		buf0[0] = len - 1;
+	else
+		put_unaligned_be16(len - 2, buf0);
+	return len;
+}
+
+
+static int do_start_stop(struct fsg_common *common)
+{
+	struct fsg_lun	*curlun = &common->luns[common->lun];
+
+	if (!curlun) {
+		return -EINVAL;
+	} else if (!curlun->removable) {
+		curlun->sense_data = SS_INVALID_COMMAND;
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int do_prevent_allow(struct fsg_common *common)
+{
+	struct fsg_lun	*curlun = &common->luns[common->lun];
+	int		prevent;
+
+	if (!curlun->removable) {
+		curlun->sense_data = SS_INVALID_COMMAND;
+		return -EINVAL;
+	}
+
+	prevent = common->cmnd[4] & 0x01;
+	if ((common->cmnd[4] & ~0x01) != 0) {	/* Mask away Prevent */
+		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+		return -EINVAL;
+	}
+
+	if (curlun->prevent_medium_removal && !prevent)
+		fsg_lun_fsync_sub(curlun);
+	curlun->prevent_medium_removal = prevent;
+	return 0;
+}
+
+
+static int do_read_format_capacities(struct fsg_common *common,
+			struct fsg_buffhd *bh)
+{
+	struct fsg_lun	*curlun = &common->luns[common->lun];
+	u8		*buf = (u8 *) bh->buf;
+
+	buf[0] = buf[1] = buf[2] = 0;
+	buf[3] = 8;	/* Only the Current/Maximum Capacity Descriptor */
+	buf += 4;
+
+	put_unaligned_be32(curlun->num_sectors, &buf[0]);
+						/* Number of blocks */
+	put_unaligned_be32(512, &buf[4]);	/* Block length */
+	buf[4] = 0x02;				/* Current capacity */
+	return 12;
+}
+
+
+static int do_mode_select(struct fsg_common *common, struct fsg_buffhd *bh)
+{
+	struct fsg_lun	*curlun = &common->luns[common->lun];
+
+	/* We don't support MODE SELECT */
+	if (curlun)
+		curlun->sense_data = SS_INVALID_COMMAND;
+	return -EINVAL;
+}
+
+
+/*-------------------------------------------------------------------------*/
+
+static int halt_bulk_in_endpoint(struct fsg_dev *fsg)
+{
+	int	rc;
+
+	rc = fsg_set_halt(fsg, fsg->bulk_in);
+	if (rc == -EAGAIN)
+		VDBG(fsg, "delayed bulk-in endpoint halt\n");
+	while (rc != 0) {
+		if (rc != -EAGAIN) {
+			WARNING(fsg, "usb_ep_set_halt -> %d\n", rc);
+			rc = 0;
+			break;
+		}
+
+		rc = usb_ep_set_halt(fsg->bulk_in);
+	}
+	return rc;
+}
+
+static int wedge_bulk_in_endpoint(struct fsg_dev *fsg)
+{
+	int	rc;
+
+	DBG(fsg, "bulk-in set wedge\n");
+	rc = 0; /* usb_ep_set_wedge(fsg->bulk_in); */
+	if (rc == -EAGAIN)
+		VDBG(fsg, "delayed bulk-in endpoint wedge\n");
+	while (rc != 0) {
+		if (rc != -EAGAIN) {
+			WARNING(fsg, "usb_ep_set_wedge -> %d\n", rc);
+			rc = 0;
+			break;
+		}
+	}
+	return rc;
+}
+
+static int pad_with_zeros(struct fsg_dev *fsg)
+{
+	struct fsg_buffhd	*bh = fsg->common->next_buffhd_to_fill;
+	u32			nkeep = bh->inreq->length;
+	u32			nsend;
+	int			rc;
+
+	bh->state = BUF_STATE_EMPTY;		/* For the first iteration */
+	fsg->common->usb_amount_left = nkeep + fsg->common->residue;
+	while (fsg->common->usb_amount_left > 0) {
+
+		/* Wait for the next buffer to be free */
+		while (bh->state != BUF_STATE_EMPTY) {
+			rc = sleep_thread(fsg->common);
+			if (rc)
+				return rc;
+		}
+
+		nsend = min(fsg->common->usb_amount_left, FSG_BUFLEN);
+		memset(bh->buf + nkeep, 0, nsend - nkeep);
+		bh->inreq->length = nsend;
+		bh->inreq->zero = 0;
+		start_transfer(fsg, fsg->bulk_in, bh->inreq,
+				&bh->inreq_busy, &bh->state);
+		bh = fsg->common->next_buffhd_to_fill = bh->next;
+		fsg->common->usb_amount_left -= nsend;
+		nkeep = 0;
+	}
+	return 0;
+}
+
+static int throw_away_data(struct fsg_common *common)
+{
+	struct fsg_buffhd	*bh;
+	u32			amount;
+	int			rc;
+
+	for (bh = common->next_buffhd_to_drain;
+	     bh->state != BUF_STATE_EMPTY || common->usb_amount_left > 0;
+	     bh = common->next_buffhd_to_drain) {
+
+		/* Throw away the data in a filled buffer */
+		if (bh->state == BUF_STATE_FULL) {
+			bh->state = BUF_STATE_EMPTY;
+			common->next_buffhd_to_drain = bh->next;
+
+			/* A short packet or an error ends everything */
+			if (bh->outreq->actual != bh->outreq->length ||
+					bh->outreq->status != 0) {
+				raise_exception(common,
+						FSG_STATE_ABORT_BULK_OUT);
+				return -EPIPE;
+			}
+			continue;
+		}
+
+		/* Try to submit another request if we need one */
+		bh = common->next_buffhd_to_fill;
+		if (bh->state == BUF_STATE_EMPTY
+		 && common->usb_amount_left > 0) {
+			amount = min(common->usb_amount_left, FSG_BUFLEN);
+
+			/* amount is always divisible by 512, hence by
+			 * the bulk-out maxpacket size */
+			bh->outreq->length = amount;
+			bh->bulk_out_intended_length = amount;
+			bh->outreq->short_not_ok = 1;
+			START_TRANSFER_OR(common, bulk_out, bh->outreq,
+					  &bh->outreq_busy, &bh->state)
+				/* Don't know what to do if
+				 * common->fsg is NULL */
+				return -EIO;
+			common->next_buffhd_to_fill = bh->next;
+			common->usb_amount_left -= amount;
+			continue;
+		}
+
+		/* Otherwise wait for something to happen */
+		rc = sleep_thread(common);
+		if (rc)
+			return rc;
+	}
+	return 0;
+}
+
+
+static int finish_reply(struct fsg_common *common)
+{
+	struct fsg_buffhd	*bh = common->next_buffhd_to_fill;
+	int			rc = 0;
+
+	switch (common->data_dir) {
+	case DATA_DIR_NONE:
+		break;			/* Nothing to send */
+
+	/* If we don't know whether the host wants to read or write,
+	 * this must be CB or CBI with an unknown command.  We mustn't
+	 * try to send or receive any data.  So stall both bulk pipes
+	 * if we can and wait for a reset. */
+	case DATA_DIR_UNKNOWN:
+		if (!common->can_stall) {
+			/* Nothing */
+		} else if (fsg_is_set(common)) {
+			fsg_set_halt(common->fsg, common->fsg->bulk_out);
+			rc = halt_bulk_in_endpoint(common->fsg);
+		} else {
+			/* Don't know what to do if common->fsg is NULL */
+			rc = -EIO;
+		}
+		break;
+
+	/* All but the last buffer of data must have already been sent */
+	case DATA_DIR_TO_HOST:
+		if (common->data_size == 0) {
+			/* Nothing to send */
+
+		/* If there's no residue, simply send the last buffer */
+		} else if (common->residue == 0) {
+			bh->inreq->zero = 0;
+			START_TRANSFER_OR(common, bulk_in, bh->inreq,
+					  &bh->inreq_busy, &bh->state)
+				return -EIO;
+			common->next_buffhd_to_fill = bh->next;
+
+		/* For Bulk-only, if we're allowed to stall then send the
+		 * short packet and halt the bulk-in endpoint.  If we can't
+		 * stall, pad out the remaining data with 0's. */
+		} else if (common->can_stall) {
+			bh->inreq->zero = 1;
+			START_TRANSFER_OR(common, bulk_in, bh->inreq,
+					  &bh->inreq_busy, &bh->state)
+				/* Don't know what to do if
+				 * common->fsg is NULL */
+				rc = -EIO;
+			common->next_buffhd_to_fill = bh->next;
+			if (common->fsg)
+				rc = halt_bulk_in_endpoint(common->fsg);
+		} else if (fsg_is_set(common)) {
+			rc = pad_with_zeros(common->fsg);
+		} else {
+			/* Don't know what to do if common->fsg is NULL */
+			rc = -EIO;
+		}
+		break;
+
+	/* We have processed all we want from the data the host has sent.
+	 * There may still be outstanding bulk-out requests. */
+	case DATA_DIR_FROM_HOST:
+		if (common->residue == 0) {
+			/* Nothing to receive */
+
+		/* Did the host stop sending unexpectedly early? */
+		} else if (common->short_packet_received) {
+			raise_exception(common, FSG_STATE_ABORT_BULK_OUT);
+			rc = -EPIPE;
+
+		/* We haven't processed all the incoming data.  Even though
+		 * we may be allowed to stall, doing so would cause a race.
+		 * The controller may already have ACK'ed all the remaining
+		 * bulk-out packets, in which case the host wouldn't see a
+		 * STALL.  Not realizing the endpoint was halted, it wouldn't
+		 * clear the halt -- leading to problems later on. */
+#if 0
+		} else if (common->can_stall) {
+			if (fsg_is_set(common))
+				fsg_set_halt(common->fsg,
+					     common->fsg->bulk_out);
+			raise_exception(common, FSG_STATE_ABORT_BULK_OUT);
+			rc = -EPIPE;
+#endif
+
+		/* We can't stall.  Read in the excess data and throw it
+		 * all away. */
+		} else {
+			rc = throw_away_data(common);
+		}
+		break;
+	}
+	return rc;
+}
+
+
+static int send_status(struct fsg_common *common)
+{
+	struct fsg_lun		*curlun = &common->luns[common->lun];
+	struct fsg_buffhd	*bh;
+	struct bulk_cs_wrap	*csw;
+	int			rc;
+	u8			status = US_BULK_STAT_OK;
+	u32			sd, sdinfo = 0;
+
+	/* Wait for the next buffer to become available */
+	bh = common->next_buffhd_to_fill;
+	while (bh->state != BUF_STATE_EMPTY) {
+		rc = sleep_thread(common);
+		if (rc)
+			return rc;
+	}
+
+	if (curlun)
+		sd = curlun->sense_data;
+	else if (common->bad_lun_okay)
+		sd = SS_NO_SENSE;
+	else
+		sd = SS_LOGICAL_UNIT_NOT_SUPPORTED;
+
+	if (common->phase_error) {
+		DBG(common, "sending phase-error status\n");
+		status = US_BULK_STAT_PHASE;
+		sd = SS_INVALID_COMMAND;
+	} else if (sd != SS_NO_SENSE) {
+		DBG(common, "sending command-failure status\n");
+		status = US_BULK_STAT_FAIL;
+		VDBG(common, "  sense data: SK x%02x, ASC x%02x, ASCQ x%02x;"
+			"  info x%x\n",
+			SK(sd), ASC(sd), ASCQ(sd), sdinfo);
+	}
+
+	/* Store and send the Bulk-only CSW */
+	csw = (void *)bh->buf;
+
+	csw->Signature = cpu_to_le32(US_BULK_CS_SIGN);
+	csw->Tag = common->tag;
+	csw->Residue = cpu_to_le32(common->residue);
+	csw->Status = status;
+
+	bh->inreq->length = US_BULK_CS_WRAP_LEN;
+	bh->inreq->zero = 0;
+	START_TRANSFER_OR(common, bulk_in, bh->inreq,
+			  &bh->inreq_busy, &bh->state)
+		/* Don't know what to do if common->fsg is NULL */
+		return -EIO;
+
+	common->next_buffhd_to_fill = bh->next;
+	return 0;
+}
+
+
+/*-------------------------------------------------------------------------*/
+
+/* Check whether the command is properly formed and whether its data size
+ * and direction agree with the values we already have. */
+static int check_command(struct fsg_common *common, int cmnd_size,
+		enum data_direction data_dir, unsigned int mask,
+		int needs_medium, const char *name)
+{
+	int			i;
+	int			lun = common->cmnd[1] >> 5;
+	static const char	dirletter[4] = {'u', 'o', 'i', 'n'};
+	char			hdlen[20];
+	struct fsg_lun		*curlun;
+
+	hdlen[0] = 0;
+	if (common->data_dir != DATA_DIR_UNKNOWN)
+		sprintf(hdlen, ", H%c=%u", dirletter[(int) common->data_dir],
+				common->data_size);
+	VDBG(common, "SCSI command: %s;  Dc=%d, D%c=%u;  Hc=%d%s\n",
+	     name, cmnd_size, dirletter[(int) data_dir],
+	     common->data_size_from_cmnd, common->cmnd_size, hdlen);
+
+	/* We can't reply at all until we know the correct data direction
+	 * and size. */
+	if (common->data_size_from_cmnd == 0)
+		data_dir = DATA_DIR_NONE;
+	if (common->data_size < common->data_size_from_cmnd) {
+		/* Host data size < Device data size is a phase error.
+		 * Carry out the command, but only transfer as much as
+		 * we are allowed. */
+		common->data_size_from_cmnd = common->data_size;
+		common->phase_error = 1;
+	}
+	common->residue = common->data_size;
+	common->usb_amount_left = common->data_size;
+
+	/* Conflicting data directions is a phase error */
+	if (common->data_dir != data_dir
+	 && common->data_size_from_cmnd > 0) {
+		common->phase_error = 1;
+		return -EINVAL;
+	}
+
+	/* Verify the length of the command itself */
+	if (cmnd_size != common->cmnd_size) {
+
+		/* Special case workaround: There are plenty of buggy SCSI
+		 * implementations. Many have issues with cbw->Length
+		 * field passing a wrong command size. For those cases we
+		 * always try to work around the problem by using the length
+		 * sent by the host side provided it is at least as large
+		 * as the correct command length.
+		 * Examples of such cases would be MS-Windows, which issues
+		 * REQUEST SENSE with cbw->Length == 12 where it should
+		 * be 6, and xbox360 issuing INQUIRY, TEST UNIT READY and
+		 * REQUEST SENSE with cbw->Length == 10 where it should
+		 * be 6 as well.
+		 */
+		if (cmnd_size <= common->cmnd_size) {
+			DBG(common, "%s is buggy! Expected length %d "
+			    "but we got %d\n", name,
+			    cmnd_size, common->cmnd_size);
+			cmnd_size = common->cmnd_size;
+		} else {
+			common->phase_error = 1;
+			return -EINVAL;
+		}
+	}
+
+	/* Check that the LUN values are consistent */
+	if (common->lun != lun)
+		DBG(common, "using LUN %d from CBW, not LUN %d from CDB\n",
+		    common->lun, lun);
+
+	/* Check the LUN */
+	if (common->lun < common->nluns) {
+		curlun = &common->luns[common->lun];
+		if (common->cmnd[0] != SCSI_REQ_SENSE) {
+			curlun->sense_data = SS_NO_SENSE;
+			curlun->info_valid = 0;
+		}
+	} else {
+		curlun = NULL;
+		common->bad_lun_okay = 0;
+
+		/* INQUIRY and REQUEST SENSE commands are explicitly allowed
+		 * to use unsupported LUNs; all others may not. */
+		if (common->cmnd[0] != SCSI_INQUIRY &&
+		    common->cmnd[0] != SCSI_REQ_SENSE) {
+			DBG(common, "unsupported LUN %d\n", common->lun);
+			return -EINVAL;
+		}
+	}
+#if 0
+	/* If a unit attention condition exists, only INQUIRY and
+	 * REQUEST SENSE commands are allowed; anything else must fail. */
+	if (curlun && curlun->unit_attention_data != SS_NO_SENSE &&
+			common->cmnd[0] != SCSI_INQUIRY &&
+			common->cmnd[0] != SCSI_REQ_SENSE) {
+		curlun->sense_data = curlun->unit_attention_data;
+		curlun->unit_attention_data = SS_NO_SENSE;
+		return -EINVAL;
+	}
+#endif
+	/* Check that only command bytes listed in the mask are non-zero */
+	common->cmnd[1] &= 0x1f;			/* Mask away the LUN */
+	for (i = 1; i < cmnd_size; ++i) {
+		if (common->cmnd[i] && !(mask & (1 << i))) {
+			if (curlun)
+				curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int do_scsi_command(struct fsg_common *common)
+{
+	struct fsg_buffhd	*bh;
+	int			rc;
+	int			reply = -EINVAL;
+	int			i;
+	static char		unknown[16];
+	struct fsg_lun		*curlun = &common->luns[common->lun];
+
+	dump_cdb(common);
+
+	/* Wait for the next buffer to become available for data or status */
+	bh = common->next_buffhd_to_fill;
+	common->next_buffhd_to_drain = bh;
+	while (bh->state != BUF_STATE_EMPTY) {
+		rc = sleep_thread(common);
+		if (rc)
+			return rc;
+	}
+	common->phase_error = 0;
+	common->short_packet_received = 0;
+
+	down_read(&common->filesem);	/* We're using the backing file */
+	switch (common->cmnd[0]) {
+
+	case SCSI_INQUIRY:
+		common->data_size_from_cmnd = common->cmnd[4];
+		reply = check_command(common, 6, DATA_DIR_TO_HOST,
+				      (1<<4), 0,
+				      "INQUIRY");
+		if (reply == 0)
+			reply = do_inquiry(common, bh);
+		break;
+
+	case SCSI_MODE_SEL6:
+		common->data_size_from_cmnd = common->cmnd[4];
+		reply = check_command(common, 6, DATA_DIR_FROM_HOST,
+				      (1<<1) | (1<<4), 0,
+				      "MODE SELECT(6)");
+		if (reply == 0)
+			reply = do_mode_select(common, bh);
+		break;
+
+	case SCSI_MODE_SEL10:
+		common->data_size_from_cmnd =
+			get_unaligned_be16(&common->cmnd[7]);
+		reply = check_command(common, 10, DATA_DIR_FROM_HOST,
+				      (1<<1) | (3<<7), 0,
+				      "MODE SELECT(10)");
+		if (reply == 0)
+			reply = do_mode_select(common, bh);
+		break;
+
+	case SCSI_MODE_SEN6:
+		common->data_size_from_cmnd = common->cmnd[4];
+		reply = check_command(common, 6, DATA_DIR_TO_HOST,
+				      (1<<1) | (1<<2) | (1<<4), 0,
+				      "MODE SENSE(6)");
+		if (reply == 0)
+			reply = do_mode_sense(common, bh);
+		break;
+
+	case SCSI_MODE_SEN10:
+		common->data_size_from_cmnd =
+			get_unaligned_be16(&common->cmnd[7]);
+		reply = check_command(common, 10, DATA_DIR_TO_HOST,
+				      (1<<1) | (1<<2) | (3<<7), 0,
+				      "MODE SENSE(10)");
+		if (reply == 0)
+			reply = do_mode_sense(common, bh);
+		break;
+
+	case SCSI_MED_REMOVL:
+		common->data_size_from_cmnd = 0;
+		reply = check_command(common, 6, DATA_DIR_NONE,
+				      (1<<4), 0,
+				      "PREVENT-ALLOW MEDIUM REMOVAL");
+		if (reply == 0)
+			reply = do_prevent_allow(common);
+		break;
+
+	case SCSI_READ6:
+		i = common->cmnd[4];
+		common->data_size_from_cmnd = (i == 0 ? 256 : i) << 9;
+		reply = check_command(common, 6, DATA_DIR_TO_HOST,
+				      (7<<1) | (1<<4), 1,
+				      "READ(6)");
+		if (reply == 0)
+			reply = do_read(common);
+		break;
+
+	case SCSI_READ10:
+		common->data_size_from_cmnd =
+				get_unaligned_be16(&common->cmnd[7]) << 9;
+		reply = check_command(common, 10, DATA_DIR_TO_HOST,
+				      (1<<1) | (0xf<<2) | (3<<7), 1,
+				      "READ(10)");
+		if (reply == 0)
+			reply = do_read(common);
+		break;
+
+	case SCSI_READ12:
+		common->data_size_from_cmnd =
+				get_unaligned_be32(&common->cmnd[6]) << 9;
+		reply = check_command(common, 12, DATA_DIR_TO_HOST,
+				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
+				      "READ(12)");
+		if (reply == 0)
+			reply = do_read(common);
+		break;
+
+	case SCSI_RD_CAPAC:
+		common->data_size_from_cmnd = 8;
+		reply = check_command(common, 10, DATA_DIR_TO_HOST,
+				      (0xf<<2) | (1<<8), 1,
+				      "READ CAPACITY");
+		if (reply == 0)
+			reply = do_read_capacity(common, bh);
+		break;
+
+	case SCSI_RD_HEADER:
+		if (!common->luns[common->lun].cdrom)
+			goto unknown_cmnd;
+		common->data_size_from_cmnd =
+			get_unaligned_be16(&common->cmnd[7]);
+		reply = check_command(common, 10, DATA_DIR_TO_HOST,
+				      (3<<7) | (0x1f<<1), 1,
+				      "READ HEADER");
+		if (reply == 0)
+			reply = do_read_header(common, bh);
+		break;
+
+	case SCSI_RD_TOC:
+		if (!common->luns[common->lun].cdrom)
+			goto unknown_cmnd;
+		common->data_size_from_cmnd =
+			get_unaligned_be16(&common->cmnd[7]);
+		reply = check_command(common, 10, DATA_DIR_TO_HOST,
+				      (7<<6) | (1<<1), 1,
+				      "READ TOC");
+		if (reply == 0)
+			reply = do_read_toc(common, bh);
+		break;
+
+	case SCSI_RD_FMT_CAPAC:
+		common->data_size_from_cmnd =
+			get_unaligned_be16(&common->cmnd[7]);
+		reply = check_command(common, 10, DATA_DIR_TO_HOST,
+				      (3<<7), 1,
+				      "READ FORMAT CAPACITIES");
+		if (reply == 0)
+			reply = do_read_format_capacities(common, bh);
+		break;
+
+	case SCSI_REQ_SENSE:
+		common->data_size_from_cmnd = common->cmnd[4];
+		reply = check_command(common, 6, DATA_DIR_TO_HOST,
+				      (1<<4), 0,
+				      "REQUEST SENSE");
+		if (reply == 0)
+			reply = do_request_sense(common, bh);
+		break;
+
+	case SCSI_START_STP:
+		common->data_size_from_cmnd = 0;
+		reply = check_command(common, 6, DATA_DIR_NONE,
+				      (1<<1) | (1<<4), 0,
+				      "START-STOP UNIT");
+		if (reply == 0)
+			reply = do_start_stop(common);
+		break;
+
+	case SCSI_SYNC_CACHE:
+		common->data_size_from_cmnd = 0;
+		reply = check_command(common, 10, DATA_DIR_NONE,
+				      (0xf<<2) | (3<<7), 1,
+				      "SYNCHRONIZE CACHE");
+		if (reply == 0)
+			reply = do_synchronize_cache(common);
+		break;
+
+	case SCSI_TST_U_RDY:
+		common->data_size_from_cmnd = 0;
+		reply = check_command(common, 6, DATA_DIR_NONE,
+				0, 1,
+				"TEST UNIT READY");
+		break;
+
+	/* Although optional, this command is used by MS-Windows.  We
+	 * support a minimal version: BytChk must be 0. */
+	case SCSI_VERIFY:
+		common->data_size_from_cmnd = 0;
+		reply = check_command(common, 10, DATA_DIR_NONE,
+				      (1<<1) | (0xf<<2) | (3<<7), 1,
+				      "VERIFY");
+		if (reply == 0)
+			reply = do_verify(common);
+		break;
+
+	case SCSI_WRITE6:
+		i = common->cmnd[4];
+		common->data_size_from_cmnd = (i == 0 ? 256 : i) << 9;
+		reply = check_command(common, 6, DATA_DIR_FROM_HOST,
+				      (7<<1) | (1<<4), 1,
+				      "WRITE(6)");
+		if (reply == 0)
+			reply = do_write(common);
+		break;
+
+	case SCSI_WRITE10:
+		common->data_size_from_cmnd =
+				get_unaligned_be16(&common->cmnd[7]) << 9;
+		reply = check_command(common, 10, DATA_DIR_FROM_HOST,
+				      (1<<1) | (0xf<<2) | (3<<7), 1,
+				      "WRITE(10)");
+		if (reply == 0)
+			reply = do_write(common);
+		break;
+
+	case SCSI_WRITE12:
+		common->data_size_from_cmnd =
+				get_unaligned_be32(&common->cmnd[6]) << 9;
+		reply = check_command(common, 12, DATA_DIR_FROM_HOST,
+				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
+				      "WRITE(12)");
+		if (reply == 0)
+			reply = do_write(common);
+		break;
+
+	/* Some mandatory commands that we recognize but don't implement.
+	 * They don't mean much in this setting.  It's left as an exercise
+	 * for anyone interested to implement RESERVE and RELEASE in terms
+	 * of Posix locks. */
+	case SCSI_FORMAT:
+	case SCSI_RELEASE:
+	case SCSI_RESERVE:
+	case SCSI_SEND_DIAG:
+		/* Fall through */
+
+	default:
+unknown_cmnd:
+		common->data_size_from_cmnd = 0;
+		sprintf(unknown, "Unknown x%02x", common->cmnd[0]);
+		reply = check_command(common, common->cmnd_size,
+				      DATA_DIR_UNKNOWN, 0xff, 0, unknown);
+		if (reply == 0) {
+			curlun->sense_data = SS_INVALID_COMMAND;
+			reply = -EINVAL;
+		}
+		break;
+	}
+	up_read(&common->filesem);
+
+	if (reply == -EPIPE)
+		return -EPIPE;
+
+	/* Set up the single reply buffer for finish_reply() */
+	if (reply == -EINVAL)
+		reply = 0;		/* Error reply length */
+	if (reply >= 0 && common->data_dir == DATA_DIR_TO_HOST) {
+		reply = min((u32) reply, common->data_size_from_cmnd);
+		bh->inreq->length = reply;
+		bh->state = BUF_STATE_FULL;
+		common->residue -= reply;
+	}				/* Otherwise it's already set */
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int received_cbw(struct fsg_dev *fsg, struct fsg_buffhd *bh)
+{
+	struct usb_request	*req = bh->outreq;
+	struct bulk_cb_wrap	*cbw = req->buf;
+	struct fsg_common	*common = fsg->common;
+
+	/* Was this a real packet?  Should it be ignored? */
+	if (req->status || test_bit(IGNORE_BULK_OUT, &fsg->atomic_bitflags))
+		return -EINVAL;
+
+	/* Is the CBW valid? */
+	if (req->actual != US_BULK_CB_WRAP_LEN ||
+			cbw->Signature != cpu_to_le32(
+				US_BULK_CB_SIGN)) {
+		DBG(fsg, "invalid CBW: len %u sig 0x%x\n",
+				req->actual,
+				le32_to_cpu(cbw->Signature));
+
+		/* The Bulk-only spec says we MUST stall the IN endpoint
+		 * (6.6.1), so it's unavoidable.  It also says we must
+		 * retain this state until the next reset, but there's
+		 * no way to tell the controller driver it should ignore
+		 * Clear-Feature(HALT) requests.
+		 *
+		 * We aren't required to halt the OUT endpoint; instead
+		 * we can simply accept and discard any data received
+		 * until the next reset. */
+		wedge_bulk_in_endpoint(fsg);
+		set_bit(IGNORE_BULK_OUT, &fsg->atomic_bitflags);
+		return -EINVAL;
+	}
+
+	/* Is the CBW meaningful? */
+	if (cbw->Lun >= FSG_MAX_LUNS || cbw->Flags & ~US_BULK_FLAG_IN ||
+			cbw->Length <= 0 || cbw->Length > MAX_COMMAND_SIZE) {
+		DBG(fsg, "non-meaningful CBW: lun = %u, flags = 0x%x, "
+				"cmdlen %u\n",
+				cbw->Lun, cbw->Flags, cbw->Length);
+
+		/* We can do anything we want here, so let's stall the
+		 * bulk pipes if we are allowed to. */
+		if (common->can_stall) {
+			fsg_set_halt(fsg, fsg->bulk_out);
+			halt_bulk_in_endpoint(fsg);
+		}
+		return -EINVAL;
+	}
+
+	/* Save the command for later */
+	common->cmnd_size = cbw->Length;
+	memcpy(common->cmnd, cbw->CDB, common->cmnd_size);
+	if (cbw->Flags & US_BULK_FLAG_IN)
+		common->data_dir = DATA_DIR_TO_HOST;
+	else
+		common->data_dir = DATA_DIR_FROM_HOST;
+	common->data_size = le32_to_cpu(cbw->DataTransferLength);
+	if (common->data_size == 0)
+		common->data_dir = DATA_DIR_NONE;
+	common->lun = cbw->Lun;
+	common->tag = cbw->Tag;
+	return 0;
+}
+
+
+static int get_next_command(struct fsg_common *common)
+{
+	struct fsg_buffhd	*bh;
+	int			rc = 0;
+
+	/* Wait for the next buffer to become available */
+	bh = common->next_buffhd_to_fill;
+	while (bh->state != BUF_STATE_EMPTY) {
+		rc = sleep_thread(common);
+		if (rc)
+			return rc;
+	}
+
+	/* Queue a request to read a Bulk-only CBW */
+	set_bulk_out_req_length(common, bh, US_BULK_CB_WRAP_LEN);
+	bh->outreq->short_not_ok = 1;
+	START_TRANSFER_OR(common, bulk_out, bh->outreq,
+			  &bh->outreq_busy, &bh->state)
+		/* Don't know what to do if common->fsg is NULL */
+		return -EIO;
+
+	/* We will drain the buffer in software, which means we
+	 * can reuse it for the next filling.  No need to advance
+	 * next_buffhd_to_fill. */
+
+	/* Wait for the CBW to arrive */
+	while (bh->state != BUF_STATE_FULL) {
+		rc = sleep_thread(common);
+		if (rc)
+			return rc;
+	}
+
+	rc = fsg_is_set(common) ? received_cbw(common->fsg, bh) : -EIO;
+	bh->state = BUF_STATE_EMPTY;
+
+	return rc;
+}
+
+
+/*-------------------------------------------------------------------------*/
+
+static int enable_endpoint(struct fsg_common *common, struct usb_ep *ep)
+{
+	int	rc;
+
+	ep->driver_data = common;
+	rc = usb_ep_enable(ep);
+	if (rc)
+		ERROR(common, "can't enable %s, result %d\n", ep->name, rc);
+	return rc;
+}
+
+static int alloc_request(struct fsg_common *common, struct usb_ep *ep,
+		struct usb_request **preq)
+{
+	*preq = usb_ep_alloc_request(ep);
+	if (*preq)
+		return 0;
+	ERROR(common, "can't allocate request for %s\n", ep->name);
+	return -ENOMEM;
+}
+
+/* Reset interface setting and re-init endpoint state (toggle etc). */
+static int do_set_interface(struct fsg_common *common, struct fsg_dev *new_fsg)
+{
+	struct fsg_dev *fsg;
+	int i, rc = 0;
+
+	if (common->running)
+		DBG(common, "reset interface\n");
+
+reset:
+	/* Deallocate the requests */
+	if (common->fsg) {
+		fsg = common->fsg;
+
+		for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
+			struct fsg_buffhd *bh = &common->buffhds[i];
+
+			if (bh->inreq) {
+				usb_ep_free_request(fsg->bulk_in, bh->inreq);
+				bh->inreq = NULL;
+			}
+			if (bh->outreq) {
+				usb_ep_free_request(fsg->bulk_out, bh->outreq);
+				bh->outreq = NULL;
+			}
+		}
+
+		/* Disable the endpoints */
+		if (fsg->bulk_in_enabled) {
+			usb_ep_disable(fsg->bulk_in);
+			fsg->bulk_in_enabled = 0;
+		}
+		if (fsg->bulk_out_enabled) {
+			usb_ep_disable(fsg->bulk_out);
+			fsg->bulk_out_enabled = 0;
+		}
+
+		common->fsg = NULL;
+	}
+
+	common->running = 0;
+	if (!new_fsg || rc)
+		return rc;
+
+	common->fsg = new_fsg;
+	fsg = common->fsg;
+
+	/* Enable the endpoints */
+	fsg->bulk_in->desc = fsg_ep_desc(common->gadget,
+					 &fsg_fs_bulk_in_desc, &fsg_hs_bulk_in_desc);
+	rc = enable_endpoint(common, fsg->bulk_in);
+	if (rc)
+		goto reset;
+	fsg->bulk_in_enabled = 1;
+
+	fsg->bulk_out->desc = fsg_ep_desc(common->gadget,
+					  &fsg_fs_bulk_out_desc, &fsg_hs_bulk_out_desc);
+	rc = enable_endpoint(common, fsg->bulk_out);
+	if (rc)
+		goto reset;
+	fsg->bulk_out_enabled = 1;
+	common->bulk_out_maxpacket = 512;
+	clear_bit(IGNORE_BULK_OUT, &fsg->atomic_bitflags);
+
+	/* Allocate the requests */
+	for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
+		struct fsg_buffhd	*bh = &common->buffhds[i];
+
+		rc = alloc_request(common, fsg->bulk_in, &bh->inreq);
+		if (rc)
+			goto reset;
+		rc = alloc_request(common, fsg->bulk_out, &bh->outreq);
+		if (rc)
+			goto reset;
+		bh->inreq->buf = bh->outreq->buf = bh->buf;
+		bh->inreq->context = bh->outreq->context = bh;
+		bh->inreq->complete = bulk_in_complete;
+		bh->outreq->complete = bulk_out_complete;
+	}
+
+	common->running = 1;
+
+	return rc;
+}
+
+
+/****************************** ALT CONFIGS ******************************/
+
+
+static int fsg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
+{
+	struct fsg_dev *fsg = fsg_from_func(f);
+	fsg->common->new_fsg = fsg;
+	raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
+	return 0;
+}
+
+static void fsg_disable(struct usb_function *f)
+{
+	struct fsg_dev *fsg = fsg_from_func(f);
+	fsg->common->new_fsg = NULL;
+	raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static void handle_exception(struct fsg_common *common)
+{
+	int			i;
+	struct fsg_buffhd	*bh;
+	enum fsg_state		old_state;
+	struct fsg_lun		*curlun;
+	unsigned int		exception_req_tag;
+
+	/* Cancel all the pending transfers */
+	if (common->fsg) {
+		for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
+			bh = &common->buffhds[i];
+			if (bh->inreq_busy)
+				usb_ep_dequeue(common->fsg->bulk_in, bh->inreq);
+			if (bh->outreq_busy)
+				usb_ep_dequeue(common->fsg->bulk_out,
+					       bh->outreq);
+		}
+
+		/* Wait until everything is idle */
+		for (;;) {
+			int num_active = 0;
+			for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
+				bh = &common->buffhds[i];
+				num_active += bh->inreq_busy + bh->outreq_busy;
+			}
+			if (num_active == 0)
+				break;
+			if (sleep_thread(common))
+				return;
+		}
+
+		/* Clear out the controller's fifos */
+		if (common->fsg->bulk_in_enabled)
+			usb_ep_fifo_flush(common->fsg->bulk_in);
+		if (common->fsg->bulk_out_enabled)
+			usb_ep_fifo_flush(common->fsg->bulk_out);
+	}
+
+	/* Reset the I/O buffer states and pointers, the SCSI
+	 * state, and the exception.  Then invoke the handler. */
+
+	for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
+		bh = &common->buffhds[i];
+		bh->state = BUF_STATE_EMPTY;
+	}
+	common->next_buffhd_to_fill = &common->buffhds[0];
+	common->next_buffhd_to_drain = &common->buffhds[0];
+	exception_req_tag = common->exception_req_tag;
+	old_state = common->state;
+
+	report_exception("handling", old_state);
+
+	if (old_state == FSG_STATE_ABORT_BULK_OUT)
+		common->state = FSG_STATE_STATUS_PHASE;
+	else {
+		for (i = 0; i < common->nluns; ++i) {
+			curlun = &common->luns[i];
+			curlun->sense_data = SS_NO_SENSE;
+			curlun->info_valid = 0;
+		}
+		common->state = FSG_STATE_IDLE;
+	}
+
+	/* Carry out any extra actions required for the exception */
+	switch (old_state) {
+	case FSG_STATE_ABORT_BULK_OUT:
+		send_status(common);
+
+		if (common->state == FSG_STATE_STATUS_PHASE)
+			common->state = FSG_STATE_IDLE;
+		break;
+
+	case FSG_STATE_RESET:
+		/* In case we were forced against our will to halt a
+		 * bulk endpoint, clear the halt now.  (The SuperH UDC
+		 * requires this.) */
+		if (!fsg_is_set(common))
+			break;
+		if (test_and_clear_bit(IGNORE_BULK_OUT,
+				       &common->fsg->atomic_bitflags))
+			usb_ep_clear_halt(common->fsg->bulk_in);
+
+		if (common->ep0_req_tag == exception_req_tag)
+			ep0_queue(common);	/* Complete the status stage */
+
+		break;
+
+	case FSG_STATE_CONFIG_CHANGE:
+		do_set_interface(common, common->new_fsg);
+		break;
+
+	case FSG_STATE_EXIT:
+	case FSG_STATE_TERMINATED:
+		do_set_interface(common, NULL);		/* Free resources */
+		common->state = FSG_STATE_TERMINATED;	/* Stop the thread */
+		break;
+
+	case FSG_STATE_INTERFACE_CHANGE:
+	case FSG_STATE_DISCONNECT:
+	case FSG_STATE_COMMAND_PHASE:
+	case FSG_STATE_DATA_PHASE:
+	case FSG_STATE_STATUS_PHASE:
+	case FSG_STATE_IDLE:
+		break;
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+
+static void fsg_main_thread(void *fsg_)
+{
+	struct fsg_dev *fsg = fsg_;
+	struct fsg_common *common = fsg->common;
+	struct fsg_buffhd *bh;
+	unsigned i;
+	int ret = 0;
+
+	/* The main loop */
+	while (common->state != FSG_STATE_TERMINATED) {
+		if (exception_in_progress(common)) {
+			handle_exception(common);
+			continue;
+		}
+
+		if (!common->running) {
+			ret = sleep_thread(common);
+			if (ret)
+				break;
+			continue;
+		}
+
+		ret = get_next_command(common);
+		if (ret)
+			continue;
+
+		if (!exception_in_progress(common))
+			common->state = FSG_STATE_DATA_PHASE;
+
+		if (do_scsi_command(common) || finish_reply(common))
+			continue;
+
+		if (!exception_in_progress(common))
+			common->state = FSG_STATE_STATUS_PHASE;
+
+		if (send_status(common))
+			continue;
+
+		if (!exception_in_progress(common))
+			common->state = FSG_STATE_IDLE;
+	}
+
+	if (ret && ret != -ERESTARTSYS)
+		pr_warn("%s: error %pe\n", __func__, ERR_PTR(ret));
+
+	usb_free_all_descriptors(&fsg->function);
+
+	for (i = 0; i < ums_count; i++)
+		close(ums[i].fd);
+
+	bh = common->buffhds;
+	i = FSG_NUM_BUFFERS;
+
+	do {
+		dma_free(bh->buf);
+	} while (++bh, --i);
+
+	ums_count = 0;
+	ums_files = NULL;
+}
+
+static void fsg_common_release(struct fsg_common *common);
+
+static struct fsg_common *fsg_common_setup(void)
+{
+	struct fsg_common *common;
+
+	/* Allocate? */
+	common = calloc(sizeof(*common), 1);
+	if (!common)
+		return NULL;
+
+	common->ops = NULL;
+	common->private_data = NULL;
+
+	return common;
+}
+
+static int fsg_common_init(struct fsg_common *common,
+			   struct usb_composite_dev *cdev)
+{
+	struct usb_gadget *gadget = cdev->gadget;
+	struct file_list_entry *fentry;
+	struct fsg_buffhd *bh;
+	int nluns, i, fd = -1, rc;
+
+	ums_count = 0;
+
+	common->gadget = gadget;
+	common->ep0 = gadget->ep0;
+	common->ep0req = cdev->req;
+
+	thread_task = bthread_run(fsg_main_thread, common->fsg, "mass-storage-gadget");
+	if (IS_ERR(thread_task))
+		return PTR_ERR(thread_task);
+
+	file_list_detect_all(ums_files);
+
+	file_list_for_each_entry(ums_files, fentry) {
+		unsigned flags = O_RDWR;
+		struct stat st;
+
+		if (fentry->flags) {
+			pr_err("flags not supported\n");
+			return -ENOSYS;
+		}
+
+		fd = open(fentry->filename, flags);
+		if (fd < 0) {
+			pr_err("open('%s') failed: %pe\n",
+			       fentry->filename, ERR_PTR(fd));
+			return fd;
+		}
+
+		rc = fstat(fd, &st);
+		if (rc < 0) {
+			pr_err("stat('%s') failed: %pe\n",
+			       fentry->filename, ERR_PTR(rc));
+			goto close;
+		}
+
+		if (st.st_size % SECTOR_SIZE != 0) {
+			pr_err("exporting '%s' failed: invalid block size\n",
+			       fentry->filename);
+			goto close;
+		}
+
+		ums[ums_count].fd = fd;
+		ums[ums_count].num_sectors = st.st_size / SECTOR_SIZE;
+
+		strlcpy(ums[ums_count].name, fentry->name, sizeof(ums[ums_count].name));
+
+		DBG(common, "LUN %d, %s sector_count %#x\n",
+			ums_count, fentry->name, ums[ums_count].num_sectors);
+
+		ums_count++;
+	}
+
+	/* Find out how many LUNs there should be */
+	nluns = ums_count;
+	if (nluns < 1 || nluns > FSG_MAX_LUNS) {
+		pr_warn("invalid number of LUNs: %u\n", nluns);
+		rc = -EINVAL;
+		goto close;
+	}
+
+	/* Maybe allocate device-global string IDs, and patch descriptors */
+	if (fsg_strings[FSG_STRING_INTERFACE].id == 0) {
+		rc = usb_string_id(cdev);
+		if (unlikely(rc < 0))
+			goto error_release;
+		fsg_strings[FSG_STRING_INTERFACE].id = rc;
+		fsg_intf_desc.iInterface = rc;
+	}
+
+	common->nluns = nluns;
+
+	for (i = 0; i < nluns; i++) {
+		common->luns[i].removable = 1;
+
+		rc = fsg_lun_open(&common->luns[i], ums[i].num_sectors, "");
+		if (rc)
+			goto error_luns;
+	}
+	common->lun = 0;
+
+	/* Data buffers cyclic list */
+	bh = common->buffhds;
+
+	i = FSG_NUM_BUFFERS;
+	goto buffhds_first_it;
+	do {
+		bh->next = bh + 1;
+		++bh;
+buffhds_first_it:
+		bh->inreq_busy = 0;
+		bh->outreq_busy = 0;
+		bh->buf = dma_alloc(FSG_BUFLEN);
+		if (unlikely(!bh->buf)) {
+			rc = -ENOMEM;
+			goto error_release;
+		}
+	} while (--i);
+	bh->next = common->buffhds;
+
+	snprintf(common->inquiry_string, sizeof common->inquiry_string,
+		 "%-8s%-16s%04x",
+		 "Linux   ",
+		 "File-Store Gadget",
+		 0xffff);
+
+	/* Some peripheral controllers are known not to be able to
+	 * halt bulk endpoints correctly.  If one of them is present,
+	 * disable stalls.
+	 */
+
+	/* Information */
+	DBG(common, FSG_DRIVER_DESC ", version: " FSG_DRIVER_VERSION "\n");
+	DBG(common, "Number of LUNs=%d\n", common->nluns);
+
+	return 0;
+
+error_luns:
+	common->nluns = i + 1;
+error_release:
+	common->state = FSG_STATE_TERMINATED;	/* The thread is dead */
+	fsg_common_release(common);
+close:
+	close(fd);
+	return rc;
+}
+
+static void fsg_common_release(struct fsg_common *common)
+{
+	/* If the thread isn't already dead, tell it to exit now */
+	if (common->state != FSG_STATE_TERMINATED) {
+		raise_exception(common, FSG_STATE_EXIT);
+	}
+
+	bthread_cancel(thread_task);
+}
+
+
+static void fsg_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct fsg_dev		*fsg = fsg_from_func(f);
+
+	DBG(fsg, "unbind\n");
+
+	if (fsg->common->fsg == fsg) {
+		fsg->common->new_fsg = NULL;
+		raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
+	}
+
+	fsg_common_release(fsg->common);
+}
+
+static int fsg_bind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct fsg_dev		*fsg = fsg_from_func(f);
+	struct usb_gadget	*gadget = c->cdev->gadget;
+	int			ret;
+	struct usb_ep		*ep;
+	struct usb_descriptor_header **hs_function = NULL;
+	struct fsg_common	*common = fsg->common;
+
+	if (!ums_files) {
+		struct f_ums_opts *opts = container_of(f->fi, struct f_ums_opts, func_inst);
+
+		ums_files = opts->files;
+	}
+
+	fsg->gadget = gadget;
+
+	DBG(fsg, "bind\n");
+
+	ret = fsg_common_init(common, c->cdev);
+	if (ret)
+		goto remove_ums_files;
+
+	/* New interface */
+	ret = usb_interface_id(c, f);
+	if (ret < 0)
+		goto fsg_common_release;
+	fsg_intf_desc.bInterfaceNumber = ret;
+	fsg->interface_number = ret;
+
+	ret = -EOPNOTSUPP;
+
+	/* Find all the endpoints we will use */
+	ep = usb_ep_autoconfig(gadget, &fsg_fs_bulk_in_desc);
+	if (!ep)
+		goto autoconf_fail;
+
+	ep->driver_data = common;	/* claim the endpoint */
+	fsg->bulk_in = ep;
+
+	ep = usb_ep_autoconfig(gadget, &fsg_fs_bulk_out_desc);
+	if (!ep)
+		goto autoconf_fail;
+
+	ep->driver_data = common;	/* claim the endpoint */
+	fsg->bulk_out = ep;
+
+	if (gadget_is_dualspeed(gadget)) {
+		/* Assume endpoint addresses are the same for both speeds */
+		fsg_hs_bulk_in_desc.bEndpointAddress =
+			fsg_fs_bulk_in_desc.bEndpointAddress;
+		fsg_hs_bulk_out_desc.bEndpointAddress =
+			fsg_fs_bulk_out_desc.bEndpointAddress;
+		hs_function = fsg_hs_function;
+	}
+
+	/* Copy descriptors */
+	return usb_assign_descriptors(f, fsg_fs_function, hs_function, NULL);
+
+autoconf_fail:
+	ERROR(fsg, "unable to autoconfigure all endpoints\n");
+fsg_common_release:
+	fsg_common_release(common);
+remove_ums_files:
+	ums_files = NULL;
+
+	return ret;
+}
+
+
+/****************************** ADD FUNCTION ******************************/
+
+static struct usb_gadget_strings *fsg_strings_array[] = {
+	&fsg_stringtab,
+	NULL,
+};
+
+static void fsg_free(struct usb_function *f)
+{
+	struct fsg_dev *fsg;
+
+	fsg = container_of(f, struct fsg_dev, function);
+
+	kfree(fsg);
+}
+
+static struct usb_function *fsg_alloc(struct usb_function_instance *fi)
+{
+	struct f_ums_opts *opts = fsg_opts_from_func_inst(fi);
+	struct fsg_common *common = opts->common;
+	struct fsg_dev *fsg;
+
+	fsg = kzalloc(sizeof(*fsg), GFP_KERNEL);
+	if (!fsg)
+		return ERR_PTR(-ENOMEM);
+
+	fsg->function.name = FSG_DRIVER_DESC;
+	fsg->function.strings = fsg_strings_array;
+	/* descriptors are per-instance copies */
+	fsg->function.bind = fsg_bind;
+	fsg->function.set_alt = fsg_set_alt;
+	fsg->function.setup = fsg_setup;
+	fsg->function.disable = fsg_disable;
+	fsg->function.unbind = fsg_unbind;
+	fsg->function.free_func = fsg_free;
+
+	fsg->common = common;
+	common->fsg = fsg;
+
+	return &fsg->function;
+}
+
+static void fsg_free_instance(struct usb_function_instance *fi)
+{
+	struct f_ums_opts *opts = fsg_opts_from_func_inst(fi);
+
+	kfree(opts->common);
+	kfree(opts);
+}
+
+static struct usb_function_instance *fsg_alloc_inst(void)
+{
+	struct f_ums_opts *opts;
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+
+	opts->func_inst.free_func_inst = fsg_free_instance;
+
+	opts->common = fsg_common_setup();
+	if (!opts->common) {
+		free(opts);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	return &opts->func_inst;
+}
+
+DECLARE_USB_FUNCTION_INIT(ums, fsg_alloc_inst, fsg_alloc);
diff --git a/drivers/usb/gadget/multi.c b/drivers/usb/gadget/multi.c
index 144ac0624b..04b3c2604e 100644
--- a/drivers/usb/gadget/multi.c
+++ b/drivers/usb/gadget/multi.c
@@ -60,6 +60,8 @@ static struct usb_function_instance *fi_dfu;
 static struct usb_function *f_dfu;
 static struct usb_function_instance *fi_fastboot;
 static struct usb_function *f_fastboot;
+static struct usb_function_instance *fi_ums;
+static struct usb_function *f_ums;
 
 static struct usb_configuration config = {
 	.bConfigurationValue	= 1,
@@ -139,6 +141,31 @@ static int multi_bind_fastboot(struct usb_composite_dev *cdev)
 	return usb_add_function(&config, f_fastboot);
 }
 
+static int multi_bind_ums(struct usb_composite_dev *cdev)
+{
+	int ret;
+	struct f_ums_opts *opts;
+
+	fi_ums = usb_get_function_instance("ums");
+	if (IS_ERR(fi_ums)) {
+		ret = PTR_ERR(fi_ums);
+		fi_ums = NULL;
+		return ret;
+	}
+
+	opts = container_of(fi_ums, struct f_ums_opts, func_inst);
+	opts->files = gadget_multi_opts->ums_opts.files;
+
+	f_ums = usb_get_function(fi_ums);
+	if (IS_ERR(f_ums)) {
+		ret = PTR_ERR(f_ums);
+		f_ums = NULL;
+		return ret;
+	}
+
+	return usb_add_function(&config, f_ums);
+}
+
 static int multi_unbind(struct usb_composite_dev *cdev)
 {
 	if (gadget_multi_opts->create_acm) {
@@ -205,6 +232,13 @@ static int multi_bind(struct usb_composite_dev *cdev)
 			goto out;
 	}
 
+	if (gadget_multi_opts->ums_opts.files) {
+		printf("%s: creating USB Mass Storage function\n", __func__);
+		ret = multi_bind_ums(cdev);
+		if (ret)
+			goto out;
+	}
+
 	if (gadget_multi_opts->create_acm) {
 		printf("%s: creating ACM function\n", __func__);
 		ret = multi_bind_acm(cdev);
@@ -273,6 +307,7 @@ unsigned usb_multi_count_functions(struct f_multi_opts *opts)
 	count += !file_list_empty(opts->fastboot_opts.files) ||
 		opts->fastboot_opts.export_bbu;
 	count += !file_list_empty(opts->dfu_opts.files);
+	count += !file_list_empty(opts->ums_opts.files);
 	count += opts->create_acm;
 
 	return count;
@@ -282,6 +317,7 @@ void usb_multi_opts_release(struct f_multi_opts *opts)
 {
 	file_list_free(opts->fastboot_opts.files);
 	file_list_free(opts->dfu_opts.files);
+	file_list_free(opts->ums_opts.files);
 
 	free(opts);
 }
diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
new file mode 100644
index 0000000000..88cd745063
--- /dev/null
+++ b/drivers/usb/gadget/storage_common.c
@@ -0,0 +1,173 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * storage_common.c -- Common definitions for mass storage functionality
+ *
+ * Copyright (C) 2003-2008 Alan Stern
+ * Copyeight (C) 2009 Samsung Electronics
+ * Author: Michal Nazarewicz (m.nazarewicz@samsung.com)
+ *
+ * Ported to u-boot:
+ * Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * Code refactoring & cleanup:
+ * Łukasz Majewski <l.majewski@samsung.com>
+ */
+
+#include "storage_common.h"
+
+/*
+ * This file requires the following identifiers used in USB strings to
+ * be defined (each of type pointer to char):
+ *  - fsg_string_manufacturer -- name of the manufacturer
+ *  - fsg_string_product      -- name of the product
+ *  - fsg_string_serial       -- product's serial
+ *  - fsg_string_config       -- name of the configuration
+ *  - fsg_string_interface    -- name of the interface
+ * The first four are only needed when FSG_DESCRIPTORS_DEVICE_STRINGS
+ * macro is defined prior to including this file.
+ */
+
+/* There is only one interface. */
+
+struct usb_interface_descriptor fsg_intf_desc = {
+	.bLength =		sizeof fsg_intf_desc,
+	.bDescriptorType =	USB_DT_INTERFACE,
+
+	.bNumEndpoints =	2,		/* Adjusted during fsg_bind() */
+	.bInterfaceClass =	USB_CLASS_MASS_STORAGE,
+	.bInterfaceSubClass =	USB_SC_SCSI,	/* Adjusted during fsg_bind() */
+	.bInterfaceProtocol =	USB_PR_BULK,	/* Adjusted during fsg_bind() */
+	.iInterface =		FSG_STRING_INTERFACE,
+};
+
+/*
+ * Three full-speed endpoint descriptors: bulk-in, bulk-out, and
+ * interrupt-in.
+ */
+
+struct usb_endpoint_descriptor fsg_fs_bulk_in_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	/* wMaxPacketSize set by autoconfiguration */
+};
+
+struct usb_endpoint_descriptor fsg_fs_bulk_out_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	/* wMaxPacketSize set by autoconfiguration */
+};
+
+struct usb_descriptor_header *fsg_fs_function[] = {
+	(struct usb_descriptor_header *) &fsg_intf_desc,
+	(struct usb_descriptor_header *) &fsg_fs_bulk_in_desc,
+	(struct usb_descriptor_header *) &fsg_fs_bulk_out_desc,
+	NULL,
+};
+
+/*
+ * USB 2.0 devices need to expose both high speed and full speed
+ * descriptors, unless they only run at full speed.
+ *
+ * That means alternate endpoint descriptors (bigger packets)
+ * and a "device qualifier" ... plus more construction options
+ * for the configuration descriptor.
+ */
+struct usb_endpoint_descriptor fsg_hs_bulk_in_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	/* bEndpointAddress copied from fs_bulk_in_desc during fsg_bind() */
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+};
+
+struct usb_endpoint_descriptor fsg_hs_bulk_out_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	/* bEndpointAddress copied from fs_bulk_out_desc during fsg_bind() */
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+	.bInterval =		1,	/* NAK every 1 uframe */
+};
+
+struct usb_descriptor_header *fsg_hs_function[] = {
+	(struct usb_descriptor_header *) &fsg_intf_desc,
+	(struct usb_descriptor_header *) &fsg_hs_bulk_in_desc,
+	(struct usb_descriptor_header *) &fsg_hs_bulk_out_desc,
+	NULL,
+};
+
+/* Maxpacket and other transfer characteristics vary by speed. */
+struct usb_endpoint_descriptor *
+fsg_ep_desc(struct usb_gadget *g, struct usb_endpoint_descriptor *fs,
+		struct usb_endpoint_descriptor *hs)
+{
+	if (gadget_is_dualspeed(g) && g->speed == USB_SPEED_HIGH)
+		return hs;
+	return fs;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * If the next two routines are called while the gadget is registered,
+ * the caller must own fsg->filesem for writing.
+ */
+
+int fsg_lun_open(struct fsg_lun *curlun, unsigned int num_sectors,
+		 const char *filename)
+{
+	int				ro;
+
+	/* R/W if we can, R/O if we must */
+	ro = curlun->initially_ro;
+
+	curlun->ro = ro;
+	curlun->file_length = num_sectors << 9;
+	curlun->num_sectors = num_sectors;
+	debug("open backing file: %s\n", filename);
+
+	return 0;
+}
+
+void fsg_lun_close(struct fsg_lun *curlun)
+{
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * Sync the file data, don't bother with the metadata.
+ * This code was copied from fs/buffer.c:sys_fdatasync().
+ */
+int fsg_lun_fsync_sub(struct fsg_lun *curlun)
+{
+	return 0;
+}
+
+void store_cdrom_address(u8 *dest, int msf, u32 addr)
+{
+	if (msf) {
+		/* Convert to Minutes-Seconds-Frames */
+		addr >>= 2;		/* Convert to 2048-byte frames */
+		addr += 2*75;		/* Lead-in occupies 2 seconds */
+		dest[3] = addr % 75;	/* Frames */
+		addr /= 75;
+		dest[2] = addr % 60;	/* Seconds */
+		addr /= 60;
+		dest[1] = addr;		/* Minutes */
+		dest[0] = 0;		/* Reserved */
+	} else {
+		/* Absolute sector */
+		put_unaligned_be32(addr, dest);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
diff --git a/drivers/usb/gadget/storage_common.h b/drivers/usb/gadget/storage_common.h
new file mode 100644
index 0000000000..ce07a7dac7
--- /dev/null
+++ b/drivers/usb/gadget/storage_common.h
@@ -0,0 +1,245 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+#ifndef USB_STORAGE_COMMON_H
+#define USB_STORAGE_COMMON_H
+
+#include <driver.h>
+#include <usb/storage.h>
+#include <asm/unaligned.h>
+#include <usb/mass_storage.h>
+
+#ifndef DEBUG
+#undef VERBOSE_DEBUG
+#undef DUMP_MSGS
+#endif /* !DEBUG */
+
+#define VLDBG(lun, fmt, args...)  dev_vdbg(&(lun)->dev, fmt, ## args)
+#define LDBG(lun, fmt, args...)   dev_dbg (&(lun)->dev, fmt, ## args)
+#define LERROR(lun, fmt, args...) dev_err (&(lun)->dev, fmt, ## args)
+#define LWARN(lun, fmt, args...)  dev_warn(&(lun)->dev, fmt, ## args)
+#define LINFO(lun, fmt, args...)  dev_info(&(lun)->dev, fmt, ## args)
+
+/*
+ * Keep those macros in sync with those in
+ * include/linux/usb/composite.h or else GCC will complain.  If they
+ * are identical (the same names of arguments, white spaces in the
+ * same places) GCC will allow redefinition otherwise (even if some
+ * white space is removed or added) warning will be issued.
+ *
+ * Those macros are needed here because File Storage Gadget does not
+ * include the composite.h header.  For composite gadgets those macros
+ * are redundant since composite.h is included any way.
+ *
+ * One could check whether those macros are already defined (which
+ * would indicate composite.h had been included) or not (which would
+ * indicate we were in FSG) but this is not done because a warning is
+ * desired if definitions here differ from the ones in composite.h.
+ *
+ * We want the definitions to match and be the same in File Storage
+ * Gadget as well as Mass Storage Function (and so composite gadgets
+ * using MSF).  If someone changes them in composite.h it will produce
+ * a warning in this file when building MSF.
+ */
+
+#define DBG(d, fmt, args...) \
+	dev_dbg(&(d)->gadget->dev , fmt , ## args)
+#define VDBG(d, fmt, args...) \
+	dev_vdbg(&(d)->gadget->dev , fmt , ## args)
+#define ERROR(d, fmt, args...) \
+	dev_err(&(d)->gadget->dev , fmt , ## args)
+#define WARNING(d, fmt, args...) \
+	dev_warn(&(d)->gadget->dev , fmt , ## args)
+#define INFO(d, fmt, args...) \
+	dev_info(&(d)->gadget->dev , fmt , ## args)
+
+#ifdef DUMP_MSGS
+
+/* dump_msg(fsg, const char * label, const u8 * buf, unsigned length); */
+# define dump_msg(fsg, label, buf, length) do {                         \
+	if (length < 512) {						\
+		DBG(fsg, "%s, length %u:\n", label, length);		\
+		print_hex_dump("", DUMP_PREFIX_OFFSET,	\
+			       16, 1, buf, length, 0);			\
+	}								\
+} while (0)
+
+#  define dump_cdb(fsg) do { } while (0)
+
+#else
+
+#  define dump_msg(fsg, /* const char * */ label, \
+		   /* const u8 * */ buf, /* unsigned */ length) do { } while (0)
+
+#  ifdef VERBOSE_DEBUG
+
+#    define dump_cdb(fsg)						\
+	print_hex_dump("SCSI CDB: ", DUMP_PREFIX_NONE,	\
+		       16, 1, (fsg)->cmnd, (fsg)->cmnd_size, 0)		\
+
+#  else
+
+#    define dump_cdb(fsg) do { } while (0)
+
+#  endif /* VERBOSE_DEBUG */
+
+#endif /* DUMP_MSGS */
+
+/*
+ * Thanks to NetChip Technologies for donating this product ID.
+ *
+ * DO NOT REUSE THESE IDs with any other driver!!  Ever!!
+ * Instead:  allocate your own, using normal USB-IF procedures.
+ */
+
+#define FSG_VENDOR_ID	0x0525	/* NetChip */
+#define FSG_PRODUCT_ID	0xa4a5	/* Linux-USB File-backed Storage Gadget */
+
+/* Length of a SCSI Command Data Block */
+#define MAX_COMMAND_SIZE	16
+
+/* SCSI Sense Key/Additional Sense Code/ASC Qualifier values */
+#define SS_NO_SENSE				0
+#define SS_COMMUNICATION_FAILURE		0x040800
+#define SS_INVALID_COMMAND			0x052000
+#define SS_INVALID_FIELD_IN_CDB			0x052400
+#define SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE	0x052100
+#define SS_LOGICAL_UNIT_NOT_SUPPORTED		0x052500
+#define SS_MEDIUM_NOT_PRESENT			0x023a00
+#define SS_MEDIUM_REMOVAL_PREVENTED		0x055302
+#define SS_NOT_READY_TO_READY_TRANSITION	0x062800
+#define SS_RESET_OCCURRED			0x062900
+#define SS_SAVING_PARAMETERS_NOT_SUPPORTED	0x053900
+#define SS_UNRECOVERED_READ_ERROR		0x031100
+#define SS_WRITE_ERROR				0x030c02
+#define SS_WRITE_PROTECTED			0x072700
+
+#define SK(x)		((u8) ((x) >> 16))	/* Sense Key byte, etc. */
+#define ASC(x)		((u8) ((x) >> 8))
+#define ASCQ(x)		((u8) (x))
+
+/*-------------------------------------------------------------------------*/
+
+struct fsg_lun {
+	loff_t		file_length;
+	loff_t		num_sectors;
+
+	unsigned int	initially_ro:1;
+	unsigned int	ro:1;
+	unsigned int	removable:1;
+	unsigned int	cdrom:1;
+	unsigned int	prevent_medium_removal:1;
+	unsigned int	registered:1;
+	unsigned int	info_valid:1;
+	unsigned int	nofua:1;
+
+	u32		sense_data;
+	u32		sense_data_info;
+	u32		unit_attention_data;
+
+	struct device_d	dev;
+};
+
+#define fsg_lun_is_open(curlun)	((curlun)->filp != NULL)
+
+/* Big enough to hold our biggest descriptor */
+#define EP0_BUFSIZE	256
+#define DELAYED_STATUS	(EP0_BUFSIZE + 999)	/* An impossibly large value */
+
+/* Number of buffers we will use.  2 is enough for double-buffering */
+#define FSG_NUM_BUFFERS	2
+
+/* Default size of buffer length. */
+#define FSG_BUFLEN	((u32)131072)
+
+/* Maximal number of LUNs supported in mass storage function */
+#define FSG_MAX_LUNS	8
+
+enum fsg_buffer_state {
+	BUF_STATE_EMPTY = 0,
+	BUF_STATE_FULL,
+	BUF_STATE_BUSY
+};
+
+/*
+ * When FSG_BUFFHD_STATIC_BUFFER is defined when this file is included
+ * the fsg_buffhd structure's buf field will be an array of FSG_BUFLEN
+ * characters rather then a pointer to void.
+ */
+
+struct fsg_buffhd {
+	void				*buf;
+	enum fsg_buffer_state		state;
+	struct fsg_buffhd		*next;
+
+	/*
+	 * The NetChip 2280 is faster, and handles some protocol faults
+	 * better, if we don't submit any short bulk-out read requests.
+	 * So we will record the intended request length here.
+	 */
+	unsigned int			bulk_out_intended_length;
+
+	struct usb_request		*inreq;
+	int				inreq_busy;
+	struct usb_request		*outreq;
+	int				outreq_busy;
+};
+
+enum fsg_state {
+	/* This one isn't used anywhere */
+	FSG_STATE_COMMAND_PHASE = -10,
+	FSG_STATE_DATA_PHASE,
+	FSG_STATE_STATUS_PHASE,
+
+	FSG_STATE_IDLE = 0,
+	FSG_STATE_ABORT_BULK_OUT,
+	FSG_STATE_RESET,
+	FSG_STATE_INTERFACE_CHANGE,
+	FSG_STATE_CONFIG_CHANGE,
+	FSG_STATE_DISCONNECT,
+	FSG_STATE_EXIT,
+	FSG_STATE_TERMINATED
+};
+
+enum data_direction {
+	DATA_DIR_UNKNOWN = 0,
+	DATA_DIR_FROM_HOST,
+	DATA_DIR_TO_HOST,
+	DATA_DIR_NONE
+};
+
+/*-------------------------------------------------------------------------*/
+
+static inline u32 get_unaligned_be24(u8 *buf)
+{
+	return 0xffffff & (u32) get_unaligned_be32(buf - 1);
+}
+
+/*-------------------------------------------------------------------------*/
+
+enum {
+	FSG_STRING_INTERFACE
+};
+
+/*-------------------------------------------------------------------------*/
+
+extern struct usb_interface_descriptor fsg_intf_desc;
+
+extern struct usb_endpoint_descriptor fsg_fs_bulk_in_desc;
+extern struct usb_endpoint_descriptor fsg_fs_bulk_out_desc;
+extern struct usb_descriptor_header *fsg_fs_function[];
+
+extern struct usb_endpoint_descriptor fsg_hs_bulk_in_desc;
+extern struct usb_endpoint_descriptor fsg_hs_bulk_out_desc;
+extern struct usb_descriptor_header *fsg_hs_function[];
+
+int fsg_lun_open(struct fsg_lun *curlun, unsigned int num_sectors,
+		 const char *filename);
+void fsg_lun_close(struct fsg_lun *curlun);
+
+struct usb_endpoint_descriptor *
+fsg_ep_desc(struct usb_gadget *g, struct usb_endpoint_descriptor *fs,
+		struct usb_endpoint_descriptor *hs);
+int fsg_lun_fsync_sub(struct fsg_lun *curlun);
+void store_cdrom_address(u8 *dest, int msf, u32 addr);
+
+#endif /* USB_STORAGE_COMMON_H */
diff --git a/include/bthread.h b/include/bthread.h
index e3871fb115..4441b53696 100644
--- a/include/bthread.h
+++ b/include/bthread.h
@@ -12,14 +12,15 @@ struct bthread;
 
 extern struct bthread *current;
 
-struct bthread *bthread_create(int (*threadfn)(void *), void *data, const char *namefmt, ...);
-void bthread_free(struct bthread *bthread);
+struct bthread *bthread_create(void (*threadfn)(void *), void *data, const char *namefmt, ...);
+void bthread_cancel(struct bthread *bthread);
 
 void bthread_schedule(struct bthread *);
 void bthread_wake(struct bthread *bthread);
 void bthread_suspend(struct bthread *bthread);
 int bthread_should_stop(void);
-int bthread_stop(struct bthread *bthread);
+void __bthread_stop(struct bthread *bthread);
+void *bthread_data(struct bthread *bthread);
 void bthread_info(void);
 const char *bthread_name(struct bthread *bthread);
 bool bthread_is_main(struct bthread *bthread);
diff --git a/include/linux/completion.h b/include/linux/completion.h
new file mode 100644
index 0000000000..e897e4f65b
--- /dev/null
+++ b/include/linux/completion.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * (C) Copyright 2021 Ahmad Fatoum
+ *
+ * Async wait-for-completion handler data structures.
+ * This allows one bthread to wait for another
+ */
+
+#ifndef __LINUX_COMPLETION_H
+#define __LINUX_COMPLETION_H
+
+#include <stdio.h>
+#include <errno.h>
+#include <bthread.h>
+
+struct completion {
+	unsigned int done;
+};
+
+static inline void init_completion(struct completion *x)
+{
+	x->done = 0;
+}
+
+static inline void reinit_completion(struct completion *x)
+{
+	x->done = 0;
+}
+
+static inline int wait_for_completion_interruptible(struct completion *x)
+{
+	while (!x->done) {
+		switch (bthread_should_stop()) {
+		case -EINTR:
+			if (!ctrlc())
+				continue;
+		case 1:
+			return -ERESTARTSYS;
+		}
+	}
+
+	return 0;
+}
+
+static inline bool completion_done(struct completion *x)
+{
+	return x->done;
+}
+
+static inline void complete(struct completion *x)
+{
+	x->done = 1;
+}
+
+#endif
diff --git a/include/poller.h b/include/poller.h
index 371dafc6f8..6e51a06133 100644
--- a/include/poller.h
+++ b/include/poller.h
@@ -7,6 +7,7 @@
 #define POLLER_H
 
 #include <linux/list.h>
+#include <linux/types.h>
 
 struct poller_struct {
 	void (*func)(struct poller_struct *poller);
@@ -39,11 +40,14 @@ static inline bool poller_async_active(struct poller_async *pa)
 	return pa->active;
 }
 
-extern int poller_active;
-
 #ifdef CONFIG_POLLER
+bool poller_active(void);
 void poller_call(void);
 #else
+static inline bool poller_active(void)
+{
+	return false;
+}
 static inline void poller_call(void)
 {
 }
diff --git a/include/sched.h b/include/sched.h
index 57be1678fd..4dadff2069 100644
--- a/include/sched.h
+++ b/include/sched.h
@@ -2,14 +2,12 @@
 #ifndef __BAREBOX_SCHED_H_
 #define __BAREBOX_SCHED_H_
 
-#include <bthread.h>
-#include <poller.h>
-
+#ifdef CONFIG_HAS_SCHED
+void resched(void);
+#else
 static inline void resched(void)
 {
-	poller_call();
-	if (!IS_ENABLED(CONFIG_POLLER) || !poller_active)
-		bthread_reschedule();
 }
+#endif
 
 #endif
diff --git a/include/scsi.h b/include/scsi.h
index f84513b813..23fc7abbaf 100644
--- a/include/scsi.h
+++ b/include/scsi.h
@@ -109,12 +109,15 @@
 #define SCSI_MED_REMOVL	0x1E		/* Prevent/Allow medium Removal (O) */
 #define SCSI_READ6	0x08		/* Read 6-byte (MANDATORY) */
 #define SCSI_READ10	0x28		/* Read 10-byte (MANDATORY) */
+#define SCSI_READ12	0xA8		/* Read 12-byte (O) */
 #define SCSI_READ16	0x88		/* Read 16-byte (O) */
 #define SCSI_RD_CAPAC	0x25		/* Read Capacity (MANDATORY) */
+#define SCSI_RD_FMT_CAPAC	0x23
 #define SCSI_RD_DEFECT	0x37		/* Read Defect Data (O) */
 #define SCSI_READ_LONG	0x3E		/* Read Long (O) */
 #define SCSI_REASS_BLK	0x07		/* Reassign Blocks (O) */
 #define SCSI_RCV_DIAG	0x1C		/* Receive Diagnostic Results (O) */
+#define SCSI_RESERVE	0x16
 #define SCSI_RELEASE	0x17		/* Release Unit (MANDATORY) */
 #define SCSI_REZERO	0x01		/* Rezero Unit (O) */
 #define SCSI_SRCH_DAT_E	0x31		/* Search Data Equal (O) */
@@ -129,10 +132,13 @@
 #define SCSI_VERIFY	0x2F		/* Verify (O) */
 #define SCSI_WRITE6	0x0A		/* Write 6-Byte (MANDATORY) */
 #define SCSI_WRITE10	0x2A		/* Write 10-Byte (MANDATORY) */
+#define SCSI_WRITE12	0xAA		/* Write 12-Byte (O) */
 #define SCSI_WRITE16	0x8A		/* Write 16-Byte (O) */
 #define SCSI_WRT_VERIFY	0x2E		/* Write and Verify (O) */
 #define SCSI_WRITE_LONG	0x3F		/* Write Long (O) */
 #define SCSI_WRITE_SAME	0x41		/* Write Same (O) */
+#define SCSI_RD_TOC	0x43
+#define SCSI_RD_HEADER	0x44
 
 #define SERVICE_ACTION_IN_16	0x9e
 /* values for service action in */
@@ -170,4 +176,10 @@ void scsi_init(void);
 #define FALSE 0
 #endif
 
+/*
+ *  DEVICE TYPES
+ */
+
+#define TYPE_DISK           0x00
+
 #endif /* _SCSI_H */
diff --git a/include/slice.h b/include/slice.h
index cf684300a8..6c4688e308 100644
--- a/include/slice.h
+++ b/include/slice.h
@@ -2,6 +2,7 @@
 #define __SLICE_H
 
 #include <bthread.h>
+#include <poller.h>
 
 enum slice_action {
 	SLICE_ACQUIRE = 1,
@@ -35,13 +36,8 @@ extern struct slice command_slice;
 void command_slice_acquire(void);
 void command_slice_release(void);
 
-extern int poller_active;
-
 #define assert_command_context() do { \
-	WARN_ONCE(IS_ENABLED(CONFIG_POLLER) && poller_active, \
-		  "%s called in poller\n", __func__); \
-	WARN_ONCE(IS_ENABLED(CONFIG_BTHREAD) && !bthread_is_main(current), \
-		  "%s called in secondary bthread\n", __func__); \
+	WARN_ONCE(poller_active(), "%s called in poller\n", __func__); \
 } while (0)
 
 #endif
diff --git a/include/usb/gadget-multi.h b/include/usb/gadget-multi.h
index f30dae5686..79b24ca4df 100644
--- a/include/usb/gadget-multi.h
+++ b/include/usb/gadget-multi.h
@@ -4,11 +4,13 @@
 #include <usb/fastboot.h>
 #include <usb/dfu.h>
 #include <usb/usbserial.h>
+#include <usb/mass_storage.h>
 
 struct f_multi_opts {
 	struct fastboot_opts fastboot_opts;
 	struct f_dfu_opts dfu_opts;
-	int create_acm;
+	struct f_ums_opts ums_opts;
+	bool create_acm;
 	void (*release)(struct f_multi_opts *opts);
 };
 
@@ -17,8 +19,19 @@ void usb_multi_unregister(void);
 void usb_multi_opts_release(struct f_multi_opts *opts);
 unsigned usb_multi_count_functions(struct f_multi_opts *opts);
 
-int usbgadget_register(bool dfu, const char *dfu_opts,
-		       bool fastboot, const char *fastboot_opts,
-		       bool acm, bool export_bbu);
+#define USBGADGET_EXPORT_BBU	(1 << 0)
+#define USBGADGET_ACM		(1 << 1)
+#define USBGADGET_DFU		(1 << 2)
+#define USBGADGET_FASTBOOT	(1 << 3)
+#define USBGADGET_MASS_STORAGE	(1 << 4)
+
+struct usbgadget_funcs {
+	int flags;
+	const char *fastboot_opts;
+	const char *dfu_opts;
+	const char *ums_opts;
+};
+
+int usbgadget_register(const struct usbgadget_funcs *funcs);
 
 #endif /* __USB_GADGET_MULTI_H */
diff --git a/include/usb/mass_storage.h b/include/usb/mass_storage.h
new file mode 100644
index 0000000000..084b3c8e8f
--- /dev/null
+++ b/include/usb/mass_storage.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2011 Samsung Electrnoics
+ * Lukasz Majewski <l.majewski@samsung.com>
+ */
+
+#ifndef __USB_MASS_STORAGE_H__
+#define __USB_MASS_STORAGE_H__
+
+#include <usb/composite.h>
+
+/* Wait at maximum 60 seconds for cable connection */
+#define UMS_CABLE_READY_TIMEOUT	60
+
+struct fsg_common;
+
+struct f_ums_opts {
+	struct usb_function_instance func_inst;
+	struct fsg_common *common;
+	struct file_list *files;
+	unsigned int num_sectors;
+	int fd;
+	char name[16];
+};
+
+int usb_ums_register(struct f_ums_opts *);
+
+#endif /* __USB_MASS_STORAGE_H__ */
diff --git a/include/usb/storage.h b/include/usb/storage.h
new file mode 100644
index 0000000000..e0240f8645
--- /dev/null
+++ b/include/usb/storage.h
@@ -0,0 +1,87 @@
+// SPDX-License-Identifier: GPL-2.0
+#ifndef __LINUX_USB_STORAGE_H
+#define __LINUX_USB_STORAGE_H
+
+/*
+ * linux/usb/storage.h
+ *
+ * Copyright Matthew Wilcox for Intel Corp, 2010
+ *
+ * This file contains definitions taken from the
+ * USB Mass Storage Class Specification Overview
+ *
+ * Distributed under the terms of the GNU GPL, version two.
+ */
+
+/* Storage subclass codes */
+
+#define USB_SC_RBC	0x01		/* Typically, flash devices */
+#define USB_SC_8020	0x02		/* CD-ROM */
+#define USB_SC_QIC	0x03		/* QIC-157 Tapes */
+#define USB_SC_UFI	0x04		/* Floppy */
+#define USB_SC_8070	0x05		/* Removable media */
+#define USB_SC_SCSI	0x06		/* Transparent */
+#define USB_SC_LOCKABLE	0x07		/* Password-protected */
+
+#define USB_SC_ISD200	0xf0		/* ISD200 ATA */
+#define USB_SC_CYP_ATACB	0xf1	/* Cypress ATACB */
+#define USB_SC_DEVICE	0xff		/* Use device's value */
+
+/* Storage protocol codes */
+
+#define USB_PR_CBI	0x00		/* Control/Bulk/Interrupt */
+#define USB_PR_CB	0x01		/* Control/Bulk w/o interrupt */
+#define USB_PR_BULK	0x50		/* bulk only */
+#define USB_PR_UAS	0x62		/* USB Attached SCSI */
+
+#define USB_PR_USBAT	0x80		/* SCM-ATAPI bridge */
+#define USB_PR_EUSB_SDDR09	0x81	/* SCM-SCSI bridge for SDDR-09 */
+#define USB_PR_SDDR55	0x82		/* SDDR-55 (made up) */
+#define USB_PR_DPCM_USB	0xf0		/* Combination CB/SDDR09 */
+#define USB_PR_FREECOM	0xf1		/* Freecom */
+#define USB_PR_DATAFAB	0xf2		/* Datafab chipsets */
+#define USB_PR_JUMPSHOT	0xf3		/* Lexar Jumpshot */
+#define USB_PR_ALAUDA	0xf4		/* Alauda chipsets */
+#define USB_PR_KARMA	0xf5		/* Rio Karma */
+
+#define USB_PR_DEVICE	0xff		/* Use device's value */
+
+/*
+ * Bulk only data structures
+ */
+
+/* command block wrapper */
+struct bulk_cb_wrap {
+	__le32	Signature;		/* contains 'USBC' */
+	__u32	Tag;			/* unique per command id */
+	__le32	DataTransferLength;	/* size of data */
+	__u8	Flags;			/* direction in bit 0 */
+	__u8	Lun;			/* LUN normally 0 */
+	__u8	Length;			/* length of the CDB */
+	__u8	CDB[16];		/* max command */
+};
+
+#define US_BULK_CB_WRAP_LEN	31
+#define US_BULK_CB_SIGN		0x43425355	/* spells out 'USBC' */
+#define US_BULK_FLAG_IN		(1 << 7)
+#define US_BULK_FLAG_OUT	0
+
+/* command status wrapper */
+struct bulk_cs_wrap {
+	__le32	Signature;	/* contains 'USBS' */
+	__u32	Tag;		/* same as original command */
+	__le32	Residue;	/* amount not transferred */
+	__u8	Status;		/* see below */
+};
+
+#define US_BULK_CS_WRAP_LEN	13
+#define US_BULK_CS_SIGN		0x53425355      /* spells out 'USBS' */
+#define US_BULK_STAT_OK		0
+#define US_BULK_STAT_FAIL	1
+#define US_BULK_STAT_PHASE	2
+
+/* bulk-only class specific requests */
+#define US_BULK_RESET_REQUEST   0xff
+#define US_BULK_GET_MAX_LUN     0xfe
+
+#endif