New ap-side power save implementation; the main change is to allow drivers

to queue frames previously encapsulated on a separate high priority list
that is dispatched before the unencapsulated frames (to preserve order).

8 files changed, 285 insertions, 113 deletions

diff --git a/sys/net80211/ieee80211_freebsd.h b/sys/net80211/ieee80211_freebsd.h
index 1f1826f..aac997a 100644
--- a/sys/net80211/ieee80211_freebsd.h
+++ b/sys/net80211/ieee80211_freebsd.h
@@ -112,41 +112,14 @@ typedef struct {
 } while (0)
 
 /*
- * Per-node power-save queue definitions. 
+ * Power-save queue definitions. 
  */
-#define	IEEE80211_NODE_SAVEQ_INIT(_ni, _name) do {		\
-	mtx_init(&(_ni)->ni_savedq.ifq_mtx, _name, "802.11 ps queue", MTX_DEF);\
-	(_ni)->ni_savedq.ifq_maxlen = IEEE80211_PS_MAX_QUEUE;	\
-} while (0)
-#define	IEEE80211_NODE_SAVEQ_DESTROY(_ni) \
-	mtx_destroy(&(_ni)->ni_savedq.ifq_mtx)
-#define	IEEE80211_NODE_SAVEQ_QLEN(_ni) \
-	_IF_QLEN(&(_ni)->ni_savedq)
-#define	IEEE80211_NODE_SAVEQ_LOCK(_ni) do {	\
-	IF_LOCK(&(_ni)->ni_savedq);				\
-} while (0)
-#define	IEEE80211_NODE_SAVEQ_UNLOCK(_ni) do {	\
-	IF_UNLOCK(&(_ni)->ni_savedq);				\
-} while (0)
-#define	IEEE80211_NODE_SAVEQ_DEQUEUE(_ni, _m, _qlen) do {	\
-	IEEE80211_NODE_SAVEQ_LOCK(_ni);				\
-	_IF_DEQUEUE(&(_ni)->ni_savedq, _m);			\
-	(_qlen) = IEEE80211_NODE_SAVEQ_QLEN(_ni);		\
-	IEEE80211_NODE_SAVEQ_UNLOCK(_ni);			\
-} while (0)
-#define	IEEE80211_NODE_SAVEQ_DRAIN(_ni, _qlen) do {		\
-	IEEE80211_NODE_SAVEQ_LOCK(_ni);				\
-	(_qlen) = IEEE80211_NODE_SAVEQ_QLEN(_ni);		\
-	_IF_DRAIN(&(_ni)->ni_savedq);				\
-	IEEE80211_NODE_SAVEQ_UNLOCK(_ni);			\
-} while (0)
-/* XXX could be optimized */
-#define	_IEEE80211_NODE_SAVEQ_DEQUEUE_HEAD(_ni, _m) do {	\
-	_IF_DEQUEUE(&(_ni)->ni_savedq, m);			\
-} while (0)
-#define	_IEEE80211_NODE_SAVEQ_ENQUEUE(_ni, _m, _qlen, _age) do {\
-	_AGEQ_ENQUEUE(&ni->ni_savedq, _m, _qlen, _age);		\
-} while (0)
+typedef struct mtx ieee80211_psq_lock_t;
+#define	IEEE80211_PSQ_INIT(_psq, _name) \
+	mtx_init(&(_psq)->psq_lock, _name, "802.11 ps q", MTX_DEF);
+#define	IEEE80211_PSQ_DESTROY(_psq)	mtx_destroy(&(_psq)->psq_lock)
+#define	IEEE80211_PSQ_LOCK(_psq)	mtx_lock(&(_psq)->psq_lock)
+#define	IEEE80211_PSQ_UNLOCK(_psq)	mtx_unlock(&(_psq)->psq_lock)
 
 #ifndef IF_PREPEND_LIST
 #define _IF_PREPEND_LIST(ifq, mhead, mtail, mcount) do {	\
