[PATCH 2/2] usbnet: add rx queue pausing

[Jussi Kivilinna <jussi.kivilinna@mbnet.fi>]

Add rx queue pausing to usbnet. This is needed by rndis_wlan so that it can
control rx queue and prevent received packets from being send forward before
rndis_wlan receives and handles 'media connect'-indication. Without this
establishing WPA connections is hard and fail often.

Cc: David Brownell <dbrownell@users.sourceforge.net>
Signed-off-by: Jussi Kivilinna <jussi.kivilinna@mbnet.fi>
---

 drivers/net/usb/usbnet.c          |   52 ++++++++++++++++++++++++++++++++++++-
 drivers/net/wireless/rndis_wlan.c |   13 +++++++++
 include/linux/usb/usbnet.h        |    6 ++++
 3 files changed, 69 insertions(+), 2 deletions(-)

diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index af1fe46..06cf18a 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -233,6 +233,19 @@ void usbnet_skb_return (struct usbnet *dev, struct sk_buff *skb)
 {
 	int	status;
 
+	if (test_bit(EVENT_RX_PAUSED, &dev->flags)) {
+		struct sk_buff *skb2;
+
+		skb2 = skb_clone(skb, GFP_ATOMIC);
+		if (unlikely(!skb2))
+			return;
+
+		skb_queue_tail(&dev->rxq_pause, skb2);
+		kfree_skb(skb);
+
+		return;
+	}
+
 	skb->protocol = eth_type_trans (skb, dev->net);
 	dev->net->stats.rx_packets++;
 	dev->net->stats.rx_bytes += skb->len;
@@ -526,6 +539,41 @@ static void intr_complete (struct urb *urb)
 }
 
 /*-------------------------------------------------------------------------*/
+void usbnet_pause_rx(struct usbnet *dev)
+{
+	set_bit(EVENT_RX_PAUSED, &dev->flags);
+
+	if (netif_msg_rx_status(dev))
+		devdbg(dev, "paused rx queue enabled");
+}
+EXPORT_SYMBOL_GPL(usbnet_pause_rx);
+
+void usbnet_resume_rx(struct usbnet *dev)
+{
+	struct sk_buff *skb;
+	int num = 0;
+
+	clear_bit(EVENT_RX_PAUSED, &dev->flags);
+
+	while ((skb = skb_dequeue(&dev->rxq_pause)) != NULL) {
+		usbnet_skb_return(dev, skb);
+		num++;
+	}
+
+	tasklet_schedule(&dev->bh);
+
+	if (netif_msg_rx_status(dev))
+		devdbg(dev, "paused rx queue disabled, %d skbs requeued", num);
+}
+EXPORT_SYMBOL_GPL(usbnet_resume_rx);
+
+void usbnet_purge_paused_rxq(struct usbnet *dev)
+{
+	skb_queue_purge(&dev->rxq_pause);
+}
+EXPORT_SYMBOL_GPL(usbnet_purge_paused_rxq);
+
+/*-------------------------------------------------------------------------*/
 
 // unlink pending rx/tx; completion handlers do all other cleanup
 
@@ -623,6 +671,8 @@ int usbnet_stop (struct net_device *net)
 
 	usb_kill_urb(dev->interrupt);
 
+	usbnet_purge_paused_rxq(dev);
+
 	/* deferred work (task, timer, softirq) must also stop.
 	 * can't flush_scheduled_work() until we drop rtnl (later),
 	 * else workers could deadlock; so make workers a NOP.
@@ -1113,7 +1163,6 @@ static void usbnet_bh (unsigned long param)
 }
 
 
-\f
 /*-------------------------------------------------------------------------
  *
  * USB Device Driver support
@@ -1210,6 +1259,7 @@ usbnet_probe (struct usb_interface *udev, const struct usb_device_id *prod)
 	skb_queue_head_init (&dev->rxq);
 	skb_queue_head_init (&dev->txq);
 	skb_queue_head_init (&dev->done);
+	skb_queue_head_init(&dev->rxq_pause);
 	dev->bh.func = usbnet_bh;
 	dev->bh.data = (unsigned long) dev;
 	INIT_WORK (&dev->kevent, kevent);
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 828dc18..d42692d 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -1764,8 +1764,15 @@ static int rndis_iw_set_essid(struct net_device *dev,
 
 	if (!wrqu->essid.flags || length == 0)
 		return disassociate(usbdev, 1);
-	else
+	else {
+		/* Pause and purge rx queue, so we don't pass packets before
+		 * 'media connect'-indication.
+		 */
+		usbnet_pause_rx(usbdev);
+		usbnet_purge_paused_rxq(usbdev);
+
 		return set_essid(usbdev, &ssid);
+	}
 }
 
 
@@ -2328,6 +2335,8 @@ get_bssid:
 			memcpy(evt.ap_addr.sa_data, bssid, ETH_ALEN);
 			wireless_send_event(usbdev->net, SIOCGIWAP, &evt, NULL);
 		}
+
+		usbnet_resume_rx(usbdev);
 	}
 
 	if (test_and_clear_bit(WORK_LINK_DOWN, &priv->work_pending)) {
@@ -2541,6 +2550,8 @@ static void rndis_wlan_indication(struct usbnet *usbdev, void *ind, int buflen)
 
 	switch (msg->status) {
 	case RNDIS_STATUS_MEDIA_CONNECT:
+		usbnet_pause_rx(usbdev);
+
 		devinfo(usbdev, "media connect");
 
 		/* queue work to avoid recursive calls into rndis_command */
diff --git a/include/linux/usb/usbnet.h b/include/linux/usb/usbnet.h
index de8b4b1..0951425 100644
--- a/include/linux/usb/usbnet.h
+++ b/include/linux/usb/usbnet.h
@@ -53,6 +53,7 @@ struct usbnet {
 	struct sk_buff_head	rxq;
 	struct sk_buff_head	txq;
 	struct sk_buff_head	done;
+	struct sk_buff_head	rxq_pause;
 	struct urb		*interrupt;
 	struct tasklet_struct	bh;
 
@@ -63,6 +64,7 @@ struct usbnet {
 #		define EVENT_RX_MEMORY	2
 #		define EVENT_STS_SPLIT	3
 #		define EVENT_LINK_RESET	4
+#		define EVENT_RX_PAUSED	5
 };
 
 static inline struct usb_driver *driver_of(struct usb_interface *intf)
@@ -190,6 +192,10 @@ extern void usbnet_defer_kevent (struct usbnet *, int);
 extern void usbnet_skb_return (struct usbnet *, struct sk_buff *);
 extern void usbnet_unlink_rx_urbs(struct usbnet *);
 
+extern void usbnet_pause_rx(struct usbnet *);
+extern void usbnet_resume_rx(struct usbnet *);
+extern void usbnet_purge_paused_rxq(struct usbnet *);
+
 extern int usbnet_get_settings (struct net_device *net, struct ethtool_cmd *cmd);
 extern int usbnet_set_settings (struct net_device *net, struct ethtool_cmd *cmd);
 extern u32 usbnet_get_link (struct net_device *net);