From 3c419c1bb59219bfd05157c96a26d7eb8ee0828f Mon Sep 17 00:00:00 2001
From: Sam Leffler <sam@FreeBSD.org>
Date: Wed, 7 Mar 2007 04:42:22 +0000
Subject: [PATCH] When dispatching frames saved on the power save queue to a
 station exiting power save mode prepend them to the driver's send q instead
 of appending them.  This insures the packets are not misordered wrt any
 packets already q'd for the station.

This corrects a problem noticed when using a VoIP phone talking
to an ath card in ap mode; the misordered packets caused noise.

Submitted by:	"J.R. Oldroyd" <jr@opal.com>
MFC after:	2 weeks
---
 sys/net80211/ieee80211_freebsd.h | 15 +++++++++++++
 sys/net80211/ieee80211_input.c   | 36 ++++++++++++++++++++------------
 2 files changed, 38 insertions(+), 13 deletions(-)

diff --git a/sys/net80211/ieee80211_freebsd.h b/sys/net80211/ieee80211_freebsd.h
index 48ffd6e8310f..9880b692db0f 100644
--- a/sys/net80211/ieee80211_freebsd.h
+++ b/sys/net80211/ieee80211_freebsd.h
@@ -115,6 +115,21 @@ typedef struct mtx ieee80211_scan_lock_t;
 	(_qlen) = ++(_ni)->ni_savedq.ifq_len; 			\
 } while (0)
 
+#ifndef IF_PREPEND_LIST
+#define _IF_PREPEND_LIST(ifq, mhead, mtail, mcount) do {	\
+	(mtail)->m_nextpkt = (ifq)->ifq_head;			\
+	if ((ifq)->ifq_tail == NULL)				\
+		(ifq)->ifq_tail = (mtail);			\
+	(ifq)->ifq_head = (mhead);				\
+	(ifq)->ifq_len += (mcount);				\
+} while (0)
+#define IF_PREPEND_LIST(ifq, mhead, mtail, mcount) do {		\
+	IF_LOCK(ifq);						\
+	_IF_PREPEND_LIST(ifq, mhead, mtail, mcount);		\
+	IF_UNLOCK(ifq);						\
+} while (0)
+#endif /* IF_PREPEND_LIST */
+
 /*
  * 802.1x MAC ACL database locking definitions.
  */
diff --git a/sys/net80211/ieee80211_input.c b/sys/net80211/ieee80211_input.c
index 9c3d60999ec4..2517edd1389b 100644
--- a/sys/net80211/ieee80211_input.c
+++ b/sys/net80211/ieee80211_input.c
@@ -2633,7 +2633,8 @@ static void
 ieee80211_node_pwrsave(struct ieee80211_node *ni, int enable)
 {
 	struct ieee80211com *ic = ni->ni_ic;
-	struct mbuf *m;
+	struct mbuf *m, *mhead, *mtail;
+	int mcount;
 
 	if (enable) {
 		if ((ni->ni_flags & IEEE80211_NODE_PWR_MGT) == 0)
@@ -2664,23 +2665,32 @@ ieee80211_node_pwrsave(struct ieee80211_node *ni, int enable)
 	IEEE80211_DPRINTF(ic, IEEE80211_MSG_POWER,
 	    "[%s] flush ps queue, %u packets queued\n",
 	    ether_sprintf(ni->ni_macaddr), IEEE80211_NODE_SAVEQ_QLEN(ni));
+	/*
+	 * Unload the frames from the ps q but don't send them
+	 * to the driver yet.  We do this in two stages to minimize
+	 * locking but also because there's no easy way to preserve
+	 * ordering given the existing ifnet access mechanisms.
+	 * XXX could be optimized
+	 */
+	IEEE80211_NODE_SAVEQ_LOCK(ni);
+	mcount = IEEE80211_NODE_SAVEQ_QLEN(ni);
+	mhead = mtail = NULL;
 	for (;;) {
-		int qlen;
-
-		IEEE80211_NODE_SAVEQ_DEQUEUE(ni, m, qlen);
+		_IEEE80211_NODE_SAVEQ_DEQUEUE_HEAD(ni, m);
 		if (m == NULL)
 			break;
-		/* 
-		 * If this is the last packet, turn off the TIM bit.
-		 * If there are more packets, set the more packets bit
-		 * in the mbuf so ieee80211_encap will mark the 802.11
-		 * head to indicate more data frames will follow.
-		 */
-		if (qlen != 0)
-			m->m_flags |= M_MORE_DATA;
+		if (mhead == NULL) {
+			mhead = m;
+			m->m_nextpkt = NULL;
+		} else
+			mtail->m_nextpkt = m;
+		mtail = m;
+	}
+	IEEE80211_NODE_SAVEQ_UNLOCK(ni);
+	if (mhead != NULL) {
 		/* XXX need different driver interface */
 		/* XXX bypasses q max */
-		IF_ENQUEUE(&ic->ic_ifp->if_snd, m);
+		IF_PREPEND_LIST(&ic->ic_ifp->if_snd, mhead, mtail, mcount);
 	}
 	if (ic->ic_set_tim != NULL)
 		ic->ic_set_tim(ni, 0);