diff --git a/sys/net80211/ieee80211_hostap.c b/sys/net80211/ieee80211_hostap.c
index c0a99a1..1c837f4 100644
--- a/sys/net80211/ieee80211_hostap.c
+++ b/sys/net80211/ieee80211_hostap.c
@@ -2171,7 +2171,7 @@ hostap_recv_pspoll(struct ieee80211_node *ni, struct mbuf *m0)
 {
 	struct ieee80211vap *vap = ni->ni_vap;
 	struct ieee80211_frame_min *wh;
-	struct ifnet *ifp = vap->iv_ifp;
+	struct ifnet *ifp;
 	struct mbuf *m;
 	uint16_t aid;
 	int qlen;
@@ -2208,7 +2208,7 @@ hostap_recv_pspoll(struct ieee80211_node *ni, struct mbuf *m0)
 	}
 
 	/* Okay, take the first queued packet and put it out... */
-	IEEE80211_NODE_SAVEQ_DEQUEUE(ni, m, qlen);
+	m = ieee80211_node_psq_dequeue(ni, &qlen);
 	if (m == NULL) {
 		IEEE80211_NOTE_MAC(vap, IEEE80211_MSG_POWER, wh->i_addr2,
 		    "%s", "recv ps-poll, but queue empty");
@@ -2234,6 +2234,11 @@ hostap_recv_pspoll(struct ieee80211_node *ni, struct mbuf *m0)
 			vap->iv_set_tim(ni, 0);
 	}
 	m->m_flags |= M_PWR_SAV;		/* bypass PS handling */
+
+	if (m->m_flags & M_ENCAP)
+		ifp = vap->iv_ic->ic_ifp;
+	else
+		ifp = vap->iv_ifp;
 	IF_ENQUEUE(&ifp->if_snd, m);
 	if_start(ifp);
 }
diff --git a/sys/net80211/ieee80211_node.c b/sys/net80211/ieee80211_node.c
index 6bbef44..562d31f 100644
--- a/sys/net80211/ieee80211_node.c
+++ b/sys/net80211/ieee80211_node.c
@@ -904,7 +904,7 @@ node_cleanup(struct ieee80211_node *ni)
 	/*
 	 * Drain power save queue and, if needed, clear TIM.
 	 */
-	if (ieee80211_node_saveq_drain(ni) != 0 && vap->iv_set_tim != NULL)
+	if (ieee80211_node_psq_drain(ni) != 0 && vap->iv_set_tim != NULL)
 		vap->iv_set_tim(ni, 0);
 
 	ni->ni_associd = 0;
@@ -943,7 +943,7 @@ node_free(struct ieee80211_node *ni)
 
 	ic->ic_node_cleanup(ni);
 	ieee80211_ies_cleanup(&ni->ni_ies);
-	IEEE80211_NODE_SAVEQ_DESTROY(ni);
+	ieee80211_psq_cleanup(&ni->ni_psq);
 	IEEE80211_NODE_WDSQ_DESTROY(ni);
 	FREE(ni, M_80211_NODE);
 }
@@ -958,9 +958,8 @@ node_age(struct ieee80211_node *ni)
 	/*
 	 * Age frames on the power save queue.
 	 */
-	if (ieee80211_node_saveq_age(ni) != 0 &&
-	    IEEE80211_NODE_SAVEQ_QLEN(ni) == 0 &&
-	    vap->iv_set_tim != NULL)
+	if (ieee80211_node_psq_age(ni) != 0 &&
+	    ni->ni_psq.psq_len == 0 && vap->iv_set_tim != NULL)
 		vap->iv_set_tim(ni, 0);
 	/*
 	 * Age frames on the wds pending queue.
@@ -1031,7 +1030,7 @@ ieee80211_alloc_node(struct ieee80211_node_table *nt,
 	ni->ni_inact_reload = nt->nt_inact_init;
 	ni->ni_inact = ni->ni_inact_reload;
 	ni->ni_ath_defkeyix = 0x7fff;
-	IEEE80211_NODE_SAVEQ_INIT(ni, "unknown");
+	ieee80211_psq_init(&ni->ni_psq, "unknown");
 	IEEE80211_NODE_WDSQ_INIT(ni, "unknown");
 
 	IEEE80211_NODE_LOCK(nt);
@@ -1081,7 +1080,7 @@ ieee80211_tmp_node(struct ieee80211vap *vap,
 			IEEE80211_KEYIX_NONE);
 		ni->ni_txpower = bss->ni_txpower;
 		/* XXX optimize away */
