5 files changed, 446 insertions, 1 deletions

diff --git a/net/wireless/core.c b/net/wireless/core.c
index 71b6b3a9cf1f..51908dc2ea00 100644
--- a/net/wireless/core.c
+++ b/net/wireless/core.c
@@ -677,6 +677,9 @@ static int cfg80211_netdev_notifier_call(struct notifier_block * nb,
                 INIT_WORK(&wdev->cleanup_work, wdev_cleanup_work);
                 INIT_LIST_HEAD(&wdev->event_list);
                 spin_lock_init(&wdev->event_lock);
+                INIT_LIST_HEAD(&wdev->action_registrations);
+                spin_lock_init(&wdev->action_registrations_lock);
+
                 mutex_lock(&rdev->devlist_mtx);
                 list_add_rcu(&wdev->list, &rdev->netdev_list);
                 rdev->devlist_generation++;
@@ -792,6 +795,7 @@ static int cfg80211_netdev_notifier_call(struct notifier_block * nb,
                         sysfs_remove_link(&dev->dev.kobj, "phy80211");
                         list_del_rcu(&wdev->list);
                         rdev->devlist_generation++;
+                        cfg80211_mlme_purge_actions(wdev);
 #ifdef CONFIG_CFG80211_WEXT
                         kfree(wdev->wext.keys);
 #endif
diff --git a/net/wireless/core.h b/net/wireless/core.h
index c326a667022a..d52da913145a 100644
--- a/net/wireless/core.h
+++ b/net/wireless/core.h
@@ -329,6 +329,15 @@ void __cfg80211_connect_result(struct net_device *dev, const u8 *bssid,
                                const u8 *resp_ie, size_t resp_ie_len,
                                u16 status, bool wextev,
                                struct cfg80211_bss *bss);
+int cfg80211_mlme_register_action(struct wireless_dev *wdev, u32 snd_pid,
+                                  const u8 *match_data, int match_len);
+void cfg80211_mlme_unregister_actions(struct wireless_dev *wdev, u32 nlpid);
+void cfg80211_mlme_purge_actions(struct wireless_dev *wdev);
+int cfg80211_mlme_action(struct cfg80211_registered_device *rdev,
+                         struct net_device *dev,
+                         struct ieee80211_channel *chan,
+                         enum nl80211_channel_type channel_type,
+                         const u8 *buf, size_t len, u64 *cookie);
 
 /* SME */
 int __cfg80211_connect(struct cfg80211_registered_device *rdev,
diff --git a/net/wireless/mlme.c b/net/wireless/mlme.c
index 94d151f6f73e..62bc8855e123 100644
--- a/net/wireless/mlme.c
+++ b/net/wireless/mlme.c
@@ -728,3 +728,169 @@ void cfg80211_new_sta(struct net_device *dev, const u8 *mac_addr,
         nl80211_send_sta_event(rdev, dev, mac_addr, sinfo, gfp);
 }
 EXPORT_SYMBOL(cfg80211_new_sta);
+
+struct cfg80211_action_registration {
+        struct list_head list;
+
+        u32 nlpid;
+
+        int match_len;
+
+        u8 match[];
+};
+
+int cfg80211_mlme_register_action(struct wireless_dev *wdev, u32 snd_pid,
+                                  const u8 *match_data, int match_len)
+{
+        struct cfg80211_action_registration *reg, *nreg;
+        int err = 0;
+
+        nreg = kzalloc(sizeof(*reg) + match_len, GFP_KERNEL);
+        if (!nreg)
+                return -ENOMEM;
+
+        spin_lock_bh(&wdev->action_registrations_lock);
+
+        list_for_each_entry(reg, &wdev->action_registrations, list) {
+                int mlen = min(match_len, reg->match_len);
+
+                if (memcmp(reg->match, match_data, mlen) == 0) {
+                        err = -EALREADY;
+                        break;
+                }
+        }
+
+        if (err) {
+                kfree(nreg);
+                goto out;
+        }
+
+        memcpy(nreg->match, match_data, match_len);
+        nreg->match_len = match_len;
+        nreg->nlpid = snd_pid;
+        list_add(&nreg->list, &wdev->action_registrations);
+
+ out:
+        spin_unlock_bh(&wdev->action_registrations_lock);
+        return err;
+}
+
+void cfg80211_mlme_unregister_actions(struct wireless_dev *wdev, u32 nlpid)
+{
+        struct cfg80211_action_registration *reg, *tmp;
+
+        spin_lock_bh(&wdev->action_registrations_lock);
+
+        list_for_each_entry_safe(reg, tmp, &wdev->action_registrations, list) {
+                if (reg->nlpid == nlpid) {
+                        list_del(&reg->list);
+                        kfree(reg);
+                }
+        }
+
+        spin_unlock_bh(&wdev->action_registrations_lock);
+}
+
+void cfg80211_mlme_purge_actions(struct wireless_dev *wdev)
+{
+        struct cfg80211_action_registration *reg, *tmp;
+
+        spin_lock_bh(&wdev->action_registrations_lock);
+
+        list_for_each_entry_safe(reg, tmp, &wdev->action_registrations, list) {
+                list_del(&reg->list);
+                kfree(reg);
+        }
+
+        spin_unlock_bh(&wdev->action_registrations_lock);
+}
+
+int cfg80211_mlme_action(struct cfg80211_registered_device *rdev,
+                         struct net_device *dev,
+                         struct ieee80211_channel *chan,
+                         enum nl80211_channel_type channel_type,
+                         const u8 *buf, size_t len, u64 *cookie)
+{
+        struct wireless_dev *wdev = dev->ieee80211_ptr;
+        const struct ieee80211_mgmt *mgmt;
+
+        if (rdev->ops->action == NULL)
+                return -EOPNOTSUPP;
+        if (len < 24 + 1)
+                return -EINVAL;
+
+        mgmt = (const struct ieee80211_mgmt *) buf;
+        if (!ieee80211_is_action(mgmt->frame_control))
+                return -EINVAL;
+        if (mgmt->u.action.category != WLAN_CATEGORY_PUBLIC) {
+                /* Verify that we are associated with the destination AP */
+                if (!wdev->current_bss ||
+                    memcmp(wdev->current_bss->pub.bssid, mgmt->bssid,
+                           ETH_ALEN) != 0 ||
+                    memcmp(wdev->current_bss->pub.bssid, mgmt->da,
+                           ETH_ALEN) != 0)
+                        return -ENOTCONN;
+        }
+
+        if (memcmp(mgmt->sa, dev->dev_addr, ETH_ALEN) != 0)
+                return -EINVAL;
+
+        /* Transmit the Action frame as requested by user space */
+        return rdev->ops->action(&rdev->wiphy, dev, chan, channel_type,
+                                 buf, len, cookie);
+}
+
+bool cfg80211_rx_action(struct net_device *dev, int freq, const u8 *buf,
+                        size_t len, gfp_t gfp)
+{
+        struct wireless_dev *wdev = dev->ieee80211_ptr;
+        struct wiphy *wiphy = wdev->wiphy;
+        struct cfg80211_registered_device *rdev = wiphy_to_dev(wiphy);
+        struct cfg80211_action_registration *reg;
+        const u8 *action_data;
+        int action_data_len;
+        bool result = false;
+
+        /* frame length - min size excluding category */
+        action_data_len = len - (IEEE80211_MIN_ACTION_SIZE - 1);
+
+        /* action data starts with category */
+        action_data = buf + IEEE80211_MIN_ACTION_SIZE - 1;
+
+        spin_lock_bh(&wdev->action_registrations_lock);
+
+        list_for_each_entry(reg, &wdev->action_registrations, list) {
+                if (reg->match_len > action_data_len)
+                        continue;
+
+                if (memcmp(reg->match, action_data, reg->match_len))
+                        continue;
+
+                /* found match! */
+
+                /* Indicate the received Action frame to user space */
+                if (nl80211_send_action(rdev, dev, reg->nlpid, freq,
+                                        buf, len, gfp))
+                        continue;
+
+                result = true;
+                break;
+        }
+
+        spin_unlock_bh(&wdev->action_registrations_lock);
+
+        return result;
+}
+EXPORT_SYMBOL(cfg80211_rx_action);
+
+void cfg80211_action_tx_status(struct net_device *dev, u64 cookie,
+                               const u8 *buf, size_t len, bool ack, gfp_t gfp)
+{
+        struct wireless_dev *wdev = dev->ieee80211_ptr;
+        struct wiphy *wiphy = wdev->wiphy;
+        struct cfg80211_registered_device *rdev = wiphy_to_dev(wiphy);
+
+        /* Indicate TX status of the Action frame to user space */
+        nl80211_send_action_tx_status(rdev, dev, cookie, buf, len, ack, gfp);
+}
+EXPORT_SYMBOL(cfg80211_action_tx_status);
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index a95ab9e4c19e..328112081358 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -1,7 +1,7 @@
 /*
  * This is the new netlink-based wireless configuration interface.
  *
- * Copyright 2006-2009  Johannes Berg <johannes@sipsolutions.net>
+ * Copyright 2006-2010  Johannes Berg <johannes@sipsolutions.net>
  */
 
 #include <linux/if.h>
@@ -145,6 +145,9 @@ static struct nla_policy nl80211_policy[NL80211_ATTR_MAX+1] __read_mostly = {
         [NL80211_ATTR_DURATION] = { .type = NLA_U32 },
         [NL80211_ATTR_COOKIE] = { .type = NLA_U64 },
         [NL80211_ATTR_TX_RATES] = { .type = NLA_NESTED },
+        [NL80211_ATTR_FRAME] = { .type = NLA_BINARY,
+                                 .len = IEEE80211_MAX_DATA_LEN },
+        [NL80211_ATTR_FRAME_MATCH] = { .type = NLA_BINARY, },
 };
 
 /* policy for the attributes */
@@ -577,6 +580,7 @@ static int nl80211_send_wiphy(struct sk_buff *msg, u32 pid, u32 seq, int flags,
         CMD(flush_pmksa, FLUSH_PMKSA);
         CMD(remain_on_channel, REMAIN_ON_CHANNEL);
         CMD(set_bitrate_mask, SET_TX_BITRATE_MASK);
+        CMD(action, ACTION);
         if (dev->wiphy.flags & WIPHY_FLAG_NETNS_OK) {
                 i++;
                 NLA_PUT_U32(msg, i, NL80211_CMD_SET_WIPHY_NETNS);
@@ -4526,6 +4530,139 @@ static int nl80211_set_tx_bitrate_mask(struct sk_buff *skb,
         return err;
 }
 
+static int nl80211_register_action(struct sk_buff *skb, struct genl_info *info)
+{
+        struct cfg80211_registered_device *rdev;
+        struct net_device *dev;
+        int err;
+
+        if (!info->attrs[NL80211_ATTR_FRAME_MATCH])
+                return -EINVAL;
+
+        if (nla_len(info->attrs[NL80211_ATTR_FRAME_MATCH]) < 1)
+                return -EINVAL;
+
+        rtnl_lock();
+
+        err = get_rdev_dev_by_info_ifindex(info, &rdev, &dev);
+        if (err)
+                goto unlock_rtnl;
+
+        if (dev->ieee80211_ptr->iftype != NL80211_IFTYPE_STATION) {
+                err = -EOPNOTSUPP;
+                goto out;
+        }
+
+        /* not much point in registering if we can't reply */
+        if (!rdev->ops->action) {
+                err = -EOPNOTSUPP;
+                goto out;
+        }
+
+        err = cfg80211_mlme_register_action(dev->ieee80211_ptr, info->snd_pid,
+                        nla_data(info->attrs[NL80211_ATTR_FRAME_MATCH]),
+                        nla_len(info->attrs[NL80211_ATTR_FRAME_MATCH]));
+ out:
+        cfg80211_unlock_rdev(rdev);
+        dev_put(dev);
+ unlock_rtnl:
+        rtnl_unlock();
+        return err;
+}
+
+static int nl80211_action(struct sk_buff *skb, struct genl_info *info)
+{
+        struct cfg80211_registered_device *rdev;
+        struct net_device *dev;
+        struct ieee80211_channel *chan;
+        enum nl80211_channel_type channel_type = NL80211_CHAN_NO_HT;
+        u32 freq;
+        int err;
+        void *hdr;
+        u64 cookie;
+        struct sk_buff *msg;
+
+        if (!info->attrs[NL80211_ATTR_FRAME] ||
+            !info->attrs[NL80211_ATTR_WIPHY_FREQ])
+                return -EINVAL;
+
+        rtnl_lock();
+
+        err = get_rdev_dev_by_info_ifindex(info, &rdev, &dev);
+        if (err)
+                goto unlock_rtnl;
+
+        if (!rdev->ops->action) {
+                err = -EOPNOTSUPP;
+                goto out;
+        }
+
+        if (dev->ieee80211_ptr->iftype != NL80211_IFTYPE_STATION) {
+                err = -EOPNOTSUPP;
+                goto out;
+        }
+
+        if (!netif_running(dev)) {
+                err = -ENETDOWN;
+                goto out;
+        }
+
+        if (info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]) {
+                channel_type = nla_get_u32(
+                        info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]);
+                if (channel_type != NL80211_CHAN_NO_HT &&
+                    channel_type != NL80211_CHAN_HT20 &&
+                    channel_type != NL80211_CHAN_HT40PLUS &&
+                    channel_type != NL80211_CHAN_HT40MINUS)
+                        err = -EINVAL;
+                        goto out;
+        }
+
+        freq = nla_get_u32(info->attrs[NL80211_ATTR_WIPHY_FREQ]);
+        chan = rdev_freq_to_chan(rdev, freq, channel_type);
+        if (chan == NULL) {
+                err = -EINVAL;
+                goto out;
+        }
+
+        msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+        if (!msg) {
+                err = -ENOMEM;
+                goto out;
+        }
+
+        hdr = nl80211hdr_put(msg, info->snd_pid, info->snd_seq, 0,
+                             NL80211_CMD_ACTION);
+
+        if (IS_ERR(hdr)) {
+                err = PTR_ERR(hdr);
+                goto free_msg;
+        }
+        err = cfg80211_mlme_action(rdev, dev, chan, channel_type,
+                                   nla_data(info->attrs[NL80211_ATTR_FRAME]),
+                                   nla_len(info->attrs[NL80211_ATTR_FRAME]),
+                                   &cookie);
+        if (err)
+                goto free_msg;
+
+        NLA_PUT_U64(msg, NL80211_ATTR_COOKIE, cookie);
+
+        genlmsg_end(msg, hdr);
+        err = genlmsg_reply(msg, info);
+        goto out;
+
+ nla_put_failure:
+        err = -ENOBUFS;
+ free_msg:
+        nlmsg_free(msg);
+ out:
+        cfg80211_unlock_rdev(rdev);
+        dev_put(dev);
+unlock_rtnl:
+        rtnl_unlock();
+        return err;
+}
+
 static struct genl_ops nl80211_ops[] = {
         {
                 .cmd = NL80211_CMD_GET_WIPHY,
@@ -4806,6 +4943,18 @@ static struct genl_ops nl80211_ops[] = {
                 .policy = nl80211_policy,
                 .flags = GENL_ADMIN_PERM,
         },
+        {
+                .cmd = NL80211_CMD_REGISTER_ACTION,
+                .doit = nl80211_register_action,
+                .policy = nl80211_policy,
+                .flags = GENL_ADMIN_PERM,
+        },
+        {
+                .cmd = NL80211_CMD_ACTION,
+                .doit = nl80211_action,
+                .policy = nl80211_policy,
+                .flags = GENL_ADMIN_PERM,
+        },
 };
 
 static struct genl_multicast_group nl80211_mlme_mcgrp = {
@@ -5478,6 +5627,110 @@ void nl80211_send_sta_event(struct cfg80211_registered_device *rdev,
                                 nl80211_mlme_mcgrp.id, gfp);
 }
 
+int nl80211_send_action(struct cfg80211_registered_device *rdev,
+                        struct net_device *netdev, u32 nlpid,
+                        int freq, const u8 *buf, size_t len, gfp_t gfp)
+{
+        struct sk_buff *msg;
+        void *hdr;
+        int err;
+
+        msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp);
+        if (!msg)
+                return -ENOMEM;
+
+        hdr = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_ACTION);
+        if (!hdr) {
+                nlmsg_free(msg);
+                return -ENOMEM;
+        }
+
+        NLA_PUT_U32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx);
+        NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, netdev->ifindex);
+        NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, freq);
+        NLA_PUT(msg, NL80211_ATTR_FRAME, len, buf);
+
+        err = genlmsg_end(msg, hdr);
+        if (err < 0) {
+                nlmsg_free(msg);
+                return err;
+        }
+
+        err = genlmsg_unicast(wiphy_net(&rdev->wiphy), msg, nlpid);
+        if (err < 0)
+                return err;
+        return 0;
+
+ nla_put_failure:
+        genlmsg_cancel(msg, hdr);
+        nlmsg_free(msg);
+        return -ENOBUFS;
+}
+
+void nl80211_send_action_tx_status(struct cfg80211_registered_device *rdev,
+                                   struct net_device *netdev, u64 cookie,
+                                   const u8 *buf, size_t len, bool ack,
+                                   gfp_t gfp)
+{
+        struct sk_buff *msg;
+        void *hdr;
+
+        msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp);
+        if (!msg)
+                return;
+
+        hdr = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_ACTION_TX_STATUS);
+        if (!hdr) {
+                nlmsg_free(msg);
+                return;
+        }
+
+        NLA_PUT_U32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx);
+        NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, netdev->ifindex);
+        NLA_PUT(msg, NL80211_ATTR_FRAME, len, buf);
+        NLA_PUT_U64(msg, NL80211_ATTR_COOKIE, cookie);
+        if (ack)
+                NLA_PUT_FLAG(msg, NL80211_ATTR_ACK);
+
+        if (genlmsg_end(msg, hdr) < 0) {
+                nlmsg_free(msg);
+                return;
+        }
+
+        genlmsg_multicast(msg, 0, nl80211_mlme_mcgrp.id, gfp);
+        return;
+
+ nla_put_failure:
+        genlmsg_cancel(msg, hdr);
+        nlmsg_free(msg);
+}
+
+static int nl80211_netlink_notify(struct notifier_block * nb,
+                                  unsigned long state,
+                                  void *_notify)
+{
+        struct netlink_notify *notify = _notify;
+        struct cfg80211_registered_device *rdev;
+        struct wireless_dev *wdev;
+
+        if (state != NETLINK_URELEASE)
+                return NOTIFY_DONE;
+
+        rcu_read_lock();
+
+        list_for_each_entry_rcu(rdev, &cfg80211_rdev_list, list)
+                list_for_each_entry_rcu(wdev, &rdev->netdev_list, list)
+                        cfg80211_mlme_unregister_actions(wdev, notify->pid);
+
+        rcu_read_unlock();
+
+        return NOTIFY_DONE;
+}
+
+static struct notifier_block nl80211_netlink_notifier = {
+        .notifier_call = nl80211_netlink_notify,
+};
+
 /* initialisation/exit functions */
 
 int nl80211_init(void)
