nl80211: New command for adding extra IE(s) into management frames

A new nl80211 command, NL80211_CMD_SET_MGMT_EXTRA_IE, can be used to
add arbitrary IE data into the end of management frames. The interface
allows extra IEs to be configured for each management frame subtype, but
only some of them (ProbeReq, ProbeResp, Auth, (Re)AssocReq, Deauth,
Disassoc) are currently accepted in mac80211 implementation.

This makes it easier to implement IEEE 802.11 extensions like WPS and
FT that add IE(s) into some management frames. In addition, this can
be useful for testing and experimentation purposes.

Signed-off-by: Jouni Malinen <jouni.malinen@atheros.com>
Acked-by: Johannes Berg <johannes@sipsolutions.net>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

5 files changed, 198 insertions, 6 deletions

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 72c106915433..d1ac3ab2c515 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -1175,6 +1175,87 @@ static int ieee80211_set_channel(struct wiphy *wiphy,
         return ieee80211_hw_config(local, IEEE80211_CONF_CHANGE_CHANNEL);
 }
 
+static int set_mgmt_extra_ie_sta(struct ieee80211_if_sta *ifsta, u8 subtype,
+                                 u8 *ies, size_t ies_len)
+{
+        switch (subtype) {
+        case IEEE80211_STYPE_PROBE_REQ >> 4:
+                kfree(ifsta->ie_probereq);
+                ifsta->ie_probereq = ies;
+                ifsta->ie_probereq_len = ies_len;
+                return 0;
+        case IEEE80211_STYPE_PROBE_RESP >> 4:
+                kfree(ifsta->ie_proberesp);
+                ifsta->ie_proberesp = ies;
+                ifsta->ie_proberesp_len = ies_len;
+                return 0;
+        case IEEE80211_STYPE_AUTH >> 4:
+                kfree(ifsta->ie_auth);
+                ifsta->ie_auth = ies;
+                ifsta->ie_auth_len = ies_len;
+                return 0;
+        case IEEE80211_STYPE_ASSOC_REQ >> 4:
+                kfree(ifsta->ie_assocreq);
+                ifsta->ie_assocreq = ies;
+                ifsta->ie_assocreq_len = ies_len;
+                return 0;
+        case IEEE80211_STYPE_REASSOC_REQ >> 4:
+                kfree(ifsta->ie_reassocreq);
+                ifsta->ie_reassocreq = ies;
+                ifsta->ie_reassocreq_len = ies_len;
+                return 0;
+        case IEEE80211_STYPE_DEAUTH >> 4:
+                kfree(ifsta->ie_deauth);
+                ifsta->ie_deauth = ies;
+                ifsta->ie_deauth_len = ies_len;
+                return 0;
+        case IEEE80211_STYPE_DISASSOC >> 4:
+                kfree(ifsta->ie_disassoc);
+                ifsta->ie_disassoc = ies;
+                ifsta->ie_disassoc_len = ies_len;
+                return 0;
+        }
+
+        return -EOPNOTSUPP;
+}
+
+static int ieee80211_set_mgmt_extra_ie(struct wiphy *wiphy,
+                                       struct net_device *dev,
+                                       struct mgmt_extra_ie_params *params)
+{
+        struct ieee80211_sub_if_data *sdata;
+        u8 *ies;
+        size_t ies_len;
+        int ret = -EOPNOTSUPP;
+
+        if (params->ies) {
+                ies = kmemdup(params->ies, params->ies_len, GFP_KERNEL);
+                if (ies == NULL)
+                        return -ENOMEM;
+                ies_len = params->ies_len;
+        } else {
+                ies = NULL;
+                ies_len = 0;
+        }
+
+        sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+
+        switch (sdata->vif.type) {
+        case NL80211_IFTYPE_STATION:
+        case NL80211_IFTYPE_ADHOC:
+                ret = set_mgmt_extra_ie_sta(&sdata->u.sta, params->subtype,
+                                            ies, ies_len);
+                break;
+        default:
+                ret = -EOPNOTSUPP;
+                break;
+        }
+
+        if (ret)
+                kfree(ies);
+        return ret;
+}
+
 struct cfg80211_ops mac80211_config_ops = {
         .add_virtual_intf = ieee80211_add_iface,
         .del_virtual_intf = ieee80211_del_iface,
@@ -1204,4 +1285,5 @@ struct cfg80211_ops mac80211_config_ops = {
         .change_bss = ieee80211_change_bss,
         .set_txq_params = ieee80211_set_txq_params,
         .set_channel = ieee80211_set_channel,
+        .set_mgmt_extra_ie = ieee80211_set_mgmt_extra_ie,
 };
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index c9ffadb55d36..5eafd3affe27 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -331,6 +331,22 @@ struct ieee80211_if_sta {
         u32 supp_rates_bits[IEEE80211_NUM_BANDS];
 
         int wmm_last_param_set;
+
+        /* Extra IE data for management frames */
+        u8 *ie_probereq;
+        size_t ie_probereq_len;
+        u8 *ie_proberesp;
+        size_t ie_proberesp_len;
+        u8 *ie_auth;
+        size_t ie_auth_len;
+        u8 *ie_assocreq;
+        size_t ie_assocreq_len;
+        u8 *ie_reassocreq;
+        size_t ie_reassocreq_len;
+        u8 *ie_deauth;
+        size_t ie_deauth_len;
+        u8 *ie_disassoc;
+        size_t ie_disassoc_len;
 };
 
 struct ieee80211_if_mesh {
diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c
index 5d5a029228be..8dc2c2188d92 100644
--- a/net/mac80211/iface.c
+++ b/net/mac80211/iface.c
@@ -632,6 +632,13 @@ static void ieee80211_teardown_sdata(struct net_device *dev)
                 kfree(sdata->u.sta.assocreq_ies);
                 kfree(sdata->u.sta.assocresp_ies);
                 kfree_skb(sdata->u.sta.probe_resp);
+                kfree(sdata->u.sta.ie_probereq);
+                kfree(sdata->u.sta.ie_proberesp);
+                kfree(sdata->u.sta.ie_auth);
+                kfree(sdata->u.sta.ie_assocreq);
+                kfree(sdata->u.sta.ie_reassocreq);
+                kfree(sdata->u.sta.ie_deauth);
+                kfree(sdata->u.sta.ie_disassoc);
                 break;
         case NL80211_IFTYPE_WDS:
         case NL80211_IFTYPE_AP_VLAN:
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index f0d42498c257..43da6227b37c 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -131,6 +131,12 @@ u64 ieee80211_sta_get_rates(struct ieee80211_local *local,
 
 /* frame sending functions */
 
+static void add_extra_ies(struct sk_buff *skb, u8 *ies, size_t ies_len)
+{
+        if (ies)
+                memcpy(skb_put(skb, ies_len), ies, ies_len);
+}
+
 /* also used by scanning code */
 void ieee80211_send_probe_req(struct ieee80211_sub_if_data *sdata, u8 *dst,
                               u8 *ssid, size_t ssid_len)
@@ -142,7 +148,8 @@ void ieee80211_send_probe_req(struct ieee80211_sub_if_data *sdata, u8 *dst,
         u8 *pos, *supp_rates, *esupp_rates = NULL;
         int i;
 
-        skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*mgmt) + 200);
+        skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*mgmt) + 200 +
+                            sdata->u.sta.ie_probereq_len);
         if (!skb) {
                 printk(KERN_DEBUG "%s: failed to allocate buffer for probe "
                        "request\n", sdata->dev->name);
@@ -189,6 +196,9 @@ void ieee80211_send_probe_req(struct ieee80211_sub_if_data *sdata, u8 *dst,
                 *pos = rate->bitrate / 5;
         }
 
+        add_extra_ies(skb, sdata->u.sta.ie_probereq,
+                      sdata->u.sta.ie_probereq_len);
+
         ieee80211_tx_skb(sdata, skb, 0);
 }
 
