mac80211: use QoS NDP for AP probing

When connected to a QoS/WMM AP, mac80211 should use a QoS NDP
for probing it, instead of a regular non-QoS one, fix this.

Change all the drivers to *not* allow QoS NDP for now, even
though it looks like most of them should be OK with that.

Signed-off-by: Johannes Berg <johannes.berg@intel.com>

1 files changed, 27 insertions, 2 deletions

diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c
index 7b8154474b9e..3160954fc406 100644
--- a/net/mac80211/tx.c
+++ b/net/mac80211/tx.c
@@ -4438,13 +4438,15 @@ struct sk_buff *ieee80211_pspoll_get(struct ieee80211_hw *hw,
 EXPORT_SYMBOL(ieee80211_pspoll_get);
 
 struct sk_buff *ieee80211_nullfunc_get(struct ieee80211_hw *hw,
-                                       struct ieee80211_vif *vif)
+                                       struct ieee80211_vif *vif,
+                                       bool qos_ok)
 {
         struct ieee80211_hdr_3addr *nullfunc;
         struct ieee80211_sub_if_data *sdata;
         struct ieee80211_if_managed *ifmgd;
         struct ieee80211_local *local;
         struct sk_buff *skb;
+        bool qos = false;
 
         if (WARN_ON(vif->type != NL80211_IFTYPE_STATION))
                 return NULL;
@@ -4453,7 +4455,17 @@ struct sk_buff *ieee80211_nullfunc_get(struct ieee80211_hw *hw,
         ifmgd = &sdata->u.mgd;
         local = sdata->local;
 
-        skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*nullfunc));
+        if (qos_ok) {
+                struct sta_info *sta;
+
+                rcu_read_lock();
+                sta = sta_info_get(sdata, ifmgd->bssid);
+                qos = sta && sta->sta.wme;
+                rcu_read_unlock();
+        }
+
+        skb = dev_alloc_skb(local->hw.extra_tx_headroom +
+                            sizeof(*nullfunc) + 2);
         if (!skb)
                 return NULL;
 
@@ -4463,6 +4475,19 @@ struct sk_buff *ieee80211_nullfunc_get(struct ieee80211_hw *hw,
         nullfunc->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
                                               IEEE80211_STYPE_NULLFUNC |
                                               IEEE80211_FCTL_TODS);
+        if (qos) {
+                __le16 qos = cpu_to_le16(7);
+
+                BUILD_BUG_ON((IEEE80211_STYPE_QOS_NULLFUNC |
+                              IEEE80211_STYPE_NULLFUNC) !=
+                             IEEE80211_STYPE_QOS_NULLFUNC);
+                nullfunc->frame_control |=
+                        cpu_to_le16(IEEE80211_STYPE_QOS_NULLFUNC);
+                skb->priority = 7;
+                skb_set_queue_mapping(skb, IEEE80211_AC_VO);
+                skb_put_data(skb, &qos, sizeof(qos));
+        }
+
         memcpy(nullfunc->addr1, ifmgd->bssid, ETH_ALEN);
         memcpy(nullfunc->addr2, vif->addr, ETH_ALEN);
         memcpy(nullfunc->addr3, ifmgd->bssid, ETH_ALEN);