mac80211: implement uAPSD

Add uAPSD support to mac80211. This is probably not
possible with all devices, so advertising it with
the cfg80211 flag will be left up to drivers that
want it.

Due to my previous patches it is now a fairly
straight-forward extension. Drivers need to have
accurate TX status reporting for the EOSP frame.
For drivers that buffer themselves, the provided
APIs allow releasing the right number of frames,
but then drivers need to set EOSP and more-data
themselves. This is documented in more detail in
the new code itself.

Signed-off-by: Johannes Berg <johannes.berg@intel.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

5 files changed, 224 insertions, 79 deletions

diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index db46601e50b..9a703f00b5f 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -1163,6 +1163,79 @@ int ieee80211_sta_ps_transition(struct ieee80211_sta *sta, bool start)
 EXPORT_SYMBOL(ieee80211_sta_ps_transition);
 
 static ieee80211_rx_result debug_noinline
+ieee80211_rx_h_uapsd_and_pspoll(struct ieee80211_rx_data *rx)
+{
+	struct ieee80211_sub_if_data *sdata = rx->sdata;
+	struct ieee80211_hdr *hdr = (void *)rx->skb->data;
+	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(rx->skb);
+	int tid, ac;
+
+	if (!rx->sta || !(status->rx_flags & IEEE80211_RX_RA_MATCH))
+		return RX_CONTINUE;
+
+	if (sdata->vif.type != NL80211_IFTYPE_AP &&
+	    sdata->vif.type != NL80211_IFTYPE_AP_VLAN)
+		return RX_CONTINUE;
+
+	/*
+	 * The device handles station powersave, so don't do anything about
+	 * uAPSD and PS-Poll frames (the latter shouldn't even come up from
+	 * it to mac80211 since they're handled.)
+	 */
+	if (sdata->local->hw.flags & IEEE80211_HW_AP_LINK_PS)
+		return RX_CONTINUE;
+
+	/*
+	 * Don't do anything if the station isn't already asleep. In
+	 * the uAPSD case, the station will probably be marked asleep,
+	 * in the PS-Poll case the station must be confused ...
+	 */
+	if (!test_sta_flags(rx->sta, WLAN_STA_PS_STA))
+		return RX_CONTINUE;
+
+	if (unlikely(ieee80211_is_pspoll(hdr->frame_control))) {
+		if (!test_sta_flags(rx->sta, WLAN_STA_PS_DRIVER))
+			ieee80211_sta_ps_deliver_poll_response(rx->sta);
+		else
+			set_sta_flags(rx->sta, WLAN_STA_PSPOLL);
+
+		/* Free PS Poll skb here instead of returning RX_DROP that would
+		 * count as an dropped frame. */
+		dev_kfree_skb(rx->skb);
+
+		return RX_QUEUED;
+	} else if (!ieee80211_has_morefrags(hdr->frame_control) &&
+		   !(status->rx_flags & IEEE80211_RX_DEFERRED_RELEASE) &&
+		   ieee80211_has_pm(hdr->frame_control) &&
+		   (ieee80211_is_data_qos(hdr->frame_control) ||
+		    ieee80211_is_qos_nullfunc(hdr->frame_control))) {
+		tid = *ieee80211_get_qos_ctl(hdr) & IEEE80211_QOS_CTL_TID_MASK;
+		ac = ieee802_1d_to_ac[tid & 7];
+
+		/*
+		 * If this AC is not trigger-enabled do nothing.
+		 *
+		 * NB: This could/should check a separate bitmap of trigger-
+		 * enabled queues, but for now we only implement uAPSD w/o
+		 * TSPEC changes to the ACs, so they're always the same.
+		 */
+		if (!(rx->sta->sta.uapsd_queues & BIT(ac)))
+			return RX_CONTINUE;
+
+		/* if we are in a service period, do nothing */
+		if (test_sta_flags(rx->sta, WLAN_STA_SP))
+			return RX_CONTINUE;
+
+		if (!test_sta_flags(rx->sta, WLAN_STA_PS_DRIVER))
+			ieee80211_sta_ps_deliver_uapsd(rx->sta);
+		else
+			set_sta_flags(rx->sta, WLAN_STA_UAPSD);
+	}
+
+	return RX_CONTINUE;
+}
+
+static ieee80211_rx_result debug_noinline
 ieee80211_rx_h_sta_process(struct ieee80211_rx_data *rx)
 {
 	struct sta_info *sta = rx->sta;
@@ -1473,33 +1546,6 @@ ieee80211_rx_h_defragment(struct ieee80211_rx_data *rx)
 }
 
 static ieee80211_rx_result debug_noinline
