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>

7 files changed, 246 insertions, 6 deletions

diff --git a/include/linux/nl80211.h b/include/linux/nl80211.h
index 4e7a7986a521..76aae3d8e97e 100644
--- a/include/linux/nl80211.h
+++ b/include/linux/nl80211.h
@@ -133,6 +133,14 @@
  * @NL80211_CMD_SET_MESH_PARAMS: Set mesh networking properties for the
  *      interface identified by %NL80211_ATTR_IFINDEX
  *
+ * @NL80211_CMD_SET_MGMT_EXTRA_IE: Set extra IEs for management frames. The
+ *      interface is identified with %NL80211_ATTR_IFINDEX and the management
+ *      frame subtype with %NL80211_ATTR_MGMT_SUBTYPE. The extra IE data to be
+ *      added to the end of the specified management frame is specified with
+ *      %NL80211_ATTR_IE. If the command succeeds, the requested data will be
+ *      added to all specified management frames generated by
+ *      kernel/firmware/driver.
+ *
  * @NL80211_CMD_MAX: highest used command number
  * @__NL80211_CMD_AFTER_LAST: internal use
  */
@@ -178,6 +186,8 @@ enum nl80211_commands {
         NL80211_CMD_GET_MESH_PARAMS,
         NL80211_CMD_SET_MESH_PARAMS,
 
+        NL80211_CMD_SET_MGMT_EXTRA_IE,
+
         /* add new commands above here */
 
         /* used to define NL80211_CMD_MAX below */
@@ -190,6 +200,7 @@ enum nl80211_commands {
  * here
  */
 #define NL80211_CMD_SET_BSS NL80211_CMD_SET_BSS
+#define NL80211_CMD_SET_MGMT_EXTRA_IE NL80211_CMD_SET_MGMT_EXTRA_IE
 
 /**
  * enum nl80211_attrs - nl80211 netlink attributes
@@ -284,6 +295,12 @@ enum nl80211_commands {
  *      supported interface types, each a flag attribute with the number
  *      of the interface mode.
  *
+ * @NL80211_ATTR_MGMT_SUBTYPE: Management frame subtype for
+ *      %NL80211_CMD_SET_MGMT_EXTRA_IE.
+ *
+ * @NL80211_ATTR_IE: Information element(s) data (used, e.g., with
+ *      %NL80211_CMD_SET_MGMT_EXTRA_IE).
+ *
  * @NL80211_ATTR_MAX: highest attribute number currently defined
  * @__NL80211_ATTR_AFTER_LAST: internal use
  */
@@ -348,6 +365,9 @@ enum nl80211_attrs {
 
         NL80211_ATTR_KEY_DEFAULT_MGMT,
 
+        NL80211_ATTR_MGMT_SUBTYPE,
+        NL80211_ATTR_IE,
+
         /* add attributes here, update the policy in nl80211.c */
 
         __NL80211_ATTR_AFTER_LAST,
@@ -363,6 +383,8 @@ enum nl80211_attrs {
 #define NL80211_ATTR_WIPHY_TXQ_PARAMS NL80211_ATTR_WIPHY_TXQ_PARAMS
 #define NL80211_ATTR_WIPHY_FREQ NL80211_ATTR_WIPHY_FREQ
 #define NL80211_ATTR_WIPHY_CHANNEL_TYPE NL80211_ATTR_WIPHY_CHANNEL_TYPE
+#define NL80211_ATTR_MGMT_SUBTYPE NL80211_ATTR_MGMT_SUBTYPE
+#define NL80211_ATTR_IE NL80211_ATTR_IE
 
 #define NL80211_MAX_SUPP_RATES                  32
 #define NL80211_MAX_SUPP_REG_RULES              32
diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index df78abc496f1..c7da88fb15b7 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -433,6 +433,26 @@ struct ieee80211_txq_params {
         u8 aifs;
 };
 
+/**
+ * struct mgmt_extra_ie_params - Extra management frame IE parameters
+ *
+ * Used to add extra IE(s) into management frames. If the driver cannot add the
+ * requested data into all management frames of the specified subtype that are
+ * generated in kernel or firmware/hardware, it must reject the configuration
+ * call. The IE data buffer is added to the end of the specified management
+ * frame body after all other IEs. This addition is not applied to frames that
+ * are injected through a monitor interface.
+ *
+ * @subtype: Management frame subtype
+ * @ies: IE data buffer or %NULL to remove previous data
+ * @ies_len: Length of @ies in octets
+ */
+struct mgmt_extra_ie_params {
+        u8 subtype;
+        u8 *ies;
+        int ies_len;
+};
+
 /* from net/wireless.h */
 struct wiphy;
 
@@ -501,6 +521,8 @@ struct ieee80211_channel;
  * @set_txq_params: Set TX queue parameters
  *
  * @set_channel: Set channel
+ *
+ * @set_mgmt_extra_ie: Set extra IE data for management frames
  */
 struct cfg80211_ops {
         int     (*add_virtual_intf)(struct wiphy *wiphy, char *name,
@@ -571,6 +593,10 @@ struct cfg80211_ops {
         int     (*set_channel)(struct wiphy *wiphy,
                                struct ieee80211_channel *chan,
                                enum nl80211_channel_type channel_type);
+
+        int     (*set_mgmt_extra_ie)(struct wiphy *wiphy,
+                                     struct net_device *dev,
+                                     struct mgmt_extra_ie_params *params);
 };
 
 /* temporary wext handlers */
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 */