-		IEEE80211_NODE_SAVEQ_INIT(ni, "unknown");
+		ieee80211_psq_init(&ni->ni_psq, "unknown");
 		IEEE80211_NODE_WDSQ_INIT(ni, "unknown");
 	} else {
 		/* XXX msg */
diff --git a/sys/net80211/ieee80211_node.h b/sys/net80211/ieee80211_node.h
index 37d6372..b8a30ad 100644
--- a/sys/net80211/ieee80211_node.h
+++ b/sys/net80211/ieee80211_node.h
@@ -185,7 +185,7 @@ struct ieee80211_node {
 	short			ni_inact;	/* inactivity mark count */
 	short			ni_inact_reload;/* inactivity reload value */
 	int			ni_txrate;	/* legacy rate/MCS */
-	struct ifqueue		ni_savedq;	/* ps-poll queue */
+	struct ieee80211_psq	ni_psq;		/* power save queue */
 	struct ieee80211_nodestats ni_stats;	/* per-node statistics */
 
 	struct ieee80211vap	*ni_wdsvap;	/* associated WDS vap */
diff --git a/sys/net80211/ieee80211_output.c b/sys/net80211/ieee80211_output.c
index 713dc6b..f29b47e 100644
--- a/sys/net80211/ieee80211_output.c
+++ b/sys/net80211/ieee80211_output.c
@@ -225,7 +225,7 @@ ieee80211_start(struct ifnet *ifp)
 			 * the frame back when the time is right.
 			 * XXX lose WDS vap linkage?
 			 */
-			ieee80211_pwrsave(ni, m);
+			(void) ieee80211_pwrsave(ni, m);
 			ieee80211_free_node(ni);
 			continue;
 		}
diff --git a/sys/net80211/ieee80211_power.c b/sys/net80211/ieee80211_power.c
index 2a7dda8..b2ace4a 100644
--- a/sys/net80211/ieee80211_power.c
+++ b/sys/net80211/ieee80211_power.c
@@ -99,24 +99,115 @@ ieee80211_power_vdetach(struct ieee80211vap *vap)
 	}
 }
 
+void
+ieee80211_psq_init(struct ieee80211_psq *psq, const char *name)
+{
+	memset(psq, 0, sizeof(psq));
+	psq->psq_maxlen = IEEE80211_PS_MAX_QUEUE;
+	IEEE80211_PSQ_INIT(psq, name);		/* OS-dependent setup */
+}
+
+void
+ieee80211_psq_cleanup(struct ieee80211_psq *psq)
+{
+#if 0
+	psq_drain(psq);				/* XXX should not be needed? */
+#else
+	KASSERT(psq->psq_len == 0, ("%d frames on ps q", psq->psq_len));
+#endif
+	IEEE80211_PSQ_DESTROY(psq);		/* OS-dependent cleanup */
+}
+
+/*
+ * Return the highest priority frame in the ps queue.
+ */
+struct mbuf *
+ieee80211_node_psq_dequeue(struct ieee80211_node *ni, int *qlen)
+{
+	struct ieee80211_psq *psq = &ni->ni_psq;
+	struct ieee80211_psq_head *qhead;
+	struct mbuf *m;
+
+	IEEE80211_PSQ_LOCK(psq);
+	qhead = &psq->psq_head[0];
+again:
+	if ((m = qhead->head) != NULL) {
+		if ((qhead->head = m->m_nextpkt) == NULL)
+			qhead->tail = NULL;
+		KASSERT(qhead->len > 0, ("qhead len %d", qhead->len));
+		qhead->len--;
+		KASSERT(psq->psq_len > 0, ("psq len %d", psq->psq_len));
+		psq->psq_len--;
+		m->m_nextpkt = NULL;
+	}
+	if (m == NULL && qhead == &psq->psq_head[0]) {
+		/* Algol-68 style for loop */
+		qhead = &psq->psq_head[1];
+		goto again;
+	}
+	if (qlen != NULL)
+		*qlen = psq->psq_len;
+	IEEE80211_PSQ_UNLOCK(psq);
+	return m;
+}
+
+/*
+ * Reclaim an mbuf from the ps q.  If marked with M_ENCAP
+ * we assume there is a node reference that must be relcaimed.
+ */
+static void
+psq_mfree(struct mbuf *m)
+{
+	if (m->m_flags & M_ENCAP) {
+		struct ieee80211_node *ni = (void *) m->m_pkthdr.rcvif;
+		ieee80211_free_node(ni);
+	}
+	m->m_nextpkt = NULL;
+	m_freem(m);
+}
+
 /*
- * Clear any frames queued on a node's power save queue.
+ * Clear any frames queued in the power save queue.
  * The number of frames that were present is returned.
  */
