diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index 45f0504b6e..6f39c24503 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -652,15 +652,17 @@ static void busy_indicator(void)
 		state = 0;
 }
 
-static int sleep_thread(struct fsg_common *common)
+static int sleep_thread(struct fsg_common *common, struct fsg_buffhd *bh)
 {
-	int	rc = 0;
 	int i = 0, k = 0;
 
-	/* Wait until a signal arrives or we are woken up */
-	for (;;) {
-		if (common->thread_wakeup_needed)
-			break;
+	/*
+	 * Wait until a signal arrives, we are woken up, or the buffer is no
+	 * longer busy.
+	 */
+	while (!bh || bh->state == BUF_STATE_BUSY) {
+		if (exception_in_progress(common))
+			return -EINTR;
 
 		if (++i == 20000) {
 			busy_indicator();
@@ -682,8 +684,7 @@ static int sleep_thread(struct fsg_common *common)
 
 		usb_gadget_handle_interrupts(controller_index);
 	}
-	common->thread_wakeup_needed = 0;
-	return rc;
+	return 0;
 }
 
 /*-------------------------------------------------------------------------*/
@@ -744,11 +745,9 @@ static int do_read(struct fsg_common *common)
 
 		/* 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;
-		}
+		rc = sleep_thread(common, bh);
+		if (rc)
+			return rc;
 
 		/* If we were asked to read past the end of file,
 		 * end with an empty buffer. */
@@ -922,67 +921,63 @@ static int do_write(struct fsg_common *common)
 		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;
-			}
+		rc = sleep_thread(common, bh);
+		if (rc)
+			return rc;
 
-			amount = bh->outreq->actual;
+		common->next_buffhd_to_drain = bh->next;
+		bh->state = BUF_STATE_EMPTY;
 
-			/* Perform the write */
-			rc = ums[common->lun].write_sector(&ums[common->lun],
-					       file_offset / SECTOR_SIZE,
-					       amount / SECTOR_SIZE,
-					       (char __user *)bh->buf);
-			if (!rc)
-				return -EIO;
-			nwritten = rc * SECTOR_SIZE;
-
-			VLDBG(curlun, "file write %u @ %llu -> %d\n", amount,
-					(unsigned long long) file_offset,
-					(int) nwritten);
-
-			if (nwritten < 0) {
-				LDBG(curlun, "error in file write: %d\n",
-						(int) 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) {
-				printf("nwritten:%zd amount:%u\n", nwritten,
-				       amount);
-				curlun->sense_data = SS_WRITE_ERROR;
-				curlun->info_valid = 1;
-				break;
-			}
+		/* Did something go wrong with the transfer? */
+		if (bh->outreq->status != 0) {
+			curlun->sense_data = SS_COMMUNICATION_FAILURE;
+			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;
+		amount = bh->outreq->actual;
+
+		/* Perform the write */
+		rc = ums[common->lun].write_sector(&ums[common->lun],
+				       file_offset / SECTOR_SIZE,
+				       amount / SECTOR_SIZE,
+				       (char __user *)bh->buf);
+		if (!rc)
+			return -EIO;
+		nwritten = rc * SECTOR_SIZE;
+
+		VLDBG(curlun, "file write %u @ %llu -> %d\n", amount,
+		      (unsigned long long)file_offset, (int)nwritten);
+
+		if (nwritten < 0) {
+			LDBG(curlun, "error in file write: %d\n",
+			     (int)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;
 
-		/* Wait for something to happen */
-		rc = sleep_thread(common);
-		if (rc)
-			return rc;
+		/* If an error occurred, report it and its position */
+		if (nwritten < amount) {
+			printf("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;
+		}
 	}
 
 	return -EIO;		/* No default reply */
@@ -1430,13 +1425,10 @@ static int pad_with_zeros(struct fsg_dev *fsg)
 	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;
-		}
+		rc = sleep_thread(fsg->common, bh);
+		if (rc)
+			return rc;
 
 		nsend = min(fsg->common->usb_amount_left, FSG_BUFLEN);
 		memset(bh->buf + nkeep, 0, nsend - nkeep);
@@ -1461,23 +1453,7 @@ static int throw_away_data(struct fsg_common *common)
 	     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 -EINTR;
-			}
-			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);
@@ -1494,13 +1470,24 @@ static int throw_away_data(struct fsg_common *common)
 				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);
+		/* Wait for the data to be received */
+		rc = sleep_thread(common, bh);
 		if (rc)
 			return rc;
+
+		/* Throw away the data in a filled buffer */
+		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 -EINTR;
+		}
 	}
 	return 0;
 }
@@ -1613,11 +1600,9 @@ static int send_status(struct fsg_common *common)
 
 	/* 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;
-	}
+	rc = sleep_thread(common, bh);
+	if (rc)
+		return rc;
 
 	if (curlun)
 		sd = curlun->sense_data;
@@ -1790,11 +1775,9 @@ static int do_scsi_command(struct fsg_common *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;
-	}
+	rc = sleep_thread(common, bh);
+	if (rc)
+		return rc;
 	common->phase_error = 0;
 	common->short_packet_received = 0;
 
@@ -2118,11 +2101,9 @@ static int get_next_command(struct fsg_common *common)
 
 	/* 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;
-	}
+	rc = sleep_thread(common, bh);
+	if (rc)
+		return rc;
 
 	/* Queue a request to read a Bulk-only CBW */
 	set_bulk_out_req_length(common, bh, USB_BULK_CB_WRAP_LEN);
@@ -2137,11 +2118,9 @@ static int get_next_command(struct fsg_common *common)
 	 * 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 = sleep_thread(common, bh);
+	if (rc)
+		return rc;
 
 	rc = fsg_is_set(common) ? received_cbw(common->fsg, bh) : -EIO;
 	bh->state = BUF_STATE_EMPTY;
@@ -2291,6 +2270,7 @@ static void handle_exception(struct fsg_common *common)
 	struct fsg_lun		*curlun;
 	unsigned int		exception_req_tag;
 
+	common->thread_wakeup_needed = 0;
 	/* Cancel all the pending transfers */
 	if (common->fsg) {
 		for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
@@ -2300,18 +2280,9 @@ static void handle_exception(struct fsg_common *common)
 			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))
+			/* Wait until the transfer is idle */
+			if (sleep_thread(common, bh))
 				return;
 		}
 
@@ -2403,15 +2374,16 @@ int fsg_main_thread(void *common_)
 		}
 
 		if (!common->running) {
-			ret = sleep_thread(common);
-			if (ret)
+			ret = sleep_thread(common, NULL);
+			if (ret != -EINTR)
 				return ret;
-
 			continue;
 		}
 
 		ret = get_next_command(common);
-		if (ret)
+		if (ret == -EINTR)
+			continue;
+		else if (ret)
 			return ret;
 
 		if (!exception_in_progress(common))
diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
index 5674e8fe49..723549ef09 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -318,7 +318,7 @@ static struct fsg_lun *fsg_lun_from_dev(struct device *dev)
 enum fsg_buffer_state {
 	BUF_STATE_EMPTY = 0,
 	BUF_STATE_FULL,
-	BUF_STATE_BUSY
+	BUF_STATE_BUSY,
 };
 
 struct fsg_buffhd {
