LXR linux/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
  25void 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
  43static 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
  71static u8
  72ieee80211_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
 128static void
 129ieee80211_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
 156static 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
 174static 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
 184static 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
 202static 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
 227static void
 228ieee80211_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 */
 239static 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
 256static 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
 267static 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
 273static 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
 306static void
 307ieee80211_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
 362static void
 363ieee80211_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
 543static void
 544ieee80211_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
 645static void
 646ieee80211_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
 684static void
 685ieee80211_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
 698static 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
 745static int
 746ieee80211_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
 827static int
 828ieee80211_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
 862static struct sk_buff *
 863ieee80211_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
 928fail:
 929        dev_kfree_skb(skb);
 930        return NULL;
 931}
 932
 933static int
 934ieee80211_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
1064fail:
1065        dev_kfree_skb(skb);
1066        return ret;
1067}
1068
1069static int
1070ieee80211_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
1136out_unlock:
1137        mutex_unlock(&local->mtx);
1138        return ret;
1139}
1140
1141static int
1142ieee80211_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
1188int 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
1249static 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
1291static 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
1311static void
1312iee80211_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
1343int 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
1443void 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}
1457EXPORT_SYMBOL(ieee80211_tdls_oper_request);
1458
1459static void
1460iee80211_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 */
1473static 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
1490static struct sk_buff *
1491ieee80211_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
1559int
1560ieee80211_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
1600out:
1601        mutex_unlock(&local->sta_mtx);
1602        dev_kfree_skb_any(skb);
1603        return ret;
1604}
1605
1606void
1607ieee80211_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
1633out:
1634        mutex_unlock(&local->sta_mtx);
1635}
1636
1637static struct sk_buff *
1638ieee80211_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
1680static int
1681ieee80211_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;
1754call_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
1761out:
1762        mutex_unlock(&local->sta_mtx);
1763        dev_kfree_skb_any(params.tmpl_skb);
1764        return ret;
1765}
1766
1767static int
1768ieee80211_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);
1915out:
1916        mutex_unlock(&local->sta_mtx);
1917        dev_kfree_skb_any(params.tmpl_skb);
1918        return ret;
1919}
1920
1921static void
1922ieee80211_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
1954void 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
1972void 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}
1997