-ieee80211_rx_h_ps_poll(struct ieee80211_rx_data *rx)
-{
-	struct ieee80211_sub_if_data *sdata = rx->sdata;
-	__le16 fc = ((struct ieee80211_hdr *)rx->skb->data)->frame_control;
-	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(rx->skb);
-
-	if (likely(!rx->sta || !ieee80211_is_pspoll(fc) ||
-		   !(status->rx_flags & IEEE80211_RX_RA_MATCH)))
-		return RX_CONTINUE;
-
-	if ((sdata->vif.type != NL80211_IFTYPE_AP) &&
-	    (sdata->vif.type != NL80211_IFTYPE_AP_VLAN))
-		return RX_DROP_UNUSABLE;
-
-	if (!test_sta_flags(rx->sta, WLAN_STA_PS_DRIVER))
-		ieee80211_sta_ps_deliver_poll_response(rx->sta);
-	else
-		set_sta_flags(rx->sta, WLAN_STA_PSPOLL);
-
-	/* Free PS Poll skb here instead of returning RX_DROP that would
-	 * count as an dropped frame. */
-	dev_kfree_skb(rx->skb);
-
-	return RX_QUEUED;
-}
-
-static ieee80211_rx_result debug_noinline
 ieee80211_rx_h_remove_qos_control(struct ieee80211_rx_data *rx)
 {
 	u8 *data = rx->skb->data;
@@ -2567,9 +2613,9 @@ static void ieee80211_rx_handlers(struct ieee80211_rx_data *rx)
 
 		CALL_RXH(ieee80211_rx_h_decrypt)
 		CALL_RXH(ieee80211_rx_h_check_more_data)
+		CALL_RXH(ieee80211_rx_h_uapsd_and_pspoll)
 		CALL_RXH(ieee80211_rx_h_sta_process)
 		CALL_RXH(ieee80211_rx_h_defragment)
-		CALL_RXH(ieee80211_rx_h_ps_poll)
 		CALL_RXH(ieee80211_rx_h_michael_mic_verify)
 		/* must be after MMIC verify so header is counted in MPDU mic */
 #ifdef CONFIG_MAC80211_MESH
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index b3f841948c0..f9079e478f7 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -248,6 +248,9 @@ static void sta_unblock(struct work_struct *wk)
 	else if (test_and_clear_sta_flags(sta, WLAN_STA_PSPOLL)) {
 		clear_sta_flags(sta, WLAN_STA_PS_DRIVER);
 		ieee80211_sta_ps_deliver_poll_response(sta);
+	} else if (test_and_clear_sta_flags(sta, WLAN_STA_UAPSD)) {
+		clear_sta_flags(sta, WLAN_STA_PS_DRIVER);
+		ieee80211_sta_ps_deliver_uapsd(sta);
 	} else
 		clear_sta_flags(sta, WLAN_STA_PS_DRIVER);
 }
@@ -1117,6 +1120,8 @@ void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta)
 	struct sk_buff_head pending;
 	int filtered = 0, buffered = 0, ac;
 
+	clear_sta_flags(sta, WLAN_STA_SP);
+
 	BUILD_BUG_ON(BITS_TO_LONGS(STA_TID_NUM) > 1);
 	sta->driver_buffered_tids = 0;
 
@@ -1152,32 +1157,28 @@ void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta)
 #endif /* CONFIG_MAC80211_VERBOSE_PS_DEBUG */
 }
 
