net/mac80211/mesh.c

   1 /*
   2  * Copyright (c) 2008, 2009 open80211s Ltd.
   3  * Copyright (C) 2018 - 2019 Intel Corporation
   4  * Authors:    Luis Carlos Cobo <luisca@cozybit.com>
   5  *             Javier Cardona <javier@cozybit.com>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License version 2 as
   9  * published by the Free Software Foundation.
  10  */
  11
  12 #include <linux/slab.h>
  13 #include <asm/unaligned.h>
  14 #include "ieee80211_i.h"
  15 #include "mesh.h"
  16 #include "driver-ops.h"
  17
  18 static int mesh_allocated;
  19 static struct kmem_cache *rm_cache;
  20
  21 bool mesh_action_is_path_sel(struct ieee80211_mgmt *mgmt)
  22 {
  23         return (mgmt->u.action.u.mesh_action.action_code ==
  24                         WLAN_MESH_ACTION_HWMP_PATH_SELECTION);
  25 }
  26
  27 void ieee80211s_init(void)
  28 {
  29         mesh_allocated = 1;
  30         rm_cache = kmem_cache_create("mesh_rmc", sizeof(struct rmc_entry),
  31                                      0, 0, NULL);
  32 }
  33
  34 void ieee80211s_stop(void)
  35 {
  36         if (!mesh_allocated)
  37                 return;
  38         kmem_cache_destroy(rm_cache);
  39 }
  40
  41 static void ieee80211_mesh_housekeeping_timer(struct timer_list *t)
  42 {
  43         struct ieee80211_sub_if_data *sdata =
  44                 from_timer(sdata, t, u.mesh.housekeeping_timer);
  45         struct ieee80211_local *local = sdata->local;
  46         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
  47
  48         set_bit(MESH_WORK_HOUSEKEEPING, &ifmsh->wrkq_flags);
  49
  50         ieee80211_queue_work(&local->hw, &sdata->work);
  51 }
  52
  53 /**
  54  * mesh_matches_local - check if the config of a mesh point matches ours
  55  *
  56  * @sdata: local mesh subif
  57  * @ie: information elements of a management frame from the mesh peer
  58  *
  59  * This function checks if the mesh configuration of a mesh point matches the
  60  * local mesh configuration, i.e. if both nodes belong to the same mesh network.
  61  */
  62 bool mesh_matches_local(struct ieee80211_sub_if_data *sdata,
  63                         struct ieee802_11_elems *ie)
  64 {
  65         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
  66         u32 basic_rates = 0;
  67         struct cfg80211_chan_def sta_chan_def;
  68         struct ieee80211_supported_band *sband;
  69
  70         /*
  71          * As support for each feature is added, check for matching
  72          * - On mesh config capabilities
  73          *   - Power Save Support En
  74          *   - Sync support enabled
  75          *   - Sync support active
  76          *   - Sync support required from peer
  77          *   - MDA enabled
  78          * - Power management control on fc
  79          */
  80         if (!(ifmsh->mesh_id_len == ie->mesh_id_len &&
  81              memcmp(ifmsh->mesh_id, ie->mesh_id, ie->mesh_id_len) == 0 &&
  82              (ifmsh->mesh_pp_id == ie->mesh_config->meshconf_psel) &&
  83              (ifmsh->mesh_pm_id == ie->mesh_config->meshconf_pmetric) &&
  84              (ifmsh->mesh_cc_id == ie->mesh_config->meshconf_congest) &&
  85              (ifmsh->mesh_sp_id == ie->mesh_config->meshconf_synch) &&
  86              (ifmsh->mesh_auth_id == ie->mesh_config->meshconf_auth)))
  87                 return false;
  88
  89         sband = ieee80211_get_sband(sdata);
  90         if (!sband)
  91                 return false;
  92
  93         ieee80211_sta_get_rates(sdata, ie, sband->band,
  94                                 &basic_rates);
  95
  96         if (sdata->vif.bss_conf.basic_rates != basic_rates)
  97                 return false;
  98
  99         cfg80211_chandef_create(&sta_chan_def, sdata->vif.bss_conf.chandef.chan,
 100                                 NL80211_CHAN_NO_HT);
 101         ieee80211_chandef_ht_oper(ie->ht_operation, &sta_chan_def);
 102         ieee80211_chandef_vht_oper(&sdata->local->hw,
 103                                    ie->vht_operation, ie->ht_operation,
 104                                    &sta_chan_def);
 105
 106         if (!cfg80211_chandef_compatible(&sdata->vif.bss_conf.chandef,
 107                                          &sta_chan_def))
 108                 return false;
 109
 110         return true;
 111 }
 112
 113 /**
 114  * mesh_peer_accepts_plinks - check if an mp is willing to establish peer links
 115  *
 116  * @ie: information elements of a management frame from the mesh peer
 117  */
 118 bool mesh_peer_accepts_plinks(struct ieee802_11_elems *ie)
 119 {
 120         return (ie->mesh_config->meshconf_cap &
 121                         IEEE80211_MESHCONF_CAPAB_ACCEPT_PLINKS) != 0;
 122 }
 123
 124 /**
 125  * mesh_accept_plinks_update - update accepting_plink in local mesh beacons
 126  *
 127  * @sdata: mesh interface in which mesh beacons are going to be updated
 128  *
 129  * Returns: beacon changed flag if the beacon content changed.
 130  */
 131 u32 mesh_accept_plinks_update(struct ieee80211_sub_if_data *sdata)
 132 {
 133         bool free_plinks;
 134         u32 changed = 0;
 135
 136         /* In case mesh_plink_free_count > 0 and mesh_plinktbl_capacity == 0,
 137          * the mesh interface might be able to establish plinks with peers that
 138          * are already on the table but are not on PLINK_ESTAB state. However,
 139          * in general the mesh interface is not accepting peer link requests
 140          * from new peers, and that must be reflected in the beacon
 141          */
 142         free_plinks = mesh_plink_availables(sdata);
 143
 144         if (free_plinks != sdata->u.mesh.accepting_plinks) {
 145                 sdata->u.mesh.accepting_plinks = free_plinks;
 146                 changed = BSS_CHANGED_BEACON;
 147         }
 148
 149         return changed;
 150 }
 151
 152 /*
 153  * mesh_sta_cleanup - clean up any mesh sta state
 154  *
 155  * @sta: mesh sta to clean up.
 156  */
 157 void mesh_sta_cleanup(struct sta_info *sta)
 158 {
 159         struct ieee80211_sub_if_data *sdata = sta->sdata;
 160         u32 changed = mesh_plink_deactivate(sta);
 161
 162         if (changed)
 163                 ieee80211_mbss_info_change_notify(sdata, changed);
 164 }
 165
 166 int mesh_rmc_init(struct ieee80211_sub_if_data *sdata)
 167 {
 168         int i;
 169
 170         sdata->u.mesh.rmc = kmalloc(sizeof(struct mesh_rmc), GFP_KERNEL);
 171         if (!sdata->u.mesh.rmc)
 172                 return -ENOMEM;
 173         sdata->u.mesh.rmc->idx_mask = RMC_BUCKETS - 1;
 174         for (i = 0; i < RMC_BUCKETS; i++)
 175                 INIT_HLIST_HEAD(&sdata->u.mesh.rmc->bucket[i]);
 176         return 0;
 177 }
 178
 179 void mesh_rmc_free(struct ieee80211_sub_if_data *sdata)
 180 {
 181         struct mesh_rmc *rmc = sdata->u.mesh.rmc;
 182         struct rmc_entry *p;
 183         struct hlist_node *n;
 184         int i;
 185
 186         if (!sdata->u.mesh.rmc)
 187                 return;
 188
 189         for (i = 0; i < RMC_BUCKETS; i++) {
 190                 hlist_for_each_entry_safe(p, n, &rmc->bucket[i], list) {
 191                         hlist_del(&p->list);
 192                         kmem_cache_free(rm_cache, p);
 193                 }
 194         }
 195
 196         kfree(rmc);
 197         sdata->u.mesh.rmc = NULL;
 198 }
 199
 200 /**
 201  * mesh_rmc_check - Check frame in recent multicast cache and add if absent.
 202  *
 203  * @sdata:      interface
 204  * @sa:         source address
 205  * @mesh_hdr:   mesh_header
 206  *
 207  * Returns: 0 if the frame is not in the cache, nonzero otherwise.
 208  *
 209  * Checks using the source address and the mesh sequence number if we have
 210  * received this frame lately. If the frame is not in the cache, it is added to
 211  * it.
 212  */
 213 int mesh_rmc_check(struct ieee80211_sub_if_data *sdata,
 214                    const u8 *sa, struct ieee80211s_hdr *mesh_hdr)
 215 {
 216         struct mesh_rmc *rmc = sdata->u.mesh.rmc;
 217         u32 seqnum = 0;
 218         int entries = 0;
 219         u8 idx;
 220         struct rmc_entry *p;
 221         struct hlist_node *n;
 222
 223         if (!rmc)
 224                 return -1;
 225
 226         /* Don't care about endianness since only match matters */
 227         memcpy(&seqnum, &mesh_hdr->seqnum, sizeof(mesh_hdr->seqnum));
 228         idx = le32_to_cpu(mesh_hdr->seqnum) & rmc->idx_mask;
 229         hlist_for_each_entry_safe(p, n, &rmc->bucket[idx], list) {
 230                 ++entries;
 231                 if (time_after(jiffies, p->exp_time) ||
 232                     entries == RMC_QUEUE_MAX_LEN) {
 233                         hlist_del(&p->list);
 234                         kmem_cache_free(rm_cache, p);
 235                         --entries;
 236                 } else if ((seqnum == p->seqnum) && ether_addr_equal(sa, p->sa))
 237                         return -1;
 238         }
 239
 240         p = kmem_cache_alloc(rm_cache, GFP_ATOMIC);
 241         if (!p)
 242                 return 0;
 243
 244         p->seqnum = seqnum;
 245         p->exp_time = jiffies + RMC_TIMEOUT;
 246         memcpy(p->sa, sa, ETH_ALEN);
 247         hlist_add_head(&p->list, &rmc->bucket[idx]);
 248         return 0;
 249 }
 250
 251 int mesh_add_meshconf_ie(struct ieee80211_sub_if_data *sdata,
 252                          struct sk_buff *skb)
 253 {
 254         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 255         u8 *pos, neighbors;
 256         u8 meshconf_len = sizeof(struct ieee80211_meshconf_ie);
 257         bool is_connected_to_gate = ifmsh->num_gates > 0 ||
 258                 ifmsh->mshcfg.dot11MeshGateAnnouncementProtocol ||
 259                 ifmsh->mshcfg.dot11MeshConnectedToMeshGate;
 260
 261         if (skb_tailroom(skb) < 2 + meshconf_len)
 262                 return -ENOMEM;
 263
 264         pos = skb_put(skb, 2 + meshconf_len);
 265         *pos++ = WLAN_EID_MESH_CONFIG;
 266         *pos++ = meshconf_len;
 267
 268         /* save a pointer for quick updates in pre-tbtt */
 269         ifmsh->meshconf_offset = pos - skb->data;
 270
 271         /* Active path selection protocol ID */
 272         *pos++ = ifmsh->mesh_pp_id;
 273         /* Active path selection metric ID   */
 274         *pos++ = ifmsh->mesh_pm_id;
 275         /* Congestion control mode identifier */
 276         *pos++ = ifmsh->mesh_cc_id;
 277         /* Synchronization protocol identifier */
 278         *pos++ = ifmsh->mesh_sp_id;
 279         /* Authentication Protocol identifier */
 280         *pos++ = ifmsh->mesh_auth_id;
 281         /* Mesh Formation Info - number of neighbors */
 282         neighbors = atomic_read(&ifmsh->estab_plinks);
 283         neighbors = min_t(int, neighbors, IEEE80211_MAX_MESH_PEERINGS);
 284         *pos++ = (neighbors << 1) | is_connected_to_gate;
 285         /* Mesh capability */
 286         *pos = 0x00;
 287         *pos |= ifmsh->mshcfg.dot11MeshForwarding ?
 288                         IEEE80211_MESHCONF_CAPAB_FORWARDING : 0x00;
 289         *pos |= ifmsh->accepting_plinks ?
 290                         IEEE80211_MESHCONF_CAPAB_ACCEPT_PLINKS : 0x00;
 291         /* Mesh PS mode. See IEEE802.11-2012 8.4.2.100.8 */
 292         *pos |= ifmsh->ps_peers_deep_sleep ?
 293                         IEEE80211_MESHCONF_CAPAB_POWER_SAVE_LEVEL : 0x00;
 294         return 0;
 295 }
 296
 297 int mesh_add_meshid_ie(struct ieee80211_sub_if_data *sdata, struct sk_buff *skb)
 298 {
 299         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 300         u8 *pos;
 301
 302         if (skb_tailroom(skb) < 2 + ifmsh->mesh_id_len)
 303                 return -ENOMEM;
 304
 305         pos = skb_put(skb, 2 + ifmsh->mesh_id_len);
 306         *pos++ = WLAN_EID_MESH_ID;
 307         *pos++ = ifmsh->mesh_id_len;
 308         if (ifmsh->mesh_id_len)
 309                 memcpy(pos, ifmsh->mesh_id, ifmsh->mesh_id_len);
 310
 311         return 0;
 312 }
 313
 314 static int mesh_add_awake_window_ie(struct ieee80211_sub_if_data *sdata,
 315                                     struct sk_buff *skb)
 316 {
 317         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 318         u8 *pos;
 319
 320         /* see IEEE802.11-2012 13.14.6 */
 321         if (ifmsh->ps_peers_light_sleep == 0 &&
 322             ifmsh->ps_peers_deep_sleep == 0 &&
 323             ifmsh->nonpeer_pm == NL80211_MESH_POWER_ACTIVE)
 324                 return 0;
 325
 326         if (skb_tailroom(skb) < 4)
 327                 return -ENOMEM;
 328
 329         pos = skb_put(skb, 2 + 2);
 330         *pos++ = WLAN_EID_MESH_AWAKE_WINDOW;
 331         *pos++ = 2;
 332         put_unaligned_le16(ifmsh->mshcfg.dot11MeshAwakeWindowDuration, pos);
 333
 334         return 0;
 335 }
 336
 337 int mesh_add_vendor_ies(struct ieee80211_sub_if_data *sdata,
 338                         struct sk_buff *skb)
 339 {
 340         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 341         u8 offset, len;
 342         const u8 *data;
 343
 344         if (!ifmsh->ie || !ifmsh->ie_len)
 345                 return 0;
 346
 347         /* fast-forward to vendor IEs */
 348         offset = ieee80211_ie_split_vendor(ifmsh->ie, ifmsh->ie_len, 0);
 349
 350         if (offset < ifmsh->ie_len) {
 351                 len = ifmsh->ie_len - offset;
 352                 data = ifmsh->ie + offset;
 353                 if (skb_tailroom(skb) < len)
 354                         return -ENOMEM;
 355                 skb_put_data(skb, data, len);
 356         }
 357
 358         return 0;
 359 }
 360
 361 int mesh_add_rsn_ie(struct ieee80211_sub_if_data *sdata, struct sk_buff *skb)
 362 {
 363         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 364         u8 len = 0;
 365         const u8 *data;
 366
 367         if (!ifmsh->ie || !ifmsh->ie_len)
 368                 return 0;
 369
 370         /* find RSN IE */
 371         data = cfg80211_find_ie(WLAN_EID_RSN, ifmsh->ie, ifmsh->ie_len);
 372         if (!data)
 373                 return 0;
 374
 375         len = data[1] + 2;
 376
 377         if (skb_tailroom(skb) < len)
 378                 return -ENOMEM;
 379         skb_put_data(skb, data, len);
 380
 381         return 0;
 382 }
 383
 384 static int mesh_add_ds_params_ie(struct ieee80211_sub_if_data *sdata,
 385                                  struct sk_buff *skb)
 386 {
 387         struct ieee80211_chanctx_conf *chanctx_conf;
 388         struct ieee80211_channel *chan;
 389         u8 *pos;
 390
 391         if (skb_tailroom(skb) < 3)
 392                 return -ENOMEM;
 393
 394         rcu_read_lock();
 395         chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
 396         if (WARN_ON(!chanctx_conf)) {
 397                 rcu_read_unlock();
 398                 return -EINVAL;
 399         }
 400         chan = chanctx_conf->def.chan;
 401         rcu_read_unlock();
 402
 403         pos = skb_put(skb, 2 + 1);
 404         *pos++ = WLAN_EID_DS_PARAMS;
 405         *pos++ = 1;
 406         *pos++ = ieee80211_frequency_to_channel(chan->center_freq);
 407
 408         return 0;
 409 }
 410
 411 int mesh_add_ht_cap_ie(struct ieee80211_sub_if_data *sdata,
 412                        struct sk_buff *skb)
 413 {
 414         struct ieee80211_supported_band *sband;
 415         u8 *pos;
 416
 417         sband = ieee80211_get_sband(sdata);
 418         if (!sband)
 419                 return -EINVAL;
 420
 421         if (!sband->ht_cap.ht_supported ||
 422             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_20_NOHT ||
 423             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_5 ||
 424             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_10)
 425                 return 0;
 426
 427         if (skb_tailroom(skb) < 2 + sizeof(struct ieee80211_ht_cap))
 428                 return -ENOMEM;
 429
 430         pos = skb_put(skb, 2 + sizeof(struct ieee80211_ht_cap));
 431         ieee80211_ie_build_ht_cap(pos, &sband->ht_cap, sband->ht_cap.cap);
 432
 433         return 0;
 434 }
 435
 436 int mesh_add_ht_oper_ie(struct ieee80211_sub_if_data *sdata,
 437                         struct sk_buff *skb)
 438 {
 439         struct ieee80211_local *local = sdata->local;
 440         struct ieee80211_chanctx_conf *chanctx_conf;
 441         struct ieee80211_channel *channel;
 442         struct ieee80211_supported_band *sband;
 443         struct ieee80211_sta_ht_cap *ht_cap;
 444         u8 *pos;
 445
 446         rcu_read_lock();
 447         chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
 448         if (WARN_ON(!chanctx_conf)) {
 449                 rcu_read_unlock();
 450                 return -EINVAL;
 451         }
 452         channel = chanctx_conf->def.chan;
 453         rcu_read_unlock();
 454
 455         sband = local->hw.wiphy->bands[channel->band];
 456         ht_cap = &sband->ht_cap;
 457
 458         if (!ht_cap->ht_supported ||
 459             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_20_NOHT ||
 460             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_5 ||
 461             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_10)
 462                 return 0;
 463
 464         if (skb_tailroom(skb) < 2 + sizeof(struct ieee80211_ht_operation))
 465                 return -ENOMEM;
 466
 467         pos = skb_put(skb, 2 + sizeof(struct ieee80211_ht_operation));
 468         ieee80211_ie_build_ht_oper(pos, ht_cap, &sdata->vif.bss_conf.chandef,
 469                                    sdata->vif.bss_conf.ht_operation_mode,
 470                                    false);
 471
 472         return 0;
 473 }
 474
 475 int mesh_add_vht_cap_ie(struct ieee80211_sub_if_data *sdata,
 476                         struct sk_buff *skb)
 477 {
 478         struct ieee80211_supported_band *sband;
 479         u8 *pos;
 480
 481         sband = ieee80211_get_sband(sdata);
 482         if (!sband)
 483                 return -EINVAL;
 484
 485         if (!sband->vht_cap.vht_supported ||
 486             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_20_NOHT ||
 487             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_5 ||
 488             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_10)
 489                 return 0;
 490
 491         if (skb_tailroom(skb) < 2 + sizeof(struct ieee80211_vht_cap))
 492                 return -ENOMEM;
 493
 494         pos = skb_put(skb, 2 + sizeof(struct ieee80211_vht_cap));
 495         ieee80211_ie_build_vht_cap(pos, &sband->vht_cap, sband->vht_cap.cap);
 496
 497         return 0;
 498 }
 499
 500 int mesh_add_vht_oper_ie(struct ieee80211_sub_if_data *sdata,
 501                          struct sk_buff *skb)
 502 {
 503         struct ieee80211_local *local = sdata->local;
 504         struct ieee80211_chanctx_conf *chanctx_conf;
 505         struct ieee80211_channel *channel;
 506         struct ieee80211_supported_band *sband;
 507         struct ieee80211_sta_vht_cap *vht_cap;
 508         u8 *pos;
 509
 510         rcu_read_lock();
 511         chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
 512         if (WARN_ON(!chanctx_conf)) {
 513                 rcu_read_unlock();
 514                 return -EINVAL;
 515         }
 516         channel = chanctx_conf->def.chan;
 517         rcu_read_unlock();
 518
 519         sband = local->hw.wiphy->bands[channel->band];
 520         vht_cap = &sband->vht_cap;
 521
 522         if (!vht_cap->vht_supported ||
 523             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_20_NOHT ||
 524             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_5 ||
 525             sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_10)
 526                 return 0;
 527
 528         if (skb_tailroom(skb) < 2 + sizeof(struct ieee80211_vht_operation))
 529                 return -ENOMEM;
 530
 531         pos = skb_put(skb, 2 + sizeof(struct ieee80211_vht_operation));
 532         ieee80211_ie_build_vht_oper(pos, vht_cap,
 533                                     &sdata->vif.bss_conf.chandef);
 534
 535         return 0;
 536 }
 537
 538 static void ieee80211_mesh_path_timer(struct timer_list *t)
 539 {
 540         struct ieee80211_sub_if_data *sdata =
 541                 from_timer(sdata, t, u.mesh.mesh_path_timer);
 542
 543         ieee80211_queue_work(&sdata->local->hw, &sdata->work);
 544 }
 545
 546 static void ieee80211_mesh_path_root_timer(struct timer_list *t)
 547 {
 548         struct ieee80211_sub_if_data *sdata =
 549                 from_timer(sdata, t, u.mesh.mesh_path_root_timer);
 550         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 551
 552         set_bit(MESH_WORK_ROOT, &ifmsh->wrkq_flags);
 553
 554         ieee80211_queue_work(&sdata->local->hw, &sdata->work);
 555 }
 556
 557 void ieee80211_mesh_root_setup(struct ieee80211_if_mesh *ifmsh)
 558 {
 559         if (ifmsh->mshcfg.dot11MeshHWMPRootMode > IEEE80211_ROOTMODE_ROOT)
 560                 set_bit(MESH_WORK_ROOT, &ifmsh->wrkq_flags);
 561         else {
 562                 clear_bit(MESH_WORK_ROOT, &ifmsh->wrkq_flags);
 563                 /* stop running timer */
 564                 del_timer_sync(&ifmsh->mesh_path_root_timer);
 565         }
 566 }
 567
 568 /**
 569  * ieee80211_fill_mesh_addresses - fill addresses of a locally originated mesh frame
 570  * @hdr:        802.11 frame header
 571  * @fc:         frame control field
 572  * @meshda:     destination address in the mesh
 573  * @meshsa:     source address address in the mesh.  Same as TA, as frame is
 574  *              locally originated.
 575  *
 576  * Return the length of the 802.11 (does not include a mesh control header)
 577  */
 578 int ieee80211_fill_mesh_addresses(struct ieee80211_hdr *hdr, __le16 *fc,
 579                                   const u8 *meshda, const u8 *meshsa)
 580 {
 581         if (is_multicast_ether_addr(meshda)) {
 582                 *fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
 583                 /* DA TA SA */
 584                 memcpy(hdr->addr1, meshda, ETH_ALEN);
 585                 memcpy(hdr->addr2, meshsa, ETH_ALEN);
 586                 memcpy(hdr->addr3, meshsa, ETH_ALEN);
 587                 return 24;
 588         } else {
 589                 *fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS);
 590                 /* RA TA DA SA */
 591                 eth_zero_addr(hdr->addr1);   /* RA is resolved later */
 592                 memcpy(hdr->addr2, meshsa, ETH_ALEN);
 593                 memcpy(hdr->addr3, meshda, ETH_ALEN);
 594                 memcpy(hdr->addr4, meshsa, ETH_ALEN);
 595                 return 30;
 596         }
 597 }
 598
 599 /**
 600  * ieee80211_new_mesh_header - create a new mesh header
 601  * @sdata:      mesh interface to be used
 602  * @meshhdr:    uninitialized mesh header
 603  * @addr4or5:   1st address in the ae header, which may correspond to address 4
 604  *              (if addr6 is NULL) or address 5 (if addr6 is present). It may
 605  *              be NULL.
 606  * @addr6:      2nd address in the ae header, which corresponds to addr6 of the
 607  *              mesh frame
 608  *
 609  * Return the header length.
 610  */
 611 unsigned int ieee80211_new_mesh_header(struct ieee80211_sub_if_data *sdata,
 612                                        struct ieee80211s_hdr *meshhdr,
 613                                        const char *addr4or5, const char *addr6)
 614 {
 615         if (WARN_ON(!addr4or5 && addr6))
 616                 return 0;
 617
 618         memset(meshhdr, 0, sizeof(*meshhdr));
 619
 620         meshhdr->ttl = sdata->u.mesh.mshcfg.dot11MeshTTL;
 621
 622         /* FIXME: racy -- TX on multiple queues can be concurrent */
 623         put_unaligned(cpu_to_le32(sdata->u.mesh.mesh_seqnum), &meshhdr->seqnum);
 624         sdata->u.mesh.mesh_seqnum++;
 625
 626         if (addr4or5 && !addr6) {
 627                 meshhdr->flags |= MESH_FLAGS_AE_A4;
 628                 memcpy(meshhdr->eaddr1, addr4or5, ETH_ALEN);
 629                 return 2 * ETH_ALEN;
 630         } else if (addr4or5 && addr6) {
 631                 meshhdr->flags |= MESH_FLAGS_AE_A5_A6;
 632                 memcpy(meshhdr->eaddr1, addr4or5, ETH_ALEN);
 633                 memcpy(meshhdr->eaddr2, addr6, ETH_ALEN);
 634                 return 3 * ETH_ALEN;
 635         }
 636
 637         return ETH_ALEN;
 638 }
 639
 640 static void ieee80211_mesh_housekeeping(struct ieee80211_sub_if_data *sdata)
 641 {
 642         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 643         u32 changed;
 644
 645         if (ifmsh->mshcfg.plink_timeout > 0)
 646                 ieee80211_sta_expire(sdata, ifmsh->mshcfg.plink_timeout * HZ);
 647         mesh_path_expire(sdata);
 648
 649         changed = mesh_accept_plinks_update(sdata);
 650         ieee80211_mbss_info_change_notify(sdata, changed);
 651
 652         mod_timer(&ifmsh->housekeeping_timer,
 653                   round_jiffies(jiffies +
 654                                 IEEE80211_MESH_HOUSEKEEPING_INTERVAL));
 655 }
 656
 657 static void ieee80211_mesh_rootpath(struct ieee80211_sub_if_data *sdata)
 658 {
 659         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 660         u32 interval;
 661
 662         mesh_path_tx_root_frame(sdata);
 663
 664         if (ifmsh->mshcfg.dot11MeshHWMPRootMode == IEEE80211_PROACTIVE_RANN)
 665                 interval = ifmsh->mshcfg.dot11MeshHWMPRannInterval;
 666         else
 667                 interval = ifmsh->mshcfg.dot11MeshHWMProotInterval;
 668
 669         mod_timer(&ifmsh->mesh_path_root_timer,
 670                   round_jiffies(TU_TO_EXP_TIME(interval)));
 671 }
 672
 673 static int
 674 ieee80211_mesh_build_beacon(struct ieee80211_if_mesh *ifmsh)
 675 {
 676         struct beacon_data *bcn;
 677         int head_len, tail_len;
 678         struct sk_buff *skb;
 679         struct ieee80211_mgmt *mgmt;
 680         struct ieee80211_chanctx_conf *chanctx_conf;
 681         struct mesh_csa_settings *csa;
 682         enum nl80211_band band;
 683         u8 *pos;
 684         struct ieee80211_sub_if_data *sdata;
 685         int hdr_len = offsetofend(struct ieee80211_mgmt, u.beacon);
 686
 687         sdata = container_of(ifmsh, struct ieee80211_sub_if_data, u.mesh);
 688         rcu_read_lock();
 689         chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
 690         band = chanctx_conf->def.chan->band;
 691         rcu_read_unlock();
 692
 693         head_len = hdr_len +
 694                    2 + /* NULL SSID */
 695                    /* Channel Switch Announcement */
 696                    2 + sizeof(struct ieee80211_channel_sw_ie) +
 697                    /* Mesh Channel Switch Parameters */
 698                    2 + sizeof(struct ieee80211_mesh_chansw_params_ie) +
 699                    /* Channel Switch Wrapper + Wide Bandwidth CSA IE */
 700                    2 + 2 + sizeof(struct ieee80211_wide_bw_chansw_ie) +
 701                    2 + sizeof(struct ieee80211_sec_chan_offs_ie) +
 702                    2 + 8 + /* supported rates */
 703                    2 + 3; /* DS params */
 704         tail_len = 2 + (IEEE80211_MAX_SUPP_RATES - 8) +
 705                    2 + sizeof(struct ieee80211_ht_cap) +
 706                    2 + sizeof(struct ieee80211_ht_operation) +
 707                    2 + ifmsh->mesh_id_len +
 708                    2 + sizeof(struct ieee80211_meshconf_ie) +
 709                    2 + sizeof(__le16) + /* awake window */
 710                    2 + sizeof(struct ieee80211_vht_cap) +
 711                    2 + sizeof(struct ieee80211_vht_operation) +
 712                    ifmsh->ie_len;
 713
 714         bcn = kzalloc(sizeof(*bcn) + head_len + tail_len, GFP_KERNEL);
 715         /* need an skb for IE builders to operate on */
 716         skb = dev_alloc_skb(max(head_len, tail_len));
 717
 718         if (!bcn || !skb)
 719                 goto out_free;
 720
 721         /*
 722          * pointers go into the block we allocated,
 723          * memory is | beacon_data | head | tail |
 724          */
 725         bcn->head = ((u8 *) bcn) + sizeof(*bcn);
 726
 727         /* fill in the head */
 728         mgmt = skb_put_zero(skb, hdr_len);
 729         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
 730                                           IEEE80211_STYPE_BEACON);
 731         eth_broadcast_addr(mgmt->da);
 732         memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
 733         memcpy(mgmt->bssid, sdata->vif.addr, ETH_ALEN);
 734         ieee80211_mps_set_frame_flags(sdata, NULL, (void *) mgmt);
 735         mgmt->u.beacon.beacon_int =
 736                 cpu_to_le16(sdata->vif.bss_conf.beacon_int);
 737         mgmt->u.beacon.capab_info |= cpu_to_le16(
 738                 sdata->u.mesh.security ? WLAN_CAPABILITY_PRIVACY : 0);
 739
 740         pos = skb_put(skb, 2);
 741         *pos++ = WLAN_EID_SSID;
 742         *pos++ = 0x0;
 743
 744         rcu_read_lock();
 745         csa = rcu_dereference(ifmsh->csa);
 746         if (csa) {
 747                 enum nl80211_channel_type ct;
 748                 struct cfg80211_chan_def *chandef;
 749                 int ie_len = 2 + sizeof(struct ieee80211_channel_sw_ie) +
 750                              2 + sizeof(struct ieee80211_mesh_chansw_params_ie);
 751
 752                 pos = skb_put_zero(skb, ie_len);
 753                 *pos++ = WLAN_EID_CHANNEL_SWITCH;
 754                 *pos++ = 3;
 755                 *pos++ = 0x0;
 756                 *pos++ = ieee80211_frequency_to_channel(
 757                                 csa->settings.chandef.chan->center_freq);
 758                 bcn->csa_current_counter = csa->settings.count;
 759                 bcn->csa_counter_offsets[0] = hdr_len + 6;
 760                 *pos++ = csa->settings.count;
 761                 *pos++ = WLAN_EID_CHAN_SWITCH_PARAM;
 762                 *pos++ = 6;
 763                 if (ifmsh->csa_role == IEEE80211_MESH_CSA_ROLE_INIT) {
 764                         *pos++ = ifmsh->mshcfg.dot11MeshTTL;
 765                         *pos |= WLAN_EID_CHAN_SWITCH_PARAM_INITIATOR;
 766                 } else {
 767                         *pos++ = ifmsh->chsw_ttl;
 768                 }
 769                 *pos++ |= csa->settings.block_tx ?
 770                           WLAN_EID_CHAN_SWITCH_PARAM_TX_RESTRICT : 0x00;
 771                 put_unaligned_le16(WLAN_REASON_MESH_CHAN, pos);
 772                 pos += 2;
 773                 put_unaligned_le16(ifmsh->pre_value, pos);
 774                 pos += 2;
 775
 776                 switch (csa->settings.chandef.width) {
 777                 case NL80211_CHAN_WIDTH_40:
 778                         ie_len = 2 + sizeof(struct ieee80211_sec_chan_offs_ie);
 779                         pos = skb_put_zero(skb, ie_len);
 780
 781                         *pos++ = WLAN_EID_SECONDARY_CHANNEL_OFFSET; /* EID */
 782                         *pos++ = 1;                                 /* len */
 783                         ct = cfg80211_get_chandef_type(&csa->settings.chandef);
 784                         if (ct == NL80211_CHAN_HT40PLUS)
 785                                 *pos++ = IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
 786                         else
 787                                 *pos++ = IEEE80211_HT_PARAM_CHA_SEC_BELOW;
 788                         break;
 789                 case NL80211_CHAN_WIDTH_80:
 790                 case NL80211_CHAN_WIDTH_80P80:
 791                 case NL80211_CHAN_WIDTH_160:
 792                         /* Channel Switch Wrapper + Wide Bandwidth CSA IE */
 793                         ie_len = 2 + 2 +
 794                                  sizeof(struct ieee80211_wide_bw_chansw_ie);
 795                         pos = skb_put_zero(skb, ie_len);
 796
 797                         *pos++ = WLAN_EID_CHANNEL_SWITCH_WRAPPER; /* EID */
 798                         *pos++ = 5;                               /* len */
 799                         /* put sub IE */
 800                         chandef = &csa->settings.chandef;
 801                         ieee80211_ie_build_wide_bw_cs(pos, chandef);
 802                         break;
 803                 default:
 804                         break;
 805                 }
 806         }
 807         rcu_read_unlock();
 808
 809         if (ieee80211_add_srates_ie(sdata, skb, true, band) ||
 810             mesh_add_ds_params_ie(sdata, skb))
 811                 goto out_free;
 812
 813         bcn->head_len = skb->len;
 814         memcpy(bcn->head, skb->data, bcn->head_len);
 815
 816         /* now the tail */
 817         skb_trim(skb, 0);
 818         bcn->tail = bcn->head + bcn->head_len;
 819
 820         if (ieee80211_add_ext_srates_ie(sdata, skb, true, band) ||
 821             mesh_add_rsn_ie(sdata, skb) ||
 822             mesh_add_ht_cap_ie(sdata, skb) ||
 823             mesh_add_ht_oper_ie(sdata, skb) ||
 824             mesh_add_meshid_ie(sdata, skb) ||
 825             mesh_add_meshconf_ie(sdata, skb) ||
 826             mesh_add_awake_window_ie(sdata, skb) ||
 827             mesh_add_vht_cap_ie(sdata, skb) ||
 828             mesh_add_vht_oper_ie(sdata, skb) ||
 829             mesh_add_vendor_ies(sdata, skb))
 830                 goto out_free;
 831
 832         bcn->tail_len = skb->len;
 833         memcpy(bcn->tail, skb->data, bcn->tail_len);
 834         bcn->meshconf = (struct ieee80211_meshconf_ie *)
 835                                         (bcn->tail + ifmsh->meshconf_offset);
 836
 837         dev_kfree_skb(skb);
 838         rcu_assign_pointer(ifmsh->beacon, bcn);
 839         return 0;
 840 out_free:
 841         kfree(bcn);
 842         dev_kfree_skb(skb);
 843         return -ENOMEM;
 844 }
 845
 846 static int
 847 ieee80211_mesh_rebuild_beacon(struct ieee80211_sub_if_data *sdata)
 848 {
 849         struct beacon_data *old_bcn;
 850         int ret;
 851
 852         old_bcn = rcu_dereference_protected(sdata->u.mesh.beacon,
 853                                             lockdep_is_held(&sdata->wdev.mtx));
 854         ret = ieee80211_mesh_build_beacon(&sdata->u.mesh);
 855         if (ret)
 856                 /* just reuse old beacon */
 857                 return ret;
 858
 859         if (old_bcn)
 860                 kfree_rcu(old_bcn, rcu_head);
 861         return 0;
 862 }
 863
 864 void ieee80211_mbss_info_change_notify(struct ieee80211_sub_if_data *sdata,
 865                                        u32 changed)
 866 {
 867         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 868         unsigned long bits = changed;
 869         u32 bit;
 870
 871         if (!bits)
 872                 return;
 873
 874         /* if we race with running work, worst case this work becomes a noop */
 875         for_each_set_bit(bit, &bits, sizeof(changed) * BITS_PER_BYTE)
 876                 set_bit(bit, &ifmsh->mbss_changed);
 877         set_bit(MESH_WORK_MBSS_CHANGED, &ifmsh->wrkq_flags);
 878         ieee80211_queue_work(&sdata->local->hw, &sdata->work);
 879 }
 880
 881 int ieee80211_start_mesh(struct ieee80211_sub_if_data *sdata)
 882 {
 883         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 884         struct ieee80211_local *local = sdata->local;
 885         u32 changed = BSS_CHANGED_BEACON |
 886                       BSS_CHANGED_BEACON_ENABLED |
 887                       BSS_CHANGED_HT |
 888                       BSS_CHANGED_BASIC_RATES |
 889                       BSS_CHANGED_BEACON_INT |
 890                       BSS_CHANGED_MCAST_RATE;
 891
 892         local->fif_other_bss++;
 893         /* mesh ifaces must set allmulti to forward mcast traffic */
 894         atomic_inc(&local->iff_allmultis);
 895         ieee80211_configure_filter(local);
 896
 897         ifmsh->mesh_cc_id = 0;  /* Disabled */
 898         /* register sync ops from extensible synchronization framework */
 899         ifmsh->sync_ops = ieee80211_mesh_sync_ops_get(ifmsh->mesh_sp_id);
 900         ifmsh->sync_offset_clockdrift_max = 0;
 901         set_bit(MESH_WORK_HOUSEKEEPING, &ifmsh->wrkq_flags);
 902         ieee80211_mesh_root_setup(ifmsh);
 903         ieee80211_queue_work(&local->hw, &sdata->work);
 904         sdata->vif.bss_conf.ht_operation_mode =
 905                                 ifmsh->mshcfg.ht_opmode;
 906         sdata->vif.bss_conf.enable_beacon = true;
 907
 908         changed |= ieee80211_mps_local_status_update(sdata);
 909
 910         if (ieee80211_mesh_build_beacon(ifmsh)) {
 911                 ieee80211_stop_mesh(sdata);
 912                 return -ENOMEM;
 913         }
 914
 915         ieee80211_recalc_dtim(local, sdata);
 916         ieee80211_bss_info_change_notify(sdata, changed);
 917
 918         netif_carrier_on(sdata->dev);
 919         return 0;
 920 }
 921
 922 void ieee80211_stop_mesh(struct ieee80211_sub_if_data *sdata)
 923 {
 924         struct ieee80211_local *local = sdata->local;
 925         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 926         struct beacon_data *bcn;
 927
 928         netif_carrier_off(sdata->dev);
 929
 930         /* flush STAs and mpaths on this iface */
 931         sta_info_flush(sdata);
 932         mesh_path_flush_by_iface(sdata);
 933
 934         /* stop the beacon */
 935         ifmsh->mesh_id_len = 0;
 936         sdata->vif.bss_conf.enable_beacon = false;
 937         clear_bit(SDATA_STATE_OFFCHANNEL_BEACON_STOPPED, &sdata->state);
 938         ieee80211_bss_info_change_notify(sdata, BSS_CHANGED_BEACON_ENABLED);
 939
 940         /* remove beacon */
 941         bcn = rcu_dereference_protected(ifmsh->beacon,
 942                                         lockdep_is_held(&sdata->wdev.mtx));
 943         RCU_INIT_POINTER(ifmsh->beacon, NULL);
 944         kfree_rcu(bcn, rcu_head);
 945
 946         /* free all potentially still buffered group-addressed frames */
 947         local->total_ps_buffered -= skb_queue_len(&ifmsh->ps.bc_buf);
 948         skb_queue_purge(&ifmsh->ps.bc_buf);
 949
 950         del_timer_sync(&sdata->u.mesh.housekeeping_timer);
 951         del_timer_sync(&sdata->u.mesh.mesh_path_root_timer);
 952         del_timer_sync(&sdata->u.mesh.mesh_path_timer);
 953
 954         /* clear any mesh work (for next join) we may have accrued */
 955         ifmsh->wrkq_flags = 0;
 956         ifmsh->mbss_changed = 0;
 957
 958         local->fif_other_bss--;
 959         atomic_dec(&local->iff_allmultis);
 960         ieee80211_configure_filter(local);
 961 }
 962
 963 static void ieee80211_mesh_csa_mark_radar(struct ieee80211_sub_if_data *sdata)
 964 {
 965         int err;
 966
 967         /* if the current channel is a DFS channel, mark the channel as
 968          * unavailable.
 969          */
 970         err = cfg80211_chandef_dfs_required(sdata->local->hw.wiphy,
 971                                             &sdata->vif.bss_conf.chandef,
 972                                             NL80211_IFTYPE_MESH_POINT);
 973         if (err > 0)
 974                 cfg80211_radar_event(sdata->local->hw.wiphy,
 975                                      &sdata->vif.bss_conf.chandef, GFP_ATOMIC);
 976 }
 977
 978 static bool
 979 ieee80211_mesh_process_chnswitch(struct ieee80211_sub_if_data *sdata,
 980                                  struct ieee802_11_elems *elems, bool beacon)
 981 {
 982         struct cfg80211_csa_settings params;
 983         struct ieee80211_csa_ie csa_ie;
 984         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 985         struct ieee80211_supported_band *sband;
 986         int err;
 987         u32 sta_flags;
 988
 989         sdata_assert_lock(sdata);
 990
 991         sband = ieee80211_get_sband(sdata);
 992         if (!sband)
 993                 return false;
 994
 995         sta_flags = 0;
 996         switch (sdata->vif.bss_conf.chandef.width) {
 997         case NL80211_CHAN_WIDTH_20_NOHT:
 998                 sta_flags |= IEEE80211_STA_DISABLE_HT;
 999                 /* fall through */
1000         case NL80211_CHAN_WIDTH_20:
1001                 sta_flags |= IEEE80211_STA_DISABLE_40MHZ;
1002                 /* fall through */
1003         case NL80211_CHAN_WIDTH_40:
1004                 sta_flags |= IEEE80211_STA_DISABLE_VHT;
1005                 break;
1006         default:
1007                 break;
1008         }
1009
1010         memset(&params, 0, sizeof(params));
1011         err = ieee80211_parse_ch_switch_ie(sdata, elems, sband->band,
1012                                            sta_flags, sdata->vif.addr,
1013                                            &csa_ie);
1014         if (err < 0)
1015                 return false;
1016         if (err)
1017                 return false;
1018
1019         /* Mark the channel unavailable if the reason for the switch is
1020          * regulatory.
1021          */
1022         if (csa_ie.reason_code == WLAN_REASON_MESH_CHAN_REGULATORY)
1023                 ieee80211_mesh_csa_mark_radar(sdata);
1024
1025         params.chandef = csa_ie.chandef;
1026         params.count = csa_ie.count;
1027
1028         if (!cfg80211_chandef_usable(sdata->local->hw.wiphy, &params.chandef,
1029                                      IEEE80211_CHAN_DISABLED) ||
1030             !cfg80211_reg_can_beacon(sdata->local->hw.wiphy, &params.chandef,
1031                                      NL80211_IFTYPE_MESH_POINT)) {
1032                 sdata_info(sdata,
1033                            "mesh STA %pM switches to unsupported channel (%d MHz, width:%d, CF1/2: %d/%d MHz), aborting\n",
1034                            sdata->vif.addr,
1035                            params.chandef.chan->center_freq,
1036                            params.chandef.width,
1037                            params.chandef.center_freq1,
1038                            params.chandef.center_freq2);
1039                 return false;
1040         }
1041
1042         err = cfg80211_chandef_dfs_required(sdata->local->hw.wiphy,
1043                                             &params.chandef,
1044                                             NL80211_IFTYPE_MESH_POINT);
1045         if (err < 0)
1046                 return false;
1047         if (err > 0 && !ifmsh->userspace_handles_dfs) {
1048                 sdata_info(sdata,
1049                            "mesh STA %pM switches to channel requiring DFS (%d MHz, width:%d, CF1/2: %d/%d MHz), aborting\n",
1050                            sdata->vif.addr,
1051                            params.chandef.chan->center_freq,
1052                            params.chandef.width,
1053                            params.chandef.center_freq1,
1054                            params.chandef.center_freq2);
1055                 return false;
1056         }
1057
1058         params.radar_required = err;
1059
1060         if (cfg80211_chandef_identical(&params.chandef,
1061                                        &sdata->vif.bss_conf.chandef)) {
1062                 mcsa_dbg(sdata,
1063                          "received csa with an identical chandef, ignoring\n");
1064                 return true;
1065         }
1066
1067         mcsa_dbg(sdata,
1068                  "received channel switch announcement to go to channel %d MHz\n",
1069                  params.chandef.chan->center_freq);
1070
1071         params.block_tx = csa_ie.mode & WLAN_EID_CHAN_SWITCH_PARAM_TX_RESTRICT;
1072         if (beacon) {
1073                 ifmsh->chsw_ttl = csa_ie.ttl - 1;
1074                 if (ifmsh->pre_value >= csa_ie.pre_value)
1075                         return false;
1076                 ifmsh->pre_value = csa_ie.pre_value;
1077         }
1078
1079         if (ifmsh->chsw_ttl >= ifmsh->mshcfg.dot11MeshTTL)
1080                 return false;
1081
1082         ifmsh->csa_role = IEEE80211_MESH_CSA_ROLE_REPEATER;
1083
1084         if (ieee80211_channel_switch(sdata->local->hw.wiphy, sdata->dev,
1085                                      &params) < 0)
1086                 return false;
1087
1088         return true;
1089 }
1090
1091 static void
1092 ieee80211_mesh_rx_probe_req(struct ieee80211_sub_if_data *sdata,
1093                             struct ieee80211_mgmt *mgmt, size_t len)
1094 {
1095         struct ieee80211_local *local = sdata->local;
1096         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
1097         struct sk_buff *presp;
1098         struct beacon_data *bcn;
1099         struct ieee80211_mgmt *hdr;
1100         struct ieee802_11_elems elems;
1101         size_t baselen;
1102         u8 *pos;
1103
1104         pos = mgmt->u.probe_req.variable;
1105         baselen = (u8 *) pos - (u8 *) mgmt;
1106         if (baselen > len)
1107                 return;
1108
1109         ieee802_11_parse_elems(pos, len - baselen, false, &elems, mgmt->bssid,
1110                                NULL);
1111
1112         if (!elems.mesh_id)
1113                 return;
1114
1115         /* 802.11-2012 10.1.4.3.2 */
1116         if ((!ether_addr_equal(mgmt->da, sdata->vif.addr) &&
1117              !is_broadcast_ether_addr(mgmt->da)) ||
1118             elems.ssid_len != 0)
1119                 return;
1120
1121         if (elems.mesh_id_len != 0 &&
1122             (elems.mesh_id_len != ifmsh->mesh_id_len ||
1123              memcmp(elems.mesh_id, ifmsh->mesh_id, ifmsh->mesh_id_len)))
1124                 return;
1125
1126         rcu_read_lock();
1127         bcn = rcu_dereference(ifmsh->beacon);
1128
1129         if (!bcn)
1130                 goto out;
1131
1132         presp = dev_alloc_skb(local->tx_headroom +
1133                               bcn->head_len + bcn->tail_len);
1134         if (!presp)
1135                 goto out;
1136
1137         skb_reserve(presp, local->tx_headroom);
1138         skb_put_data(presp, bcn->head, bcn->head_len);
1139         skb_put_data(presp, bcn->tail, bcn->tail_len);
1140         hdr = (struct ieee80211_mgmt *) presp->data;
1141         hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
1142                                          IEEE80211_STYPE_PROBE_RESP);
1143         memcpy(hdr->da, mgmt->sa, ETH_ALEN);
1144         IEEE80211_SKB_CB(presp)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT;
1145         ieee80211_tx_skb(sdata, presp);
1146 out:
1147         rcu_read_unlock();
1148 }
1149
1150 static void ieee80211_mesh_rx_bcn_presp(struct ieee80211_sub_if_data *sdata,
1151                                         u16 stype,
1152                                         struct ieee80211_mgmt *mgmt,
1153                                         size_t len,
1154                                         struct ieee80211_rx_status *rx_status)
1155 {
1156         struct ieee80211_local *local = sdata->local;
1157         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
1158         struct ieee802_11_elems elems;
1159         struct ieee80211_channel *channel;
1160         size_t baselen;
1161         int freq;
1162         enum nl80211_band band = rx_status->band;
1163
1164         /* ignore ProbeResp to foreign address */
1165         if (stype == IEEE80211_STYPE_PROBE_RESP &&
1166             !ether_addr_equal(mgmt->da, sdata->vif.addr))
1167                 return;
1168
1169         baselen = (u8 *) mgmt->u.probe_resp.variable - (u8 *) mgmt;
1170         if (baselen > len)
1171                 return;
1172
1173         ieee802_11_parse_elems(mgmt->u.probe_resp.variable, len - baselen,
1174                                false, &elems, mgmt->bssid, NULL);
1175
1176         /* ignore non-mesh or secure / unsecure mismatch */
1177         if ((!elems.mesh_id || !elems.mesh_config) ||
1178             (elems.rsn && sdata->u.mesh.security == IEEE80211_MESH_SEC_NONE) ||
1179             (!elems.rsn && sdata->u.mesh.security != IEEE80211_MESH_SEC_NONE))
1180                 return;
1181
1182         if (elems.ds_params)
1183                 freq = ieee80211_channel_to_frequency(elems.ds_params[0], band);
1184         else
1185                 freq = rx_status->freq;
1186
1187         channel = ieee80211_get_channel(local->hw.wiphy, freq);
1188
1189         if (!channel || channel->flags & IEEE80211_CHAN_DISABLED)
1190                 return;
1191
1192         if (mesh_matches_local(sdata, &elems)) {
1193                 mpl_dbg(sdata, "rssi_threshold=%d,rx_status->signal=%d\n",
1194                         sdata->u.mesh.mshcfg.rssi_threshold, rx_status->signal);
1195                 if (!sdata->u.mesh.user_mpm ||
1196                     sdata->u.mesh.mshcfg.rssi_threshold == 0 ||
1197                     sdata->u.mesh.mshcfg.rssi_threshold < rx_status->signal)
1198                         mesh_neighbour_update(sdata, mgmt->sa, &elems,
1199                                               rx_status);
1200         }
1201
1202         if (ifmsh->sync_ops)
1203                 ifmsh->sync_ops->rx_bcn_presp(sdata,
1204                         stype, mgmt, &elems, rx_status);
1205
1206         if (ifmsh->csa_role != IEEE80211_MESH_CSA_ROLE_INIT &&
1207             !sdata->vif.csa_active)
1208                 ieee80211_mesh_process_chnswitch(sdata, &elems, true);
1209 }
1210
1211 int ieee80211_mesh_finish_csa(struct ieee80211_sub_if_data *sdata)
1212 {
1213         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
1214         struct mesh_csa_settings *tmp_csa_settings;
1215         int ret = 0;
1216         int changed = 0;
1217
1218         /* Reset the TTL value and Initiator flag */
1219         ifmsh->csa_role = IEEE80211_MESH_CSA_ROLE_NONE;
1220         ifmsh->chsw_ttl = 0;
1221
1222         /* Remove the CSA and MCSP elements from the beacon */
1223         tmp_csa_settings = rcu_dereference(ifmsh->csa);
1224         RCU_INIT_POINTER(ifmsh->csa, NULL);
1225         if (tmp_csa_settings)
1226                 kfree_rcu(tmp_csa_settings, rcu_head);
1227         ret = ieee80211_mesh_rebuild_beacon(sdata);
1228         if (ret)
1229                 return -EINVAL;
1230
1231         changed |= BSS_CHANGED_BEACON;
1232
1233         mcsa_dbg(sdata, "complete switching to center freq %d MHz",
1234                  sdata->vif.bss_conf.chandef.chan->center_freq);
1235         return changed;
1236 }
1237
1238 int ieee80211_mesh_csa_beacon(struct ieee80211_sub_if_data *sdata,
1239                               struct cfg80211_csa_settings *csa_settings)
1240 {
1241         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
1242         struct mesh_csa_settings *tmp_csa_settings;
1243         int ret = 0;
1244
1245         tmp_csa_settings = kmalloc(sizeof(*tmp_csa_settings),
1246                                    GFP_ATOMIC);
1247         if (!tmp_csa_settings)
1248                 return -ENOMEM;
1249
1250         memcpy(&tmp_csa_settings->settings, csa_settings,
1251                sizeof(struct cfg80211_csa_settings));
1252
1253         rcu_assign_pointer(ifmsh->csa, tmp_csa_settings);
1254
1255         ret = ieee80211_mesh_rebuild_beacon(sdata);
1256         if (ret) {
1257                 tmp_csa_settings = rcu_dereference(ifmsh->csa);
1258                 RCU_INIT_POINTER(ifmsh->csa, NULL);
1259                 kfree_rcu(tmp_csa_settings, rcu_head);
1260                 return ret;
1261         }
1262
1263         return BSS_CHANGED_BEACON;
1264 }
1265
1266 static int mesh_fwd_csa_frame(struct ieee80211_sub_if_data *sdata,
1267                                struct ieee80211_mgmt *mgmt, size_t len,
1268                                struct ieee802_11_elems *elems)
1269 {
1270         struct ieee80211_mgmt *mgmt_fwd;
1271         struct sk_buff *skb;
1272         struct ieee80211_local *local = sdata->local;
1273
1274         skb = dev_alloc_skb(local->tx_headroom + len);
1275         if (!skb)
1276                 return -ENOMEM;
1277         skb_reserve(skb, local->tx_headroom);
1278         mgmt_fwd = skb_put(skb, len);
1279
1280         elems->mesh_chansw_params_ie->mesh_ttl--;
1281         elems->mesh_chansw_params_ie->mesh_flags &=
1282                 ~WLAN_EID_CHAN_SWITCH_PARAM_INITIATOR;
1283
1284         memcpy(mgmt_fwd, mgmt, len);
1285         eth_broadcast_addr(mgmt_fwd->da);
1286         memcpy(mgmt_fwd->sa, sdata->vif.addr, ETH_ALEN);
1287         memcpy(mgmt_fwd->bssid, sdata->vif.addr, ETH_ALEN);
1288
1289         ieee80211_tx_skb(sdata, skb);
1290         return 0;
1291 }
1292
1293 static void mesh_rx_csa_frame(struct ieee80211_sub_if_data *sdata,
1294                               struct ieee80211_mgmt *mgmt, size_t len)
1295 {
1296         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
1297         struct ieee802_11_elems elems;
1298         u16 pre_value;
1299         bool fwd_csa = true;
1300         size_t baselen;
1301         u8 *pos;
1302
1303         if (mgmt->u.action.u.measurement.action_code !=
1304             WLAN_ACTION_SPCT_CHL_SWITCH)
1305                 return;
1306
1307         pos = mgmt->u.action.u.chan_switch.variable;
1308         baselen = offsetof(struct ieee80211_mgmt,
1309                            u.action.u.chan_switch.variable);
1310         ieee802_11_parse_elems(pos, len - baselen, true, &elems,
1311                                mgmt->bssid, NULL);
1312
1313         ifmsh->chsw_ttl = elems.mesh_chansw_params_ie->mesh_ttl;
1314         if (!--ifmsh->chsw_ttl)
1315                 fwd_csa = false;
1316
1317         pre_value = le16_to_cpu(elems.mesh_chansw_params_ie->mesh_pre_value);
1318         if (ifmsh->pre_value >= pre_value)
1319                 return;
1320
1321         ifmsh->pre_value = pre_value;
1322
1323         if (!sdata->vif.csa_active &&
1324             !ieee80211_mesh_process_chnswitch(sdata, &elems, false)) {
1325                 mcsa_dbg(sdata, "Failed to process CSA action frame");
1326                 return;
1327         }
1328
1329         /* forward or re-broadcast the CSA frame */
1330         if (fwd_csa) {
1331                 if (mesh_fwd_csa_frame(sdata, mgmt, len, &elems) < 0)
1332                         mcsa_dbg(sdata, "Failed to forward the CSA frame");
1333         }
1334 }
1335
1336 static void ieee80211_mesh_rx_mgmt_action(struct ieee80211_sub_if_data *sdata,
1337                                           struct ieee80211_mgmt *mgmt,
1338                                           size_t len,
1339                                           struct ieee80211_rx_status *rx_status)
1340 {
1341         switch (mgmt->u.action.category) {
1342         case WLAN_CATEGORY_SELF_PROTECTED:
1343                 switch (mgmt->u.action.u.self_prot.action_code) {
1344                 case WLAN_SP_MESH_PEERING_OPEN:
1345                 case WLAN_SP_MESH_PEERING_CLOSE:
1346                 case WLAN_SP_MESH_PEERING_CONFIRM:
1347                         mesh_rx_plink_frame(sdata, mgmt, len, rx_status);
1348                         break;
1349                 }
1350                 break;
1351         case WLAN_CATEGORY_MESH_ACTION:
1352                 if (mesh_action_is_path_sel(mgmt))
1353                         mesh_rx_path_sel_frame(sdata, mgmt, len);
1354                 break;
1355         case WLAN_CATEGORY_SPECTRUM_MGMT:
1356                 mesh_rx_csa_frame(sdata, mgmt, len);
1357                 break;
1358         }
1359 }
1360
1361 void ieee80211_mesh_rx_queued_mgmt(struct ieee80211_sub_if_data *sdata,
1362                                    struct sk_buff *skb)
1363 {
1364         struct ieee80211_rx_status *rx_status;
1365         struct ieee80211_mgmt *mgmt;
1366         u16 stype;
1367
1368         sdata_lock(sdata);
1369
1370         /* mesh already went down */
1371         if (!sdata->u.mesh.mesh_id_len)
1372                 goto out;
1373
1374         rx_status = IEEE80211_SKB_RXCB(skb);
1375         mgmt = (struct ieee80211_mgmt *) skb->data;
1376         stype = le16_to_cpu(mgmt->frame_control) & IEEE80211_FCTL_STYPE;
1377
1378         switch (stype) {
1379         case IEEE80211_STYPE_PROBE_RESP:
1380         case IEEE80211_STYPE_BEACON:
1381                 ieee80211_mesh_rx_bcn_presp(sdata, stype, mgmt, skb->len,
1382                                             rx_status);
1383                 break;
1384         case IEEE80211_STYPE_PROBE_REQ:
1385                 ieee80211_mesh_rx_probe_req(sdata, mgmt, skb->len);
1386                 break;
1387         case IEEE80211_STYPE_ACTION:
1388                 ieee80211_mesh_rx_mgmt_action(sdata, mgmt, skb->len, rx_status);
1389                 break;
1390         }
1391 out:
1392         sdata_unlock(sdata);
1393 }
1394
1395 static void mesh_bss_info_changed(struct ieee80211_sub_if_data *sdata)
1396 {
1397         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
1398         u32 bit, changed = 0;
1399
1400         for_each_set_bit(bit, &ifmsh->mbss_changed,
1401                          sizeof(changed) * BITS_PER_BYTE) {
1402                 clear_bit(bit, &ifmsh->mbss_changed);
1403                 changed |= BIT(bit);
1404         }
1405
1406         if (sdata->vif.bss_conf.enable_beacon &&
1407             (changed & (BSS_CHANGED_BEACON |
1408                         BSS_CHANGED_HT |
1409                         BSS_CHANGED_BASIC_RATES |
1410                         BSS_CHANGED_BEACON_INT)))
1411                 if (ieee80211_mesh_rebuild_beacon(sdata))
1412                         return;
1413
1414         ieee80211_bss_info_change_notify(sdata, changed);
1415 }
1416
1417 void ieee80211_mesh_work(struct ieee80211_sub_if_data *sdata)
1418 {
1419         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
1420
1421         sdata_lock(sdata);
1422
1423         /* mesh already went down */
1424         if (!sdata->u.mesh.mesh_id_len)
1425                 goto out;
1426
1427         if (ifmsh->preq_queue_len &&
1428             time_after(jiffies,
1429                        ifmsh->last_preq + msecs_to_jiffies(ifmsh->mshcfg.dot11MeshHWMPpreqMinInterval)))
1430                 mesh_path_start_discovery(sdata);
1431
1432         if (test_and_clear_bit(MESH_WORK_HOUSEKEEPING, &ifmsh->wrkq_flags))
1433                 ieee80211_mesh_housekeeping(sdata);
1434
1435         if (test_and_clear_bit(MESH_WORK_ROOT, &ifmsh->wrkq_flags))
1436                 ieee80211_mesh_rootpath(sdata);
1437
1438         if (test_and_clear_bit(MESH_WORK_DRIFT_ADJUST, &ifmsh->wrkq_flags))
1439                 mesh_sync_adjust_tsf(sdata);
1440
1441         if (test_and_clear_bit(MESH_WORK_MBSS_CHANGED, &ifmsh->wrkq_flags))
1442                 mesh_bss_info_changed(sdata);
1443 out:
1444         sdata_unlock(sdata);
1445 }
1446
1447
1448 void ieee80211_mesh_init_sdata(struct ieee80211_sub_if_data *sdata)
1449 {
1450         struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
1451         static u8 zero_addr[ETH_ALEN] = {};
1452
1453         timer_setup(&ifmsh->housekeeping_timer,
1454                     ieee80211_mesh_housekeeping_timer, 0);
1455
1456         ifmsh->accepting_plinks = true;
1457         atomic_set(&ifmsh->mpaths, 0);
1458         mesh_rmc_init(sdata);
1459         ifmsh->last_preq = jiffies;
1460         ifmsh->next_perr = jiffies;
1461         ifmsh->csa_role = IEEE80211_MESH_CSA_ROLE_NONE;
1462         /* Allocate all mesh structures when creating the first mesh interface. */
1463         if (!mesh_allocated)
1464                 ieee80211s_init();
1465
1466         mesh_pathtbl_init(sdata);
1467
1468         timer_setup(&ifmsh->mesh_path_timer, ieee80211_mesh_path_timer, 0);
1469         timer_setup(&ifmsh->mesh_path_root_timer,
1470                     ieee80211_mesh_path_root_timer, 0);
1471         INIT_LIST_HEAD(&ifmsh->preq_queue.list);
1472         skb_queue_head_init(&ifmsh->ps.bc_buf);
1473         spin_lock_init(&ifmsh->mesh_preq_queue_lock);
1474         spin_lock_init(&ifmsh->sync_offset_lock);
1475         RCU_INIT_POINTER(ifmsh->beacon, NULL);
1476
1477         sdata->vif.bss_conf.bssid = zero_addr;
1478 }
1479
1480 void ieee80211_mesh_teardown_sdata(struct ieee80211_sub_if_data *sdata)
1481 {
1482         mesh_rmc_free(sdata);
1483         mesh_pathtbl_unregister(sdata);
1484 }