mac80211: add channel switch command and beacon callbacks

The count field in CSA must be decremented with each beacon
transmitted. This patch implements the functionality for drivers
using ieee80211_beacon_get(). Other drivers must call back manually
after reaching count == 0.

This patch also contains the handling and finish worker for the channel
switch command, and mac80211/chanctx code to allow to change a channel
definition of an active channel context.

Signed-off-by: Simon Wunderlich <siwu@hrz.tu-chemnitz.de>
Signed-off-by: Mathias Kretschmer <mathias.kretschmer@fokus.fraunhofer.de>
[small cleanups, catch identical chandef]
Signed-off-by: Johannes Berg <johannes.berg@intel.com>

1 files changed, 185 insertions, 2 deletions

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index b82fff6c0b30..44449ceb7966 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -860,8 +860,8 @@ static int ieee80211_set_probe_resp(struct ieee80211_sub_if_data *sdata,
         return 0;
 }
 
-static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
-                                   struct cfg80211_beacon_data *params)
+int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
+                            struct cfg80211_beacon_data *params)
 {
         struct beacon_data *new, *old;
         int new_head_len, new_tail_len;
@@ -1024,6 +1024,12 @@ static int ieee80211_change_beacon(struct wiphy *wiphy, struct net_device *dev,
 
         sdata = IEEE80211_DEV_TO_SUB_IF(dev);
 
+        /* don't allow changing the beacon while CSA is in place - offset
+         * of channel switch counter may change
+         */
+        if (sdata->vif.csa_active)
+                return -EBUSY;
+
         old = rtnl_dereference(sdata->u.ap.beacon);
         if (!old)
                 return -ENOENT;
@@ -1048,6 +1054,10 @@ static int ieee80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
                 return -ENOENT;
         old_probe_resp = rtnl_dereference(sdata->u.ap.probe_resp);
 
+        /* abort any running channel switch */
+        sdata->vif.csa_active = false;
+        cancel_work_sync(&sdata->csa_finalize_work);
+
         /* turn off carrier for this interface and dependent VLANs */
         list_for_each_entry(vlan, &sdata->u.ap.vlans, u.vlan.list)
                 netif_carrier_off(vlan->dev);
@@ -2775,6 +2785,178 @@ static int ieee80211_start_radar_detection(struct wiphy *wiphy,
         return 0;
 }
 
+static struct cfg80211_beacon_data *
+cfg80211_beacon_dup(struct cfg80211_beacon_data *beacon)
+{
+        struct cfg80211_beacon_data *new_beacon;
+        u8 *pos;
+        int len;
+
+        len = beacon->head_len + beacon->tail_len + beacon->beacon_ies_len +
+              beacon->proberesp_ies_len + beacon->assocresp_ies_len +
+              beacon->probe_resp_len;
+
+        new_beacon = kzalloc(sizeof(*new_beacon) + len, GFP_KERNEL);
+        if (!new_beacon)
+                return NULL;
+
+        pos = (u8 *)(new_beacon + 1);
+        if (beacon->head_len) {
+                new_beacon->head_len = beacon->head_len;
+                new_beacon->head = pos;
+                memcpy(pos, beacon->head, beacon->head_len);
+                pos += beacon->head_len;
+        }
+        if (beacon->tail_len) {
+                new_beacon->tail_len = beacon->tail_len;
+                new_beacon->tail = pos;
+                memcpy(pos, beacon->tail, beacon->tail_len);
+                pos += beacon->tail_len;
+        }
+        if (beacon->beacon_ies_len) {
+                new_beacon->beacon_ies_len = beacon->beacon_ies_len;
+                new_beacon->beacon_ies = pos;
+                memcpy(pos, beacon->beacon_ies, beacon->beacon_ies_len);
+                pos += beacon->beacon_ies_len;
+        }
+        if (beacon->proberesp_ies_len) {
+                new_beacon->proberesp_ies_len = beacon->proberesp_ies_len;
+                new_beacon->proberesp_ies = pos;
+                memcpy(pos, beacon->proberesp_ies, beacon->proberesp_ies_len);
+                pos += beacon->proberesp_ies_len;
+        }
+        if (beacon->assocresp_ies_len) {
+                new_beacon->assocresp_ies_len = beacon->assocresp_ies_len;
+                new_beacon->assocresp_ies = pos;
+                memcpy(pos, beacon->assocresp_ies, beacon->assocresp_ies_len);
+                pos += beacon->assocresp_ies_len;
+        }
+        if (beacon->probe_resp_len) {
+                new_beacon->probe_resp_len = beacon->probe_resp_len;
+                beacon->probe_resp = pos;
+                memcpy(pos, beacon->probe_resp, beacon->probe_resp_len);
+                pos += beacon->probe_resp_len;
+        }
+
+        return new_beacon;
+}
+
+void ieee80211_csa_finalize_work(struct work_struct *work)
+{
+        struct ieee80211_sub_if_data *sdata =
+                container_of(work, struct ieee80211_sub_if_data,
+                             csa_finalize_work);
+        struct ieee80211_local *local = sdata->local;
+        int err, changed;
+
+        if (!ieee80211_sdata_running(sdata))
+                return;
+
+        if (WARN_ON(sdata->vif.type != NL80211_IFTYPE_AP))
+                return;
+
+        sdata->radar_required = sdata->csa_radar_required;
+        err = ieee80211_vif_change_channel(sdata, &local->csa_chandef,
+                                           &changed);
+        if (WARN_ON(err < 0))
+                return;
+
+        err = ieee80211_assign_beacon(sdata, sdata->u.ap.next_beacon);
+        if (err < 0)
+                return;
+
+        changed |= err;
+        kfree(sdata->u.ap.next_beacon);
+        sdata->u.ap.next_beacon = NULL;
+        sdata->vif.csa_active = false;
+
+        ieee80211_wake_queues_by_reason(&sdata->local->hw,
+                                        IEEE80211_MAX_QUEUE_MAP,
+                                        IEEE80211_QUEUE_STOP_REASON_CSA);
+
+        ieee80211_bss_info_change_notify(sdata, changed);
+
+        cfg80211_ch_switch_notify(sdata->dev, &local->csa_chandef);
+}
+
+static int ieee80211_channel_switch(struct wiphy *wiphy, struct net_device *dev,
+                                    struct cfg80211_csa_settings *params)
+{
+        struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+        struct ieee80211_local *local = sdata->local;
+        struct ieee80211_chanctx_conf *chanctx_conf;
+        struct ieee80211_chanctx *chanctx;
+        int err, num_chanctx;
+
+        if (!list_empty(&local->roc_list) || local->scanning)
+                return -EBUSY;
+
+        if (sdata->wdev.cac_started)
+                return -EBUSY;
+
+        if (cfg80211_chandef_identical(&params->chandef,
+                                       &sdata->vif.bss_conf.chandef))
+                return -EINVAL;
+
+        rcu_read_lock();
+        chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
+        if (!chanctx_conf) {
+                rcu_read_unlock();
+                return -EBUSY;
+        }
+
+        /* don't handle for multi-VIF cases */
+        chanctx = container_of(chanctx_conf, struct ieee80211_chanctx, conf);
+        if (chanctx->refcount > 1) {
+                rcu_read_unlock();
+                return -EBUSY;
+        }
+        num_chanctx = 0;
+        list_for_each_entry_rcu(chanctx, &local->chanctx_list, list)
+                num_chanctx++;
+        rcu_read_unlock();
+
+        if (num_chanctx > 1)
+                return -EBUSY;
+
+        /* don't allow another channel switch if one is already active. */
+        if (sdata->vif.csa_active)
+                return -EBUSY;
+
+        /* only handle AP for now. */
+        switch (sdata->vif.type) {
+        case NL80211_IFTYPE_AP:
+                break;
+        default:
+                return -EOPNOTSUPP;
+        }
+
+        sdata->u.ap.next_beacon = cfg80211_beacon_dup(&params->beacon_after);
+        if (!sdata->u.ap.next_beacon)
+                return -ENOMEM;
+
+        sdata->csa_counter_offset_beacon = params->counter_offset_beacon;
+        sdata->csa_counter_offset_presp = params->counter_offset_presp;
+        sdata->csa_radar_required = params->radar_required;
+
+        if (params->block_tx)
+                ieee80211_stop_queues_by_reason(&local->hw,
+                                IEEE80211_MAX_QUEUE_MAP,
+                                IEEE80211_QUEUE_STOP_REASON_CSA);
+
+        err = ieee80211_assign_beacon(sdata, &params->beacon_csa);
+        if (err < 0)
+                return err;
+
+        local->csa_chandef = params->chandef;
+        sdata->vif.csa_active = true;
+
+        ieee80211_bss_info_change_notify(sdata, err);
+        drv_channel_switch_beacon(sdata, &params->chandef);
+
+        return 0;
+}
+
 static int ieee80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
                              struct ieee80211_channel *chan, bool offchan,
                              unsigned int wait, const u8 *buf, size_t len,
@@ -3492,4 +3674,5 @@ struct cfg80211_ops mac80211_config_ops = {
         .get_et_strings = ieee80211_get_et_strings,
         .get_channel = ieee80211_cfg_get_channel,
         .start_radar_detection = ieee80211_start_radar_detection,
+        .channel_switch = ieee80211_channel_switch,
 };