mac80211: support client probe

Support probing clients with null data frames
in AP mode.

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

1 files changed, 68 insertions, 0 deletions

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 1f1056172ef1..e072fea69a30 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -2507,6 +2507,73 @@ static int ieee80211_tdls_oper(struct wiphy *wiphy, struct net_device *dev,
         return 0;
 }
 
+static int ieee80211_probe_client(struct wiphy *wiphy, struct net_device *dev,
+                                  const u8 *peer, u64 *cookie)
+{
+        struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+        struct ieee80211_local *local = sdata->local;
+        struct ieee80211_qos_hdr *nullfunc;
+        struct sk_buff *skb;
+        int size = sizeof(*nullfunc);
+        __le16 fc;
+        bool qos;
+        struct ieee80211_tx_info *info;
+        struct sta_info *sta;
+
+        rcu_read_lock();
+        sta = sta_info_get(sdata, peer);
+        if (sta)
+                qos = test_sta_flag(sta, WLAN_STA_WME);
+        rcu_read_unlock();
+
+        if (!sta)
+                return -ENOLINK;
+
+        if (qos) {
+                fc = cpu_to_le16(IEEE80211_FTYPE_DATA |
+                                 IEEE80211_STYPE_QOS_NULLFUNC |
+                                 IEEE80211_FCTL_FROMDS);
+        } else {
+                size -= 2;
+                fc = cpu_to_le16(IEEE80211_FTYPE_DATA |
+                                 IEEE80211_STYPE_NULLFUNC |
+                                 IEEE80211_FCTL_FROMDS);
+        }
+
+        skb = dev_alloc_skb(local->hw.extra_tx_headroom + size);
+        if (!skb)
+                return -ENOMEM;
+
+        skb->dev = dev;
+
+        skb_reserve(skb, local->hw.extra_tx_headroom);
+
+        nullfunc = (void *) skb_put(skb, size);
+        nullfunc->frame_control = fc;
+        nullfunc->duration_id = 0;
+        memcpy(nullfunc->addr1, sta->sta.addr, ETH_ALEN);
+        memcpy(nullfunc->addr2, sdata->vif.addr, ETH_ALEN);
+        memcpy(nullfunc->addr3, sdata->vif.addr, ETH_ALEN);
+        nullfunc->seq_ctrl = 0;
+
+        info = IEEE80211_SKB_CB(skb);
+
+        info->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS |
+                       IEEE80211_TX_INTFL_NL80211_FRAME_TX;
+
+        skb_set_queue_mapping(skb, IEEE80211_AC_VO);
+        skb->priority = 7;
+        if (qos)
+                nullfunc->qos_ctrl = cpu_to_le16(7);
+
+        local_bh_disable();
+        ieee80211_xmit(sdata, skb);
+        local_bh_enable();
+
+        *cookie = (unsigned long) skb;
+        return 0;
+}
+
 struct cfg80211_ops mac80211_config_ops = {
         .add_virtual_intf = ieee80211_add_iface,
         .del_virtual_intf = ieee80211_del_iface,
@@ -2572,4 +2639,5 @@ struct cfg80211_ops mac80211_config_ops = {
         .set_rekey_data = ieee80211_set_rekey_data,
         .tdls_oper = ieee80211_tdls_oper,
         .tdls_mgmt = ieee80211_tdls_mgmt,
+        .probe_client = ieee80211_probe_client,
 };