@@ -5511,6 +5764,10 @@ int nl80211_init(void)
                 goto err_out;
 #endif
 
+        err = netlink_register_notifier(&nl80211_netlink_notifier);
+        if (err)
+                goto err_out;
+
         return 0;
  err_out:
         genl_unregister_family(&nl80211_fam);
@@ -5519,5 +5776,6 @@ int nl80211_init(void)
 
 void nl80211_exit(void)
 {
+        netlink_unregister_notifier(&nl80211_netlink_notifier);
         genl_unregister_family(&nl80211_fam);
 }
diff --git a/net/wireless/nl80211.h b/net/wireless/nl80211.h
index 14855b8fb430..4ca511102c6c 100644
--- a/net/wireless/nl80211.h
+++ b/net/wireless/nl80211.h
@@ -74,4 +74,12 @@ void nl80211_send_sta_event(struct cfg80211_registered_device *rdev,
                             struct net_device *dev, const u8 *mac_addr,
                             struct station_info *sinfo, gfp_t gfp);
 
+int nl80211_send_action(struct cfg80211_registered_device *rdev,
+                        struct net_device *netdev, u32 nlpid, int freq,
+                        const u8 *buf, size_t len, gfp_t gfp);
+void nl80211_send_action_tx_status(struct cfg80211_registered_device *rdev,
+                                   struct net_device *netdev, u64 cookie,
+                                   const u8 *buf, size_t len, bool ack,
+                                   gfp_t gfp);
+
 #endif /* __NET_WIRELESS_NL80211_H */