Merge "usb: gadget: gsi: Improve notify_req handling"

2 files changed, 155 insertions, 80 deletions

diff --git a/drivers/usb/gadget/function/f_gsi.c b/drivers/usb/gadget/function/f_gsi.c
index d8f0f98d00ef..8c80a8d80ac9 100644
--- a/drivers/usb/gadget/function/f_gsi.c
+++ b/drivers/usb/gadget/function/f_gsi.c
@@ -34,14 +34,19 @@ module_param(num_out_bufs, uint, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(num_out_bufs,
 		"Number of OUT buffers");
 
+static bool qti_packet_debug;
+module_param(qti_packet_debug, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(qti_packet_debug, "Print QTI Packet's Raw Data");
+
 static struct workqueue_struct *ipa_usb_wq;
 
 static void gsi_rndis_ipa_reset_trigger(struct f_gsi *rndis);
 static void ipa_disconnect_handler(struct gsi_data_port *d_port);
-static int gsi_ctrl_send_notification(struct f_gsi *gsi,
-		enum gsi_ctrl_notify_state);
+static int gsi_ctrl_send_notification(struct f_gsi *gsi);
 static int gsi_alloc_trb_buffer(struct f_gsi *gsi);
 static void gsi_free_trb_buffer(struct f_gsi *gsi);
+static struct gsi_ctrl_pkt *gsi_ctrl_pkt_alloc(unsigned len, gfp_t flags);
+static void gsi_ctrl_pkt_free(struct gsi_ctrl_pkt *pkt);
 
 void post_event(struct gsi_data_port *port, u8 event)
 {
@@ -173,6 +178,7 @@ int ipa_usb_notify_cb(enum ipa_usb_notify_event event,
 {
 	struct f_gsi *gsi = driver_data;
 	unsigned long flags;
+	struct gsi_ctrl_pkt *cpkt_notify_connect, *cpkt_notify_speed;
 
 	if (!gsi) {
 		log_event_err("%s: invalid driver data", __func__);
@@ -185,17 +191,43 @@ int ipa_usb_notify_cb(enum ipa_usb_notify_event event,
 	case IPA_USB_DEVICE_READY:
 
 		if (gsi->d_port.net_ready_trigger) {
-			log_event_err("%s: Already triggered", __func__);
 			spin_unlock_irqrestore(&gsi->d_port.lock, flags);
+			log_event_dbg("%s: Already triggered", __func__);
 			return 1;
 		}
 
 		log_event_err("%s: Set net_ready_trigger", __func__);
 		gsi->d_port.net_ready_trigger = true;
 
-		if (gsi->prot_id == IPA_USB_ECM)
-			gsi_ctrl_send_notification(gsi,
-					GSI_CTRL_NOTIFY_CONNECT);
+		if (gsi->prot_id == IPA_USB_ECM) {
+			cpkt_notify_connect = gsi_ctrl_pkt_alloc(0, GFP_ATOMIC);
+			if (IS_ERR(cpkt_notify_connect)) {
+				spin_unlock_irqrestore(&gsi->d_port.lock,
+								flags);
+				log_event_dbg("%s: err cpkt_notify_connect\n",
+								__func__);
+				return -ENOMEM;
+			}
+			cpkt_notify_connect->type = GSI_CTRL_NOTIFY_CONNECT;
+
+			cpkt_notify_speed = gsi_ctrl_pkt_alloc(0, GFP_ATOMIC);
+			if (IS_ERR(cpkt_notify_speed)) {
+				spin_unlock_irqrestore(&gsi->d_port.lock,
+								flags);
+				gsi_ctrl_pkt_free(cpkt_notify_connect);
+				log_event_dbg("%s: err cpkt_notify_speed\n",
+								__func__);
+				return -ENOMEM;
+			}
+			cpkt_notify_speed->type = GSI_CTRL_NOTIFY_SPEED;
+			spin_lock_irqsave(&gsi->c_port.lock, flags);
+			list_add_tail(&cpkt_notify_connect->list,
+					&gsi->c_port.cpkt_resp_q);
+			list_add_tail(&cpkt_notify_speed->list,
+					&gsi->c_port.cpkt_resp_q);
+			spin_unlock_irqrestore(&gsi->c_port.lock, flags);
+			gsi_ctrl_send_notification(gsi);
+		}
 
 		/* Do not post EVT_CONNECTED for RNDIS.
 		   Data path for RNDIS is enabled on EVT_HOST_READY.
@@ -969,6 +1001,9 @@ gsi_ctrl_dev_read(struct file *fp, char __user *buf, size_t count, loff_t *pos)
 	}
 
 	log_event_dbg("%s: cpkt size:%d", __func__, cpkt->len);
+	if (qti_packet_debug)
+		print_hex_dump(KERN_DEBUG, "READ:", DUMP_PREFIX_OFFSET, 16, 1,
+			buf, min_t(int, 30, cpkt->len), false);
 
 	ret = copy_to_user(buf, cpkt->buf, cpkt->len);
 	if (ret) {
@@ -1037,14 +1072,17 @@ static ssize_t gsi_ctrl_dev_write(struct file *fp, const char __user *buf,
 		gsi_ctrl_pkt_free(cpkt);
 		return ret;
 	}
+	cpkt->type = GSI_CTRL_NOTIFY_RESPONSE_AVAILABLE;
 	c_port->copied_from_modem++;
+	if (qti_packet_debug)
+		print_hex_dump(KERN_DEBUG, "WRITE:", DUMP_PREFIX_OFFSET, 16, 1,
+			buf, min_t(int, 30, count), false);
 
 	spin_lock_irqsave(&c_port->lock, flags);
 	list_add_tail(&cpkt->list, &c_port->cpkt_resp_q);
 	spin_unlock_irqrestore(&c_port->lock, flags);
 
-	ret = gsi_ctrl_send_notification(gsi,
-			GSI_CTRL_NOTIFY_RESPONSE_AVAILABLE);
+	ret = gsi_ctrl_send_notification(gsi);
 
 	c_port->modem_to_host++;
 	log_event_dbg("Exit %zu", count);
@@ -1059,8 +1097,10 @@ static long gsi_ctrl_dev_ioctl(struct file *fp, unsigned cmd,
 						struct gsi_ctrl_port,
 						ctrl_device);
 	struct f_gsi *gsi = c_port_to_gsi(c_port);
+	struct gsi_ctrl_pkt *cpkt;
 	struct ep_info info;
 	int val, ret = 0;
+	unsigned long flags;
 
 	if (!c_port) {
 		log_event_err("%s: gsi ctrl port %pK", __func__, c_port);
@@ -1074,8 +1114,17 @@ static long gsi_ctrl_dev_ioctl(struct file *fp, unsigned cmd,
 			goto exit_ioctl;
 		}
 		atomic_set(&c_port->ctrl_online, 0);
-		gsi_ctrl_send_notification(gsi, GSI_CTRL_NOTIFY_OFFLINE);
 		gsi_ctrl_clear_cpkt_queues(gsi, true);
+		cpkt = gsi_ctrl_pkt_alloc(0, GFP_KERNEL);
+		if (IS_ERR(cpkt)) {
+			log_event_err("%s: err allocating cpkt\n", __func__);
+			return -ENOMEM;
+		}
+		cpkt->type = GSI_CTRL_NOTIFY_OFFLINE;
+		spin_lock_irqsave(&c_port->lock, flags);
+		list_add_tail(&cpkt->list, &c_port->cpkt_resp_q);
+		spin_unlock_irqrestore(&c_port->lock, flags);
+		gsi_ctrl_send_notification(gsi);
 		break;
 	case QTI_CTRL_MODEM_ONLINE:
 		if (gsi->prot_id == IPA_USB_DIAG) {
@@ -1346,13 +1395,13 @@ static int queue_notification_request(struct f_gsi *gsi)
 			   gsi->c_port.notify_req, GFP_ATOMIC);
 	if (ret == -ENOTSUPP || (ret < 0 && ret != -EAGAIN)) {
 		spin_lock_irqsave(&gsi->c_port.lock, flags);
+		gsi->c_port.notify_req_queued = false;
 		/* check if device disconnected while we dropped lock */
 		if (atomic_read(&gsi->connected) &&
 			!list_empty(&gsi->c_port.cpkt_resp_q)) {
 			cpkt = list_first_entry(&gsi->c_port.cpkt_resp_q,
 					struct gsi_ctrl_pkt, list);
 			list_del(&cpkt->list);
-			atomic_dec(&gsi->c_port.notify_count);
 			log_event_err("%s: drop ctrl pkt of len %d error %d",
 						__func__, cpkt->len, ret);
 			gsi_ctrl_pkt_free(cpkt);
@@ -1368,36 +1417,53 @@ static int queue_notification_request(struct f_gsi *gsi)
 
 	return ret;
 }
-static int gsi_ctrl_send_notification(struct f_gsi *gsi,
-		enum gsi_ctrl_notify_state state)
+
+static int gsi_ctrl_send_notification(struct f_gsi *gsi)
 {
 	__le32 *data;
 	struct usb_cdc_notification *event;
 	struct usb_request *req = gsi->c_port.notify_req;
 	struct usb_composite_dev *cdev = gsi->function.config->cdev;
+	struct gsi_ctrl_pkt *cpkt;
+	unsigned long flags;
+	bool del_free_cpkt = false;
 
 	if (!atomic_read(&gsi->connected)) {
 		log_event_dbg("%s: cable disconnect", __func__);
 		return -ENODEV;
 	}
 
-	event = req->buf;
+	spin_lock_irqsave(&gsi->c_port.lock, flags);
+	if (list_empty(&gsi->c_port.cpkt_resp_q)) {
+		spin_unlock_irqrestore(&gsi->c_port.lock, flags);
+		log_event_dbg("%s: cpkt_resp_q is empty\n", __func__);
+		return 0;
+	}
 
-	switch (state) {
-	case GSI_CTRL_NOTIFY_NONE:
-		if (atomic_read(&gsi->c_port.notify_count) > 0)
-			log_event_dbg("GSI_CTRL_NOTIFY_NONE %d",
-			atomic_read(&gsi->c_port.notify_count));
-		else
-			log_event_dbg("No pending notifications");
+	log_event_dbg("%s: notify_req_queued:%d\n",
+		__func__, gsi->c_port.notify_req_queued);
+
+	if (gsi->c_port.notify_req_queued) {
+		spin_unlock_irqrestore(&gsi->c_port.lock, flags);
+		log_event_dbg("%s: notify_req is already queued.\n", __func__);
 		return 0;
+	}
+
+	cpkt = list_first_entry(&gsi->c_port.cpkt_resp_q,
+			struct gsi_ctrl_pkt, list);
+	log_event_dbg("%s: cpkt->type:%d\n", __func__, cpkt->type);
+
+	event = req->buf;
+
+	switch (cpkt->type) {
 	case GSI_CTRL_NOTIFY_CONNECT:
+		del_free_cpkt = true;
 		event->bNotificationType = USB_CDC_NOTIFY_NETWORK_CONNECTION;
 		event->wValue = cpu_to_le16(1);
 		event->wLength = cpu_to_le16(0);
-		gsi->c_port.notify_state = GSI_CTRL_NOTIFY_SPEED;
 		break;
 	case GSI_CTRL_NOTIFY_SPEED:
+		del_free_cpkt = true;
 		event->bNotificationType = USB_CDC_NOTIFY_SPEED_CHANGE;
 		event->wValue = cpu_to_le16(0);
 		event->wLength = cpu_to_le16(8);
@@ -1409,39 +1475,57 @@ static int gsi_ctrl_send_notification(struct f_gsi *gsi,
 
 		log_event_dbg("notify speed %d",
 				gsi_xfer_bitrate(cdev->gadget));
-		gsi->c_port.notify_state = GSI_CTRL_NOTIFY_NONE;
 		break;
 	case GSI_CTRL_NOTIFY_OFFLINE:
+		del_free_cpkt = true;
 		event->bNotificationType = USB_CDC_NOTIFY_NETWORK_CONNECTION;
 		event->wValue = cpu_to_le16(0);
 		event->wLength = cpu_to_le16(0);
-		gsi->c_port.notify_state = GSI_CTRL_NOTIFY_NONE;
 		break;
 	case GSI_CTRL_NOTIFY_RESPONSE_AVAILABLE:
 		event->bNotificationType = USB_CDC_NOTIFY_RESPONSE_AVAILABLE;
 		event->wValue = cpu_to_le16(0);
 		event->wLength = cpu_to_le16(0);
-		gsi->c_port.notify_state = GSI_CTRL_NOTIFY_RESPONSE_AVAILABLE;
 
 		if (gsi->prot_id == IPA_USB_RNDIS) {
 			data = req->buf;
 			data[0] = cpu_to_le32(1);
 			data[1] = cpu_to_le32(0);
+			/*
+			 * we need to free dummy packet for RNDIS as sending
+			 * notification about response available multiple time,
+			 * RNDIS host driver doesn't like. All SEND/GET
+			 * ENCAPSULATED response is one-to-one for RNDIS case
+			 * and host expects to have below sequence:
+			 * ep0: USB_CDC_SEND_ENCAPSULATED_COMMAND
+			 * int_ep: device->host: RESPONSE_AVAILABLE
+			 * ep0: USB_GET_SEND_ENCAPSULATED_COMMAND
+			 * For RMNET case: host ignores multiple notification.
+			 */
+			del_free_cpkt = true;
 		}
 		break;
 	default:
+		spin_unlock_irqrestore(&gsi->c_port.lock, flags);
 		log_event_err("%s:unknown notify state", __func__);
+		WARN_ON(1);
 		return -EINVAL;
 	}
 
-	log_event_dbg("send Notify type %02x", event->bNotificationType);
-
-	if (atomic_inc_return(&gsi->c_port.notify_count) != 1) {
-		log_event_dbg("delay ep_queue: notify req is busy %d",
-			atomic_read(&gsi->c_port.notify_count));
-		return 0;
+	/*
+	 * Delete and free cpkt related to non NOTIFY_RESPONSE_AVAILABLE
+	 * notification whereas NOTIFY_RESPONSE_AVAILABLE related cpkt is
+	 * deleted from USB_CDC_GET_ENCAPSULATED_RESPONSE setup request
+	 */
+	if (del_free_cpkt) {
+		list_del(&cpkt->list);
+		gsi_ctrl_pkt_free(cpkt);
 	}
 
+	gsi->c_port.notify_req_queued = true;
+	spin_unlock_irqrestore(&gsi->c_port.lock, flags);
+	log_event_dbg("send Notify type %02x", event->bNotificationType);
+
 	return queue_notification_request(gsi);
 }
 
@@ -1451,13 +1535,16 @@ static void gsi_ctrl_notify_resp_complete(struct usb_ep *ep,
 	struct f_gsi *gsi = req->context;
 	struct usb_cdc_notification *event = req->buf;
 	int status = req->status;
+	unsigned long flags;
+
+	spin_lock_irqsave(&gsi->c_port.lock, flags);
+	gsi->c_port.notify_req_queued = false;
+	spin_unlock_irqrestore(&gsi->c_port.lock, flags);
 
 	switch (status) {
 	case -ECONNRESET:
 	case -ESHUTDOWN:
 		/* connection gone */
-		gsi->c_port.notify_state = GSI_CTRL_NOTIFY_NONE;
-		atomic_set(&gsi->c_port.notify_count, 0);
 		log_event_dbg("ESHUTDOWN/ECONNRESET, connection gone");
 		gsi_ctrl_clear_cpkt_queues(gsi, false);
 		gsi_ctrl_send_cpkt_tomodem(gsi, NULL, 0);
@@ -1467,30 +1554,13 @@ static void gsi_ctrl_notify_resp_complete(struct usb_ep *ep,
 			event->bNotificationType, req->status);
 		/* FALLTHROUGH */
 	case 0:
-		/* no need to handle multiple resp available for RNDIS */
-		if (gsi->prot_id == IPA_USB_RNDIS) {
-			atomic_set(&gsi->c_port.notify_count, 0);
-			log_event_dbg("notify_count = %d",
-			atomic_read(&gsi->c_port.notify_count));
-			break;
-		}
-
 		 /*
 		  * handle multiple pending resp available
 		  * notifications by queuing same until we're done,
 		  * rest of the notification require queuing new
 		  * request.
 		  */
-		if (!atomic_dec_and_test(&gsi->c_port.notify_count)) {
-			log_event_dbg("notify_count = %d",
-			atomic_read(&gsi->c_port.notify_count));
-			 queue_notification_request(gsi);
-		} else if (gsi->c_port.notify_state != GSI_CTRL_NOTIFY_NONE &&
-				gsi->c_port.notify_state !=
-				GSI_CTRL_NOTIFY_RESPONSE_AVAILABLE) {
-			gsi_ctrl_send_notification(gsi,
-					gsi->c_port.notify_state);
-		}
+		gsi_ctrl_send_notification(gsi);
 		break;
 	}
 }
@@ -1498,8 +1568,20 @@ static void gsi_ctrl_notify_resp_complete(struct usb_ep *ep,
 static void gsi_rndis_response_available(void *_rndis)
 {
 	struct f_gsi *gsi = _rndis;
+	struct gsi_ctrl_pkt *cpkt;
+	unsigned long flags;
 
-	gsi_ctrl_send_notification(gsi, GSI_CTRL_NOTIFY_RESPONSE_AVAILABLE);
+	cpkt = gsi_ctrl_pkt_alloc(0, GFP_ATOMIC);
+	if (IS_ERR(cpkt)) {
+		log_event_err("%s: err allocating cpkt\n", __func__);
+		return;
+	}
+
+	cpkt->type = GSI_CTRL_NOTIFY_RESPONSE_AVAILABLE;
+	spin_lock_irqsave(&gsi->c_port.lock, flags);
+	list_add_tail(&cpkt->list, &gsi->c_port.cpkt_resp_q);
+	spin_unlock_irqrestore(&gsi->c_port.lock, flags);
+	gsi_ctrl_send_notification(gsi);
 }
 
 static void gsi_rndis_command_complete(struct usb_ep *ep,
@@ -1652,6 +1734,7 @@ gsi_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
 		cpkt = list_first_entry(&gsi->c_port.cpkt_resp_q,
 					struct gsi_ctrl_pkt, list);
 		list_del(&cpkt->list);
+		gsi->c_port.get_encap_cnt++;
 		spin_unlock(&gsi->c_port.lock);
 
 		value = min_t(unsigned, w_length, cpkt->len);
@@ -2019,15 +2102,12 @@ static void gsi_disable(struct usb_function *f)
 		gsi->c_port.notify->driver_data) {
 		usb_ep_disable(gsi->c_port.notify);
 		gsi->c_port.notify->driver_data = NULL;
-		gsi->c_port.notify_state = GSI_CTRL_NOTIFY_NONE;
 	}
 
-	atomic_set(&gsi->c_port.notify_count, 0);
-
 	gsi_ctrl_clear_cpkt_queues(gsi, false);
 	/* send 0 len pkt to qti/qbi to notify state change */
 	gsi_ctrl_send_cpkt_tomodem(gsi, NULL, 0);
-
+	gsi->c_port.notify_req_queued = false;
 	/* Disable Data Path  - only if it was initialized already (alt=1) */
 	if (!gsi->data_interface_up) {
 		log_event_dbg("%s: data intf is closed", __func__);
@@ -2114,6 +2194,12 @@ static void gsi_resume(struct usb_function *f)
 	else
 		remote_wakeup_allowed = f->config->cdev->gadget->remote_wakeup;
 
+	if (gsi->c_port.notify && !gsi->c_port.notify->desc)
+		config_ep_by_speed(cdev->gadget, f, gsi->c_port.notify);
+
+	/* Check any pending cpkt, and queue immediately on resume */
+	gsi_ctrl_send_notification(gsi);
+
 	if (!remote_wakeup_allowed) {
 
 		/* Configure EPs for GSI */
@@ -2145,10 +2231,7 @@ static void gsi_resume(struct usb_function *f)
 
 	queue_work(gsi->d_port.ipa_usb_wq, &gsi->d_port.usb_ipa_w);
 
-	if (gsi->c_port.notify && !gsi->c_port.notify->desc)
-		config_ep_by_speed(cdev->gadget, f, gsi->c_port.notify);
 
-	atomic_set(&gsi->c_port.notify_count, 0);
 	log_event_dbg("%s: completed", __func__);
 }
 
@@ -2274,8 +2357,6 @@ skip_string_id_alloc:
 		gsi->c_port.notify = ep;
 		ep->driver_data = cdev;	/* claim */
 
-		atomic_set(&gsi->c_port.notify_count, 0);
-
 		/* allocate notification request and buffer */
 		gsi->c_port.notify_req = usb_ep_alloc_request(ep, GFP_KERNEL);
 		if (!gsi->c_port.notify_req)
@@ -2300,7 +2381,6 @@ skip_string_id_alloc:
 			event->wIndex = cpu_to_le16(gsi->ctrl_id);
 
 		event->wLength = cpu_to_le16(0);
-		gsi->c_port.notify_state = GSI_CTRL_NOTIFY_NONE;
 	}
 
 	gsi->d_port.in_request.buf_len = info->in_req_buf_len;
@@ -2837,12 +2917,6 @@ static ssize_t gsi_info_show(struct config_item *item, char *page)
 		len += scnprintf(buf + len, PAGE_SIZE - len,
 		"%25s %10s\n", "Ctrl Name: ", gsi->c_port.name);
 		len += scnprintf(buf + len, PAGE_SIZE - len,
-		"%25s %10u\n", "Notify State: ",
-				gsi->c_port.notify_state);
-		len += scnprintf(buf + len, PAGE_SIZE - len,
-		"%25s %10u\n", "Notify Count: ",
-				gsi->c_port.notify_count.counter);
-		len += scnprintf(buf + len, PAGE_SIZE - len,
 		"%25s %10u\n", "Ctrl Online: ",
 				gsi->c_port.ctrl_online.counter);
 		len += scnprintf(buf + len, PAGE_SIZE - len,
diff --git a/drivers/usb/gadget/function/f_gsi.h b/drivers/usb/gadget/function/f_gsi.h
index f058ab4cedaa..3baf65572afc 100644
--- a/drivers/usb/gadget/function/f_gsi.h
+++ b/drivers/usb/gadget/function/f_gsi.h
@@ -93,6 +93,14 @@ enum connection_state {
 	STATE_SUSPENDED
 };
 
+enum gsi_ctrl_notify_state {
+	GSI_CTRL_NOTIFY_NONE,
+	GSI_CTRL_NOTIFY_CONNECT,
+	GSI_CTRL_NOTIFY_SPEED,
+	GSI_CTRL_NOTIFY_OFFLINE,
+	GSI_CTRL_NOTIFY_RESPONSE_AVAILABLE,
+};
+
 #define MAXQUEUELEN 128
 struct event_queue {
 	u8 event[MAXQUEUELEN];
@@ -107,9 +115,10 @@ struct gsi_ntb_info {
 };
 
 struct gsi_ctrl_pkt {
-	void			*buf;
-	int			len;
-	struct list_head	list;
+	void				*buf;
+	int				len;
+	enum gsi_ctrl_notify_state	type;
+	struct list_head		list;
 };
 
 struct gsi_function_bind_info {
@@ -147,22 +156,13 @@ struct gsi_function_bind_info {
 	u32 notify_buf_len;
 };
 
-enum gsi_ctrl_notify_state {
-	GSI_CTRL_NOTIFY_NONE,
-	GSI_CTRL_NOTIFY_CONNECT,
-	GSI_CTRL_NOTIFY_SPEED,
-	GSI_CTRL_NOTIFY_OFFLINE,
-	GSI_CTRL_NOTIFY_RESPONSE_AVAILABLE,
-};
-
 struct gsi_ctrl_port {
 	char name[GSI_CTRL_NAME_LEN];
 	struct miscdevice ctrl_device;
 
 	struct usb_ep *notify;
 	struct usb_request *notify_req;
-	int notify_state;
-	atomic_t notify_count;
+	bool notify_req_queued;
 
 	atomic_t ctrl_online;
 
@@ -184,6 +184,7 @@ struct gsi_ctrl_port {
 	unsigned copied_from_modem;
 	unsigned modem_to_host;
 	unsigned cpkt_drop_cnt;
+	unsigned get_encap_cnt;
 };
 
 struct gsi_data_port {