-void ieee80211_sta_ps_deliver_poll_response(struct sta_info *sta)
+static void
+ieee80211_sta_ps_deliver_response(struct sta_info *sta,
+				  int n_frames, u8 ignored_acs,
+				  enum ieee80211_frame_release_type reason)
 {
 	struct ieee80211_sub_if_data *sdata = sta->sdata;
 	struct ieee80211_local *local = sdata->local;
-	struct sk_buff *skb = NULL;
 	bool found = false;
 	bool more_data = false;
 	int ac;
 	unsigned long driver_release_tids = 0;
-	u8 ignore_for_response = sta->sta.uapsd_queues;
+	struct sk_buff_head frames;
 
-	/*
-	 * If all ACs are delivery-enabled then we should reply
-	 * from any of them, if only some are enabled we reply
-	 * only from the non-enabled ones.
-	 */
-	if (ignore_for_response == BIT(IEEE80211_NUM_ACS) - 1)
-		ignore_for_response = 0;
+	__skb_queue_head_init(&frames);
 
 	/*
-	 * Get response frame and more data bit for it.
+	 * Get response frame(s) and more data bit for it.
 	 */
 	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
 		unsigned long tids;
 
-		if (ignore_for_response & BIT(ac))
+		if (ignored_acs & BIT(ac))
 			continue;
 
 		tids = ieee80211_tids_for_ac(ac);
@@ -1187,14 +1188,22 @@ void ieee80211_sta_ps_deliver_poll_response(struct sta_info *sta)
 			if (driver_release_tids) {
 				found = true;
 			} else {
-				skb = skb_dequeue(&sta->tx_filtered[ac]);
-				if (!skb) {
-					skb = skb_dequeue(&sta->ps_tx_buf[ac]);
-					if (skb)
-						local->total_ps_buffered--;
-				}
-				if (skb)
+				struct sk_buff *skb;
+
+				while (n_frames > 0) {
+					skb = skb_dequeue(&sta->tx_filtered[ac]);
+					if (!skb) {
+						skb = skb_dequeue(
+							&sta->ps_tx_buf[ac]);
+						if (skb)
+							local->total_ps_buffered--;
+					}
+					if (!skb)
+						break;
+					n_frames--;
 					found = true;
+					__skb_queue_tail(&frames, skb);
+				}
 			}
 
 			/*
@@ -1202,7 +1211,8 @@ void ieee80211_sta_ps_deliver_poll_response(struct sta_info *sta)
 			 * certainly there's more data if we release just a
 			 * single frame now (from a single TID).
 			 */
-			if (hweight16(driver_release_tids) > 1) {
+			if (reason == IEEE80211_FRAME_RELEASE_PSPOLL &&
+			    hweight16(driver_release_tids) > 1) {
 				more_data = true;
 				driver_release_tids =
 					BIT(ffs(driver_release_tids) - 1);
@@ -1225,38 +1235,56 @@ void ieee80211_sta_ps_deliver_poll_response(struct sta_info *sta)
 		 *	  Should we send it a null-func frame indicating we
 		 *	  have nothing buffered for it?
 		 */
-		printk(KERN_DEBUG "%s: STA %pM sent PS Poll even "
-		       "though there are no buffered frames for it\n",
-		       sdata->name, sta->sta.addr);
+		if (reason == IEEE80211_FRAME_RELEASE_PSPOLL)
+			printk(KERN_DEBUG "%s: STA %pM sent PS Poll even "
+			       "though there are no buffered frames for it\n",
+			       sdata->name, sta->sta.addr);
 #endif /* CONFIG_MAC80211_VERBOSE_PS_DEBUG */
 
 		return;
 	}
 
-	if (skb) {
-		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-		struct ieee80211_hdr *hdr =
-			(struct ieee80211_hdr *) skb->data;
+	if (!driver_release_tids) {
+		struct sk_buff_head pending;
+		struct sk_buff *skb;
 
-		/*
-		 * Tell TX path to send this frame even though the STA may
-		 * still remain is PS mode after this frame exchange.
-		 */
-		info->flags |= IEEE80211_TX_CTL_PSPOLL_RESPONSE;
+		skb_queue_head_init(&pending);
 
-#ifdef CONFIG_MAC80211_VERBOSE_PS_DEBUG
-		printk(KERN_DEBUG "STA %pM aid %d: PS Poll\n",
-		       sta->sta.addr, sta->sta.aid);
-#endif /* CONFIG_MAC80211_VERBOSE_PS_DEBUG */
+		while ((skb = __skb_dequeue(&frames))) {
+			struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+			struct ieee80211_hdr *hdr = (void *) skb->data;
 
-		/* Use MoreData flag to indicate whether there are more
-		 * buffered frames for this STA */
-		if (!more_data)
-			hdr->frame_control &= cpu_to_le16(~IEEE80211_FCTL_MOREDATA);
-		else
-			hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+			/*
+			 * Tell TX path to send this frame even though the
+			 * STA may still remain is PS mode after this frame
+			 * exchange.
+			 */
+			info->flags |= IEEE80211_TX_CTL_POLL_RESPONSE;
+
+			/*
+			 * Use MoreData flag to indicate whether there are
+			 * more buffered frames for this STA
+			 */
+			if (!more_data)
+				hdr->frame_control &=
+					cpu_to_le16(~IEEE80211_FCTL_MOREDATA);
+			else
+				hdr->frame_control |=
+					cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+
+			if (reason == IEEE80211_FRAME_RELEASE_UAPSD &&
+			    skb_queue_empty(&frames)) {
+				/* set EOSP for the frame */
+				u8 *p = ieee80211_get_qos_ctl(hdr);
+				*p |= IEEE80211_QOS_CTL_EOSP;
+				info->flags |= IEEE80211_TX_STATUS_EOSP |
+					       IEEE80211_TX_CTL_REQ_TX_STATUS;
+			}
 
-		ieee80211_add_pending_skb(local, skb);
+			__skb_queue_tail(&pending, skb);
+		}
+
+		ieee80211_add_pending_skbs(local, &pending);
 
 		sta_info_recalc_tim(sta);
 	} else {
@@ -1271,8 +1299,7 @@ void ieee80211_sta_ps_deliver_poll_response(struct sta_info *sta)
 		 * needs to be set anyway.
 		 */
 		drv_release_buffered_frames(local, sta, driver_release_tids,
-					    1, IEEE80211_FRAME_RELEASE_PSPOLL,
-					    more_data);
+					    n_frames, reason, more_data);
 
 		/*
 		 * Note that we don't recalculate the TIM bit here as it would
@@ -1285,6 +1312,59 @@ void ieee80211_sta_ps_deliver_poll_response(struct sta_info *sta)
 	}
 }
 
+void ieee80211_sta_ps_deliver_poll_response(struct sta_info *sta)
+{
+	u8 ignore_for_response = sta->sta.uapsd_queues;
+
+	/*
+	 * If all ACs are delivery-enabled then we should reply
+	 * from any of them, if only some are enabled we reply
+	 * only from the non-enabled ones.
+	 */
+	if (ignore_for_response == BIT(IEEE80211_NUM_ACS) - 1)
+		ignore_for_response = 0;
+
+	ieee80211_sta_ps_deliver_response(sta, 1, ignore_for_response,
+					  IEEE80211_FRAME_RELEASE_PSPOLL);
+}
+
+void ieee80211_sta_ps_deliver_uapsd(struct sta_info *sta)
+{
+	int n_frames = sta->sta.max_sp;
+	u8 delivery_enabled = sta->sta.uapsd_queues;
+
+	/*
+	 * If we ever grow support for TSPEC this might happen if
+	 * the TSPEC update from hostapd comes in between a trigger
+	 * frame setting WLAN_STA_UAPSD in the RX path and this
+	 * actually getting called.
+	 */
+	if (!delivery_enabled)
+		return;
+
+	/* Ohh, finally, the service period starts :-) */
+	set_sta_flags(sta, WLAN_STA_SP);
+
+	switch (sta->sta.max_sp) {
+	case 1:
+		n_frames = 2;
+		break;
+	case 2:
+		n_frames = 4;
+		break;
+	case 3:
+		n_frames = 6;
+		break;
+	case 0:
+		/* XXX: what is a good value? */
+		n_frames = 8;
+		break;
+	}
+
+	ieee80211_sta_ps_deliver_response(sta, n_frames, ~delivery_enabled,
+					  IEEE80211_FRAME_RELEASE_UAPSD);
+}
+
 void ieee80211_sta_block_awake(struct ieee80211_hw *hw,
 			       struct ieee80211_sta *pubsta, bool block)
 {
diff --git a/net/mac80211/sta_info.h b/net/mac80211/sta_info.h
index 8589afad329..751ad25f925 100644
--- a/net/mac80211/sta_info.h
+++ b/net/mac80211/sta_info.h
@@ -46,6 +46,11 @@
  * @WLAN_STA_TDLS_PEER: Station is a TDLS peer.
  * @WLAN_STA_TDLS_PEER_AUTH: This TDLS peer is authorized to send direct
  *	packets. This means the link is enabled.
+ * @WLAN_STA_UAPSD: Station requested unscheduled SP while driver was
+ *	keeping station in power-save mode, reply when the driver
+ *	unblocks the station.
+ * @WLAN_STA_SP: Station is in a service period, so don't try to
+ *	reply to other uAPSD trigger frames.
  */
 enum ieee80211_sta_info_flags {
 	WLAN_STA_AUTH		= 1<<0,
@@ -63,6 +68,8 @@ enum ieee80211_sta_info_flags {
 	WLAN_STA_PSPOLL		= 1<<13,
 	WLAN_STA_TDLS_PEER	= 1<<15,
 	WLAN_STA_TDLS_PEER_AUTH	= 1<<16,
+	WLAN_STA_UAPSD		= 1<<17,
+	WLAN_STA_SP		= 1<<18,
 };
 
 #define STA_TID_NUM 16
@@ -539,5 +546,6 @@ void ieee80211_sta_expire(struct ieee80211_sub_if_data *sdata,
 
 void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta);
 void ieee80211_sta_ps_deliver_poll_response(struct sta_info *sta);
+void ieee80211_sta_ps_deliver_uapsd(struct sta_info *sta);
 
 #endif /* STA_INFO_H */
diff --git a/net/mac80211/status.c b/net/mac80211/status.c
index 94475eb51d2..b5df9be4d04 100644
--- a/net/mac80211/status.c
+++ b/net/mac80211/status.c
@@ -76,8 +76,16 @@ static void ieee80211_handle_filtered_frame(struct ieee80211_local *local,
 		hdr->frame_control &= ~cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 
 	if (ieee80211_is_data_qos(hdr->frame_control)) {
-		int tid = *ieee80211_get_qos_ctl(hdr) &
-					IEEE80211_QOS_CTL_TID_MASK;
+		u8 *p = ieee80211_get_qos_ctl(hdr);
+		int tid = *p & IEEE80211_QOS_CTL_TID_MASK;
+
+		/*
+		 * Clear EOSP if set, this could happen e.g.
+		 * if an absence period (us being a P2P GO)
+		 * shortens the SP.
+		 */
+		if (*p & IEEE80211_QOS_CTL_EOSP)
+			*p &= ~IEEE80211_QOS_CTL_EOSP;
 		ac = ieee802_1d_to_ac[tid & 7];
 	} else {
 		ac = IEEE80211_AC_BE;
@@ -276,6 +284,9 @@ void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb)
 		if (memcmp(hdr->addr2, sta->sdata->vif.addr, ETH_ALEN))
 			continue;
 
+		if (info->flags & IEEE80211_TX_STATUS_EOSP)
+			clear_sta_flags(sta, WLAN_STA_SP);
+
 		acked = !!(info->flags & IEEE80211_TX_STAT_ACK);
 		if (!acked && test_sta_flags(sta, WLAN_STA_PS_STA)) {
 			/*
diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c
index a1029449df4..a0676d39fe8 100644
--- a/net/mac80211/tx.c
+++ b/net/mac80211/tx.c
@@ -456,7 +456,7 @@ ieee80211_tx_h_unicast_ps_buf(struct ieee80211_tx_data *tx)
 	staflags = get_sta_flags(sta);
 
 	if (unlikely((staflags & (WLAN_STA_PS_STA | WLAN_STA_PS_DRIVER)) &&
-		     !(info->flags & IEEE80211_TX_CTL_PSPOLL_RESPONSE))) {
+		     !(info->flags & IEEE80211_TX_CTL_POLL_RESPONSE))) {
 		int ac = skb_get_queue_mapping(tx->skb);
 
 #ifdef CONFIG_MAC80211_VERBOSE_PS_DEBUG
@@ -497,9 +497,9 @@ ieee80211_tx_h_unicast_ps_buf(struct ieee80211_tx_data *tx)
 	}
 #ifdef CONFIG_MAC80211_VERBOSE_PS_DEBUG
 	else if (unlikely(staflags & WLAN_STA_PS_STA)) {
-		printk(KERN_DEBUG "%s: STA %pM in PS mode, but pspoll "
-		       "set -> send frame\n", tx->sdata->name,
-		       sta->sta.addr);
+		printk(KERN_DEBUG
+		       "%s: STA %pM in PS mode, but polling/in SP -> send frame\n",
+		       tx->sdata->name, sta->sta.addr);
 	}
 #endif /* CONFIG_MAC80211_VERBOSE_PS_DEBUG */