[PATCH 2/4] usb: f_mass_storage: Check bh->state in sleep_thread

[Sean Anderson <sean.anderson@seco.com>]

Every caller of sleep_thread except one wraps it in a second loop which
waits for a buffer head to be filled. Since sleep_thread is already
(infinitely) looping, we can move this check down. Some parts of the code
have been reorganized to better take advantage of this property and and be
structured closer to their Linux counterparts.

In addition, sleep_thread now returns -EINTR if we need to wake up,
mirroring Linux. This takes advantage of the existing logic regarding
sleep_thread, which typically returns immediately on error. With this
change, any exception causes the current operation to be backed out of
immediately.

Lastly, we only clear thread_wakeup_needed in handle_exception, mirroring
Linux's signal-handling logic. Any subsequent calls to sleep_thread will
still wake up.

While these changes are conceptually different, their implementation is
interdependent, so they are all performed at once.

These changes are primarily inspired by Linux commit 225785aec726 ("USB:
f_mass_storage: improve memory barriers and synchronization").

Signed-off-by: Sean Anderson <sean.anderson@seco.com>
---

 drivers/usb/gadget/f_mass_storage.c | 225 ++++++++++++----------------
 drivers/usb/gadget/storage_common.c |   2 +-
 2 files changed, 100 insertions(+), 127 deletions(-)

diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index 45f0504b6e..99935fe9a3 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 head is
+	 * full.
+	 */
+	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,64 @@ 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;
+		/* Did something go wrong with the transfer? */
+		if (bh->outreq->status != 0) {
+			curlun->sense_data = SS_COMMUNICATION_FAILURE;
+			curlun->info_valid = 1;
+			break;
+		}
 
-			VLDBG(curlun, "file write %u @ %llu -> %d\n", amount,
-					(unsigned long long) file_offset,
-					(int) nwritten);
+		amount = bh->outreq->actual;
 
-			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;
-			}
+		/* 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;
 
-			/* Did the host decide to stop early? */
-			if (bh->outreq->actual != bh->outreq->length) {
-				common->short_packet_received = 1;
-				break;
-			}
-			continue;
+		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 +1426,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 +1454,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 +1471,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 +1601,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 +1776,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 +2102,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 +2119,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 +2271,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 +2281,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 +2375,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 {
-- 
2.25.1