diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index f866b7e72a28..4ce899c9b165 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -321,8 +321,8 @@ struct fsg_dev;
 /* Data shared by all the FSG instances. */
 struct fsg_common {
 	struct usb_gadget	*gadget;
-	struct fsg_dev		*fsg;
-	struct fsg_dev		*prev_fsg;
+	struct fsg_dev		*fsg, *new_fsg;
+	wait_queue_head_t	fsg_wait;
 
 	/* filesem protects: backing files in use */
 	struct rw_semaphore	filesem;
@@ -351,7 +351,6 @@ struct fsg_common {
 	enum fsg_state		state;		/* For exception handling */
 	unsigned int		exception_req_tag;
 
-	u8			config, new_config;
 	enum data_direction	data_dir;
 	u32			data_size;
 	u32			data_size_from_cmnd;
@@ -595,7 +594,7 @@ static int fsg_setup(struct usb_function *f,
 	u16			w_value = le16_to_cpu(ctrl->wValue);
 	u16			w_length = le16_to_cpu(ctrl->wLength);
 
-	if (!fsg->common->config)
+	if (!fsg_is_set(fsg->common))
 		return -EOPNOTSUPP;
 
 	switch (ctrl->bRequest) {
@@ -2303,24 +2302,20 @@ static int alloc_request(struct fsg_common *common, struct usb_ep *ep,
 	return -ENOMEM;
 }
 
-/*
- * Reset interface setting and re-init endpoint state (toggle etc).
- * Call with altsetting < 0 to disable the interface.  The only other
- * available altsetting is 0, which enables the interface.
- */
-static int do_set_interface(struct fsg_common *common, int altsetting)
+/* Reset interface setting and re-init endpoint state (toggle etc). */
+static int do_set_interface(struct fsg_common *common, struct fsg_dev *new_fsg)
 {
-	int	rc = 0;
-	int	i;
-	const struct usb_endpoint_descriptor	*d;
+	const struct usb_endpoint_descriptor *d;
+	struct fsg_dev *fsg;
+	int i, rc = 0;
 
 	if (common->running)
 		DBG(common, "reset interface\n");
 
 reset:
 	/* Deallocate the requests */
-	if (common->prev_fsg) {
-		struct fsg_dev *fsg = common->prev_fsg;
+	if (common->fsg) {
+		fsg = common->fsg;
 
 		for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
 			struct fsg_buffhd *bh = &common->buffhds[i];
@@ -2345,88 +2340,53 @@ reset:
 			fsg->bulk_out_enabled = 0;
 		}
 
-		common->prev_fsg = 0;
+		common->fsg = NULL;
+		wake_up(&common->fsg_wait);
 	}
 
 	common->running = 0;
-	if (altsetting < 0 || rc != 0)
+	if (!new_fsg || rc)
 		return rc;
 
-	DBG(common, "set interface %d\n", altsetting);
+	common->fsg = new_fsg;
+	fsg = common->fsg;
 
-	if (fsg_is_set(common)) {
-		struct fsg_dev *fsg = common->fsg;
-		common->prev_fsg = common->fsg;
+	/* Enable the endpoints */
+	d = fsg_ep_desc(common->gadget,
+			&fsg_fs_bulk_in_desc, &fsg_hs_bulk_in_desc);
+	rc = enable_endpoint(common, fsg->bulk_in, d);
+	if (rc)
+		goto reset;
+	fsg->bulk_in_enabled = 1;
 
-		/* Enable the endpoints */
-		d = fsg_ep_desc(common->gadget,
-				&fsg_fs_bulk_in_desc, &fsg_hs_bulk_in_desc);
-		rc = enable_endpoint(common, fsg->bulk_in, d);
+	d = fsg_ep_desc(common->gadget,
+			&fsg_fs_bulk_out_desc, &fsg_hs_bulk_out_desc);
+	rc = enable_endpoint(common, fsg->bulk_out, d);
+	if (rc)
+		goto reset;
+	fsg->bulk_out_enabled = 1;
+	common->bulk_out_maxpacket = le16_to_cpu(d->wMaxPacketSize);
+	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;
-		fsg->bulk_in_enabled = 1;
-
-		d = fsg_ep_desc(common->gadget,
-				&fsg_fs_bulk_out_desc, &fsg_hs_bulk_out_desc);
-		rc = enable_endpoint(common, fsg->bulk_out, d);
+		rc = alloc_request(common, fsg->bulk_out, &bh->outreq);
 		if (rc)
 			goto reset;
-		fsg->bulk_out_enabled = 1;
-		common->bulk_out_maxpacket = le16_to_cpu(d->wMaxPacketSize);
-		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;
-		for (i = 0; i < common->nluns; ++i)
-			common->luns[i].unit_attention_data = SS_RESET_OCCURRED;
-		return rc;
-	} else {
-		return -EIO;
-	}
-}
-
-
-/*
- * Change our operational configuration.  This code must agree with the code
- * that returns config descriptors, and with interface altsetting code.
- *
- * It's also responsible for power management interactions.  Some
- * configurations might not work with our current power sources.
- * For now we just assume the gadget is always self-powered.
- */
-static int do_set_config(struct fsg_common *common, u8 new_config)
-{
-	int	rc = 0;
-
-	/* Disable the single interface */
-	if (common->config != 0) {
-		DBG(common, "reset config\n");
-		common->config = 0;
-		rc = do_set_interface(common, -1);
+		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;
 	}
 
-	/* Enable the interface */
-	if (new_config != 0) {
-		common->config = new_config;
-		rc = do_set_interface(common, 0);
-		if (rc != 0)
-			common->config = 0;	/* Reset on errors */
-	}
+	common->running = 1;
+	for (i = 0; i < common->nluns; ++i)
+		common->luns[i].unit_attention_data = SS_RESET_OCCURRED;
 	return rc;
 }
 
@@ -2437,9 +2397,7 @@ static int do_set_config(struct fsg_common *common, u8 new_config)
 static int fsg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 {
 	struct fsg_dev *fsg = fsg_from_func(f);
-	fsg->common->prev_fsg = fsg->common->fsg;
-	fsg->common->fsg = fsg;
-	fsg->common->new_config = 1;
+	fsg->common->new_fsg = fsg;
 	raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
 	return 0;
 }
@@ -2447,9 +2405,7 @@ static int fsg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 static void fsg_disable(struct usb_function *f)
 {
 	struct fsg_dev *fsg = fsg_from_func(f);
-	fsg->common->prev_fsg = fsg->common->fsg;
-	fsg->common->fsg = fsg;
-	fsg->common->new_config = 0;
+	fsg->common->new_fsg = NULL;
 	raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
 }
 
@@ -2459,19 +2415,17 @@ static void fsg_disable(struct usb_function *f)
 static void handle_exception(struct fsg_common *common)
 {
 	siginfo_t		info;
-	int			sig;
 	int			i;
 	struct fsg_buffhd	*bh;
 	enum fsg_state		old_state;
-	u8			new_config;
 	struct fsg_lun		*curlun;
 	unsigned int		exception_req_tag;
-	int			rc;
 
 	/* Clear the existing signals.  Anything but SIGUSR1 is converted
 	 * into a high-priority EXIT exception. */
 	for (;;) {
-		sig = dequeue_signal_lock(current, &current->blocked, &info);
+		int sig =
+			dequeue_signal_lock(current, &current->blocked, &info);
 		if (!sig)
 			break;
 		if (sig != SIGUSR1) {
@@ -2482,7 +2436,7 @@ static void handle_exception(struct fsg_common *common)
 	}
 
 	/* Cancel all the pending transfers */
-	if (fsg_is_set(common)) {
+	if (likely(common->fsg)) {
 		for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
 			bh = &common->buffhds[i];
 			if (bh->inreq_busy)
@@ -2523,7 +2477,6 @@ static void handle_exception(struct fsg_common *common)
 	common->next_buffhd_to_fill = &common->buffhds[0];
 	common->next_buffhd_to_drain = &common->buffhds[0];
 	exception_req_tag = common->exception_req_tag;
-	new_config = common->new_config;
 	old_state = common->state;
 
 	if (old_state == FSG_STATE_ABORT_BULK_OUT)
@@ -2573,12 +2526,12 @@ static void handle_exception(struct fsg_common *common)
 		break;
 
 	case FSG_STATE_CONFIG_CHANGE:
-		rc = do_set_config(common, new_config);
+		do_set_interface(common, common->new_fsg);
 		break;
 
 	case FSG_STATE_EXIT:
 	case FSG_STATE_TERMINATED:
-		do_set_config(common, 0);		/* Free resources */
+		do_set_interface(common, NULL);		/* Free resources */
 		spin_lock_irq(&common->lock);
 		common->state = FSG_STATE_TERMINATED;	/* Stop the thread */
 		spin_unlock_irq(&common->lock);
@@ -2863,6 +2816,7 @@ buffhds_first_it:
 		goto error_release;
 	}
 	init_completion(&common->thread_notifier);
+	init_waitqueue_head(&common->fsg_wait);
 #undef OR
 
 
@@ -2957,9 +2911,17 @@ static void fsg_common_release(struct kref *ref)
 static void fsg_unbind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct fsg_dev		*fsg = fsg_from_func(f);
+	struct fsg_common	*common = fsg->common;
 
 	DBG(fsg, "unbind\n");
-	fsg_common_put(fsg->common);
+	if (fsg->common->fsg == fsg) {
+		fsg->common->new_fsg = NULL;
+		raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
+		/* FIXME: make interruptible or killable somehow? */
+		wait_event(common->fsg_wait, common->fsg != fsg);
+	}
+
+	fsg_common_put(common);
 	usb_free_descriptors(fsg->function.descriptors);
 	usb_free_descriptors(fsg->function.hs_descriptors);
 	kfree(fsg);