@@ -202,7 +212,8 @@ static void ieee80211_send_auth(struct ieee80211_sub_if_data *sdata,
         struct ieee80211_mgmt *mgmt;
 
         skb = dev_alloc_skb(local->hw.extra_tx_headroom +
-                            sizeof(*mgmt) + 6 + extra_len);
+                            sizeof(*mgmt) + 6 + extra_len +
+                            sdata->u.sta.ie_auth_len);
         if (!skb) {
                 printk(KERN_DEBUG "%s: failed to allocate buffer for auth "
                        "frame\n", sdata->dev->name);
@@ -225,6 +236,7 @@ static void ieee80211_send_auth(struct ieee80211_sub_if_data *sdata,
         mgmt->u.auth.status_code = cpu_to_le16(0);
         if (extra)
                 memcpy(skb_put(skb, extra_len), extra, extra_len);
+        add_extra_ies(skb, sdata->u.sta.ie_auth, sdata->u.sta.ie_auth_len);
 
         ieee80211_tx_skb(sdata, skb, encrypt);
 }
@@ -235,17 +247,26 @@ static void ieee80211_send_assoc(struct ieee80211_sub_if_data *sdata,
         struct ieee80211_local *local = sdata->local;
         struct sk_buff *skb;
         struct ieee80211_mgmt *mgmt;
-        u8 *pos, *ies, *ht_ie;
+        u8 *pos, *ies, *ht_ie, *e_ies;
         int i, len, count, rates_len, supp_rates_len;
         u16 capab;
         struct ieee80211_bss *bss;
         int wmm = 0;
         struct ieee80211_supported_band *sband;
         u64 rates = 0;
+        size_t e_ies_len;
+
+        if (ifsta->flags & IEEE80211_STA_PREV_BSSID_SET) {
+                e_ies = sdata->u.sta.ie_reassocreq;
+                e_ies_len = sdata->u.sta.ie_reassocreq_len;
+        } else {
+                e_ies = sdata->u.sta.ie_assocreq;
+                e_ies_len = sdata->u.sta.ie_assocreq_len;
+        }
 
         skb = dev_alloc_skb(local->hw.extra_tx_headroom +
                             sizeof(*mgmt) + 200 + ifsta->extra_ie_len +
-                            ifsta->ssid_len);
+                            ifsta->ssid_len + e_ies_len);
         if (!skb) {
                 printk(KERN_DEBUG "%s: failed to allocate buffer for assoc "
                        "frame\n", sdata->dev->name);
@@ -436,6 +457,8 @@ static void ieee80211_send_assoc(struct ieee80211_sub_if_data *sdata,
                 memcpy(pos, &sband->ht_cap.mcs, sizeof(sband->ht_cap.mcs));
         }
 
+        add_extra_ies(skb, e_ies, e_ies_len);
+
         kfree(ifsta->assocreq_ies);
         ifsta->assocreq_ies_len = (skb->data + skb->len) - ies;
         ifsta->assocreq_ies = kmalloc(ifsta->assocreq_ies_len, GFP_KERNEL);
@@ -453,8 +476,19 @@ static void ieee80211_send_deauth_disassoc(struct ieee80211_sub_if_data *sdata,
         struct ieee80211_if_sta *ifsta = &sdata->u.sta;
         struct sk_buff *skb;
         struct ieee80211_mgmt *mgmt;
+        u8 *ies;
+        size_t ies_len;
 
-        skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*mgmt));
+        if (stype == IEEE80211_STYPE_DEAUTH) {
+                ies = sdata->u.sta.ie_deauth;
+                ies_len = sdata->u.sta.ie_deauth_len;
+        } else {
+                ies = sdata->u.sta.ie_disassoc;
+                ies_len = sdata->u.sta.ie_disassoc_len;
+        }
+
+        skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*mgmt) +
+                            ies_len);
         if (!skb) {
                 printk(KERN_DEBUG "%s: failed to allocate buffer for "
                        "deauth/disassoc frame\n", sdata->dev->name);
@@ -472,6 +506,8 @@ static void ieee80211_send_deauth_disassoc(struct ieee80211_sub_if_data *sdata,
         /* u.deauth.reason_code == u.disassoc.reason_code */
         mgmt->u.deauth.reason_code = cpu_to_le16(reason);
 
+        add_extra_ies(skb, ies, ies_len);
+
         ieee80211_tx_skb(sdata, skb, ifsta->flags & IEEE80211_STA_MFP_ENABLED);
 }
 
@@ -1473,7 +1509,8 @@ static int ieee80211_sta_join_ibss(struct ieee80211_sub_if_data *sdata,
         struct ieee80211_supported_band *sband;
         union iwreq_data wrqu;
 
-        skb = dev_alloc_skb(local->hw.extra_tx_headroom + 400);
+        skb = dev_alloc_skb(local->hw.extra_tx_headroom + 400 +
+                            sdata->u.sta.ie_proberesp_len);
         if (!skb) {
                 printk(KERN_DEBUG "%s: failed to allocate buffer for probe "
                        "response\n", sdata->dev->name);
@@ -1556,6 +1593,9 @@ static int ieee80211_sta_join_ibss(struct ieee80211_sub_if_data *sdata,
                 memcpy(pos, &bss->supp_rates[8], rates);
         }
 
+        add_extra_ies(skb, sdata->u.sta.ie_proberesp,
+                      sdata->u.sta.ie_proberesp_len);
+
         ifsta->probe_resp = skb;
 
         ieee80211_if_config(sdata, IEEE80211_IFCC_BEACON);
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index 123d3b160fad..09a5d0f1d6dc 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -105,6 +105,10 @@ static struct nla_policy nl80211_policy[NL80211_ATTR_MAX+1] __read_mostly = {
 
         [NL80211_ATTR_HT_CAPABILITY] = { .type = NLA_BINARY,
                                          .len = NL80211_HT_CAPABILITY_LEN },
+
+        [NL80211_ATTR_MGMT_SUBTYPE] = { .type = NLA_U8 },
+        [NL80211_ATTR_IE] = { .type = NLA_BINARY,
+                              .len = IEEE80211_MAX_DATA_LEN },
 };
 
 /* message building helper */
@@ -2149,6 +2153,43 @@ static int nl80211_set_reg(struct sk_buff *skb, struct genl_info *info)
         return -EINVAL;
 }
 
+static int nl80211_set_mgmt_extra_ie(struct sk_buff *skb,
+                                     struct genl_info *info)
+{
+        struct cfg80211_registered_device *drv;
+        int err;
+        struct net_device *dev;
+        struct mgmt_extra_ie_params params;
+
+        memset(&params, 0, sizeof(params));
+
+        if (!info->attrs[NL80211_ATTR_MGMT_SUBTYPE])
+                return -EINVAL;
+        params.subtype = nla_get_u8(info->attrs[NL80211_ATTR_MGMT_SUBTYPE]);
+        if (params.subtype > 15)
+                return -EINVAL; /* FC Subtype field is 4 bits (0..15) */
+
+        if (info->attrs[NL80211_ATTR_IE]) {
+                params.ies = nla_data(info->attrs[NL80211_ATTR_IE]);
+                params.ies_len = nla_len(info->attrs[NL80211_ATTR_IE]);
+        }
+
+        err = get_drv_dev_by_info_ifindex(info->attrs, &drv, &dev);
+        if (err)
+                return err;
+
+        if (drv->ops->set_mgmt_extra_ie) {
+                rtnl_lock();
+                err = drv->ops->set_mgmt_extra_ie(&drv->wiphy, dev, &params);
+                rtnl_unlock();
+        } else
+                err = -EOPNOTSUPP;
+
+        cfg80211_put_dev(drv);
+        dev_put(dev);
+        return err;
+}
+
 static struct genl_ops nl80211_ops[] = {
         {
                 .cmd = NL80211_CMD_GET_WIPHY,
@@ -2310,6 +2351,12 @@ static struct genl_ops nl80211_ops[] = {
                 .policy = nl80211_policy,
                 .flags = GENL_ADMIN_PERM,
         },
+        {
+                .cmd = NL80211_CMD_SET_MGMT_EXTRA_IE,
+                .doit = nl80211_set_mgmt_extra_ie,
+                .policy = nl80211_policy,
+                .flags = GENL_ADMIN_PERM,
+        },
 };
 
 /* multicast groups */