-int
-ieee80211_node_saveq_drain(struct ieee80211_node *ni)
+static int
+psq_drain(struct ieee80211_psq *psq)
 {
+	struct ieee80211_psq_head *qhead;
+	struct mbuf *m;
 	int qlen;
 
-	IEEE80211_NODE_SAVEQ_LOCK(ni);
-	qlen = IEEE80211_NODE_SAVEQ_QLEN(ni);
-	_IF_DRAIN(&ni->ni_savedq);
-	IEEE80211_NODE_SAVEQ_UNLOCK(ni);
+	IEEE80211_PSQ_LOCK(psq);
+	qlen = psq->psq_len;
+	qhead = &psq->psq_head[0];
+again:
+	while ((m = qhead->head) != NULL) {
+		qhead->head = m->m_nextpkt;
+		psq_mfree(m);
+	}
+	qhead->tail = NULL;
+	qhead->len = 0;
+	if (qhead == &psq->psq_head[0]) {	/* Algol-68 style for loop */
+		qhead = &psq->psq_head[1];
+		goto again;
+	}
+	psq->psq_len = 0;
+	IEEE80211_PSQ_UNLOCK(psq);
 
 	return qlen;
 }
 
 /*
+ * Clear any frames queued in the power save queue.
+ * The number of frames that were present is returned.
+ */
+int
+ieee80211_node_psq_drain(struct ieee80211_node *ni)
+{
+	return psq_drain(&ni->ni_psq);
+}
+
+/*
  * Age frames on the power save queue. The aging interval is
  * 4 times the listen interval specified by the station.  This
  * number is factored into the age calculations when the frame
@@ -127,28 +218,41 @@ ieee80211_node_saveq_drain(struct ieee80211_node *ni)
  * can check if it needs to adjust the tim.
  */
 int
