1 files changed, 98 insertions, 0 deletions

diff --git a/net/mac80211/mesh.c b/net/mac80211/mesh.c
index 0a3ccaa275f9..6eb31d6fd8e1 100644
--- a/net/mac80211/mesh.c
+++ b/net/mac80211/mesh.c
@@ -12,6 +12,7 @@
 #include <asm/unaligned.h>
 #include "ieee80211_i.h"
 #include "mesh.h"
+#include "driver-ops.h"
 
 static int mesh_allocated;
 static struct kmem_cache *rm_cache;
@@ -610,6 +611,7 @@ ieee80211_mesh_build_beacon(struct ieee80211_if_mesh *ifmsh)
         struct sk_buff *skb;
         struct ieee80211_mgmt *mgmt;
         struct ieee80211_chanctx_conf *chanctx_conf;
+        struct mesh_csa_settings *csa;
         enum ieee80211_band band;
         u8 *pos;
         struct ieee80211_sub_if_data *sdata;
@@ -624,6 +626,10 @@ ieee80211_mesh_build_beacon(struct ieee80211_if_mesh *ifmsh)
 
         head_len = hdr_len +
                    2 + /* NULL SSID */
+                   /* Channel Switch Announcement */
+                   2 + sizeof(struct ieee80211_channel_sw_ie) +
+                   /* Mesh Channel Swith Parameters */
+                   2 + sizeof(struct ieee80211_mesh_chansw_params_ie) +
                    2 + 8 + /* supported rates */
                    2 + 3; /* DS params */
         tail_len = 2 + (IEEE80211_MAX_SUPP_RATES - 8) +
@@ -665,6 +671,38 @@ ieee80211_mesh_build_beacon(struct ieee80211_if_mesh *ifmsh)
         *pos++ = WLAN_EID_SSID;
         *pos++ = 0x0;
 
+        rcu_read_lock();
+        csa = rcu_dereference(ifmsh->csa);
+        if (csa) {
+                __le16 pre_value;
+
+                pos = skb_put(skb, 13);
+                memset(pos, 0, 13);
+                *pos++ = WLAN_EID_CHANNEL_SWITCH;
+                *pos++ = 3;
+                *pos++ = 0x0;
+                *pos++ = ieee80211_frequency_to_channel(
+                                csa->settings.chandef.chan->center_freq);
+                sdata->csa_counter_offset_beacon = hdr_len + 6;
+                *pos++ = csa->settings.count;
+                *pos++ = WLAN_EID_CHAN_SWITCH_PARAM;
+                *pos++ = 6;
+                if (ifmsh->chsw_init) {
+                        *pos++ = ifmsh->mshcfg.dot11MeshTTL;
+                        *pos |= WLAN_EID_CHAN_SWITCH_PARAM_INITIATOR;
+                } else {
+                        *pos++ = ifmsh->chsw_ttl;
+                }
+                *pos++ |= csa->settings.block_tx ?
+                          WLAN_EID_CHAN_SWITCH_PARAM_TX_RESTRICT : 0x00;
+                put_unaligned_le16(WLAN_REASON_MESH_CHAN, pos);
+                pos += 2;
+                pre_value = cpu_to_le16(ifmsh->pre_value);
+                memcpy(pos, &pre_value, 2);
+                pos += 2;
+        }
+        rcu_read_unlock();
+
         if (ieee80211_add_srates_ie(sdata, skb, true, band) ||
             mesh_add_ds_params_ie(sdata, skb))
                 goto out_free;
@@ -920,6 +958,65 @@ static void ieee80211_mesh_rx_bcn_presp(struct ieee80211_sub_if_data *sdata,
                         stype, mgmt, &elems, rx_status);
 }
 
+int ieee80211_mesh_finish_csa(struct ieee80211_sub_if_data *sdata)
+{
+        struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
+        struct mesh_csa_settings *tmp_csa_settings;
+        int ret = 0;
+
+        /* Reset the TTL value and Initiator flag */
+        ifmsh->chsw_init = false;
+        ifmsh->chsw_ttl = 0;
+
+        /* Remove the CSA and MCSP elements from the beacon */
+        tmp_csa_settings = rcu_dereference(ifmsh->csa);
+        rcu_assign_pointer(ifmsh->csa, NULL);
+        kfree_rcu(tmp_csa_settings, rcu_head);
+        ret = ieee80211_mesh_rebuild_beacon(sdata);
+        if (ret)
+                return -EINVAL;
+
+        ieee80211_bss_info_change_notify(sdata, BSS_CHANGED_BEACON);
+
+        mcsa_dbg(sdata, "complete switching to center freq %d MHz",
+                 sdata->vif.bss_conf.chandef.chan->center_freq);
+        return 0;
+}
+
+int ieee80211_mesh_csa_beacon(struct ieee80211_sub_if_data *sdata,
+                              struct cfg80211_csa_settings *csa_settings,
+                              bool csa_action)
+{
+        struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
+        struct mesh_csa_settings *tmp_csa_settings;
+        int ret = 0;
+
+        tmp_csa_settings = kmalloc(sizeof(*tmp_csa_settings),
+                                   GFP_ATOMIC);
+        if (!tmp_csa_settings)
+                return -ENOMEM;
+
+        memcpy(&tmp_csa_settings->settings, csa_settings,
+               sizeof(struct cfg80211_csa_settings));
+
+        rcu_assign_pointer(ifmsh->csa, tmp_csa_settings);
+
+        ret = ieee80211_mesh_rebuild_beacon(sdata);
+        if (ret) {
+                tmp_csa_settings = rcu_dereference(ifmsh->csa);
+                rcu_assign_pointer(ifmsh->csa, NULL);
+                kfree_rcu(tmp_csa_settings, rcu_head);
+                return ret;
+        }
+
+        ieee80211_bss_info_change_notify(sdata, BSS_CHANGED_BEACON);
+
+        if (csa_action)
+                ieee80211_send_action_csa(sdata, csa_settings);
+
+        return 0;
+}
+
 static int mesh_fwd_csa_frame(struct ieee80211_sub_if_data *sdata,
                                struct ieee80211_mgmt *mgmt, size_t len)
 {
@@ -942,6 +1039,7 @@ static int mesh_fwd_csa_frame(struct ieee80211_sub_if_data *sdata,
         offset_ttl = (len < 42) ? 7 : 10;
         *(pos + offset_ttl) -= 1;
         *(pos + offset_ttl + 1) &= ~WLAN_EID_CHAN_SWITCH_PARAM_INITIATOR;
+        sdata->u.mesh.chsw_ttl = *(pos + offset_ttl);
 
         memcpy(mgmt_fwd, mgmt, len);
         eth_broadcast_addr(mgmt_fwd->da);