--- f8e70223af4fae6bca555caf1a66b0b524bced27
+++ 40abba500d79b509d1b6721e95ddb8707009317a
@@ -28,7 +28,6 @@
 
 #include "u_serial.h"
 
-
 /*
  * This component encapsulates the TTY layer glue needed to provide basic
  * "serial port" functionality through the USB gadget stack.  Each such
@@ -77,9 +76,12 @@
  * next layer of buffering.  For TX that's a circular buffer; for RX
  * consider it a NOP.  A third layer is provided by the TTY code.
  */
-#define QUEUE_SIZE		16
+#define QUEUE_SIZE		32
+#define RX_QUEUE_SIZE	96
 #define WRITE_BUF_SIZE		8192		/* TX only */
 
+bool MODEM_DEBUG_ON;
+
 static struct workqueue_struct *gs_tty_wq;
 
 /* circular buffer */
@@ -496,7 +498,9 @@ static void gs_rx_push(struct work_struc
 
 	/* hand any queued data to the tty */
 	spin_lock_irq(&port->port_lock);
+
 	tty = port->port_tty;
+
 	while (!list_empty(queue)) {
 		struct usb_request	*req;
 
@@ -506,26 +510,6 @@ static void gs_rx_push(struct work_struc
 		if (!tty)
 			goto recycle;
 
-		/* leave data queued if tty was rx throttled */
-		if (test_bit(TTY_THROTTLED, &tty->flags))
-			break;
-
-		switch (req->status) {
-		case -ESHUTDOWN:
-			disconnect = true;
-			pr_vdebug(PREFIX "%d: shutdown\n", port->port_num);
-			break;
-
-		default:
-			/* presumably a transient fault */
-			pr_warning(PREFIX "%d: unexpected RX status %d\n",
-					port->port_num, req->status);
-			/* FALLTHROUGH */
-		case 0:
-			/* normal completion */
-			break;
-		}
-
 		/* push data to (open) tty */
 		if (req->actual) {
 			char		*packet = req->buf;
@@ -539,18 +523,23 @@ static void gs_rx_push(struct work_struc
 				packet += n;
 				size -= n;
 			}
-			pr_vdebug(PREFIX "%d: tty_insert %d\n", port->port_num,
-					size);
+
+			if (MODEM_DEBUG_ON)
+				printk("%d: tty_insert %d\n", port->port_num, size);
+
 			count = tty_insert_flip_string(tty, packet, size);
 			if (count)
 				do_push = true;
 			if (count != size) {
 				/* stop pushing; TTY layer can't handle more */
 				port->n_read += count;
-				pr_vdebug(PREFIX "%d: rx block %d/%d\n",
-						port->port_num,
-						count, req->actual);
-				break;
+				if (MODEM_DEBUG_ON) {
+					printk("%d: rx block %d/%d\n",
+							port->port_num,
+							count, req->actual);
+					list_move(&req->list, &port->read_pool);
+					break;
+				}
 			}
 			port->n_read = 0;
 		}
@@ -561,7 +550,7 @@ recycle:
 	/* Push from tty to ldisc; this is immediate with low_latency, and
 	 * may trigger callbacks to this driver ... so drop the spinlock.
 	 */
-	if (tty && do_push) {
+	if ((tty && do_push)) {
 		spin_unlock_irq(&port->port_lock);
 		tty_flip_buffer_push(tty);
 		wake_up_interruptible(&tty->read_wait);
@@ -599,22 +588,61 @@ recycle:
 
 static void gs_read_complete(struct usb_ep *ep, struct usb_request *req)
 {
-	struct gs_port	*port = ep->driver_data;
-	unsigned long flags;
-	pr_vdebug("%s: %d bytes\n", __func__, req->actual);
+	/* used global variable */
+	struct gs_port *port = ep->driver_data;
+	struct tty_struct *tty;
 
-	/* Queue all received data until the tty layer is ready for it. */
-	spin_lock_irqsave(&port->port_lock, flags);
-	list_add_tail(&req->list, &port->read_queue);
-	queue_work(gs_tty_wq, &port->push_work);
-	spin_unlock_irqrestore(&port->port_lock, flags);
+	tty = port->port_tty;
+	if (tty == NULL) {
+		printk(KERN_ERR "port_tty NULL\n");
+		return;
+	}
+
+	if (MODEM_DEBUG_ON)
+		printk("%s: %d bytes\n", __func__, req->actual);
+
+	switch (req->status) {
+	case 0:
+		spin_lock(&port->port_lock);
+		if (!test_bit(TTY_THROTTLED, &tty->flags)) {
+			list_add_tail(&req->list, &port->read_queue);
+			queue_work(gs_tty_wq, &port->push_work);
+		} else {
+			printk("%s: TTY_THROTTLED\n", __func__);
+			list_add_tail(&req->list, &port->read_queue);
+		}
+		spin_unlock(&port->port_lock);
+		break;
+	case -ESHUTDOWN:
+		/* disconnect */
+		printk("%s: ESHUTDOWN\n", __func__);
+		break;
+	case -ENODEV:
+		spin_lock(&port->port_lock);
+		printk("%s: ENODEV\n", __func__);
+		list_add_tail(&req->list, &port->read_pool);
+		/* Implemented handling in future if needed */
+		spin_unlock(&port->port_lock);
+		break;
+	default:
+		spin_lock(&port->port_lock);
+		list_add_tail(&req->list, &port->read_pool);
+		printk(KERN_ERR
+		"gs_read_complete: unexpected status error, status=%d\n",
+			req->status);
+		spin_unlock(&port->port_lock);
+		/* goto requeue; */
+		break;
+	}
 }
 
 static void gs_write_complete(struct usb_ep *ep, struct usb_request *req)
 {
 	struct gs_port	*port = ep->driver_data;
 	unsigned long flags;
-	pr_vdebug("%s: %d bytes\n", __func__, req->actual);
+
+	if (MODEM_DEBUG_ON)
+		printk("%s: %d bytes\n", __func__, req->actual);
 
 	spin_lock_irqsave(&port->port_lock, flags);
 	list_add(&req->list, &port->write_pool);
@@ -652,7 +680,7 @@ static void gs_free_requests(struct usb_
 }
 
 static int gs_alloc_requests(struct usb_ep *ep, struct list_head *head,
-		void (*fn)(struct usb_ep *, struct usb_request *))
+		void (*fn)(struct usb_ep *, struct usb_request *), int size)
 {
 	int			i;
 	struct usb_request	*req;
@@ -661,7 +689,7 @@ static int gs_alloc_requests(struct usb_
 	 * do quite that many this time, don't fail ... we just won't
 	 * be as speedy as we might otherwise be.
 	 */
-	for (i = 0; i < QUEUE_SIZE; i++) {
+	for (i = 0; i < size; i++) {
 		req = gs_alloc_req(ep, ep->maxpacket, GFP_ATOMIC);
 		if (!req)
 			return list_empty(head) ? -ENOMEM : 0;
@@ -693,12 +721,12 @@ static int gs_start_io(struct gs_port *p
 	 * configurations may use different endpoints with a given port;
 	 * and high speed vs full speed changes packet sizes too.
 	 */
-	status = gs_alloc_requests(ep, head, gs_read_complete);
+	status = gs_alloc_requests(ep, head, gs_read_complete, RX_QUEUE_SIZE);
 	if (status)
 		return status;
 
 	status = gs_alloc_requests(port->port_usb->in, &port->write_pool,
-			gs_write_complete);
+			gs_write_complete, QUEUE_SIZE);
 	if (status) {
 		gs_free_requests(ep, head);
 		return status;
@@ -1007,7 +1035,8 @@ static void gs_unthrottle(struct tty_str
 		 * read queue backs up enough we'll be NAKing OUT packets.
 		 */
 		queue_work(gs_tty_wq, &port->push_work);
-		pr_vdebug(PREFIX "%d: unthrottle\n", port->port_num);
+		if (MODEM_DEBUG_ON)
+			printk("%d: unthrottle\n", port->port_num);
 	}
 	spin_unlock_irqrestore(&port->port_lock, flags);
 }
@@ -1174,6 +1203,9 @@ int gserial_setup(struct usb_gadget *g,
 	if (count == 0 || count > N_PORTS)
 		return -EINVAL;
 
+	if (gs_tty_driver)
+		return -EBUSY;
+
 	gs_tty_wq = create_singlethread_workqueue("gs_tty");
 	if (gs_tty_wq == 0)
 		return -ENOMEM;
@@ -1418,9 +1450,9 @@ void gserial_disconnect(struct gserial *
 
 	/* REVISIT as above: how best to track this? */
 	port->port_line_coding = gser->port_line_coding;
-#if 0
 	port->port_usb = NULL;
 	gser->ioport = NULL;
+#if 0
 	if (port->open_count > 0 || port->openclose) {
 		wake_up_interruptible(&port->drain_wait);
 		if (port->port_tty)
@@ -1447,3 +1479,14 @@ void gserial_disconnect(struct gserial *
 	gs_free_requests(gser->in, &port->write_pool);
 	spin_unlock_irqrestore(&port->port_lock, flags);
 }
+
+static int modem_debug_enabled(const char *val, struct kernel_param *kp)
+{
+	int enabled = simple_strtol(val, NULL, 0);
+
+	MODEM_DEBUG_ON = (enabled) ? true : false;
+	return 0;
+}
+
+module_param_call(modem_debug, modem_debug_enabled, NULL, NULL, 0664);
+