net/mac80211/tdls.c

   1 /*
   2  * mac80211 TDLS handling code
   3  *
   4  * Copyright 2006-2010  Johannes Berg <johannes@sipsolutions.net>
   5  * Copyright 2014, Intel Corporation
   6  * Copyright 2014  Intel Mobile Communications GmbH
   7  * Copyright 2015 - 2016 Intel Deutschland GmbH
   8  * Copyright (C) 2019 Intel Corporation
   9  *
  10  * This file is GPLv2 as found in COPYING.
  11  */
  12
  13 #include <linux/ieee80211.h>
  14 #include <linux/log2.h>
  15 #include <net/cfg80211.h>
  16 #include <linux/rtnetlink.h>
  17 #include "ieee80211_i.h"
  18 #include "driver-ops.h"
  19 #include "rate.h"
  20 #include "wme.h"
  21
  22 /* give usermode some time for retries in setting up the TDLS session */
  23 #define TDLS_PEER_SETUP_TIMEOUT (15 * HZ)
  24
  25 void ieee80211_tdls_peer_del_work(struct work_struct *wk)
  26 {
  27         struct ieee80211_sub_if_data *sdata;
  28         struct ieee80211_local *local;
  29
  30         sdata = container_of(wk, struct ieee80211_sub_if_data,
  31                              u.mgd.tdls_peer_del_work.work);
  32         local = sdata->local;
  33
  34         mutex_lock(&local->mtx);
  35         if (!is_zero_ether_addr(sdata->u.mgd.tdls_peer)) {
  36                 tdls_dbg(sdata, "TDLS del peer %pM\n", sdata->u.mgd.tdls_peer);
  37                 sta_info_destroy_addr(sdata, sdata->u.mgd.tdls_peer);
  38                 eth_zero_addr(sdata->u.mgd.tdls_peer);
  39         }
  40         mutex_unlock(&local->mtx);
  41 }
  42
  43 static void ieee80211_tdls_add_ext_capab(struct ieee80211_sub_if_data *sdata,
  44                                          struct sk_buff *skb)
  45 {
  46         struct ieee80211_local *local = sdata->local;
  47         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
  48         bool chan_switch = local->hw.wiphy->features &
  49                            NL80211_FEATURE_TDLS_CHANNEL_SWITCH;
  50         bool wider_band = ieee80211_hw_check(&local->hw, TDLS_WIDER_BW) &&
  51                           !ifmgd->tdls_wider_bw_prohibited;
  52         bool buffer_sta = ieee80211_hw_check(&local->hw,
  53                                              SUPPORTS_TDLS_BUFFER_STA);
  54         struct ieee80211_supported_band *sband = ieee80211_get_sband(sdata);
  55         bool vht = sband && sband->vht_cap.vht_supported;
  56         u8 *pos = skb_put(skb, 10);
  57
  58         *pos++ = WLAN_EID_EXT_CAPABILITY;
  59         *pos++ = 8; /* len */
  60         *pos++ = 0x0;
  61         *pos++ = 0x0;
  62         *pos++ = 0x0;
  63         *pos++ = (chan_switch ? WLAN_EXT_CAPA4_TDLS_CHAN_SWITCH : 0) |
  64                  (buffer_sta ? WLAN_EXT_CAPA4_TDLS_BUFFER_STA : 0);
  65         *pos++ = WLAN_EXT_CAPA5_TDLS_ENABLED;
  66         *pos++ = 0;
  67         *pos++ = 0;
  68         *pos++ = (vht && wider_band) ? WLAN_EXT_CAPA8_TDLS_WIDE_BW_ENABLED : 0;
  69 }
  70
  71 static u8
  72 ieee80211_tdls_add_subband(struct ieee80211_sub_if_data *sdata,
  73                            struct sk_buff *skb, u16 start, u16 end,
  74                            u16 spacing)
  75 {
  76         u8 subband_cnt = 0, ch_cnt = 0;
  77         struct ieee80211_channel *ch;
  78         struct cfg80211_chan_def chandef;
  79         int i, subband_start;
  80         struct wiphy *wiphy = sdata->local->hw.wiphy;
  81
  82         for (i = start; i <= end; i += spacing) {
  83                 if (!ch_cnt)
  84                         subband_start = i;
  85
  86                 ch = ieee80211_get_channel(sdata->local->hw.wiphy, i);
  87                 if (ch) {
  88                         /* we will be active on the channel */
  89                         cfg80211_chandef_create(&chandef, ch,
  90                                                 NL80211_CHAN_NO_HT);
  91                         if (cfg80211_reg_can_beacon_relax(wiphy, &chandef,
  92                                                           sdata->wdev.iftype)) {
  93                                 ch_cnt++;
  94                                 /*
  95                                  * check if the next channel is also part of
  96                                  * this allowed range
  97                                  */
  98                                 continue;
  99                         }
 100                 }
 101
 102                 /*
 103                  * we've reached the end of a range, with allowed channels
 104                  * found
 105                  */
 106                 if (ch_cnt) {
 107                         u8 *pos = skb_put(skb, 2);
 108                         *pos++ = ieee80211_frequency_to_channel(subband_start);
 109                         *pos++ = ch_cnt;
 110
 111                         subband_cnt++;
 112                         ch_cnt = 0;
 113                 }
 114         }
 115
 116         /* all channels in the requested range are allowed - add them here */
 117         if (ch_cnt) {
 118                 u8 *pos = skb_put(skb, 2);
 119                 *pos++ = ieee80211_frequency_to_channel(subband_start);
 120                 *pos++ = ch_cnt;
 121
 122                 subband_cnt++;
 123         }
 124
 125         return subband_cnt;
 126 }
 127
 128 static void
 129 ieee80211_tdls_add_supp_channels(struct ieee80211_sub_if_data *sdata,
 130                                  struct sk_buff *skb)
 131 {
 132         /*
 133          * Add possible channels for TDLS. These are channels that are allowed
 134          * to be active.
 135          */
 136         u8 subband_cnt;
 137         u8 *pos = skb_put(skb, 2);
 138
 139         *pos++ = WLAN_EID_SUPPORTED_CHANNELS;
 140
 141         /*
 142          * 5GHz and 2GHz channels numbers can overlap. Ignore this for now, as
 143          * this doesn't happen in real world scenarios.
 144          */
 145
 146         /* 2GHz, with 5MHz spacing */
 147         subband_cnt = ieee80211_tdls_add_subband(sdata, skb, 2412, 2472, 5);
 148
 149         /* 5GHz, with 20MHz spacing */
 150         subband_cnt += ieee80211_tdls_add_subband(sdata, skb, 5000, 5825, 20);
 151
 152         /* length */
 153         *pos = 2 * subband_cnt;
 154 }
 155
 156 static void ieee80211_tdls_add_oper_classes(struct ieee80211_sub_if_data *sdata,
 157                                             struct sk_buff *skb)
 158 {
 159         u8 *pos;
 160         u8 op_class;
 161
 162         if (!ieee80211_chandef_to_operating_class(&sdata->vif.bss_conf.chandef,
 163                                                   &op_class))
 164                 return;
 165
 166         pos = skb_put(skb, 4);
 167         *pos++ = WLAN_EID_SUPPORTED_REGULATORY_CLASSES;
 168         *pos++ = 2; /* len */
 169
 170         *pos++ = op_class;
 171         *pos++ = op_class; /* give current operating class as alternate too */
 172 }
 173
 174 static void ieee80211_tdls_add_bss_coex_ie(struct sk_buff *skb)
 175 {
 176         u8 *pos = skb_put(skb, 3);
 177
 178         *pos++ = WLAN_EID_BSS_COEX_2040;
 179         *pos++ = 1; /* len */
 180
 181         *pos++ = WLAN_BSS_COEX_INFORMATION_REQUEST;
 182 }
 183
 184 static u16 ieee80211_get_tdls_sta_capab(struct ieee80211_sub_if_data *sdata,
 185                                         u16 status_code)
 186 {
 187         struct ieee80211_supported_band *sband;
 188
 189         /* The capability will be 0 when sending a failure code */
 190         if (status_code != 0)
 191                 return 0;
 192
 193         sband = ieee80211_get_sband(sdata);
 194         if (sband && sband->band == NL80211_BAND_2GHZ) {
 195                 return WLAN_CAPABILITY_SHORT_SLOT_TIME |
 196                        WLAN_CAPABILITY_SHORT_PREAMBLE;
 197         }
 198
 199         return 0;
 200 }
 201
 202 static void ieee80211_tdls_add_link_ie(struct ieee80211_sub_if_data *sdata,
 203                                        struct sk_buff *skb, const u8 *peer,
 204                                        bool initiator)
 205 {
 206         struct ieee80211_tdls_lnkie *lnkid;
 207         const u8 *init_addr, *rsp_addr;
 208
 209         if (initiator) {
 210                 init_addr = sdata->vif.addr;
 211                 rsp_addr = peer;
 212         } else {
 213                 init_addr = peer;
 214                 rsp_addr = sdata->vif.addr;
 215         }
 216
 217         lnkid = skb_put(skb, sizeof(struct ieee80211_tdls_lnkie));
 218
 219         lnkid->ie_type = WLAN_EID_LINK_ID;
 220         lnkid->ie_len = sizeof(struct ieee80211_tdls_lnkie) - 2;
 221
 222         memcpy(lnkid->bssid, sdata->u.mgd.bssid, ETH_ALEN);
 223         memcpy(lnkid->init_sta, init_addr, ETH_ALEN);
 224         memcpy(lnkid->resp_sta, rsp_addr, ETH_ALEN);
 225 }
 226
 227 static void
 228 ieee80211_tdls_add_aid(struct ieee80211_sub_if_data *sdata, struct sk_buff *skb)
 229 {
 230         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
 231         u8 *pos = skb_put(skb, 4);
 232
 233         *pos++ = WLAN_EID_AID;
 234         *pos++ = 2; /* len */
 235         put_unaligned_le16(ifmgd->aid, pos);
 236 }
 237
 238 /* translate numbering in the WMM parameter IE to the mac80211 notation */
 239 static enum ieee80211_ac_numbers ieee80211_ac_from_wmm(int ac)
 240 {
 241         switch (ac) {
 242         default:
 243                 WARN_ON_ONCE(1);
 244                 /* fall through */
 245         case 0:
 246                 return IEEE80211_AC_BE;
 247         case 1:
 248                 return IEEE80211_AC_BK;
 249         case 2:
 250                 return IEEE80211_AC_VI;
 251         case 3:
 252                 return IEEE80211_AC_VO;
 253         }
 254 }
 255
 256 static u8 ieee80211_wmm_aci_aifsn(int aifsn, bool acm, int aci)
 257 {
 258         u8 ret;
 259
 260         ret = aifsn & 0x0f;
 261         if (acm)
 262                 ret |= 0x10;
 263         ret |= (aci << 5) & 0x60;
 264         return ret;
 265 }
 266
 267 static u8 ieee80211_wmm_ecw(u16 cw_min, u16 cw_max)
 268 {
 269         return ((ilog2(cw_min + 1) << 0x0) & 0x0f) |
 270                ((ilog2(cw_max + 1) << 0x4) & 0xf0);
 271 }
 272
 273 static void ieee80211_tdls_add_wmm_param_ie(struct ieee80211_sub_if_data *sdata,
 274                                             struct sk_buff *skb)
 275 {
 276         struct ieee80211_wmm_param_ie *wmm;
 277         struct ieee80211_tx_queue_params *txq;
 278         int i;
 279
 280         wmm = skb_put_zero(skb, sizeof(*wmm));
 281
 282         wmm->element_id = WLAN_EID_VENDOR_SPECIFIC;
 283         wmm->len = sizeof(*wmm) - 2;
 284
 285         wmm->oui[0] = 0x00; /* Microsoft OUI 00:50:F2 */
 286         wmm->oui[1] = 0x50;
 287         wmm->oui[2] = 0xf2;
 288         wmm->oui_type = 2; /* WME */
 289         wmm->oui_subtype = 1; /* WME param */
 290         wmm->version = 1; /* WME ver */
 291         wmm->qos_info = 0; /* U-APSD not in use */
 292
 293         /*
 294          * Use the EDCA parameters defined for the BSS, or default if the AP
 295          * doesn't support it, as mandated by 802.11-2012 section 10.22.4
 296          */
 297         for (i = 0; i < IEEE80211_NUM_ACS; i++) {
 298                 txq = &sdata->tx_conf[ieee80211_ac_from_wmm(i)];
 299                 wmm->ac[i].aci_aifsn = ieee80211_wmm_aci_aifsn(txq->aifs,
 300                                                                txq->acm, i);
 301                 wmm->ac[i].cw = ieee80211_wmm_ecw(txq->cw_min, txq->cw_max);
 302                 wmm->ac[i].txop_limit = cpu_to_le16(txq->txop);
 303         }
 304 }
 305
 306 static void
 307 ieee80211_tdls_chandef_vht_upgrade(struct ieee80211_sub_if_data *sdata,
 308                                    struct sta_info *sta)
 309 {
 310         /* IEEE802.11ac-2013 Table E-4 */
 311         u16 centers_80mhz[] = { 5210, 5290, 5530, 5610, 5690, 5775 };
 312         struct cfg80211_chan_def uc = sta->tdls_chandef;
 313         enum nl80211_chan_width max_width = ieee80211_sta_cap_chan_bw(sta);
 314         int i;
 315
 316         /* only support upgrading non-narrow channels up to 80Mhz */
 317         if (max_width == NL80211_CHAN_WIDTH_5 ||
 318             max_width == NL80211_CHAN_WIDTH_10)
 319                 return;
 320
 321         if (max_width > NL80211_CHAN_WIDTH_80)
 322                 max_width = NL80211_CHAN_WIDTH_80;
 323
 324         if (uc.width >= max_width)
 325                 return;
 326         /*
 327          * Channel usage constrains in the IEEE802.11ac-2013 specification only
 328          * allow expanding a 20MHz channel to 80MHz in a single way. In
 329          * addition, there are no 40MHz allowed channels that are not part of
 330          * the allowed 80MHz range in the 5GHz spectrum (the relevant one here).
 331          */
 332         for (i = 0; i < ARRAY_SIZE(centers_80mhz); i++)
 333                 if (abs(uc.chan->center_freq - centers_80mhz[i]) <= 30) {
 334                         uc.center_freq1 = centers_80mhz[i];
 335                         uc.center_freq2 = 0;
 336                         uc.width = NL80211_CHAN_WIDTH_80;
 337                         break;
 338                 }
 339
 340         if (!uc.center_freq1)
 341                 return;
 342
 343         /* proceed to downgrade the chandef until usable or the same as AP BW */
 344         while (uc.width > max_width ||
 345                (uc.width > sta->tdls_chandef.width &&
 346                 !cfg80211_reg_can_beacon_relax(sdata->local->hw.wiphy, &uc,
 347                                                sdata->wdev.iftype)))
 348                 ieee80211_chandef_downgrade(&uc);
 349
 350         if (!cfg80211_chandef_identical(&uc, &sta->tdls_chandef)) {
 351                 tdls_dbg(sdata, "TDLS ch width upgraded %d -> %d\n",
 352                          sta->tdls_chandef.width, uc.width);
 353
 354                 /*
 355                  * the station is not yet authorized when BW upgrade is done,
 356                  * locking is not required
 357                  */
 358                 sta->tdls_chandef = uc;
 359         }
 360 }
 361
 362 static void
 363 ieee80211_tdls_add_setup_start_ies(struct ieee80211_sub_if_data *sdata,
 364                                    struct sk_buff *skb, const u8 *peer,
 365                                    u8 action_code, bool initiator,
 366                                    const u8 *extra_ies, size_t extra_ies_len)
 367 {
 368         struct ieee80211_supported_band *sband;
 369         struct ieee80211_local *local = sdata->local;
 370         struct ieee80211_sta_ht_cap ht_cap;
 371         struct ieee80211_sta_vht_cap vht_cap;
 372         struct sta_info *sta = NULL;
 373         size_t offset = 0, noffset;
 374         u8 *pos;
 375
 376         sband = ieee80211_get_sband(sdata);
 377         if (!sband)
 378                 return;
 379
 380         ieee80211_add_srates_ie(sdata, skb, false, sband->band);
 381         ieee80211_add_ext_srates_ie(sdata, skb, false, sband->band);
 382         ieee80211_tdls_add_supp_channels(sdata, skb);
 383
 384         /* add any custom IEs that go before Extended Capabilities */
 385         if (extra_ies_len) {
 386                 static const u8 before_ext_cap[] = {
 387                         WLAN_EID_SUPP_RATES,
 388                         WLAN_EID_COUNTRY,
 389                         WLAN_EID_EXT_SUPP_RATES,
 390                         WLAN_EID_SUPPORTED_CHANNELS,
 391                         WLAN_EID_RSN,
 392                 };
 393                 noffset = ieee80211_ie_split(extra_ies, extra_ies_len,
 394                                              before_ext_cap,
 395                                              ARRAY_SIZE(before_ext_cap),
 396                                              offset);
 397                 skb_put_data(skb, extra_ies + offset, noffset - offset);
 398                 offset = noffset;
 399         }
 400
 401         ieee80211_tdls_add_ext_capab(sdata, skb);
 402
 403         /* add the QoS element if we support it */
 404         if (local->hw.queues >= IEEE80211_NUM_ACS &&
 405             action_code != WLAN_PUB_ACTION_TDLS_DISCOVER_RES)
 406                 ieee80211_add_wmm_info_ie(skb_put(skb, 9), 0); /* no U-APSD */
 407
 408         /* add any custom IEs that go before HT capabilities */
 409         if (extra_ies_len) {
 410                 static const u8 before_ht_cap[] = {
 411                         WLAN_EID_SUPP_RATES,
 412                         WLAN_EID_COUNTRY,
 413                         WLAN_EID_EXT_SUPP_RATES,
 414                         WLAN_EID_SUPPORTED_CHANNELS,
 415                         WLAN_EID_RSN,
 416                         WLAN_EID_EXT_CAPABILITY,
 417                         WLAN_EID_QOS_CAPA,
 418                         WLAN_EID_FAST_BSS_TRANSITION,
 419                         WLAN_EID_TIMEOUT_INTERVAL,
 420                         WLAN_EID_SUPPORTED_REGULATORY_CLASSES,
 421                 };
 422                 noffset = ieee80211_ie_split(extra_ies, extra_ies_len,
 423                                              before_ht_cap,
 424                                              ARRAY_SIZE(before_ht_cap),
 425                                              offset);
 426                 skb_put_data(skb, extra_ies + offset, noffset - offset);
 427                 offset = noffset;
 428         }
 429
 430         mutex_lock(&local->sta_mtx);
 431
 432         /* we should have the peer STA if we're already responding */
 433         if (action_code == WLAN_TDLS_SETUP_RESPONSE) {
 434                 sta = sta_info_get(sdata, peer);
 435                 if (WARN_ON_ONCE(!sta)) {
 436                         mutex_unlock(&local->sta_mtx);
 437                         return;
 438                 }
 439
 440                 sta->tdls_chandef = sdata->vif.bss_conf.chandef;
 441         }
 442
 443         ieee80211_tdls_add_oper_classes(sdata, skb);
 444
 445         /*
 446          * with TDLS we can switch channels, and HT-caps are not necessarily
 447          * the same on all bands. The specification limits the setup to a
 448          * single HT-cap, so use the current band for now.
 449          */
 450         memcpy(&ht_cap, &sband->ht_cap, sizeof(ht_cap));
 451
 452         if ((action_code == WLAN_TDLS_SETUP_REQUEST ||
 453              action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) &&
 454             ht_cap.ht_supported) {
 455                 ieee80211_apply_htcap_overrides(sdata, &ht_cap);
 456
 457                 /* disable SMPS in TDLS initiator */
 458                 ht_cap.cap |= WLAN_HT_CAP_SM_PS_DISABLED
 459                                 << IEEE80211_HT_CAP_SM_PS_SHIFT;
 460
 461                 pos = skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
 462                 ieee80211_ie_build_ht_cap(pos, &ht_cap, ht_cap.cap);
 463         } else if (action_code == WLAN_TDLS_SETUP_RESPONSE &&
 464                    ht_cap.ht_supported && sta->sta.ht_cap.ht_supported) {
 465                 /* the peer caps are already intersected with our own */
 466                 memcpy(&ht_cap, &sta->sta.ht_cap, sizeof(ht_cap));
 467
 468                 pos = skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
 469                 ieee80211_ie_build_ht_cap(pos, &ht_cap, ht_cap.cap);
 470         }
 471
 472         if (ht_cap.ht_supported &&
 473             (ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
 474                 ieee80211_tdls_add_bss_coex_ie(skb);
 475
 476         ieee80211_tdls_add_link_ie(sdata, skb, peer, initiator);
 477
 478         /* add any custom IEs that go before VHT capabilities */
 479         if (extra_ies_len) {
 480                 static const u8 before_vht_cap[] = {
 481                         WLAN_EID_SUPP_RATES,
 482                         WLAN_EID_COUNTRY,
 483                         WLAN_EID_EXT_SUPP_RATES,
 484                         WLAN_EID_SUPPORTED_CHANNELS,
 485                         WLAN_EID_RSN,
 486                         WLAN_EID_EXT_CAPABILITY,
 487                         WLAN_EID_QOS_CAPA,
 488                         WLAN_EID_FAST_BSS_TRANSITION,
 489                         WLAN_EID_TIMEOUT_INTERVAL,
 490                         WLAN_EID_SUPPORTED_REGULATORY_CLASSES,
 491                         WLAN_EID_MULTI_BAND,
 492                 };
 493                 noffset = ieee80211_ie_split(extra_ies, extra_ies_len,
 494                                              before_vht_cap,
 495                                              ARRAY_SIZE(before_vht_cap),
 496                                              offset);
 497                 skb_put_data(skb, extra_ies + offset, noffset - offset);
 498                 offset = noffset;
 499         }
 500
 501         /* build the VHT-cap similarly to the HT-cap */
 502         memcpy(&vht_cap, &sband->vht_cap, sizeof(vht_cap));
 503         if ((action_code == WLAN_TDLS_SETUP_REQUEST ||
 504              action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) &&
 505             vht_cap.vht_supported) {
 506                 ieee80211_apply_vhtcap_overrides(sdata, &vht_cap);
 507
 508                 /* the AID is present only when VHT is implemented */
 509                 if (action_code == WLAN_TDLS_SETUP_REQUEST)
 510                         ieee80211_tdls_add_aid(sdata, skb);
 511
 512                 pos = skb_put(skb, sizeof(struct ieee80211_vht_cap) + 2);
 513                 ieee80211_ie_build_vht_cap(pos, &vht_cap, vht_cap.cap);
 514         } else if (action_code == WLAN_TDLS_SETUP_RESPONSE &&
 515                    vht_cap.vht_supported && sta->sta.vht_cap.vht_supported) {
 516                 /* the peer caps are already intersected with our own */
 517                 memcpy(&vht_cap, &sta->sta.vht_cap, sizeof(vht_cap));
 518
 519                 /* the AID is present only when VHT is implemented */
 520                 ieee80211_tdls_add_aid(sdata, skb);
 521
 522                 pos = skb_put(skb, sizeof(struct ieee80211_vht_cap) + 2);
 523                 ieee80211_ie_build_vht_cap(pos, &vht_cap, vht_cap.cap);
 524
 525                 /*
 526                  * if both peers support WIDER_BW, we can expand the chandef to
 527                  * a wider compatible one, up to 80MHz
 528                  */
 529                 if (test_sta_flag(sta, WLAN_STA_TDLS_WIDER_BW))
 530                         ieee80211_tdls_chandef_vht_upgrade(sdata, sta);
 531         }
 532
 533         mutex_unlock(&local->sta_mtx);
 534
 535         /* add any remaining IEs */
 536         if (extra_ies_len) {
 537                 noffset = extra_ies_len;
 538                 skb_put_data(skb, extra_ies + offset, noffset - offset);
 539         }
 540
 541 }
 542
 543 static void
 544 ieee80211_tdls_add_setup_cfm_ies(struct ieee80211_sub_if_data *sdata,
 545                                  struct sk_buff *skb, const u8 *peer,
 546                                  bool initiator, const u8 *extra_ies,
 547                                  size_t extra_ies_len)
 548 {
 549         struct ieee80211_local *local = sdata->local;
 550         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
 551         size_t offset = 0, noffset;
 552         struct sta_info *sta, *ap_sta;
 553         struct ieee80211_supported_band *sband;
 554         u8 *pos;
 555
 556         sband = ieee80211_get_sband(sdata);
 557         if (!sband)
 558                 return;
 559
 560         mutex_lock(&local->sta_mtx);
 561
 562         sta = sta_info_get(sdata, peer);
 563         ap_sta = sta_info_get(sdata, ifmgd->bssid);
 564         if (WARN_ON_ONCE(!sta || !ap_sta)) {
 565                 mutex_unlock(&local->sta_mtx);
 566                 return;
 567         }
 568
 569         sta->tdls_chandef = sdata->vif.bss_conf.chandef;
 570
 571         /* add any custom IEs that go before the QoS IE */
 572         if (extra_ies_len) {
 573                 static const u8 before_qos[] = {
 574                         WLAN_EID_RSN,
 575                 };
 576                 noffset = ieee80211_ie_split(extra_ies, extra_ies_len,
 577                                              before_qos,
 578                                              ARRAY_SIZE(before_qos),
 579                                              offset);
 580                 skb_put_data(skb, extra_ies + offset, noffset - offset);
 581                 offset = noffset;
 582         }
 583
 584         /* add the QoS param IE if both the peer and we support it */
 585         if (local->hw.queues >= IEEE80211_NUM_ACS && sta->sta.wme)
 586                 ieee80211_tdls_add_wmm_param_ie(sdata, skb);
 587
 588         /* add any custom IEs that go before HT operation */
 589         if (extra_ies_len) {
 590                 static const u8 before_ht_op[] = {
 591                         WLAN_EID_RSN,
 592                         WLAN_EID_QOS_CAPA,
 593                         WLAN_EID_FAST_BSS_TRANSITION,
 594                         WLAN_EID_TIMEOUT_INTERVAL,
 595                 };
 596                 noffset = ieee80211_ie_split(extra_ies, extra_ies_len,
 597                                              before_ht_op,
 598                                              ARRAY_SIZE(before_ht_op),
 599                                              offset);
 600                 skb_put_data(skb, extra_ies + offset, noffset - offset);
 601                 offset = noffset;
 602         }
 603
 604         /*
 605          * if HT support is only added in TDLS, we need an HT-operation IE.
 606          * add the IE as required by IEEE802.11-2012 9.23.3.2.
 607          */
 608         if (!ap_sta->sta.ht_cap.ht_supported && sta->sta.ht_cap.ht_supported) {
 609                 u16 prot = IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED |
 610                            IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT |
 611                            IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
 612
 613                 pos = skb_put(skb, 2 + sizeof(struct ieee80211_ht_operation));
 614                 ieee80211_ie_build_ht_oper(pos, &sta->sta.ht_cap,
 615                                            &sdata->vif.bss_conf.chandef, prot,
 616                                            true);
 617         }
 618
 619         ieee80211_tdls_add_link_ie(sdata, skb, peer, initiator);
 620
 621         /* only include VHT-operation if not on the 2.4GHz band */
 622         if (sband->band != NL80211_BAND_2GHZ &&
 623             sta->sta.vht_cap.vht_supported) {
 624                 /*
 625                  * if both peers support WIDER_BW, we can expand the chandef to
 626                  * a wider compatible one, up to 80MHz
 627                  */
 628                 if (test_sta_flag(sta, WLAN_STA_TDLS_WIDER_BW))
 629                         ieee80211_tdls_chandef_vht_upgrade(sdata, sta);
 630
 631                 pos = skb_put(skb, 2 + sizeof(struct ieee80211_vht_operation));
 632                 ieee80211_ie_build_vht_oper(pos, &sta->sta.vht_cap,
 633                                             &sta->tdls_chandef);
 634         }
 635
 636         mutex_unlock(&local->sta_mtx);
 637
 638         /* add any remaining IEs */
 639         if (extra_ies_len) {
 640                 noffset = extra_ies_len;
 641                 skb_put_data(skb, extra_ies + offset, noffset - offset);
 642         }
 643 }
 644
 645 static void
 646 ieee80211_tdls_add_chan_switch_req_ies(struct ieee80211_sub_if_data *sdata,
 647                                        struct sk_buff *skb, const u8 *peer,
 648                                        bool initiator, const u8 *extra_ies,
 649                                        size_t extra_ies_len, u8 oper_class,
 650                                        struct cfg80211_chan_def *chandef)
 651 {
 652         struct ieee80211_tdls_data *tf;
 653         size_t offset = 0, noffset;
 654
 655         if (WARN_ON_ONCE(!chandef))
 656                 return;
 657
 658         tf = (void *)skb->data;
 659         tf->u.chan_switch_req.target_channel =
 660                 ieee80211_frequency_to_channel(chandef->chan->center_freq);
 661         tf->u.chan_switch_req.oper_class = oper_class;
 662
 663         if (extra_ies_len) {
 664                 static const u8 before_lnkie[] = {
 665                         WLAN_EID_SECONDARY_CHANNEL_OFFSET,
 666                 };
 667                 noffset = ieee80211_ie_split(extra_ies, extra_ies_len,
 668                                              before_lnkie,
 669                                              ARRAY_SIZE(before_lnkie),
 670                                              offset);
 671                 skb_put_data(skb, extra_ies + offset, noffset - offset);
 672                 offset = noffset;
 673         }
 674
 675         ieee80211_tdls_add_link_ie(sdata, skb, peer, initiator);
 676
 677         /* add any remaining IEs */
 678         if (extra_ies_len) {
 679                 noffset = extra_ies_len;
 680                 skb_put_data(skb, extra_ies + offset, noffset - offset);
 681         }
 682 }
 683
 684 static void
 685 ieee80211_tdls_add_chan_switch_resp_ies(struct ieee80211_sub_if_data *sdata,
 686                                         struct sk_buff *skb, const u8 *peer,
 687                                         u16 status_code, bool initiator,
 688                                         const u8 *extra_ies,
 689                                         size_t extra_ies_len)
 690 {
 691         if (status_code == 0)
 692                 ieee80211_tdls_add_link_ie(sdata, skb, peer, initiator);
 693
 694         if (extra_ies_len)
 695                 skb_put_data(skb, extra_ies, extra_ies_len);
 696 }
 697
 698 static void ieee80211_tdls_add_ies(struct ieee80211_sub_if_data *sdata,
 699                                    struct sk_buff *skb, const u8 *peer,
 700                                    u8 action_code, u16 status_code,
 701                                    bool initiator, const u8 *extra_ies,
 702                                    size_t extra_ies_len, u8 oper_class,
 703                                    struct cfg80211_chan_def *chandef)
 704 {
 705         switch (action_code) {
 706         case WLAN_TDLS_SETUP_REQUEST:
 707         case WLAN_TDLS_SETUP_RESPONSE:
 708         case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
 709                 if (status_code == 0)
 710                         ieee80211_tdls_add_setup_start_ies(sdata, skb, peer,
 711                                                            action_code,
 712                                                            initiator,
 713                                                            extra_ies,
 714                                                            extra_ies_len);
 715                 break;
 716         case WLAN_TDLS_SETUP_CONFIRM:
 717                 if (status_code == 0)
 718                         ieee80211_tdls_add_setup_cfm_ies(sdata, skb, peer,
 719                                                          initiator, extra_ies,
 720                                                          extra_ies_len);
 721                 break;
 722         case WLAN_TDLS_TEARDOWN:
 723         case WLAN_TDLS_DISCOVERY_REQUEST:
 724                 if (extra_ies_len)
 725                         skb_put_data(skb, extra_ies, extra_ies_len);
 726                 if (status_code == 0 || action_code == WLAN_TDLS_TEARDOWN)
 727                         ieee80211_tdls_add_link_ie(sdata, skb, peer, initiator);
 728                 break;
 729         case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
 730                 ieee80211_tdls_add_chan_switch_req_ies(sdata, skb, peer,
 731                                                        initiator, extra_ies,
 732                                                        extra_ies_len,
 733                                                        oper_class, chandef);
 734                 break;
 735         case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
 736                 ieee80211_tdls_add_chan_switch_resp_ies(sdata, skb, peer,
 737                                                         status_code,
 738                                                         initiator, extra_ies,
 739                                                         extra_ies_len);
 740                 break;
 741         }
 742
 743 }
 744
 745 static int
 746 ieee80211_prep_tdls_encap_data(struct wiphy *wiphy, struct net_device *dev,
 747                                const u8 *peer, u8 action_code, u8 dialog_token,
 748                                u16 status_code, struct sk_buff *skb)
 749 {
 750         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
 751         struct ieee80211_tdls_data *tf;
 752
 753         tf = skb_put(skb, offsetof(struct ieee80211_tdls_data, u));
 754
 755         memcpy(tf->da, peer, ETH_ALEN);
 756         memcpy(tf->sa, sdata->vif.addr, ETH_ALEN);
 757         tf->ether_type = cpu_to_be16(ETH_P_TDLS);
 758         tf->payload_type = WLAN_TDLS_SNAP_RFTYPE;
 759
 760         /* network header is after the ethernet header */
 761         skb_set_network_header(skb, ETH_HLEN);
 762
 763         switch (action_code) {
 764         case WLAN_TDLS_SETUP_REQUEST:
 765                 tf->category = WLAN_CATEGORY_TDLS;
 766                 tf->action_code = WLAN_TDLS_SETUP_REQUEST;
 767
 768                 skb_put(skb, sizeof(tf->u.setup_req));
 769                 tf->u.setup_req.dialog_token = dialog_token;
 770                 tf->u.setup_req.capability =
 771                         cpu_to_le16(ieee80211_get_tdls_sta_capab(sdata,
 772                                                                  status_code));
 773                 break;
 774         case WLAN_TDLS_SETUP_RESPONSE:
 775                 tf->category = WLAN_CATEGORY_TDLS;
 776                 tf->action_code = WLAN_TDLS_SETUP_RESPONSE;
 777
 778                 skb_put(skb, sizeof(tf->u.setup_resp));
 779                 tf->u.setup_resp.status_code = cpu_to_le16(status_code);
 780                 tf->u.setup_resp.dialog_token = dialog_token;
 781                 tf->u.setup_resp.capability =
 782                         cpu_to_le16(ieee80211_get_tdls_sta_capab(sdata,
 783                                                                  status_code));
 784                 break;
 785         case WLAN_TDLS_SETUP_CONFIRM:
 786                 tf->category = WLAN_CATEGORY_TDLS;
 787                 tf->action_code = WLAN_TDLS_SETUP_CONFIRM;
 788
 789                 skb_put(skb, sizeof(tf->u.setup_cfm));
 790                 tf->u.setup_cfm.status_code = cpu_to_le16(status_code);
 791                 tf->u.setup_cfm.dialog_token = dialog_token;
 792                 break;
 793         case WLAN_TDLS_TEARDOWN:
 794                 tf->category = WLAN_CATEGORY_TDLS;
 795                 tf->action_code = WLAN_TDLS_TEARDOWN;
 796
 797                 skb_put(skb, sizeof(tf->u.teardown));
 798                 tf->u.teardown.reason_code = cpu_to_le16(status_code);
 799                 break;
 800         case WLAN_TDLS_DISCOVERY_REQUEST:
 801                 tf->category = WLAN_CATEGORY_TDLS;
 802                 tf->action_code = WLAN_TDLS_DISCOVERY_REQUEST;
 803
 804                 skb_put(skb, sizeof(tf->u.discover_req));
 805                 tf->u.discover_req.dialog_token = dialog_token;
 806                 break;
 807         case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
 808                 tf->category = WLAN_CATEGORY_TDLS;
 809                 tf->action_code = WLAN_TDLS_CHANNEL_SWITCH_REQUEST;
 810
 811                 skb_put(skb, sizeof(tf->u.chan_switch_req));
 812                 break;
 813         case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
 814                 tf->category = WLAN_CATEGORY_TDLS;
 815                 tf->action_code = WLAN_TDLS_CHANNEL_SWITCH_RESPONSE;
 816
 817                 skb_put(skb, sizeof(tf->u.chan_switch_resp));
 818                 tf->u.chan_switch_resp.status_code = cpu_to_le16(status_code);
 819                 break;
 820         default:
 821                 return -EINVAL;
 822         }
 823
 824         return 0;
 825 }
 826
 827 static int
 828 ieee80211_prep_tdls_direct(struct wiphy *wiphy, struct net_device *dev,
 829                            const u8 *peer, u8 action_code, u8 dialog_token,
 830                            u16 status_code, struct sk_buff *skb)
 831 {
 832         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
 833         struct ieee80211_mgmt *mgmt;
 834
 835         mgmt = skb_put_zero(skb, 24);
 836         memcpy(mgmt->da, peer, ETH_ALEN);
 837         memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
 838         memcpy(mgmt->bssid, sdata->u.mgd.bssid, ETH_ALEN);
 839
 840         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
 841                                           IEEE80211_STYPE_ACTION);
 842
 843         switch (action_code) {
 844         case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
 845                 skb_put(skb, 1 + sizeof(mgmt->u.action.u.tdls_discover_resp));
 846                 mgmt->u.action.category = WLAN_CATEGORY_PUBLIC;
 847                 mgmt->u.action.u.tdls_discover_resp.action_code =
 848                         WLAN_PUB_ACTION_TDLS_DISCOVER_RES;
 849                 mgmt->u.action.u.tdls_discover_resp.dialog_token =
 850                         dialog_token;
 851                 mgmt->u.action.u.tdls_discover_resp.capability =
 852                         cpu_to_le16(ieee80211_get_tdls_sta_capab(sdata,
 853                                                                  status_code));
 854                 break;
 855         default:
 856                 return -EINVAL;
 857         }
 858
 859         return 0;
 860 }
 861
 862 static struct sk_buff *
 863 ieee80211_tdls_build_mgmt_packet_data(struct ieee80211_sub_if_data *sdata,
 864                                       const u8 *peer, u8 action_code,
 865                                       u8 dialog_token, u16 status_code,
 866                                       bool initiator, const u8 *extra_ies,
 867                                       size_t extra_ies_len, u8 oper_class,
 868                                       struct cfg80211_chan_def *chandef)
 869 {
 870         struct ieee80211_local *local = sdata->local;
 871         struct sk_buff *skb;
 872         int ret;
 873
 874         skb = netdev_alloc_skb(sdata->dev,
 875                                local->hw.extra_tx_headroom +
 876                                max(sizeof(struct ieee80211_mgmt),
 877                                    sizeof(struct ieee80211_tdls_data)) +
 878                                50 + /* supported rates */
 879                                10 + /* ext capab */
 880                                26 + /* max(WMM-info, WMM-param) */
 881                                2 + max(sizeof(struct ieee80211_ht_cap),
 882                                        sizeof(struct ieee80211_ht_operation)) +
 883                                2 + max(sizeof(struct ieee80211_vht_cap),
 884                                        sizeof(struct ieee80211_vht_operation)) +
 885                                50 + /* supported channels */
 886                                3 + /* 40/20 BSS coex */
 887                                4 + /* AID */
 888                                4 + /* oper classes */
 889                                extra_ies_len +
 890                                sizeof(struct ieee80211_tdls_lnkie));
 891         if (!skb)
 892                 return NULL;
 893
 894         skb_reserve(skb, local->hw.extra_tx_headroom);
 895
 896         switch (action_code) {
 897         case WLAN_TDLS_SETUP_REQUEST:
 898         case WLAN_TDLS_SETUP_RESPONSE:
 899         case WLAN_TDLS_SETUP_CONFIRM:
 900         case WLAN_TDLS_TEARDOWN:
 901         case WLAN_TDLS_DISCOVERY_REQUEST:
 902         case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
 903         case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
 904                 ret = ieee80211_prep_tdls_encap_data(local->hw.wiphy,
 905                                                      sdata->dev, peer,
 906                                                      action_code, dialog_token,
 907                                                      status_code, skb);
 908                 break;
 909         case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
 910                 ret = ieee80211_prep_tdls_direct(local->hw.wiphy, sdata->dev,
 911                                                  peer, action_code,
 912                                                  dialog_token, status_code,
 913                                                  skb);
 914                 break;
 915         default:
 916                 ret = -ENOTSUPP;
 917                 break;
 918         }
 919
 920         if (ret < 0)
 921                 goto fail;
 922
 923         ieee80211_tdls_add_ies(sdata, skb, peer, action_code, status_code,
 924                                initiator, extra_ies, extra_ies_len, oper_class,
 925                                chandef);
 926         return skb;
 927
 928 fail:
 929         dev_kfree_skb(skb);
 930         return NULL;
 931 }
 932
 933 static int
 934 ieee80211_tdls_prep_mgmt_packet(struct wiphy *wiphy, struct net_device *dev,
 935                                 const u8 *peer, u8 action_code, u8 dialog_token,
 936                                 u16 status_code, u32 peer_capability,
 937                                 bool initiator, const u8 *extra_ies,
 938                                 size_t extra_ies_len, u8 oper_class,
 939                                 struct cfg80211_chan_def *chandef)
 940 {
 941         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
 942         struct sk_buff *skb = NULL;
 943         struct sta_info *sta;
 944         u32 flags = 0;
 945         int ret = 0;
 946
 947         rcu_read_lock();
 948         sta = sta_info_get(sdata, peer);
 949
 950         /* infer the initiator if we can, to support old userspace */
 951         switch (action_code) {
 952         case WLAN_TDLS_SETUP_REQUEST:
 953                 if (sta) {
 954                         set_sta_flag(sta, WLAN_STA_TDLS_INITIATOR);
 955                         sta->sta.tdls_initiator = false;
 956                 }
 957                 /* fall-through */
 958         case WLAN_TDLS_SETUP_CONFIRM:
 959         case WLAN_TDLS_DISCOVERY_REQUEST:
 960                 initiator = true;
 961                 break;
 962         case WLAN_TDLS_SETUP_RESPONSE:
 963                 /*
 964                  * In some testing scenarios, we send a request and response.
 965                  * Make the last packet sent take effect for the initiator
 966                  * value.
 967                  */
 968                 if (sta) {
 969                         clear_sta_flag(sta, WLAN_STA_TDLS_INITIATOR);
 970                         sta->sta.tdls_initiator = true;
 971                 }
 972                 /* fall-through */
 973         case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
 974                 initiator = false;
 975                 break;
 976         case WLAN_TDLS_TEARDOWN:
 977         case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
 978         case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
 979                 /* any value is ok */
 980                 break;
 981         default:
 982                 ret = -ENOTSUPP;
 983                 break;
 984         }
 985
 986         if (sta && test_sta_flag(sta, WLAN_STA_TDLS_INITIATOR))
 987                 initiator = true;
 988
 989         rcu_read_unlock();
 990         if (ret < 0)
 991                 goto fail;
 992
 993         skb = ieee80211_tdls_build_mgmt_packet_data(sdata, peer, action_code,
 994                                                     dialog_token, status_code,
 995                                                     initiator, extra_ies,
 996                                                     extra_ies_len, oper_class,
 997                                                     chandef);
 998         if (!skb) {
 999                 ret = -EINVAL;
1000                 goto fail;
1001         }
1002
1003         if (action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) {
1004                 ieee80211_tx_skb(sdata, skb);
1005                 return 0;
1006         }
1007
1008         /*
1009          * According to 802.11z: Setup req/resp are sent in AC_BK, otherwise
1010          * we should default to AC_VI.
1011          */
1012         switch (action_code) {
1013         case WLAN_TDLS_SETUP_REQUEST:
1014         case WLAN_TDLS_SETUP_RESPONSE:
1015                 skb->priority = 256 + 2;
1016                 break;
1017         default:
1018                 skb->priority = 256 + 5;
1019                 break;
1020         }
1021         skb_set_queue_mapping(skb, ieee80211_select_queue(sdata, skb));
1022
1023         /*
1024          * Set the WLAN_TDLS_TEARDOWN flag to indicate a teardown in progress.
1025          * Later, if no ACK is returned from peer, we will re-send the teardown
1026          * packet through the AP.
1027          */
1028         if ((action_code == WLAN_TDLS_TEARDOWN) &&
1029             ieee80211_hw_check(&sdata->local->hw, REPORTS_TX_ACK_STATUS)) {
1030                 bool try_resend; /* Should we keep skb for possible resend */
1031
1032                 /* If not sending directly to peer - no point in keeping skb */
1033                 rcu_read_lock();
1034                 sta = sta_info_get(sdata, peer);
1035                 try_resend = sta && test_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH);
1036                 rcu_read_unlock();
1037
1038                 spin_lock_bh(&sdata->u.mgd.teardown_lock);
1039                 if (try_resend && !sdata->u.mgd.teardown_skb) {
1040                         /* Mark it as requiring TX status callback  */
1041                         flags |= IEEE80211_TX_CTL_REQ_TX_STATUS |
1042                                  IEEE80211_TX_INTFL_MLME_CONN_TX;
1043
1044                         /*
1045                          * skb is copied since mac80211 will later set
1046                          * properties that might not be the same as the AP,
1047                          * such as encryption, QoS, addresses, etc.
1048                          *
1049                          * No problem if skb_copy() fails, so no need to check.
1050                          */
1051                         sdata->u.mgd.teardown_skb = skb_copy(skb, GFP_ATOMIC);
1052                         sdata->u.mgd.orig_teardown_skb = skb;
1053                 }
1054                 spin_unlock_bh(&sdata->u.mgd.teardown_lock);
1055         }
1056
1057         /* disable bottom halves when entering the Tx path */
1058         local_bh_disable();
1059         __ieee80211_subif_start_xmit(skb, dev, flags);
1060         local_bh_enable();
1061
1062         return ret;
1063
1064 fail:
1065         dev_kfree_skb(skb);
1066         return ret;
1067 }
1068
1069 static int
1070 ieee80211_tdls_mgmt_setup(struct wiphy *wiphy, struct net_device *dev,
1071                           const u8 *peer, u8 action_code, u8 dialog_token,
1072                           u16 status_code, u32 peer_capability, bool initiator,
1073                           const u8 *extra_ies, size_t extra_ies_len)
1074 {
1075         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1076         struct ieee80211_local *local = sdata->local;
1077         enum ieee80211_smps_mode smps_mode = sdata->u.mgd.driver_smps_mode;
1078         int ret;
1079
1080         /* don't support setup with forced SMPS mode that's not off */
1081         if (smps_mode != IEEE80211_SMPS_AUTOMATIC &&
1082             smps_mode != IEEE80211_SMPS_OFF) {
1083                 tdls_dbg(sdata, "Aborting TDLS setup due to SMPS mode %d\n",
1084                          smps_mode);
1085                 return -ENOTSUPP;
1086         }
1087
1088         mutex_lock(&local->mtx);
1089
1090         /* we don't support concurrent TDLS peer setups */
1091         if (!is_zero_ether_addr(sdata->u.mgd.tdls_peer) &&
1092             !ether_addr_equal(sdata->u.mgd.tdls_peer, peer)) {
1093                 ret = -EBUSY;
1094                 goto out_unlock;
1095         }
1096
1097         /*
1098          * make sure we have a STA representing the peer so we drop or buffer
1099          * non-TDLS-setup frames to the peer. We can't send other packets
1100          * during setup through the AP path.
1101          * Allow error packets to be sent - sometimes we don't even add a STA
1102          * before failing the setup.
1103          */
1104         if (status_code == 0) {
1105                 rcu_read_lock();
1106                 if (!sta_info_get(sdata, peer)) {
1107                         rcu_read_unlock();
1108                         ret = -ENOLINK;
1109                         goto out_unlock;
1110                 }
1111                 rcu_read_unlock();
1112         }
1113
1114         ieee80211_flush_queues(local, sdata, false);
1115         memcpy(sdata->u.mgd.tdls_peer, peer, ETH_ALEN);
1116         mutex_unlock(&local->mtx);
1117
1118         /* we cannot take the mutex while preparing the setup packet */
1119         ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer, action_code,
1120                                               dialog_token, status_code,
1121                                               peer_capability, initiator,
1122                                               extra_ies, extra_ies_len, 0,
1123                                               NULL);
1124         if (ret < 0) {
1125                 mutex_lock(&local->mtx);
1126                 eth_zero_addr(sdata->u.mgd.tdls_peer);
1127                 mutex_unlock(&local->mtx);
1128                 return ret;
1129         }
1130
1131         ieee80211_queue_delayed_work(&sdata->local->hw,
1132                                      &sdata->u.mgd.tdls_peer_del_work,
1133                                      TDLS_PEER_SETUP_TIMEOUT);
1134         return 0;
1135
1136 out_unlock:
1137         mutex_unlock(&local->mtx);
1138         return ret;
1139 }
1140
1141 static int
1142 ieee80211_tdls_mgmt_teardown(struct wiphy *wiphy, struct net_device *dev,
1143                              const u8 *peer, u8 action_code, u8 dialog_token,
1144                              u16 status_code, u32 peer_capability,
1145                              bool initiator, const u8 *extra_ies,
1146                              size_t extra_ies_len)
1147 {
1148         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1149         struct ieee80211_local *local = sdata->local;
1150         struct sta_info *sta;
1151         int ret;
1152
1153         /*
1154          * No packets can be transmitted to the peer via the AP during setup -
1155          * the STA is set as a TDLS peer, but is not authorized.
1156          * During teardown, we prevent direct transmissions by stopping the
1157          * queues and flushing all direct packets.
1158          */
1159         ieee80211_stop_vif_queues(local, sdata,
1160                                   IEEE80211_QUEUE_STOP_REASON_TDLS_TEARDOWN);
1161         ieee80211_flush_queues(local, sdata, false);
1162
1163         ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer, action_code,
1164                                               dialog_token, status_code,
1165                                               peer_capability, initiator,
1166                                               extra_ies, extra_ies_len, 0,
1167                                               NULL);
1168         if (ret < 0)
1169                 sdata_err(sdata, "Failed sending TDLS teardown packet %d\n",
1170                           ret);
1171
1172         /*
1173          * Remove the STA AUTH flag to force further traffic through the AP. If
1174          * the STA was unreachable, it was already removed.
1175          */
1176         rcu_read_lock();
1177         sta = sta_info_get(sdata, peer);
1178         if (sta)
1179                 clear_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH);
1180         rcu_read_unlock();
1181
1182         ieee80211_wake_vif_queues(local, sdata,
1183                                   IEEE80211_QUEUE_STOP_REASON_TDLS_TEARDOWN);
1184
1185         return 0;
1186 }
1187
1188 int ieee80211_tdls_mgmt(struct wiphy *wiphy, struct net_device *dev,
1189                         const u8 *peer, u8 action_code, u8 dialog_token,
1190                         u16 status_code, u32 peer_capability,
1191                         bool initiator, const u8 *extra_ies,
1192                         size_t extra_ies_len)
1193 {
1194         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1195         int ret;
1196
1197         if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS))
1198                 return -ENOTSUPP;
1199
1200         /* make sure we are in managed mode, and associated */
1201         if (sdata->vif.type != NL80211_IFTYPE_STATION ||
1202             !sdata->u.mgd.associated)
1203                 return -EINVAL;
1204
1205         switch (action_code) {
1206         case WLAN_TDLS_SETUP_REQUEST:
1207         case WLAN_TDLS_SETUP_RESPONSE:
1208                 ret = ieee80211_tdls_mgmt_setup(wiphy, dev, peer, action_code,
1209                                                 dialog_token, status_code,
1210                                                 peer_capability, initiator,
1211                                                 extra_ies, extra_ies_len);
1212                 break;
1213         case WLAN_TDLS_TEARDOWN:
1214                 ret = ieee80211_tdls_mgmt_teardown(wiphy, dev, peer,
1215                                                    action_code, dialog_token,
1216                                                    status_code,
1217                                                    peer_capability, initiator,
1218                                                    extra_ies, extra_ies_len);
1219                 break;
1220         case WLAN_TDLS_DISCOVERY_REQUEST:
1221                 /*
1222                  * Protect the discovery so we can hear the TDLS discovery
1223                  * response frame. It is transmitted directly and not buffered
1224                  * by the AP.
1225                  */
1226                 drv_mgd_protect_tdls_discover(sdata->local, sdata);
1227                 /* fall-through */
1228         case WLAN_TDLS_SETUP_CONFIRM:
1229         case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
1230                 /* no special handling */
1231                 ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer,
1232                                                       action_code,
1233                                                       dialog_token,
1234                                                       status_code,
1235                                                       peer_capability,
1236                                                       initiator, extra_ies,
1237                                                       extra_ies_len, 0, NULL);
1238                 break;
1239         default:
1240                 ret = -EOPNOTSUPP;
1241                 break;
1242         }
1243
1244         tdls_dbg(sdata, "TDLS mgmt action %d peer %pM status %d\n",
1245                  action_code, peer, ret);
1246         return ret;
1247 }
1248
1249 static void iee80211_tdls_recalc_chanctx(struct ieee80211_sub_if_data *sdata,
1250                                          struct sta_info *sta)
1251 {
1252         struct ieee80211_local *local = sdata->local;
1253         struct ieee80211_chanctx_conf *conf;
1254         struct ieee80211_chanctx *ctx;
1255         enum nl80211_chan_width width;
1256         struct ieee80211_supported_band *sband;
1257
1258         mutex_lock(&local->chanctx_mtx);
1259         conf = rcu_dereference_protected(sdata->vif.chanctx_conf,
1260                                          lockdep_is_held(&local->chanctx_mtx));
1261         if (conf) {
1262                 width = conf->def.width;
1263                 sband = local->hw.wiphy->bands[conf->def.chan->band];
1264                 ctx = container_of(conf, struct ieee80211_chanctx, conf);
1265                 ieee80211_recalc_chanctx_chantype(local, ctx);
1266
1267                 /* if width changed and a peer is given, update its BW */
1268                 if (width != conf->def.width && sta &&
1269                     test_sta_flag(sta, WLAN_STA_TDLS_WIDER_BW)) {
1270                         enum ieee80211_sta_rx_bandwidth bw;
1271
1272                         bw = ieee80211_chan_width_to_rx_bw(conf->def.width);
1273                         bw = min(bw, ieee80211_sta_cap_rx_bw(sta));
1274                         if (bw != sta->sta.bandwidth) {
1275                                 sta->sta.bandwidth = bw;
1276                                 rate_control_rate_update(local, sband, sta,
1277                                                          IEEE80211_RC_BW_CHANGED);
1278                                 /*
1279                                  * if a TDLS peer BW was updated, we need to
1280                                  * recalc the chandef width again, to get the
1281                                  * correct chanctx min_def
1282                                  */
1283                                 ieee80211_recalc_chanctx_chantype(local, ctx);
1284                         }
1285                 }
1286
1287         }
1288         mutex_unlock(&local->chanctx_mtx);
1289 }
1290
1291 static int iee80211_tdls_have_ht_peers(struct ieee80211_sub_if_data *sdata)
1292 {
1293         struct sta_info *sta;
1294         bool result = false;
1295
1296         rcu_read_lock();
1297         list_for_each_entry_rcu(sta, &sdata->local->sta_list, list) {
1298                 if (!sta->sta.tdls || sta->sdata != sdata || !sta->uploaded ||
1299                     !test_sta_flag(sta, WLAN_STA_AUTHORIZED) ||
1300                     !test_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH) ||
1301                     !sta->sta.ht_cap.ht_supported)
1302                         continue;
1303                 result = true;
1304                 break;
1305         }
1306         rcu_read_unlock();
1307
1308         return result;
1309 }
1310
1311 static void
1312 iee80211_tdls_recalc_ht_protection(struct ieee80211_sub_if_data *sdata,
1313                                    struct sta_info *sta)
1314 {
1315         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
1316         bool tdls_ht;
1317         u16 protection = IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED |
1318                          IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT |
1319                          IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
1320         u16 opmode;
1321
1322         /* Nothing to do if the BSS connection uses HT */
1323         if (!(ifmgd->flags & IEEE80211_STA_DISABLE_HT))
1324                 return;
1325
1326         tdls_ht = (sta && sta->sta.ht_cap.ht_supported) ||
1327                   iee80211_tdls_have_ht_peers(sdata);
1328
1329         opmode = sdata->vif.bss_conf.ht_operation_mode;
1330
1331         if (tdls_ht)
1332                 opmode |= protection;
1333         else
1334                 opmode &= ~protection;
1335
1336         if (opmode == sdata->vif.bss_conf.ht_operation_mode)
1337                 return;
1338
1339         sdata->vif.bss_conf.ht_operation_mode = opmode;
1340         ieee80211_bss_info_change_notify(sdata, BSS_CHANGED_HT);
1341 }
1342
1343 int ieee80211_tdls_oper(struct wiphy *wiphy, struct net_device *dev,
1344                         const u8 *peer, enum nl80211_tdls_operation oper)
1345 {
1346         struct sta_info *sta;
1347         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1348         struct ieee80211_local *local = sdata->local;
1349         int ret;
1350
1351         if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS))
1352                 return -ENOTSUPP;
1353
1354         if (sdata->vif.type != NL80211_IFTYPE_STATION)
1355                 return -EINVAL;
1356
1357         switch (oper) {
1358         case NL80211_TDLS_ENABLE_LINK:
1359         case NL80211_TDLS_DISABLE_LINK:
1360                 break;
1361         case NL80211_TDLS_TEARDOWN:
1362         case NL80211_TDLS_SETUP:
1363         case NL80211_TDLS_DISCOVERY_REQ:
1364                 /* We don't support in-driver setup/teardown/discovery */
1365                 return -ENOTSUPP;
1366         }
1367
1368         /* protect possible bss_conf changes and avoid concurrency in
1369          * ieee80211_bss_info_change_notify()
1370          */
1371         sdata_lock(sdata);
1372         mutex_lock(&local->mtx);
1373         tdls_dbg(sdata, "TDLS oper %d peer %pM\n", oper, peer);
1374
1375         switch (oper) {
1376         case NL80211_TDLS_ENABLE_LINK:
1377                 if (sdata->vif.csa_active) {
1378                         tdls_dbg(sdata, "TDLS: disallow link during CSA\n");
1379                         ret = -EBUSY;
1380                         break;
1381                 }
1382
1383                 mutex_lock(&local->sta_mtx);
1384                 sta = sta_info_get(sdata, peer);
1385                 if (!sta) {
1386                         mutex_unlock(&local->sta_mtx);
1387                         ret = -ENOLINK;
1388                         break;
1389                 }
1390
1391                 iee80211_tdls_recalc_chanctx(sdata, sta);
1392                 iee80211_tdls_recalc_ht_protection(sdata, sta);
1393
1394                 set_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH);
1395                 mutex_unlock(&local->sta_mtx);
1396
1397                 WARN_ON_ONCE(is_zero_ether_addr(sdata->u.mgd.tdls_peer) ||
1398                              !ether_addr_equal(sdata->u.mgd.tdls_peer, peer));
1399                 ret = 0;
1400                 break;
1401         case NL80211_TDLS_DISABLE_LINK:
1402                 /*
1403                  * The teardown message in ieee80211_tdls_mgmt_teardown() was
1404                  * created while the queues were stopped, so it might still be
1405                  * pending. Before flushing the queues we need to be sure the
1406                  * message is handled by the tasklet handling pending messages,
1407                  * otherwise we might start destroying the station before
1408                  * sending the teardown packet.
1409                  * Note that this only forces the tasklet to flush pendings -
1410                  * not to stop the tasklet from rescheduling itself.
1411                  */
1412                 tasklet_kill(&local->tx_pending_tasklet);
1413                 /* flush a potentially queued teardown packet */
1414                 ieee80211_flush_queues(local, sdata, false);
1415
1416                 ret = sta_info_destroy_addr(sdata, peer);
1417
1418                 mutex_lock(&local->sta_mtx);
1419                 iee80211_tdls_recalc_ht_protection(sdata, NULL);
1420                 mutex_unlock(&local->sta_mtx);
1421
1422                 iee80211_tdls_recalc_chanctx(sdata, NULL);
1423                 break;
1424         default:
1425                 ret = -ENOTSUPP;
1426                 break;
1427         }
1428
1429         if (ret == 0 && ether_addr_equal(sdata->u.mgd.tdls_peer, peer)) {
1430                 cancel_delayed_work(&sdata->u.mgd.tdls_peer_del_work);
1431                 eth_zero_addr(sdata->u.mgd.tdls_peer);
1432         }
1433
1434         if (ret == 0)
1435                 ieee80211_queue_work(&sdata->local->hw,
1436                                      &sdata->u.mgd.request_smps_work);
1437
1438         mutex_unlock(&local->mtx);
1439         sdata_unlock(sdata);
1440         return ret;
1441 }
1442
1443 void ieee80211_tdls_oper_request(struct ieee80211_vif *vif, const u8 *peer,
1444                                  enum nl80211_tdls_operation oper,
1445                                  u16 reason_code, gfp_t gfp)
1446 {
1447         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
1448
1449         if (vif->type != NL80211_IFTYPE_STATION || !vif->bss_conf.assoc) {
1450                 sdata_err(sdata, "Discarding TDLS oper %d - not STA or disconnected\n",
1451                           oper);
1452                 return;
1453         }
1454
1455         cfg80211_tdls_oper_request(sdata->dev, peer, oper, reason_code, gfp);
1456 }
1457 EXPORT_SYMBOL(ieee80211_tdls_oper_request);
1458
1459 static void
1460 iee80211_tdls_add_ch_switch_timing(u8 *buf, u16 switch_time, u16 switch_timeout)
1461 {
1462         struct ieee80211_ch_switch_timing *ch_sw;
1463
1464         *buf++ = WLAN_EID_CHAN_SWITCH_TIMING;
1465         *buf++ = sizeof(struct ieee80211_ch_switch_timing);
1466
1467         ch_sw = (void *)buf;
1468         ch_sw->switch_time = cpu_to_le16(switch_time);
1469         ch_sw->switch_timeout = cpu_to_le16(switch_timeout);
1470 }
1471
1472 /* find switch timing IE in SKB ready for Tx */
1473 static const u8 *ieee80211_tdls_find_sw_timing_ie(struct sk_buff *skb)
1474 {
1475         struct ieee80211_tdls_data *tf;
1476         const u8 *ie_start;
1477
1478         /*
1479          * Get the offset for the new location of the switch timing IE.
1480          * The SKB network header will now point to the "payload_type"
1481          * element of the TDLS data frame struct.
1482          */
1483         tf = container_of(skb->data + skb_network_offset(skb),
1484                           struct ieee80211_tdls_data, payload_type);
1485         ie_start = tf->u.chan_switch_req.variable;
1486         return cfg80211_find_ie(WLAN_EID_CHAN_SWITCH_TIMING, ie_start,
1487                                 skb->len - (ie_start - skb->data));
1488 }
1489
1490 static struct sk_buff *
1491 ieee80211_tdls_ch_sw_tmpl_get(struct sta_info *sta, u8 oper_class,
1492                               struct cfg80211_chan_def *chandef,
1493                               u32 *ch_sw_tm_ie_offset)
1494 {
1495         struct ieee80211_sub_if_data *sdata = sta->sdata;
1496         u8 extra_ies[2 + sizeof(struct ieee80211_sec_chan_offs_ie) +
1497                      2 + sizeof(struct ieee80211_ch_switch_timing)];
1498         int extra_ies_len = 2 + sizeof(struct ieee80211_ch_switch_timing);
1499         u8 *pos = extra_ies;
1500         struct sk_buff *skb;
1501
1502         /*
1503          * if chandef points to a wide channel add a Secondary-Channel
1504          * Offset information element
1505          */
1506         if (chandef->width == NL80211_CHAN_WIDTH_40) {
1507                 struct ieee80211_sec_chan_offs_ie *sec_chan_ie;
1508                 bool ht40plus;
1509
1510                 *pos++ = WLAN_EID_SECONDARY_CHANNEL_OFFSET;
1511                 *pos++ = sizeof(*sec_chan_ie);
1512                 sec_chan_ie = (void *)pos;
1513
1514                 ht40plus = cfg80211_get_chandef_type(chandef) ==
1515                                                         NL80211_CHAN_HT40PLUS;
1516                 sec_chan_ie->sec_chan_offs = ht40plus ?
1517                                              IEEE80211_HT_PARAM_CHA_SEC_ABOVE :
1518                                              IEEE80211_HT_PARAM_CHA_SEC_BELOW;
1519                 pos += sizeof(*sec_chan_ie);
1520
1521                 extra_ies_len += 2 + sizeof(struct ieee80211_sec_chan_offs_ie);
1522         }
1523
1524         /* just set the values to 0, this is a template */
1525         iee80211_tdls_add_ch_switch_timing(pos, 0, 0);
1526
1527         skb = ieee80211_tdls_build_mgmt_packet_data(sdata, sta->sta.addr,
1528                                               WLAN_TDLS_CHANNEL_SWITCH_REQUEST,
1529                                               0, 0, !sta->sta.tdls_initiator,
1530                                               extra_ies, extra_ies_len,
1531                                               oper_class, chandef);
1532         if (!skb)
1533                 return NULL;
1534
1535         skb = ieee80211_build_data_template(sdata, skb, 0);
1536         if (IS_ERR(skb)) {
1537                 tdls_dbg(sdata, "Failed building TDLS channel switch frame\n");
1538                 return NULL;
1539         }
1540
1541         if (ch_sw_tm_ie_offset) {
1542                 const u8 *tm_ie = ieee80211_tdls_find_sw_timing_ie(skb);
1543
1544                 if (!tm_ie) {
1545                         tdls_dbg(sdata, "No switch timing IE in TDLS switch\n");
1546                         dev_kfree_skb_any(skb);
1547                         return NULL;
1548                 }
1549
1550                 *ch_sw_tm_ie_offset = tm_ie - skb->data;
1551         }
1552
1553         tdls_dbg(sdata,
1554                  "TDLS channel switch request template for %pM ch %d width %d\n",
1555                  sta->sta.addr, chandef->chan->center_freq, chandef->width);
1556         return skb;
1557 }
1558
1559 int
1560 ieee80211_tdls_channel_switch(struct wiphy *wiphy, struct net_device *dev,
1561                               const u8 *addr, u8 oper_class,
1562                               struct cfg80211_chan_def *chandef)
1563 {
1564         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1565         struct ieee80211_local *local = sdata->local;
1566         struct sta_info *sta;
1567         struct sk_buff *skb = NULL;
1568         u32 ch_sw_tm_ie;
1569         int ret;
1570
1571         mutex_lock(&local->sta_mtx);
1572         sta = sta_info_get(sdata, addr);
1573         if (!sta) {
1574                 tdls_dbg(sdata,
1575                          "Invalid TDLS peer %pM for channel switch request\n",
1576                          addr);
1577                 ret = -ENOENT;
1578                 goto out;
1579         }
1580
1581         if (!test_sta_flag(sta, WLAN_STA_TDLS_CHAN_SWITCH)) {
1582                 tdls_dbg(sdata, "TDLS channel switch unsupported by %pM\n",
1583                          addr);
1584                 ret = -ENOTSUPP;
1585                 goto out;
1586         }
1587
1588         skb = ieee80211_tdls_ch_sw_tmpl_get(sta, oper_class, chandef,
1589                                             &ch_sw_tm_ie);
1590         if (!skb) {
1591                 ret = -ENOENT;
1592                 goto out;
1593         }
1594
1595         ret = drv_tdls_channel_switch(local, sdata, &sta->sta, oper_class,
1596                                       chandef, skb, ch_sw_tm_ie);
1597         if (!ret)
1598                 set_sta_flag(sta, WLAN_STA_TDLS_OFF_CHANNEL);
1599
1600 out:
1601         mutex_unlock(&local->sta_mtx);
1602         dev_kfree_skb_any(skb);
1603         return ret;
1604 }
1605
1606 void
1607 ieee80211_tdls_cancel_channel_switch(struct wiphy *wiphy,
1608                                      struct net_device *dev,
1609                                      const u8 *addr)
1610 {
1611         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1612         struct ieee80211_local *local = sdata->local;
1613         struct sta_info *sta;
1614
1615         mutex_lock(&local->sta_mtx);
1616         sta = sta_info_get(sdata, addr);
1617         if (!sta) {
1618                 tdls_dbg(sdata,
1619                          "Invalid TDLS peer %pM for channel switch cancel\n",
1620                          addr);
1621                 goto out;
1622         }
1623
1624         if (!test_sta_flag(sta, WLAN_STA_TDLS_OFF_CHANNEL)) {
1625                 tdls_dbg(sdata, "TDLS channel switch not initiated by %pM\n",
1626                          addr);
1627                 goto out;
1628         }
1629
1630         drv_tdls_cancel_channel_switch(local, sdata, &sta->sta);
1631         clear_sta_flag(sta, WLAN_STA_TDLS_OFF_CHANNEL);
1632
1633 out:
1634         mutex_unlock(&local->sta_mtx);
1635 }
1636
1637 static struct sk_buff *
1638 ieee80211_tdls_ch_sw_resp_tmpl_get(struct sta_info *sta,
1639                                    u32 *ch_sw_tm_ie_offset)
1640 {
1641         struct ieee80211_sub_if_data *sdata = sta->sdata;
1642         struct sk_buff *skb;
1643         u8 extra_ies[2 + sizeof(struct ieee80211_ch_switch_timing)];
1644
1645         /* initial timing are always zero in the template */
1646         iee80211_tdls_add_ch_switch_timing(extra_ies, 0, 0);
1647
1648         skb = ieee80211_tdls_build_mgmt_packet_data(sdata, sta->sta.addr,
1649                                         WLAN_TDLS_CHANNEL_SWITCH_RESPONSE,
1650                                         0, 0, !sta->sta.tdls_initiator,
1651                                         extra_ies, sizeof(extra_ies), 0, NULL);
1652         if (!skb)
1653                 return NULL;
1654
1655         skb = ieee80211_build_data_template(sdata, skb, 0);
1656         if (IS_ERR(skb)) {
1657                 tdls_dbg(sdata,
1658                          "Failed building TDLS channel switch resp frame\n");
1659                 return NULL;
1660         }
1661
1662         if (ch_sw_tm_ie_offset) {
1663                 const u8 *tm_ie = ieee80211_tdls_find_sw_timing_ie(skb);
1664
1665                 if (!tm_ie) {
1666                         tdls_dbg(sdata,
1667                                  "No switch timing IE in TDLS switch resp\n");
1668                         dev_kfree_skb_any(skb);
1669                         return NULL;
1670                 }
1671
1672                 *ch_sw_tm_ie_offset = tm_ie - skb->data;
1673         }
1674
1675         tdls_dbg(sdata, "TDLS get channel switch response template for %pM\n",
1676                  sta->sta.addr);
1677         return skb;
1678 }
1679
1680 static int
1681 ieee80211_process_tdls_channel_switch_resp(struct ieee80211_sub_if_data *sdata,
1682                                            struct sk_buff *skb)
1683 {
1684         struct ieee80211_local *local = sdata->local;
1685         struct ieee802_11_elems elems;
1686         struct sta_info *sta;
1687         struct ieee80211_tdls_data *tf = (void *)skb->data;
1688         bool local_initiator;
1689         struct ieee80211_rx_status *rx_status = IEEE80211_SKB_RXCB(skb);
1690         int baselen = offsetof(typeof(*tf), u.chan_switch_resp.variable);
1691         struct ieee80211_tdls_ch_sw_params params = {};
1692         int ret;
1693
1694         params.action_code = WLAN_TDLS_CHANNEL_SWITCH_RESPONSE;
1695         params.timestamp = rx_status->device_timestamp;
1696
1697         if (skb->len < baselen) {
1698                 tdls_dbg(sdata, "TDLS channel switch resp too short: %d\n",
1699                          skb->len);
1700                 return -EINVAL;
1701         }
1702
1703         mutex_lock(&local->sta_mtx);
1704         sta = sta_info_get(sdata, tf->sa);
1705         if (!sta || !test_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH)) {
1706                 tdls_dbg(sdata, "TDLS chan switch from non-peer sta %pM\n",
1707                          tf->sa);
1708                 ret = -EINVAL;
1709                 goto out;
1710         }
1711
1712         params.sta = &sta->sta;
1713         params.status = le16_to_cpu(tf->u.chan_switch_resp.status_code);
1714         if (params.status != 0) {
1715                 ret = 0;
1716                 goto call_drv;
1717         }
1718
1719         ieee802_11_parse_elems(tf->u.chan_switch_resp.variable,
1720                                skb->len - baselen, false, &elems,
1721                                NULL, NULL);
1722         if (elems.parse_error) {
1723                 tdls_dbg(sdata, "Invalid IEs in TDLS channel switch resp\n");
1724                 ret = -EINVAL;
1725                 goto out;
1726         }
1727
1728         if (!elems.ch_sw_timing || !elems.lnk_id) {
1729                 tdls_dbg(sdata, "TDLS channel switch resp - missing IEs\n");
1730                 ret = -EINVAL;
1731                 goto out;
1732         }
1733
1734         /* validate the initiator is set correctly */
1735         local_initiator =
1736                 !memcmp(elems.lnk_id->init_sta, sdata->vif.addr, ETH_ALEN);
1737         if (local_initiator == sta->sta.tdls_initiator) {
1738                 tdls_dbg(sdata, "TDLS chan switch invalid lnk-id initiator\n");
1739                 ret = -EINVAL;
1740                 goto out;
1741         }
1742
1743         params.switch_time = le16_to_cpu(elems.ch_sw_timing->switch_time);
1744         params.switch_timeout = le16_to_cpu(elems.ch_sw_timing->switch_timeout);
1745
1746         params.tmpl_skb =
1747                 ieee80211_tdls_ch_sw_resp_tmpl_get(sta, &params.ch_sw_tm_ie);
1748         if (!params.tmpl_skb) {
1749                 ret = -ENOENT;
1750                 goto out;
1751         }
1752
1753         ret = 0;
1754 call_drv:
1755         drv_tdls_recv_channel_switch(sdata->local, sdata, &params);
1756
1757         tdls_dbg(sdata,
1758                  "TDLS channel switch response received from %pM status %d\n",
1759                  tf->sa, params.status);
1760
1761 out:
1762         mutex_unlock(&local->sta_mtx);
1763         dev_kfree_skb_any(params.tmpl_skb);
1764         return ret;
1765 }
1766
1767 static int
1768 ieee80211_process_tdls_channel_switch_req(struct ieee80211_sub_if_data *sdata,
1769                                           struct sk_buff *skb)
1770 {
1771         struct ieee80211_local *local = sdata->local;
1772         struct ieee802_11_elems elems;
1773         struct cfg80211_chan_def chandef;
1774         struct ieee80211_channel *chan;
1775         enum nl80211_channel_type chan_type;
1776         int freq;
1777         u8 target_channel, oper_class;
1778         bool local_initiator;
1779         struct sta_info *sta;
1780         enum nl80211_band band;
1781         struct ieee80211_tdls_data *tf = (void *)skb->data;
1782         struct ieee80211_rx_status *rx_status = IEEE80211_SKB_RXCB(skb);
1783         int baselen = offsetof(typeof(*tf), u.chan_switch_req.variable);
1784         struct ieee80211_tdls_ch_sw_params params = {};
1785         int ret = 0;
1786
1787         params.action_code = WLAN_TDLS_CHANNEL_SWITCH_REQUEST;
1788         params.timestamp = rx_status->device_timestamp;
1789
1790         if (skb->len < baselen) {
1791                 tdls_dbg(sdata, "TDLS channel switch req too short: %d\n",
1792                          skb->len);
1793                 return -EINVAL;
1794         }
1795
1796         target_channel = tf->u.chan_switch_req.target_channel;
1797         oper_class = tf->u.chan_switch_req.oper_class;
1798
1799         /*
1800          * We can't easily infer the channel band. The operating class is
1801          * ambiguous - there are multiple tables (US/Europe/JP/Global). The
1802          * solution here is to treat channels with number >14 as 5GHz ones,
1803          * and specifically check for the (oper_class, channel) combinations
1804          * where this doesn't hold. These are thankfully unique according to
1805          * IEEE802.11-2012.
1806          * We consider only the 2GHz and 5GHz bands and 20MHz+ channels as
1807          * valid here.
1808          */
1809         if ((oper_class == 112 || oper_class == 2 || oper_class == 3 ||
1810              oper_class == 4 || oper_class == 5 || oper_class == 6) &&
1811              target_channel < 14)
1812                 band = NL80211_BAND_5GHZ;
1813         else
1814                 band = target_channel < 14 ? NL80211_BAND_2GHZ :
1815                                              NL80211_BAND_5GHZ;
1816
1817         freq = ieee80211_channel_to_frequency(target_channel, band);
1818         if (freq == 0) {
1819                 tdls_dbg(sdata, "Invalid channel in TDLS chan switch: %d\n",
1820                          target_channel);
1821                 return -EINVAL;
1822         }
1823
1824         chan = ieee80211_get_channel(sdata->local->hw.wiphy, freq);
1825         if (!chan) {
1826                 tdls_dbg(sdata,
1827                          "Unsupported channel for TDLS chan switch: %d\n",
1828                          target_channel);
1829                 return -EINVAL;
1830         }
1831
1832         ieee802_11_parse_elems(tf->u.chan_switch_req.variable,
1833                                skb->len - baselen, false, &elems, NULL, NULL);
1834         if (elems.parse_error) {
1835                 tdls_dbg(sdata, "Invalid IEs in TDLS channel switch req\n");
1836                 return -EINVAL;
1837         }
1838
1839         if (!elems.ch_sw_timing || !elems.lnk_id) {
1840                 tdls_dbg(sdata, "TDLS channel switch req - missing IEs\n");
1841                 return -EINVAL;
1842         }
1843
1844         if (!elems.sec_chan_offs) {
1845                 chan_type = NL80211_CHAN_HT20;
1846         } else {
1847                 switch (elems.sec_chan_offs->sec_chan_offs) {
1848                 case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
1849                         chan_type = NL80211_CHAN_HT40PLUS;
1850                         break;
1851                 case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
1852                         chan_type = NL80211_CHAN_HT40MINUS;
1853                         break;
1854                 default:
1855                         chan_type = NL80211_CHAN_HT20;
1856                         break;
1857                 }
1858         }
1859
1860         cfg80211_chandef_create(&chandef, chan, chan_type);
1861
1862         /* we will be active on the TDLS link */
1863         if (!cfg80211_reg_can_beacon_relax(sdata->local->hw.wiphy, &chandef,
1864                                            sdata->wdev.iftype)) {
1865                 tdls_dbg(sdata, "TDLS chan switch to forbidden channel\n");
1866                 return -EINVAL;
1867         }
1868
1869         mutex_lock(&local->sta_mtx);
1870         sta = sta_info_get(sdata, tf->sa);
1871         if (!sta || !test_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH)) {
1872                 tdls_dbg(sdata, "TDLS chan switch from non-peer sta %pM\n",
1873                          tf->sa);
1874                 ret = -EINVAL;
1875                 goto out;
1876         }
1877
1878         params.sta = &sta->sta;
1879
1880         /* validate the initiator is set correctly */
1881         local_initiator =
1882                 !memcmp(elems.lnk_id->init_sta, sdata->vif.addr, ETH_ALEN);
1883         if (local_initiator == sta->sta.tdls_initiator) {
1884                 tdls_dbg(sdata, "TDLS chan switch invalid lnk-id initiator\n");
1885                 ret = -EINVAL;
1886                 goto out;
1887         }
1888
1889         /* peer should have known better */
1890         if (!sta->sta.ht_cap.ht_supported && elems.sec_chan_offs &&
1891             elems.sec_chan_offs->sec_chan_offs) {
1892                 tdls_dbg(sdata, "TDLS chan switch - wide chan unsupported\n");
1893                 ret = -ENOTSUPP;
1894                 goto out;
1895         }
1896
1897         params.chandef = &chandef;
1898         params.switch_time = le16_to_cpu(elems.ch_sw_timing->switch_time);
1899         params.switch_timeout = le16_to_cpu(elems.ch_sw_timing->switch_timeout);
1900
1901         params.tmpl_skb =
1902                 ieee80211_tdls_ch_sw_resp_tmpl_get(sta,
1903                                                    &params.ch_sw_tm_ie);
1904         if (!params.tmpl_skb) {
1905                 ret = -ENOENT;
1906                 goto out;
1907         }
1908
1909         drv_tdls_recv_channel_switch(sdata->local, sdata, &params);
1910
1911         tdls_dbg(sdata,
1912                  "TDLS ch switch request received from %pM ch %d width %d\n",
1913                  tf->sa, params.chandef->chan->center_freq,
1914                  params.chandef->width);
1915 out:
1916         mutex_unlock(&local->sta_mtx);
1917         dev_kfree_skb_any(params.tmpl_skb);
1918         return ret;
1919 }
1920
1921 static void
1922 ieee80211_process_tdls_channel_switch(struct ieee80211_sub_if_data *sdata,
1923                                       struct sk_buff *skb)
1924 {
1925         struct ieee80211_tdls_data *tf = (void *)skb->data;
1926         struct wiphy *wiphy = sdata->local->hw.wiphy;
1927
1928         ASSERT_RTNL();
1929
1930         /* make sure the driver supports it */
1931         if (!(wiphy->features & NL80211_FEATURE_TDLS_CHANNEL_SWITCH))
1932                 return;
1933
1934         /* we want to access the entire packet */
1935         if (skb_linearize(skb))
1936                 return;
1937         /*
1938          * The packet/size was already validated by mac80211 Rx path, only look
1939          * at the action type.
1940          */
1941         switch (tf->action_code) {
1942         case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
1943                 ieee80211_process_tdls_channel_switch_req(sdata, skb);
1944                 break;
1945         case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
1946                 ieee80211_process_tdls_channel_switch_resp(sdata, skb);
1947                 break;
1948         default:
1949                 WARN_ON_ONCE(1);
1950                 return;
1951         }
1952 }
1953
1954 void ieee80211_teardown_tdls_peers(struct ieee80211_sub_if_data *sdata)
1955 {
1956         struct sta_info *sta;
1957         u16 reason = WLAN_REASON_TDLS_TEARDOWN_UNSPECIFIED;
1958
1959         rcu_read_lock();
1960         list_for_each_entry_rcu(sta, &sdata->local->sta_list, list) {
1961                 if (!sta->sta.tdls || sta->sdata != sdata || !sta->uploaded ||
1962                     !test_sta_flag(sta, WLAN_STA_AUTHORIZED))
1963                         continue;
1964
1965                 ieee80211_tdls_oper_request(&sdata->vif, sta->sta.addr,
1966                                             NL80211_TDLS_TEARDOWN, reason,
1967                                             GFP_ATOMIC);
1968         }
1969         rcu_read_unlock();
1970 }
1971
1972 void ieee80211_tdls_chsw_work(struct work_struct *wk)
1973 {
1974         struct ieee80211_local *local =
1975                 container_of(wk, struct ieee80211_local, tdls_chsw_work);
1976         struct ieee80211_sub_if_data *sdata;
1977         struct sk_buff *skb;
1978         struct ieee80211_tdls_data *tf;
1979
1980         rtnl_lock();
1981         while ((skb = skb_dequeue(&local->skb_queue_tdls_chsw))) {
1982                 tf = (struct ieee80211_tdls_data *)skb->data;
1983                 list_for_each_entry(sdata, &local->interfaces, list) {
1984                         if (!ieee80211_sdata_running(sdata) ||
1985                             sdata->vif.type != NL80211_IFTYPE_STATION ||
1986                             !ether_addr_equal(tf->da, sdata->vif.addr))
1987                                 continue;
1988
1989                         ieee80211_process_tdls_channel_switch(sdata, skb);
1990                         break;
1991                 }
1992
1993                 kfree_skb(skb);
1994         }
1995         rtnl_unlock();
1996 }