usb: gadget: f_mass_storage: Handle setup request correctly

Signed-off-by: Mike Lockwood <lockwood@android.com>

1 files changed, 39 insertions, 20 deletions

diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index d81ce9ebf842..137e27019d82 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -251,7 +251,6 @@ static struct lun *dev_to_lun(struct device *dev)
 
 /* 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 NUM_BUFFERS	2
@@ -662,7 +661,7 @@ static int fsg_function_setup(struct usb_function *f,
 			 * and reinitialize our state. */
 			DBG(fsg, "bulk reset request\n");
 			raise_exception(fsg, FSG_STATE_RESET);
-			value = DELAYED_STATUS;
+			value = 0;
 			break;
 
 		case USB_BULK_GET_MAX_LUN_REQUEST:
@@ -680,6 +679,16 @@ static int fsg_function_setup(struct usb_function *f,
 		}
 	}
 
+		/* respond with data transfer or status phase? */
+		if (value >= 0) {
+			int rc;
+			cdev->req->zero = value < w_length;
+			cdev->req->length = value;
+			rc = usb_ep_queue(cdev->gadget->ep0, cdev->req, GFP_ATOMIC);
+			if (rc < 0)
+				printk("%s setup response queue error\n", __func__);
+		}
+
 	if (value == -EOPNOTSUPP)
 		VDBG(fsg,
 			"unknown class-specific control req "
@@ -2063,8 +2072,12 @@ static int get_next_command(struct fsg_dev *fsg)
 	bh = fsg->next_buffhd_to_fill;
 	while (bh->state != BUF_STATE_EMPTY) {
 		rc = sleep_thread(fsg);
-		if (rc)
+		if (rc) {
+			usb_ep_dequeue(fsg->bulk_out, bh->outreq);
+			bh->outreq_busy = 0;
+			bh->state = BUF_STATE_EMPTY;
 			return rc;
+		}
 	}
 
 	/* Queue a request to read a Bulk-only CBW */
@@ -2135,16 +2148,16 @@ static int do_set_interface(struct fsg_dev *fsg, int altsetting)
 		DBG(fsg, "reset interface\n");
 reset:
 	 /* Disable the endpoints */
-        if (fsg->bulk_in_enabled) {
-                DBG(fsg, "usb_ep_disable %s\n", fsg->bulk_in->name);
-                usb_ep_disable(fsg->bulk_in);
-                fsg->bulk_in_enabled = 0;
-        }
-        if (fsg->bulk_out_enabled) {
-                DBG(fsg, "usb_ep_disable %s\n", fsg->bulk_out->name);
-                usb_ep_disable(fsg->bulk_out);
-                fsg->bulk_out_enabled = 0;
-        }
+	if (fsg->bulk_in_enabled) {
+		DBG(fsg, "usb_ep_disable %s\n", fsg->bulk_in->name);
+		usb_ep_disable(fsg->bulk_in);
+		fsg->bulk_in_enabled = 0;
+	}
+	if (fsg->bulk_out_enabled) {
+		DBG(fsg, "usb_ep_disable %s\n", fsg->bulk_out->name);
+		usb_ep_disable(fsg->bulk_out);
+		fsg->bulk_out_enabled = 0;
+	}
 
 	/* Deallocate the requests */
 	for (i = 0; i < NUM_BUFFERS; ++i) {
@@ -2281,12 +2294,18 @@ static void handle_exception(struct fsg_dev *fsg)
 		}
 	}
 
-	/* Clear out the controller's fifos */
-	if (fsg->bulk_in_enabled)
-		usb_ep_fifo_flush(fsg->bulk_in);
-	if (fsg->bulk_out_enabled)
-		usb_ep_fifo_flush(fsg->bulk_out);
-
+	/*
+	* Do NOT flush the fifo after set_interface()
+	* Otherwise, it results in some data being lost
+	*/
+	if ((fsg->state != FSG_STATE_CONFIG_CHANGE) ||
+		(fsg->new_config != 1))   {
+		/* Clear out the controller's fifos */
+		if (fsg->bulk_in_enabled)
+			usb_ep_fifo_flush(fsg->bulk_in);
+		if (fsg->bulk_out_enabled)
+			usb_ep_fifo_flush(fsg->bulk_out);
+	}
 	/* Reset the I/O buffer states and pointers, the SCSI
 	 * state, and the exception.  Then invoke the handler. */
 	spin_lock_irq(&fsg->lock);
@@ -2924,7 +2943,7 @@ int __init mass_storage_function_add(struct usb_composite_dev *cdev,
 		goto err_platform_driver_register;
 
 	wake_lock_init(&the_fsg->wake_lock, WAKE_LOCK_SUSPEND,
-		       "usb_mass_storage");
+			   "usb_mass_storage");
 
 	fsg->cdev = cdev;
 	fsg->function.name = shortname;