-ieee80211_node_saveq_age(struct ieee80211_node *ni)
+ieee80211_node_psq_age(struct ieee80211_node *ni)
 {
+	struct ieee80211_psq *psq = &ni->ni_psq;
 	int discard = 0;
 
-	if (IEEE80211_NODE_SAVEQ_QLEN(ni) != 0) {
+	if (psq->psq_len != 0) {
 #ifdef IEEE80211_DEBUG
 		struct ieee80211vap *vap = ni->ni_vap;
 #endif
+		struct ieee80211_psq_head *qhead;
 		struct mbuf *m;
 
-		IEEE80211_NODE_SAVEQ_LOCK(ni);
-		while (IF_POLL(&ni->ni_savedq, m) != NULL &&
-		     M_AGE_GET(m) < IEEE80211_INACT_WAIT) {
+		IEEE80211_PSQ_LOCK(psq);
+		qhead = &psq->psq_head[0];
+	again:
+		while ((m = qhead->head) != NULL &&
+		    M_AGE_GET(m) < IEEE80211_INACT_WAIT) {
 			IEEE80211_NOTE(vap, IEEE80211_MSG_POWER, ni,
 			     "discard frame, age %u", M_AGE_GET(m));
-			_IEEE80211_NODE_SAVEQ_DEQUEUE_HEAD(ni, m);
-			m_freem(m);
+			if ((qhead->head = m->m_nextpkt) == NULL)
+				qhead->tail = NULL;
+			KASSERT(qhead->len > 0, ("qhead len %d", qhead->len));
+			qhead->len--;
+			KASSERT(psq->psq_len > 0, ("psq len %d", psq->psq_len));
+			psq->psq_len--;
+			psq_mfree(m);
 			discard++;
 		}
+		if (qhead == &psq->psq_head[0]) { /* Algol-68 style for loop */
+			qhead = &psq->psq_head[1];
+			goto again;
+		}
 		if (m != NULL)
 			M_AGE_SUB(m, IEEE80211_INACT_WAIT);
-		IEEE80211_NODE_SAVEQ_UNLOCK(ni);
+		IEEE80211_PSQ_UNLOCK(psq);
 
 		IEEE80211_NOTE(vap, IEEE80211_MSG_POWER, ni,
 		    "discard %u frames for age", discard);
@@ -211,82 +315,143 @@ ieee80211_set_tim(struct ieee80211_node *ni, int set)
  * The new packet is placed on the node's saved queue, and the TIM
  * is changed, if necessary.
  */
-void
+int
 ieee80211_pwrsave(struct ieee80211_node *ni, struct mbuf *m)
 {
+	struct ieee80211_psq *psq = &ni->ni_psq;
 	struct ieee80211vap *vap = ni->ni_vap;
 	struct ieee80211com *ic = ni->ni_ic;
+	struct ieee80211_psq_head *qhead;
 	int qlen, age;
 
-	IEEE80211_NODE_SAVEQ_LOCK(ni);
-	if (_IF_QFULL(&ni->ni_savedq)) {
-		_IF_DROP(&ni->ni_savedq);
-		IEEE80211_NODE_SAVEQ_UNLOCK(ni);
+	IEEE80211_PSQ_LOCK(psq);
+	if (psq->psq_len >= psq->psq_maxlen) {
+		psq->psq_drops++;
+		IEEE80211_PSQ_UNLOCK(psq);
 		IEEE80211_NOTE(vap, IEEE80211_MSG_ANY, ni,
 		    "pwr save q overflow, drops %d (size %d)",
-		    ni->ni_savedq.ifq_drops, IEEE80211_PS_MAX_QUEUE);
+		    psq->psq_drops, psq->psq_len);
 #ifdef IEEE80211_DEBUG
 		if (ieee80211_msg_dumppkts(vap))
 			ieee80211_dump_pkt(ni->ni_ic, mtod(m, caddr_t),
 			    m->m_len, -1, -1);
 #endif
-		m_freem(m);
-		return;
+		psq_mfree(m);
+		return ENOSPC;
 	}
 	/*
-	 * Tag the frame with it's expiry time and insert
-	 * it in the queue.  The aging interval is 4 times
-	 * the listen interval specified by the station. 
-	 * Frames that sit around too long are reclaimed
-	 * using this information.
+	 * Tag the frame with it's expiry time and insert it in
+	 * the appropriate queue.  The aging interval is 4 times
+	 * the listen interval specified by the station. Frames
+	 * that sit around too long are reclaimed using this
+	 * information.
 	 */
 	/* TU -> secs.  XXX handle overflow? */
 	age = IEEE80211_TU_TO_MS((ni->ni_intval * ic->ic_bintval) << 2) / 1000;
-	_IEEE80211_NODE_SAVEQ_ENQUEUE(ni, m, qlen, age);
-	IEEE80211_NODE_SAVEQ_UNLOCK(ni);
+	/*
+	 * Encapsulated frames go on the high priority queue,
+	 * other stuff goes on the low priority queue.  We use
+	 * this to order frames returned out of the driver
+	 * ahead of frames we collect in ieee80211_start.
+	 */
+	if (m->m_flags & M_ENCAP)
+		qhead = &psq->psq_head[0];
+	else
+		qhead = &psq->psq_head[1];
+	if (qhead->tail == NULL) {
+		struct mbuf *mh;
+
+		qhead->head = m;
+		/*
+		 * Take care to adjust age when inserting the first
+		 * frame of a queue and the other queue already has
+		 * frames.  We need to preserve the age difference
+		 * relationship so ieee80211_node_psq_age works.
+		 */
+		if (qhead == &psq->psq_head[1]) {
+			mh = psq->psq_head[0].head;
+			if (mh != NULL)
+				age-= M_AGE_GET(mh);
+		} else {
+			mh = psq->psq_head[1].head;
+			if (mh != NULL) {
+				int nage = M_AGE_GET(mh) - age;
+				/* XXX is clamping to zero good 'nuf? */
+				M_AGE_SET(mh, nage < 0 ? 0 : nage);
+			}
+		}
+	} else {
+		qhead->tail->m_nextpkt = m;
+		age -= M_AGE_GET(qhead->head);
+	}
+	KASSERT(age >= 0, ("age %d", age));
+	M_AGE_SET(m, age);
+	m->m_nextpkt = NULL;
+	qhead->tail = m;
+	qhead->len++;
+	qlen = ++(psq->psq_len);
+	IEEE80211_PSQ_UNLOCK(psq);
 
 	IEEE80211_NOTE(vap, IEEE80211_MSG_POWER, ni,
 	    "save frame with age %d, %u now queued", age, qlen);
 
 	if (qlen == 1 && vap->iv_set_tim != NULL)
 		vap->iv_set_tim(ni, 1);
+
+	return 0;
 }
 
 /*
- * 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
+ * Move frames from the ps q to the vap's send queue
+ * and/or the driver's send queue; and kick the start
+ * method for each, as appropriate.  Note we're careful
+ * to preserve packet ordering here.
  */
 static void
 pwrsave_flushq(struct ieee80211_node *ni)
 {
-	struct mbuf *m, *mhead, *mtail;
-	int mcount;
-
-	IEEE80211_NODE_SAVEQ_LOCK(ni);
-	mcount = IEEE80211_NODE_SAVEQ_QLEN(ni);
-	mhead = mtail = NULL;
-	for (;;) {
-		_IEEE80211_NODE_SAVEQ_DEQUEUE_HEAD(ni, m);
-		if (m == NULL)
-			break;
-		if (mhead == NULL) {
-			mhead = m;
-			m->m_nextpkt = NULL;
-		} else
-			mtail->m_nextpkt = m;
-		mtail = m;
-	}
-	IEEE80211_NODE_SAVEQ_UNLOCK(ni);
-	if (mhead != NULL) {
+	struct ieee80211_psq *psq = &ni->ni_psq;
+	struct ieee80211vap *vap = ni->ni_vap;
+	struct ieee80211_psq_head *qhead;
+	struct ifnet *parent, *ifp;
+
+	IEEE80211_NOTE(vap, IEEE80211_MSG_POWER, ni,
+	    "flush ps queue, %u packets queued", psq->psq_len);
+
+	IEEE80211_PSQ_LOCK(psq);
+	qhead = &psq->psq_head[0];	/* 802.11 frames */
+	if (qhead->head != NULL) {
+		/* XXX could dispatch through vap and check M_ENCAP */
+		parent = vap->iv_ic->ic_ifp;
 		/* XXX need different driver interface */
 		/* XXX bypasses q max and OACTIVE */
-		struct ifnet *ifp = ni->ni_vap->iv_ifp;
-		IF_PREPEND_LIST(&ifp->if_snd, mhead, mtail, mcount);
+		IF_PREPEND_LIST(&parent->if_snd, qhead->head, qhead->tail,
+		    qhead->len);
+		qhead->head = qhead->tail = NULL;
+		qhead->len = 0;
+	} else
+		parent = NULL;
+
+	qhead = &psq->psq_head[1];	/* 802.3 frames */
+	if (qhead->head != NULL) {
+		ifp = vap->iv_ifp;
+		/* XXX need different driver interface */
+		/* XXX bypasses q max and OACTIVE */
+		IF_PREPEND_LIST(&ifp->if_snd, qhead->head, qhead->tail,
+		    qhead->len);
+		qhead->head = qhead->tail = NULL;
+		qhead->len = 0;
+	} else
+		ifp = NULL;
+	psq->psq_len = 0;
+	IEEE80211_PSQ_UNLOCK(psq);
+
+	/* NB: do this outside the psq lock */
+	/* XXX packets might get reordered if parent is OACTIVE */
+	if (parent != NULL)
+		if_start(parent);
+	if (ifp != NULL)
 		if_start(ifp);
-	}
 }
 
 /*
@@ -326,7 +491,8 @@ ieee80211_node_pwrsave(struct ieee80211_node *ni, int enable)
 			/* NB if no sta's in ps, driver should flush mc q */
 			vap->iv_update_ps(vap, vap->iv_ps_sta);
 		}
-		pwrsave_flushq(ni);
+		if (ni->ni_psq.psq_len != 0)
+			pwrsave_flushq(ni);
 	}
 }
 
@@ -337,7 +503,6 @@ void
 ieee80211_sta_pwrsave(struct ieee80211vap *vap, int enable)
 {
 	struct ieee80211_node *ni = vap->iv_bss;
-	int qlen;
 
 	if (!((enable != 0) ^ ((ni->ni_flags & IEEE80211_NODE_PWR_MGT) != 0)))
 		return;
@@ -353,12 +518,8 @@ ieee80211_sta_pwrsave(struct ieee80211vap *vap, int enable)
 		 * data frame we send the ap.
 		 * XXX can we use a data frame to take us out of ps?
 		 */
-		qlen = IEEE80211_NODE_SAVEQ_QLEN(ni);
-		if (qlen != 0) {
-			IEEE80211_NOTE(vap, IEEE80211_MSG_POWER, ni,
-			    "flush ps queue, %u packets queued", qlen);
+		if (ni->ni_psq.psq_len != 0)
 			pwrsave_flushq(ni);
-		}
 	} else {
 		ni->ni_flags |= IEEE80211_NODE_PWR_MGT;
 		ieee80211_send_nulldata(ieee80211_ref_node(ni));
diff --git a/sys/net80211/ieee80211_power.h b/sys/net80211/ieee80211_power.h
index 27f5d5a..352cdad 100644
--- a/sys/net80211/ieee80211_power.h
+++ b/sys/net80211/ieee80211_power.h
@@ -28,6 +28,39 @@
 #define _NET80211_IEEE80211_POWER_H_
 
 struct ieee80211com;
+struct ieee80211vap;
+struct ieee80211_node;
+struct mbuf;
+
+/*
+ * Power save packet queues.  There are two queues, one
+ * for frames coming from the net80211 layer and the other
+ * for frames that come from the driver. Frames from the
+ * driver are expected to have M_ENCAP marked to indicate
+ * they have already been encapsulated and are treated as
+ * higher priority: they are sent first when flushing the
+ * queue on a power save state change or in response to a
+ * ps-poll frame.
+ *
+ * Note that frames sent from the high priority queue are
+ * fed directly to the driver without going through
+ * ieee80211_start again; drivers that send up encap'd
+ * frames are required to handle them when they come back.
+ */
+struct ieee80211_psq {
+	ieee80211_psq_lock_t psq_lock;
+	int	psq_len;
+	int	psq_maxlen;
+	int	psq_drops;
+	struct ieee80211_psq_head {
+		struct mbuf *head;
+		struct mbuf *tail;
+		int len;
+	} psq_head[2];			/* 2 priorities */
+};
+
+void	ieee80211_psq_init(struct ieee80211_psq *, const char *);
+void	ieee80211_psq_cleanup(struct ieee80211_psq *);
 
 void	ieee80211_power_attach(struct ieee80211com *);
 void	ieee80211_power_detach(struct ieee80211com *);
@@ -35,9 +68,10 @@ void	ieee80211_power_vattach(struct ieee80211vap *);
 void	ieee80211_power_vdetach(struct ieee80211vap *);
 void	ieee80211_power_latevattach(struct ieee80211vap *);
 
-int	ieee80211_node_saveq_drain(struct ieee80211_node *);
-int	ieee80211_node_saveq_age(struct ieee80211_node *);
-void	ieee80211_pwrsave(struct ieee80211_node *, struct mbuf *);
+struct mbuf *ieee80211_node_psq_dequeue(struct ieee80211_node *ni, int *qlen);
+int	ieee80211_node_psq_drain(struct ieee80211_node *);
+int	ieee80211_node_psq_age(struct ieee80211_node *);
+int	ieee80211_pwrsave(struct ieee80211_node *, struct mbuf *);
 void	ieee80211_node_pwrsave(struct ieee80211_node *, int enable);
 void	ieee80211_sta_pwrsave(struct ieee80211vap *, int enable);
 
diff --git a/sys/net80211/ieee80211_var.h b/sys/net80211/ieee80211_var.h
index 0697f6f..f7d90f5 100644
--- a/sys/net80211/ieee80211_var.h
+++ b/sys/net80211/ieee80211_var.h
@@ -47,8 +47,8 @@
 #include <net80211/ieee80211_crypto.h>
 #include <net80211/ieee80211_dfs.h>
 #include <net80211/ieee80211_ioctl.h>		/* for ieee80211_stats */
-#include <net80211/ieee80211_node.h>
 #include <net80211/ieee80211_power.h>
+#include <net80211/ieee80211_node.h>
 #include <net80211/ieee80211_proto.h>
 #include <net80211/ieee80211_scan.h>