net/mac80211/mlme.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * BSS client mode implementation
   4  * Copyright 2003-2008, Jouni Malinen <[email protected]>
   5  * Copyright 2004, Instant802 Networks, Inc.
   6  * Copyright 2005, Devicescape Software, Inc.
   7  * Copyright 2006-2007  Jiri Benc <[email protected]>
   8  * Copyright 2007, Michael Wu <[email protected]>
   9  * Copyright 2013-2014  Intel Mobile Communications GmbH
  10  * Copyright (C) 2015 - 2017 Intel Deutschland GmbH
  11  * Copyright (C) 2018 - 2024 Intel Corporation
  12  */
  13
  14 #include <linux/delay.h>
  15 #include <linux/fips.h>
  16 #include <linux/if_ether.h>
  17 #include <linux/skbuff.h>
  18 #include <linux/if_arp.h>
  19 #include <linux/etherdevice.h>
  20 #include <linux/moduleparam.h>
  21 #include <linux/rtnetlink.h>
  22 #include <linux/crc32.h>
  23 #include <linux/slab.h>
  24 #include <linux/export.h>
  25 #include <net/mac80211.h>
  26 #include <linux/unaligned.h>
  27
  28 #include "ieee80211_i.h"
  29 #include "driver-ops.h"
  30 #include "rate.h"
  31 #include "led.h"
  32 #include "fils_aead.h"
  33
  34 #include <kunit/static_stub.h>
  35
  36 #define IEEE80211_AUTH_TIMEOUT          (HZ / 5)
  37 #define IEEE80211_AUTH_TIMEOUT_LONG     (HZ / 2)
  38 #define IEEE80211_AUTH_TIMEOUT_SHORT    (HZ / 10)
  39 #define IEEE80211_AUTH_TIMEOUT_SAE      (HZ * 2)
  40 #define IEEE80211_AUTH_MAX_TRIES        3
  41 #define IEEE80211_AUTH_WAIT_ASSOC       (HZ * 5)
  42 #define IEEE80211_AUTH_WAIT_SAE_RETRY   (HZ * 2)
  43 #define IEEE80211_ASSOC_TIMEOUT         (HZ / 5)
  44 #define IEEE80211_ASSOC_TIMEOUT_LONG    (HZ / 2)
  45 #define IEEE80211_ASSOC_TIMEOUT_SHORT   (HZ / 10)
  46 #define IEEE80211_ASSOC_MAX_TRIES       3
  47
  48 #define IEEE80211_ADV_TTLM_SAFETY_BUFFER_MS msecs_to_jiffies(100)
  49 #define IEEE80211_ADV_TTLM_ST_UNDERFLOW 0xff00
  50
  51 #define IEEE80211_NEG_TTLM_REQ_TIMEOUT (HZ / 5)
  52
  53 static int max_nullfunc_tries = 2;
  54 module_param(max_nullfunc_tries, int, 0644);
  55 MODULE_PARM_DESC(max_nullfunc_tries,
  56                  "Maximum nullfunc tx tries before disconnecting (reason 4).");
  57
  58 static int max_probe_tries = 5;
  59 module_param(max_probe_tries, int, 0644);
  60 MODULE_PARM_DESC(max_probe_tries,
  61                  "Maximum probe tries before disconnecting (reason 4).");
  62
  63 /*
  64  * Beacon loss timeout is calculated as N frames times the
  65  * advertised beacon interval.  This may need to be somewhat
  66  * higher than what hardware might detect to account for
  67  * delays in the host processing frames. But since we also
  68  * probe on beacon miss before declaring the connection lost
  69  * default to what we want.
  70  */
  71 static int beacon_loss_count = 7;
  72 module_param(beacon_loss_count, int, 0644);
  73 MODULE_PARM_DESC(beacon_loss_count,
  74                  "Number of beacon intervals before we decide beacon was lost.");
  75
  76 /*
  77  * Time the connection can be idle before we probe
  78  * it to see if we can still talk to the AP.
  79  */
  80 #define IEEE80211_CONNECTION_IDLE_TIME  (30 * HZ)
  81 /*
  82  * Time we wait for a probe response after sending
  83  * a probe request because of beacon loss or for
  84  * checking the connection still works.
  85  */
  86 static int probe_wait_ms = 500;
  87 module_param(probe_wait_ms, int, 0644);
  88 MODULE_PARM_DESC(probe_wait_ms,
  89                  "Maximum time(ms) to wait for probe response"
  90                  " before disconnecting (reason 4).");
  91
  92 /*
  93  * How many Beacon frames need to have been used in average signal strength
  94  * before starting to indicate signal change events.
  95  */
  96 #define IEEE80211_SIGNAL_AVE_MIN_COUNT  4
  97
  98 /*
  99  * We can have multiple work items (and connection probing)
 100  * scheduling this timer, but we need to take care to only
 101  * reschedule it when it should fire _earlier_ than it was
 102  * asked for before, or if it's not pending right now. This
 103  * function ensures that. Note that it then is required to
 104  * run this function for all timeouts after the first one
 105  * has happened -- the work that runs from this timer will
 106  * do that.
 107  */
 108 static void run_again(struct ieee80211_sub_if_data *sdata,
 109                       unsigned long timeout)
 110 {
 111         lockdep_assert_wiphy(sdata->local->hw.wiphy);
 112
 113         if (!timer_pending(&sdata->u.mgd.timer) ||
 114             time_before(timeout, sdata->u.mgd.timer.expires))
 115                 mod_timer(&sdata->u.mgd.timer, timeout);
 116 }
 117
 118 void ieee80211_sta_reset_beacon_monitor(struct ieee80211_sub_if_data *sdata)
 119 {
 120         if (sdata->vif.driver_flags & IEEE80211_VIF_BEACON_FILTER)
 121                 return;
 122
 123         if (ieee80211_hw_check(&sdata->local->hw, CONNECTION_MONITOR))
 124                 return;
 125
 126         mod_timer(&sdata->u.mgd.bcn_mon_timer,
 127                   round_jiffies_up(jiffies + sdata->u.mgd.beacon_timeout));
 128 }
 129
 130 void ieee80211_sta_reset_conn_monitor(struct ieee80211_sub_if_data *sdata)
 131 {
 132         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
 133
 134         if (unlikely(!ifmgd->associated))
 135                 return;
 136
 137         if (ifmgd->probe_send_count)
 138                 ifmgd->probe_send_count = 0;
 139
 140         if (ieee80211_hw_check(&sdata->local->hw, CONNECTION_MONITOR))
 141                 return;
 142
 143         mod_timer(&ifmgd->conn_mon_timer,
 144                   round_jiffies_up(jiffies + IEEE80211_CONNECTION_IDLE_TIME));
 145 }
 146
 147 static int ecw2cw(int ecw)
 148 {
 149         return (1 << ecw) - 1;
 150 }
 151
 152 static enum ieee80211_conn_mode
 153 ieee80211_determine_ap_chan(struct ieee80211_sub_if_data *sdata,
 154                             struct ieee80211_channel *channel,
 155                             u32 vht_cap_info,
 156                             const struct ieee802_11_elems *elems,
 157                             bool ignore_ht_channel_mismatch,
 158                             const struct ieee80211_conn_settings *conn,
 159                             struct cfg80211_chan_def *chandef)
 160 {
 161         const struct ieee80211_ht_operation *ht_oper = elems->ht_operation;
 162         const struct ieee80211_vht_operation *vht_oper = elems->vht_operation;
 163         const struct ieee80211_he_operation *he_oper = elems->he_operation;
 164         const struct ieee80211_eht_operation *eht_oper = elems->eht_operation;
 165         struct ieee80211_supported_band *sband =
 166                 sdata->local->hw.wiphy->bands[channel->band];
 167         struct cfg80211_chan_def vht_chandef;
 168         bool no_vht = false;
 169         u32 ht_cfreq;
 170
 171         *chandef = (struct cfg80211_chan_def) {
 172                 .chan = channel,
 173                 .width = NL80211_CHAN_WIDTH_20_NOHT,
 174                 .center_freq1 = channel->center_freq,
 175                 .freq1_offset = channel->freq_offset,
 176         };
 177
 178         /* get special S1G case out of the way */
 179         if (sband->band == NL80211_BAND_S1GHZ) {
 180                 if (!ieee80211_chandef_s1g_oper(elems->s1g_oper, chandef)) {
 181                         sdata_info(sdata,
 182                                    "Missing S1G Operation Element? Trying operating == primary\n");
 183                         chandef->width = ieee80211_s1g_channel_width(channel);
 184                 }
 185
 186                 return IEEE80211_CONN_MODE_S1G;
 187         }
 188
 189         /* get special 6 GHz case out of the way */
 190         if (sband->band == NL80211_BAND_6GHZ) {
 191                 enum ieee80211_conn_mode mode = IEEE80211_CONN_MODE_EHT;
 192
 193                 /* this is an error */
 194                 if (conn->mode < IEEE80211_CONN_MODE_HE)
 195                         return IEEE80211_CONN_MODE_LEGACY;
 196
 197                 if (!elems->he_6ghz_capa || !elems->he_cap) {
 198                         sdata_info(sdata,
 199                                    "HE 6 GHz AP is missing HE/HE 6 GHz band capability\n");
 200                         return IEEE80211_CONN_MODE_LEGACY;
 201                 }
 202
 203                 if (!eht_oper || !elems->eht_cap) {
 204                         eht_oper = NULL;
 205                         mode = IEEE80211_CONN_MODE_HE;
 206                 }
 207
 208                 if (!ieee80211_chandef_he_6ghz_oper(sdata->local, he_oper,
 209                                                     eht_oper, chandef)) {
 210                         sdata_info(sdata, "bad HE/EHT 6 GHz operation\n");
 211                         return IEEE80211_CONN_MODE_LEGACY;
 212                 }
 213
 214                 return mode;
 215         }
 216
 217         /* now we have the progression HT, VHT, ... */
 218         if (conn->mode < IEEE80211_CONN_MODE_HT)
 219                 return IEEE80211_CONN_MODE_LEGACY;
 220
 221         if (!ht_oper || !elems->ht_cap_elem)
 222                 return IEEE80211_CONN_MODE_LEGACY;
 223
 224         chandef->width = NL80211_CHAN_WIDTH_20;
 225
 226         ht_cfreq = ieee80211_channel_to_frequency(ht_oper->primary_chan,
 227                                                   channel->band);
 228         /* check that channel matches the right operating channel */
 229         if (!ignore_ht_channel_mismatch && channel->center_freq != ht_cfreq) {
 230                 /*
 231                  * It's possible that some APs are confused here;
 232                  * Netgear WNDR3700 sometimes reports 4 higher than
 233                  * the actual channel in association responses, but
 234                  * since we look at probe response/beacon data here
 235                  * it should be OK.
 236                  */
 237                 sdata_info(sdata,
 238                            "Wrong control channel: center-freq: %d ht-cfreq: %d ht->primary_chan: %d band: %d - Disabling HT\n",
 239                            channel->center_freq, ht_cfreq,
 240                            ht_oper->primary_chan, channel->band);
 241                 return IEEE80211_CONN_MODE_LEGACY;
 242         }
 243
 244         ieee80211_chandef_ht_oper(ht_oper, chandef);
 245
 246         if (conn->mode < IEEE80211_CONN_MODE_VHT)
 247                 return IEEE80211_CONN_MODE_HT;
 248
 249         vht_chandef = *chandef;
 250
 251         /*
 252          * having he_cap/he_oper parsed out implies we're at
 253          * least operating as HE STA
 254          */
 255         if (elems->he_cap && he_oper &&
 256             he_oper->he_oper_params & cpu_to_le32(IEEE80211_HE_OPERATION_VHT_OPER_INFO)) {
 257                 struct ieee80211_vht_operation he_oper_vht_cap;
 258
 259                 /*
 260                  * Set only first 3 bytes (other 2 aren't used in
 261                  * ieee80211_chandef_vht_oper() anyway)
 262                  */
 263                 memcpy(&he_oper_vht_cap, he_oper->optional, 3);
 264                 he_oper_vht_cap.basic_mcs_set = cpu_to_le16(0);
 265
 266                 if (!ieee80211_chandef_vht_oper(&sdata->local->hw, vht_cap_info,
 267                                                 &he_oper_vht_cap, ht_oper,
 268                                                 &vht_chandef)) {
 269                         sdata_info(sdata,
 270                                    "HE AP VHT information is invalid, disabling HE\n");
 271                         /* this will cause us to re-parse as VHT STA */
 272                         return IEEE80211_CONN_MODE_VHT;
 273                 }
 274         } else if (!vht_oper || !elems->vht_cap_elem) {
 275                 if (sband->band == NL80211_BAND_5GHZ) {
 276                         sdata_info(sdata,
 277                                    "VHT information is missing, disabling VHT\n");
 278                         return IEEE80211_CONN_MODE_HT;
 279                 }
 280                 no_vht = true;
 281         } else if (sband->band == NL80211_BAND_2GHZ) {
 282                 no_vht = true;
 283         } else if (!ieee80211_chandef_vht_oper(&sdata->local->hw,
 284                                                vht_cap_info,
 285                                                vht_oper, ht_oper,
 286                                                &vht_chandef)) {
 287                 sdata_info(sdata,
 288                            "AP VHT information is invalid, disabling VHT\n");
 289                 return IEEE80211_CONN_MODE_HT;
 290         }
 291
 292         if (!cfg80211_chandef_compatible(chandef, &vht_chandef)) {
 293                 sdata_info(sdata,
 294                            "AP VHT information doesn't match HT, disabling VHT\n");
 295                 return IEEE80211_CONN_MODE_HT;
 296         }
 297
 298         *chandef = vht_chandef;
 299
 300         /* stick to current max mode if we or the AP don't have HE */
 301         if (conn->mode < IEEE80211_CONN_MODE_HE ||
 302             !elems->he_operation || !elems->he_cap) {
 303                 if (no_vht)
 304                         return IEEE80211_CONN_MODE_HT;
 305                 return IEEE80211_CONN_MODE_VHT;
 306         }
 307
 308         /* stick to HE if we or the AP don't have EHT */
 309         if (conn->mode < IEEE80211_CONN_MODE_EHT ||
 310             !eht_oper || !elems->eht_cap)
 311                 return IEEE80211_CONN_MODE_HE;
 312
 313         /*
 314          * handle the case that the EHT operation indicates that it holds EHT
 315          * operation information (in case that the channel width differs from
 316          * the channel width reported in HT/VHT/HE).
 317          */
 318         if (eht_oper->params & IEEE80211_EHT_OPER_INFO_PRESENT) {
 319                 struct cfg80211_chan_def eht_chandef = *chandef;
 320
 321                 ieee80211_chandef_eht_oper((const void *)eht_oper->optional,
 322                                            &eht_chandef);
 323
 324                 eht_chandef.punctured =
 325                         ieee80211_eht_oper_dis_subchan_bitmap(eht_oper);
 326
 327                 if (!cfg80211_chandef_valid(&eht_chandef)) {
 328                         sdata_info(sdata,
 329                                    "AP EHT information is invalid, disabling EHT\n");
 330                         return IEEE80211_CONN_MODE_HE;
 331                 }
 332
 333                 if (!cfg80211_chandef_compatible(chandef, &eht_chandef)) {
 334                         sdata_info(sdata,
 335                                    "AP EHT information doesn't match HT/VHT/HE, disabling EHT\n");
 336                         return IEEE80211_CONN_MODE_HE;
 337                 }
 338
 339                 *chandef = eht_chandef;
 340         }
 341
 342         return IEEE80211_CONN_MODE_EHT;
 343 }
 344
 345 static bool
 346 ieee80211_verify_peer_he_mcs_support(struct ieee80211_sub_if_data *sdata,
 347                                      int link_id,
 348                                      const struct ieee80211_he_cap_elem *he_cap,
 349                                      const struct ieee80211_he_operation *he_op)
 350 {
 351         struct ieee80211_he_mcs_nss_supp *he_mcs_nss_supp;
 352         u16 mcs_80_map_tx, mcs_80_map_rx;
 353         u16 ap_min_req_set;
 354         int nss;
 355
 356         if (!he_cap)
 357                 return false;
 358
 359         /* mcs_nss is right after he_cap info */
 360         he_mcs_nss_supp = (void *)(he_cap + 1);
 361
 362         mcs_80_map_tx = le16_to_cpu(he_mcs_nss_supp->tx_mcs_80);
 363         mcs_80_map_rx = le16_to_cpu(he_mcs_nss_supp->rx_mcs_80);
 364
 365         /* P802.11-REVme/D0.3
 366          * 27.1.1 Introduction to the HE PHY
 367          * ...
 368          * An HE STA shall support the following features:
 369          * ...
 370          * Single spatial stream HE-MCSs 0 to 7 (transmit and receive) in all
 371          * supported channel widths for HE SU PPDUs
 372          */
 373         if ((mcs_80_map_tx & 0x3) == IEEE80211_HE_MCS_NOT_SUPPORTED ||
 374             (mcs_80_map_rx & 0x3) == IEEE80211_HE_MCS_NOT_SUPPORTED) {
 375                 link_id_info(sdata, link_id,
 376                              "Missing mandatory rates for 1 Nss, rx 0x%x, tx 0x%x, disable HE\n",
 377                              mcs_80_map_tx, mcs_80_map_rx);
 378                 return false;
 379         }
 380
 381         if (!he_op)
 382                 return true;
 383
 384         ap_min_req_set = le16_to_cpu(he_op->he_mcs_nss_set);
 385
 386         /*
 387          * Apparently iPhone 13 (at least iOS version 15.3.1) sets this to all
 388          * zeroes, which is nonsense, and completely inconsistent with itself
 389          * (it doesn't have 8 streams). Accept the settings in this case anyway.
 390          */
 391         if (!ap_min_req_set)
 392                 return true;
 393
 394         /* make sure the AP is consistent with itself
 395          *
 396          * P802.11-REVme/D0.3
 397          * 26.17.1 Basic HE BSS operation
 398          *
 399          * A STA that is operating in an HE BSS shall be able to receive and
 400          * transmit at each of the <HE-MCS, NSS> tuple values indicated by the
 401          * Basic HE-MCS And NSS Set field of the HE Operation parameter of the
 402          * MLME-START.request primitive and shall be able to receive at each of
 403          * the <HE-MCS, NSS> tuple values indicated by the Supported HE-MCS and
 404          * NSS Set field in the HE Capabilities parameter of the MLMESTART.request
 405          * primitive
 406          */
 407         for (nss = 8; nss > 0; nss--) {
 408                 u8 ap_op_val = (ap_min_req_set >> (2 * (nss - 1))) & 3;
 409                 u8 ap_rx_val;
 410                 u8 ap_tx_val;
 411
 412                 if (ap_op_val == IEEE80211_HE_MCS_NOT_SUPPORTED)
 413                         continue;
 414
 415                 ap_rx_val = (mcs_80_map_rx >> (2 * (nss - 1))) & 3;
 416                 ap_tx_val = (mcs_80_map_tx >> (2 * (nss - 1))) & 3;
 417
 418                 if (ap_rx_val == IEEE80211_HE_MCS_NOT_SUPPORTED ||
 419                     ap_tx_val == IEEE80211_HE_MCS_NOT_SUPPORTED ||
 420                     ap_rx_val < ap_op_val || ap_tx_val < ap_op_val) {
 421                         link_id_info(sdata, link_id,
 422                                      "Invalid rates for %d Nss, rx %d, tx %d oper %d, disable HE\n",
 423                                      nss, ap_rx_val, ap_tx_val, ap_op_val);
 424                         return false;
 425                 }
 426         }
 427
 428         return true;
 429 }
 430
 431 static bool
 432 ieee80211_verify_sta_he_mcs_support(struct ieee80211_sub_if_data *sdata,
 433                                     struct ieee80211_supported_band *sband,
 434                                     const struct ieee80211_he_operation *he_op)
 435 {
 436         const struct ieee80211_sta_he_cap *sta_he_cap =
 437                 ieee80211_get_he_iftype_cap_vif(sband, &sdata->vif);
 438         u16 ap_min_req_set;
 439         int i;
 440
 441         if (!sta_he_cap || !he_op)
 442                 return false;
 443
 444         ap_min_req_set = le16_to_cpu(he_op->he_mcs_nss_set);
 445
 446         /*
 447          * Apparently iPhone 13 (at least iOS version 15.3.1) sets this to all
 448          * zeroes, which is nonsense, and completely inconsistent with itself
 449          * (it doesn't have 8 streams). Accept the settings in this case anyway.
 450          */
 451         if (!ap_min_req_set)
 452                 return true;
 453
 454         /* Need to go over for 80MHz, 160MHz and for 80+80 */
 455         for (i = 0; i < 3; i++) {
 456                 const struct ieee80211_he_mcs_nss_supp *sta_mcs_nss_supp =
 457                         &sta_he_cap->he_mcs_nss_supp;
 458                 u16 sta_mcs_map_rx =
 459                         le16_to_cpu(((__le16 *)sta_mcs_nss_supp)[2 * i]);
 460                 u16 sta_mcs_map_tx =
 461                         le16_to_cpu(((__le16 *)sta_mcs_nss_supp)[2 * i + 1]);
 462                 u8 nss;
 463                 bool verified = true;
 464
 465                 /*
 466                  * For each band there is a maximum of 8 spatial streams
 467                  * possible. Each of the sta_mcs_map_* is a 16-bit struct built
 468                  * of 2 bits per NSS (1-8), with the values defined in enum
 469                  * ieee80211_he_mcs_support. Need to make sure STA TX and RX
 470                  * capabilities aren't less than the AP's minimum requirements
 471                  * for this HE BSS per SS.
 472                  * It is enough to find one such band that meets the reqs.
 473                  */
 474                 for (nss = 8; nss > 0; nss--) {
 475                         u8 sta_rx_val = (sta_mcs_map_rx >> (2 * (nss - 1))) & 3;
 476                         u8 sta_tx_val = (sta_mcs_map_tx >> (2 * (nss - 1))) & 3;
 477                         u8 ap_val = (ap_min_req_set >> (2 * (nss - 1))) & 3;
 478
 479                         if (ap_val == IEEE80211_HE_MCS_NOT_SUPPORTED)
 480                                 continue;
 481
 482                         /*
 483                          * Make sure the HE AP doesn't require MCSs that aren't
 484                          * supported by the client as required by spec
 485                          *
 486                          * P802.11-REVme/D0.3
 487                          * 26.17.1 Basic HE BSS operation
 488                          *
 489                          * An HE STA shall not attempt to join * (MLME-JOIN.request primitive)
 490                          * a BSS, unless it supports (i.e., is able to both transmit and
 491                          * receive using) all of the <HE-MCS, NSS> tuples in the basic
 492                          * HE-MCS and NSS set.
 493                          */
 494                         if (sta_rx_val == IEEE80211_HE_MCS_NOT_SUPPORTED ||
 495                             sta_tx_val == IEEE80211_HE_MCS_NOT_SUPPORTED ||
 496                             (ap_val > sta_rx_val) || (ap_val > sta_tx_val)) {
 497                                 verified = false;
 498                                 break;
 499                         }
 500                 }
 501
 502                 if (verified)
 503                         return true;
 504         }
 505
 506         /* If here, STA doesn't meet AP's HE min requirements */
 507         return false;
 508 }
 509
 510 static u8
 511 ieee80211_get_eht_cap_mcs_nss(const struct ieee80211_sta_he_cap *sta_he_cap,
 512                               const struct ieee80211_sta_eht_cap *sta_eht_cap,
 513                               unsigned int idx, int bw)
 514 {
 515         u8 he_phy_cap0 = sta_he_cap->he_cap_elem.phy_cap_info[0];
 516         u8 eht_phy_cap0 = sta_eht_cap->eht_cap_elem.phy_cap_info[0];
 517
 518         /* handle us being a 20 MHz-only EHT STA - with four values
 519          * for MCS 0-7, 8-9, 10-11, 12-13.
 520          */
 521         if (!(he_phy_cap0 & IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_MASK_ALL))
 522                 return sta_eht_cap->eht_mcs_nss_supp.only_20mhz.rx_tx_max_nss[idx];
 523
 524         /* the others have MCS 0-9 together, rather than separately from 0-7 */
 525         if (idx > 0)
 526                 idx--;
 527
 528         switch (bw) {
 529         case 0:
 530                 return sta_eht_cap->eht_mcs_nss_supp.bw._80.rx_tx_max_nss[idx];
 531         case 1:
 532                 if (!(he_phy_cap0 &
 533                       (IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G |
 534                        IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_80PLUS80_MHZ_IN_5G)))
 535                         return 0xff; /* pass check */
 536                 return sta_eht_cap->eht_mcs_nss_supp.bw._160.rx_tx_max_nss[idx];
 537         case 2:
 538                 if (!(eht_phy_cap0 & IEEE80211_EHT_PHY_CAP0_320MHZ_IN_6GHZ))
 539                         return 0xff; /* pass check */
 540                 return sta_eht_cap->eht_mcs_nss_supp.bw._320.rx_tx_max_nss[idx];
 541         }
 542
 543         WARN_ON(1);
 544         return 0;
 545 }
 546
 547 static bool
 548 ieee80211_verify_sta_eht_mcs_support(struct ieee80211_sub_if_data *sdata,
 549                                      struct ieee80211_supported_band *sband,
 550                                      const struct ieee80211_eht_operation *eht_op)
 551 {
 552         const struct ieee80211_sta_he_cap *sta_he_cap =
 553                 ieee80211_get_he_iftype_cap_vif(sband, &sdata->vif);
 554         const struct ieee80211_sta_eht_cap *sta_eht_cap =
 555                 ieee80211_get_eht_iftype_cap_vif(sband, &sdata->vif);
 556         const struct ieee80211_eht_mcs_nss_supp_20mhz_only *req;
 557         unsigned int i;
 558
 559         if (!sta_he_cap || !sta_eht_cap || !eht_op)
 560                 return false;
 561
 562         req = &eht_op->basic_mcs_nss;
 563
 564         for (i = 0; i < ARRAY_SIZE(req->rx_tx_max_nss); i++) {
 565                 u8 req_rx_nss, req_tx_nss;
 566                 unsigned int bw;
 567
 568                 req_rx_nss = u8_get_bits(req->rx_tx_max_nss[i],
 569                                          IEEE80211_EHT_MCS_NSS_RX);
 570                 req_tx_nss = u8_get_bits(req->rx_tx_max_nss[i],
 571                                          IEEE80211_EHT_MCS_NSS_TX);
 572
 573                 for (bw = 0; bw < 3; bw++) {
 574                         u8 have, have_rx_nss, have_tx_nss;
 575
 576                         have = ieee80211_get_eht_cap_mcs_nss(sta_he_cap,
 577                                                              sta_eht_cap,
 578                                                              i, bw);
 579                         have_rx_nss = u8_get_bits(have,
 580                                                   IEEE80211_EHT_MCS_NSS_RX);
 581                         have_tx_nss = u8_get_bits(have,
 582                                                   IEEE80211_EHT_MCS_NSS_TX);
 583
 584                         if (req_rx_nss > have_rx_nss ||
 585                             req_tx_nss > have_tx_nss)
 586                                 return false;
 587                 }
 588         }
 589
 590         return true;
 591 }
 592
 593 static void ieee80211_get_rates(struct ieee80211_supported_band *sband,
 594                                 const u8 *supp_rates,
 595                                 unsigned int supp_rates_len,
 596                                 const u8 *ext_supp_rates,
 597                                 unsigned int ext_supp_rates_len,
 598                                 u32 *rates, u32 *basic_rates,
 599                                 unsigned long *unknown_rates_selectors,
 600                                 bool *have_higher_than_11mbit,
 601                                 int *min_rate, int *min_rate_index)
 602 {
 603         int i, j;
 604
 605         for (i = 0; i < supp_rates_len + ext_supp_rates_len; i++) {
 606                 u8 supp_rate = i < supp_rates_len ?
 607                                 supp_rates[i] :
 608                                 ext_supp_rates[i - supp_rates_len];
 609                 int rate = supp_rate & 0x7f;
 610                 bool is_basic = !!(supp_rate & 0x80);
 611
 612                 if ((rate * 5) > 110 && have_higher_than_11mbit)
 613                         *have_higher_than_11mbit = true;
 614
 615                 /*
 616                  * Skip membership selectors since they're not rates.
 617                  *
 618                  * Note: Even though the membership selector and the basic
 619                  *       rate flag share the same bit, they are not exactly
 620                  *       the same.
 621                  */
 622                 if (is_basic && rate >= BSS_MEMBERSHIP_SELECTOR_MIN) {
 623                         if (unknown_rates_selectors)
 624                                 set_bit(rate, unknown_rates_selectors);
 625                         continue;
 626                 }
 627
 628                 for (j = 0; j < sband->n_bitrates; j++) {
 629                         struct ieee80211_rate *br;
 630                         int brate;
 631
 632                         br = &sband->bitrates[j];
 633
 634                         brate = DIV_ROUND_UP(br->bitrate, 5);
 635                         if (brate == rate) {
 636                                 if (rates)
 637                                         *rates |= BIT(j);
 638                                 if (is_basic && basic_rates)
 639                                         *basic_rates |= BIT(j);
 640                                 if (min_rate && (rate * 5) < *min_rate) {
 641                                         *min_rate = rate * 5;
 642                                         if (min_rate_index)
 643                                                 *min_rate_index = j;
 644                                 }
 645                                 break;
 646                         }
 647                 }
 648
 649                 /* Handle an unknown entry as if it is an unknown selector */
 650                 if (is_basic && unknown_rates_selectors && j == sband->n_bitrates)
 651                         set_bit(rate, unknown_rates_selectors);
 652         }
 653 }
 654
 655 static bool ieee80211_chandef_usable(struct ieee80211_sub_if_data *sdata,
 656                                      const struct cfg80211_chan_def *chandef,
 657                                      u32 prohibited_flags)
 658 {
 659         if (!cfg80211_chandef_usable(sdata->local->hw.wiphy,
 660                                      chandef, prohibited_flags))
 661                 return false;
 662
 663         if (chandef->punctured &&
 664             ieee80211_hw_check(&sdata->local->hw, DISALLOW_PUNCTURING))
 665                 return false;
 666
 667         if (chandef->punctured && chandef->chan->band == NL80211_BAND_5GHZ &&
 668             ieee80211_hw_check(&sdata->local->hw, DISALLOW_PUNCTURING_5GHZ))
 669                 return false;
 670
 671         return true;
 672 }
 673
 674 static int ieee80211_chandef_num_subchans(const struct cfg80211_chan_def *c)
 675 {
 676         if (c->width == NL80211_CHAN_WIDTH_80P80)
 677                 return 4 + 4;
 678
 679         return nl80211_chan_width_to_mhz(c->width) / 20;
 680 }
 681
 682 static int ieee80211_chandef_num_widths(const struct cfg80211_chan_def *c)
 683 {
 684         switch (c->width) {
 685         case NL80211_CHAN_WIDTH_20:
 686         case NL80211_CHAN_WIDTH_20_NOHT:
 687                 return 1;
 688         case NL80211_CHAN_WIDTH_40:
 689                 return 2;
 690         case NL80211_CHAN_WIDTH_80P80:
 691         case NL80211_CHAN_WIDTH_80:
 692                 return 3;
 693         case NL80211_CHAN_WIDTH_160:
 694                 return 4;
 695         case NL80211_CHAN_WIDTH_320:
 696                 return 5;
 697         default:
 698                 WARN_ON(1);
 699                 return 0;
 700         }
 701 }
 702
 703 VISIBLE_IF_MAC80211_KUNIT int
 704 ieee80211_calc_chandef_subchan_offset(const struct cfg80211_chan_def *ap,
 705                                       u8 n_partial_subchans)
 706 {
 707         int n = ieee80211_chandef_num_subchans(ap);
 708         struct cfg80211_chan_def tmp = *ap;
 709         int offset = 0;
 710
 711         /*
 712          * Given a chandef (in this context, it's the AP's) and a number
 713          * of subchannels that we want to look at ('n_partial_subchans'),
 714          * calculate the offset in number of subchannels between the full
 715          * and the subset with the desired width.
 716          */
 717
 718         /* same number of subchannels means no offset, obviously */
 719         if (n == n_partial_subchans)
 720                 return 0;
 721
 722         /* don't WARN - misconfigured APs could cause this if their N > width */
 723         if (n < n_partial_subchans)
 724                 return 0;
 725
 726         while (ieee80211_chandef_num_subchans(&tmp) > n_partial_subchans) {
 727                 u32 prev = tmp.center_freq1;
 728
 729                 ieee80211_chandef_downgrade(&tmp, NULL);
 730
 731                 /*
 732                  * if center_freq moved up, half the original channels
 733                  * are gone now but were below, so increase offset
 734                  */
 735                 if (prev < tmp.center_freq1)
 736                         offset += ieee80211_chandef_num_subchans(&tmp);
 737         }
 738
 739         /*
 740          * 80+80 with secondary 80 below primary - four subchannels for it
 741          * (we cannot downgrade *to* 80+80, so no need to consider 'tmp')
 742          */
 743         if (ap->width == NL80211_CHAN_WIDTH_80P80 &&
 744             ap->center_freq2 < ap->center_freq1)
 745                 offset += 4;
 746
 747         return offset;
 748 }
 749 EXPORT_SYMBOL_IF_MAC80211_KUNIT(ieee80211_calc_chandef_subchan_offset);
 750
 751 VISIBLE_IF_MAC80211_KUNIT void
 752 ieee80211_rearrange_tpe_psd(struct ieee80211_parsed_tpe_psd *psd,
 753                             const struct cfg80211_chan_def *ap,
 754                             const struct cfg80211_chan_def *used)
 755 {
 756         u8 needed = ieee80211_chandef_num_subchans(used);
 757         u8 have = ieee80211_chandef_num_subchans(ap);
 758         u8 tmp[IEEE80211_TPE_PSD_ENTRIES_320MHZ];
 759         u8 offset;
 760
 761         if (!psd->valid)
 762                 return;
 763
 764         /* if N is zero, all defaults were used, no point in rearranging */
 765         if (!psd->n)
 766                 goto out;
 767
 768         BUILD_BUG_ON(sizeof(tmp) != sizeof(psd->power));
 769
 770         /*
 771          * This assumes that 'N' is consistent with the HE channel, as
 772          * it should be (otherwise the AP is broken).
 773          *
 774          * In psd->power we have values in the order 0..N, 0..K, where
 775          * N+K should cover the entire channel per 'ap', but even if it
 776          * doesn't then we've pre-filled 'unlimited' as defaults.
 777          *
 778          * But this is all the wrong order, we want to have them in the
 779          * order of the 'used' channel.
 780          *
 781          * So for example, we could have a 320 MHz EHT AP, which has the
 782          * HE channel as 80 MHz (e.g. due to puncturing, which doesn't
 783          * seem to be considered for the TPE), as follows:
 784          *
 785          * EHT  320:   |  |  |  |  |  |  |  |  |  |  |  |  |  |  |  |  |
 786          * HE    80:                           |  |  |  |  |
 787          * used 160:                           |  |  |  |  |  |  |  |  |
 788          *
 789          * N entries:                          |--|--|--|--|
 790          * K entries:  |--|--|--|--|--|--|--|--|           |--|--|--|--|
 791          * power idx:   4  5  6  7  8  9  10 11 0  1  2  3  12 13 14 15
 792          * full chan:   0  1  2  3  4  5  6  7  8  9  10 11 12 13 14 15
 793          * used chan:                           0  1  2  3  4  5  6  7
 794          *
 795          * The idx in the power array ('power idx') is like this since it
 796          * comes directly from the element's N and K entries in their
 797          * element order, and those are this way for HE compatibility.
 798          *
 799          * Rearrange them as desired here, first by putting them into the
 800          * 'full chan' order, and then selecting the necessary subset for
 801          * the 'used chan'.
 802          */
 803
 804         /* first reorder according to AP channel */
 805         offset = ieee80211_calc_chandef_subchan_offset(ap, psd->n);
 806         for (int i = 0; i < have; i++) {
 807                 if (i < offset)
 808                         tmp[i] = psd->power[i + psd->n];
 809                 else if (i < offset + psd->n)
 810                         tmp[i] = psd->power[i - offset];
 811                 else
 812                         tmp[i] = psd->power[i];
 813         }
 814
 815         /*
 816          * and then select the subset for the used channel
 817          * (set everything to defaults first in case a driver is confused)
 818          */
 819         memset(psd->power, IEEE80211_TPE_PSD_NO_LIMIT, sizeof(psd->power));
 820         offset = ieee80211_calc_chandef_subchan_offset(ap, needed);
 821         for (int i = 0; i < needed; i++)
 822                 psd->power[i] = tmp[offset + i];
 823
 824 out:
 825         /* limit, but don't lie if there are defaults in the data */
 826         if (needed < psd->count)
 827                 psd->count = needed;
 828 }
 829 EXPORT_SYMBOL_IF_MAC80211_KUNIT(ieee80211_rearrange_tpe_psd);
 830
 831 static void ieee80211_rearrange_tpe(struct ieee80211_parsed_tpe *tpe,
 832                                     const struct cfg80211_chan_def *ap,
 833                                     const struct cfg80211_chan_def *used)
 834 {
 835         /* ignore this completely for narrow/invalid channels */
 836         if (!ieee80211_chandef_num_subchans(ap) ||
 837             !ieee80211_chandef_num_subchans(used)) {
 838                 ieee80211_clear_tpe(tpe);
 839                 return;
 840         }
 841
 842         for (int i = 0; i < 2; i++) {
 843                 int needed_pwr_count;
 844
 845                 ieee80211_rearrange_tpe_psd(&tpe->psd_local[i], ap, used);
 846                 ieee80211_rearrange_tpe_psd(&tpe->psd_reg_client[i], ap, used);
 847
 848                 /* limit this to the widths we actually need */
 849                 needed_pwr_count = ieee80211_chandef_num_widths(used);
 850                 if (needed_pwr_count < tpe->max_local[i].count)
 851                         tpe->max_local[i].count = needed_pwr_count;
 852                 if (needed_pwr_count < tpe->max_reg_client[i].count)
 853                         tpe->max_reg_client[i].count = needed_pwr_count;
 854         }
 855 }
 856
 857 /*
 858  * The AP part of the channel request is used to distinguish settings
 859  * to the device used for wider bandwidth OFDMA. This is used in the
 860  * channel context code to assign two channel contexts even if they're
 861  * both for the same channel, if the AP bandwidths are incompatible.
 862  * If not EHT (or driver override) then ap.chan == NULL indicates that
 863  * there's no wider BW OFDMA used.
 864  */
 865 static void ieee80211_set_chanreq_ap(struct ieee80211_sub_if_data *sdata,
 866                                      struct ieee80211_chan_req *chanreq,
 867                                      struct ieee80211_conn_settings *conn,
 868                                      struct cfg80211_chan_def *ap_chandef)
 869 {
 870         chanreq->ap.chan = NULL;
 871
 872         if (conn->mode < IEEE80211_CONN_MODE_EHT)
 873                 return;
 874         if (sdata->vif.driver_flags & IEEE80211_VIF_IGNORE_OFDMA_WIDER_BW)
 875                 return;
 876
 877         chanreq->ap = *ap_chandef;
 878 }
 879
 880 static struct ieee802_11_elems *
 881 ieee80211_determine_chan_mode(struct ieee80211_sub_if_data *sdata,
 882                               struct ieee80211_conn_settings *conn,
 883                               struct cfg80211_bss *cbss, int link_id,
 884                               struct ieee80211_chan_req *chanreq,
 885                               struct cfg80211_chan_def *ap_chandef,
 886                               unsigned long *userspace_selectors)
 887 {
 888         const struct cfg80211_bss_ies *ies = rcu_dereference(cbss->ies);
 889         struct ieee80211_bss *bss = (void *)cbss->priv;
 890         struct ieee80211_channel *channel = cbss->channel;
 891         struct ieee80211_elems_parse_params parse_params = {
 892                 .link_id = -1,
 893                 .from_ap = true,
 894                 .start = ies->data,
 895                 .len = ies->len,
 896         };
 897         struct ieee802_11_elems *elems;
 898         struct ieee80211_supported_band *sband;
 899         enum ieee80211_conn_mode ap_mode;
 900         unsigned long unknown_rates_selectors[BITS_TO_LONGS(128)] = {};
 901         unsigned long sta_selectors[BITS_TO_LONGS(128)] = {};
 902         int ret;
 903
 904 again:
 905         parse_params.mode = conn->mode;
 906         elems = ieee802_11_parse_elems_full(&parse_params);
 907         if (!elems)
 908                 return ERR_PTR(-ENOMEM);
 909
 910         ap_mode = ieee80211_determine_ap_chan(sdata, channel, bss->vht_cap_info,
 911                                               elems, false, conn, ap_chandef);
 912
 913         /* this should be impossible since parsing depends on our mode */
 914         if (WARN_ON(ap_mode > conn->mode)) {
 915                 ret = -EINVAL;
 916                 goto free;
 917         }
 918
 919         if (conn->mode != ap_mode) {
 920                 conn->mode = ap_mode;
 921                 kfree(elems);
 922                 goto again;
 923         }
 924
 925         mlme_link_id_dbg(sdata, link_id, "determined AP %pM to be %s\n",
 926                          cbss->bssid, ieee80211_conn_mode_str(ap_mode));
 927
 928         sband = sdata->local->hw.wiphy->bands[channel->band];
 929
 930         ieee80211_get_rates(sband, elems->supp_rates, elems->supp_rates_len,
 931                             elems->ext_supp_rates, elems->ext_supp_rates_len,
 932                             NULL, NULL, unknown_rates_selectors, NULL, NULL,
 933                             NULL);
 934
 935         switch (channel->band) {
 936         case NL80211_BAND_S1GHZ:
 937                 if (WARN_ON(ap_mode != IEEE80211_CONN_MODE_S1G)) {
 938                         ret = -EINVAL;
 939                         goto free;
 940                 }
 941                 return elems;
 942         case NL80211_BAND_6GHZ:
 943                 if (ap_mode < IEEE80211_CONN_MODE_HE) {
 944                         link_id_info(sdata, link_id,
 945                                      "Rejecting non-HE 6/7 GHz connection");
 946                         ret = -EINVAL;
 947                         goto free;
 948                 }
 949                 break;
 950         default:
 951                 if (WARN_ON(ap_mode == IEEE80211_CONN_MODE_S1G)) {
 952                         ret = -EINVAL;
 953                         goto free;
 954                 }
 955         }
 956
 957         switch (ap_mode) {
 958         case IEEE80211_CONN_MODE_S1G:
 959                 WARN_ON(1);
 960                 ret = -EINVAL;
 961                 goto free;
 962         case IEEE80211_CONN_MODE_LEGACY:
 963                 conn->bw_limit = IEEE80211_CONN_BW_LIMIT_20;
 964                 break;
 965         case IEEE80211_CONN_MODE_HT:
 966                 conn->bw_limit = min_t(enum ieee80211_conn_bw_limit,
 967                                        conn->bw_limit,
 968                                        IEEE80211_CONN_BW_LIMIT_40);
 969                 break;
 970         case IEEE80211_CONN_MODE_VHT:
 971         case IEEE80211_CONN_MODE_HE:
 972                 conn->bw_limit = min_t(enum ieee80211_conn_bw_limit,
 973                                        conn->bw_limit,
 974                                        IEEE80211_CONN_BW_LIMIT_160);
 975                 break;
 976         case IEEE80211_CONN_MODE_EHT:
 977                 conn->bw_limit = min_t(enum ieee80211_conn_bw_limit,
 978                                        conn->bw_limit,
 979                                        IEEE80211_CONN_BW_LIMIT_320);
 980                 break;
 981         }
 982
 983         chanreq->oper = *ap_chandef;
 984
 985         bitmap_copy(sta_selectors, userspace_selectors, 128);
 986         if (conn->mode >= IEEE80211_CONN_MODE_HT)
 987                 set_bit(BSS_MEMBERSHIP_SELECTOR_HT_PHY, sta_selectors);
 988         if (conn->mode >= IEEE80211_CONN_MODE_VHT)
 989                 set_bit(BSS_MEMBERSHIP_SELECTOR_VHT_PHY, sta_selectors);
 990         if (conn->mode >= IEEE80211_CONN_MODE_HE)
 991                 set_bit(BSS_MEMBERSHIP_SELECTOR_HE_PHY, sta_selectors);
 992         if (conn->mode >= IEEE80211_CONN_MODE_EHT)
 993                 set_bit(BSS_MEMBERSHIP_SELECTOR_EHT_PHY, sta_selectors);
 994
 995         /*
 996          * We do not support EPD or GLK so never add them.
 997          * SAE_H2E is handled through userspace_selectors.
 998          */
 999
1000         /* Check if we support all required features */
1001         if (!bitmap_subset(unknown_rates_selectors, sta_selectors, 128)) {
1002                 link_id_info(sdata, link_id,
1003                              "required basic rate or BSS membership selectors not supported or disabled, rejecting connection\n");
1004                 ret = -EINVAL;
1005                 goto free;
1006         }
1007
1008         ieee80211_set_chanreq_ap(sdata, chanreq, conn, ap_chandef);
1009
1010         while (!ieee80211_chandef_usable(sdata, &chanreq->oper,
1011                                          IEEE80211_CHAN_DISABLED)) {
1012                 if (WARN_ON(chanreq->oper.width == NL80211_CHAN_WIDTH_20_NOHT)) {
1013                         ret = -EINVAL;
1014                         goto free;
1015                 }
1016
1017                 ieee80211_chanreq_downgrade(chanreq, conn);
1018         }
1019
1020         if (conn->mode >= IEEE80211_CONN_MODE_HE &&
1021             !cfg80211_chandef_usable(sdata->wdev.wiphy, &chanreq->oper,
1022                                      IEEE80211_CHAN_NO_HE)) {
1023                 conn->mode = IEEE80211_CONN_MODE_VHT;
1024                 conn->bw_limit = min_t(enum ieee80211_conn_bw_limit,
1025                                        conn->bw_limit,
1026                                        IEEE80211_CONN_BW_LIMIT_160);
1027         }
1028
1029         if (conn->mode >= IEEE80211_CONN_MODE_EHT &&
1030             !cfg80211_chandef_usable(sdata->wdev.wiphy, &chanreq->oper,
1031                                      IEEE80211_CHAN_NO_EHT)) {
1032                 conn->mode = IEEE80211_CONN_MODE_HE;
1033                 conn->bw_limit = min_t(enum ieee80211_conn_bw_limit,
1034                                        conn->bw_limit,
1035                                        IEEE80211_CONN_BW_LIMIT_160);
1036         }
1037
1038         if (chanreq->oper.width != ap_chandef->width || ap_mode != conn->mode)
1039                 link_id_info(sdata, link_id,
1040                              "regulatory prevented using AP config, downgraded\n");
1041
1042         if (conn->mode >= IEEE80211_CONN_MODE_HE &&
1043             (!ieee80211_verify_peer_he_mcs_support(sdata, link_id,
1044                                                    (void *)elems->he_cap,
1045                                                    elems->he_operation) ||
1046              !ieee80211_verify_sta_he_mcs_support(sdata, sband,
1047                                                   elems->he_operation))) {
1048                 conn->mode = IEEE80211_CONN_MODE_VHT;
1049                 link_id_info(sdata, link_id,
1050                              "required MCSes not supported, disabling HE\n");
1051         }
1052
1053         if (conn->mode >= IEEE80211_CONN_MODE_EHT &&
1054             !ieee80211_verify_sta_eht_mcs_support(sdata, sband,
1055                                                   elems->eht_operation)) {
1056                 conn->mode = IEEE80211_CONN_MODE_HE;
1057                 conn->bw_limit = min_t(enum ieee80211_conn_bw_limit,
1058                                        conn->bw_limit,
1059                                        IEEE80211_CONN_BW_LIMIT_160);
1060                 link_id_info(sdata, link_id,
1061                              "required MCSes not supported, disabling EHT\n");
1062         }
1063
1064         /* the mode can only decrease, so this must terminate */
1065         if (ap_mode != conn->mode) {
1066                 kfree(elems);
1067                 goto again;
1068         }
1069
1070         mlme_link_id_dbg(sdata, link_id,
1071                          "connecting with %s mode, max bandwidth %d MHz\n",
1072                          ieee80211_conn_mode_str(conn->mode),
1073                          20 * (1 << conn->bw_limit));
1074
1075         if (WARN_ON_ONCE(!cfg80211_chandef_valid(&chanreq->oper))) {
1076                 ret = -EINVAL;
1077                 goto free;
1078         }
1079
1080         return elems;
1081 free:
1082         kfree(elems);
1083         return ERR_PTR(ret);
1084 }
1085
1086 static int ieee80211_config_bw(struct ieee80211_link_data *link,
1087                                struct ieee802_11_elems *elems,
1088                                bool update, u64 *changed,
1089                                const char *frame)
1090 {
1091         struct ieee80211_channel *channel = link->conf->chanreq.oper.chan;
1092         struct ieee80211_sub_if_data *sdata = link->sdata;
1093         struct ieee80211_chan_req chanreq = {};
1094         struct cfg80211_chan_def ap_chandef;
1095         enum ieee80211_conn_mode ap_mode;
1096         u32 vht_cap_info = 0;
1097         u16 ht_opmode;
1098         int ret;
1099
1100         /* don't track any bandwidth changes in legacy/S1G modes */
1101         if (link->u.mgd.conn.mode == IEEE80211_CONN_MODE_LEGACY ||
1102             link->u.mgd.conn.mode == IEEE80211_CONN_MODE_S1G)
1103                 return 0;
1104
1105         if (elems->vht_cap_elem)
1106                 vht_cap_info = le32_to_cpu(elems->vht_cap_elem->vht_cap_info);
1107
1108         ap_mode = ieee80211_determine_ap_chan(sdata, channel, vht_cap_info,
1109                                               elems, true, &link->u.mgd.conn,
1110                                               &ap_chandef);
1111
1112         if (ap_mode != link->u.mgd.conn.mode) {
1113                 link_info(link,
1114                           "AP %pM appears to change mode (expected %s, found %s) in %s, disconnect\n",
1115                           link->u.mgd.bssid,
1116                           ieee80211_conn_mode_str(link->u.mgd.conn.mode),
1117                           ieee80211_conn_mode_str(ap_mode), frame);
1118                 return -EINVAL;
1119         }
1120
1121         chanreq.oper = ap_chandef;
1122         ieee80211_set_chanreq_ap(sdata, &chanreq, &link->u.mgd.conn,
1123                                  &ap_chandef);
1124
1125         /*
1126          * if HT operation mode changed store the new one -
1127          * this may be applicable even if channel is identical
1128          */
1129         if (elems->ht_operation) {
1130                 ht_opmode = le16_to_cpu(elems->ht_operation->operation_mode);
1131                 if (link->conf->ht_operation_mode != ht_opmode) {
1132                         *changed |= BSS_CHANGED_HT;
1133                         link->conf->ht_operation_mode = ht_opmode;
1134                 }
1135         }
1136
1137         /*
1138          * Downgrade the new channel if we associated with restricted
1139          * bandwidth capabilities. For example, if we associated as a
1140          * 20 MHz STA to a 40 MHz AP (due to regulatory, capabilities
1141          * or config reasons) then switching to a 40 MHz channel now
1142          * won't do us any good -- we couldn't use it with the AP.
1143          */
1144         while (link->u.mgd.conn.bw_limit <
1145                         ieee80211_min_bw_limit_from_chandef(&chanreq.oper))
1146                 ieee80211_chandef_downgrade(&chanreq.oper, NULL);
1147
1148         if (ap_chandef.chan->band == NL80211_BAND_6GHZ &&
1149             link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_HE) {
1150                 ieee80211_rearrange_tpe(&elems->tpe, &ap_chandef,
1151                                         &chanreq.oper);
1152                 if (memcmp(&link->conf->tpe, &elems->tpe, sizeof(elems->tpe))) {
1153                         link->conf->tpe = elems->tpe;
1154                         *changed |= BSS_CHANGED_TPE;
1155                 }
1156         }
1157
1158         if (ieee80211_chanreq_identical(&chanreq, &link->conf->chanreq))
1159                 return 0;
1160
1161         link_info(link,
1162                   "AP %pM changed bandwidth in %s, new used config is %d.%03d MHz, width %d (%d.%03d/%d MHz)\n",
1163                   link->u.mgd.bssid, frame, chanreq.oper.chan->center_freq,
1164                   chanreq.oper.chan->freq_offset, chanreq.oper.width,
1165                   chanreq.oper.center_freq1, chanreq.oper.freq1_offset,
1166                   chanreq.oper.center_freq2);
1167
1168         if (!cfg80211_chandef_valid(&chanreq.oper)) {
1169                 sdata_info(sdata,
1170                            "AP %pM changed caps/bw in %s in a way we can't support - disconnect\n",
1171                            link->u.mgd.bssid, frame);
1172                 return -EINVAL;
1173         }
1174
1175         if (!update) {
1176                 link->conf->chanreq = chanreq;
1177                 return 0;
1178         }
1179
1180         /*
1181          * We're tracking the current AP here, so don't do any further checks
1182          * here. This keeps us from playing ping-pong with regulatory, without
1183          * it the following can happen (for example):
1184          *  - connect to an AP with 80 MHz, world regdom allows 80 MHz
1185          *  - AP advertises regdom US
1186          *  - CRDA loads regdom US with 80 MHz prohibited (old database)
1187          *  - we detect an unsupported channel and disconnect
1188          *  - disconnect causes CRDA to reload world regdomain and the game
1189          *    starts anew.
1190          * (see https://bugzilla.kernel.org/show_bug.cgi?id=70881)
1191          *
1192          * It seems possible that there are still scenarios with CSA or real
1193          * bandwidth changes where a this could happen, but those cases are
1194          * less common and wouldn't completely prevent using the AP.
1195          */
1196
1197         ret = ieee80211_link_change_chanreq(link, &chanreq, changed);
1198         if (ret) {
1199                 sdata_info(sdata,
1200                            "AP %pM changed bandwidth in %s to incompatible one - disconnect\n",
1201                            link->u.mgd.bssid, frame);
1202                 return ret;
1203         }
1204
1205         cfg80211_schedule_channels_check(&sdata->wdev);
1206         return 0;
1207 }
1208
1209 /* frame sending functions */
1210
1211 static void ieee80211_add_ht_ie(struct ieee80211_sub_if_data *sdata,
1212                                 struct sk_buff *skb, u8 ap_ht_param,
1213                                 struct ieee80211_supported_band *sband,
1214                                 struct ieee80211_channel *channel,
1215                                 enum ieee80211_smps_mode smps,
1216                                 const struct ieee80211_conn_settings *conn)
1217 {
1218         u8 *pos;
1219         u32 flags = channel->flags;
1220         u16 cap;
1221         struct ieee80211_sta_ht_cap ht_cap;
1222
1223         BUILD_BUG_ON(sizeof(ht_cap) != sizeof(sband->ht_cap));
1224
1225         memcpy(&ht_cap, &sband->ht_cap, sizeof(ht_cap));
1226         ieee80211_apply_htcap_overrides(sdata, &ht_cap);
1227
1228         /* determine capability flags */
1229         cap = ht_cap.cap;
1230
1231         switch (ap_ht_param & IEEE80211_HT_PARAM_CHA_SEC_OFFSET) {
1232         case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
1233                 if (flags & IEEE80211_CHAN_NO_HT40PLUS) {
1234                         cap &= ~IEEE80211_HT_CAP_SUP_WIDTH_20_40;
1235                         cap &= ~IEEE80211_HT_CAP_SGI_40;
1236                 }
1237                 break;
1238         case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
1239                 if (flags & IEEE80211_CHAN_NO_HT40MINUS) {
1240                         cap &= ~IEEE80211_HT_CAP_SUP_WIDTH_20_40;
1241                         cap &= ~IEEE80211_HT_CAP_SGI_40;
1242                 }
1243                 break;
1244         }
1245
1246         /*
1247          * If 40 MHz was disabled associate as though we weren't
1248          * capable of 40 MHz -- some broken APs will never fall
1249          * back to trying to transmit in 20 MHz.
1250          */
1251         if (conn->bw_limit <= IEEE80211_CONN_BW_LIMIT_20) {
1252                 cap &= ~IEEE80211_HT_CAP_SUP_WIDTH_20_40;
1253                 cap &= ~IEEE80211_HT_CAP_SGI_40;
1254         }
1255
1256         /* set SM PS mode properly */
1257         cap &= ~IEEE80211_HT_CAP_SM_PS;
1258         switch (smps) {
1259         case IEEE80211_SMPS_AUTOMATIC:
1260         case IEEE80211_SMPS_NUM_MODES:
1261                 WARN_ON(1);
1262                 fallthrough;
1263         case IEEE80211_SMPS_OFF:
1264                 cap |= WLAN_HT_CAP_SM_PS_DISABLED <<
1265                         IEEE80211_HT_CAP_SM_PS_SHIFT;
1266                 break;
1267         case IEEE80211_SMPS_STATIC:
1268                 cap |= WLAN_HT_CAP_SM_PS_STATIC <<
1269                         IEEE80211_HT_CAP_SM_PS_SHIFT;
1270                 break;
1271         case IEEE80211_SMPS_DYNAMIC:
1272                 cap |= WLAN_HT_CAP_SM_PS_DYNAMIC <<
1273                         IEEE80211_HT_CAP_SM_PS_SHIFT;
1274                 break;
1275         }
1276
1277         /* reserve and fill IE */
1278         pos = skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
1279         ieee80211_ie_build_ht_cap(pos, &ht_cap, cap);
1280 }
1281
1282 /* This function determines vht capability flags for the association
1283  * and builds the IE.
1284  * Note - the function returns true to own the MU-MIMO capability
1285  */
1286 static bool ieee80211_add_vht_ie(struct ieee80211_sub_if_data *sdata,
1287                                  struct sk_buff *skb,
1288                                  struct ieee80211_supported_band *sband,
1289                                  struct ieee80211_vht_cap *ap_vht_cap,
1290                                  const struct ieee80211_conn_settings *conn)
1291 {
1292         struct ieee80211_local *local = sdata->local;
1293         u8 *pos;
1294         u32 cap;
1295         struct ieee80211_sta_vht_cap vht_cap;
1296         u32 mask, ap_bf_sts, our_bf_sts;
1297         bool mu_mimo_owner = false;
1298
1299         BUILD_BUG_ON(sizeof(vht_cap) != sizeof(sband->vht_cap));
1300
1301         memcpy(&vht_cap, &sband->vht_cap, sizeof(vht_cap));
1302         ieee80211_apply_vhtcap_overrides(sdata, &vht_cap);
1303
1304         /* determine capability flags */
1305         cap = vht_cap.cap;
1306
1307         if (conn->bw_limit <= IEEE80211_CONN_BW_LIMIT_80) {
1308                 cap &= ~IEEE80211_VHT_CAP_SHORT_GI_160;
1309                 cap &= ~IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK;
1310         }
1311
1312         /*
1313          * Some APs apparently get confused if our capabilities are better
1314          * than theirs, so restrict what we advertise in the assoc request.
1315          */
1316         if (!(ap_vht_cap->vht_cap_info &
1317                         cpu_to_le32(IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE)))
1318                 cap &= ~(IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
1319                          IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE);
1320         else if (!(ap_vht_cap->vht_cap_info &
1321                         cpu_to_le32(IEEE80211_VHT_CAP_MU_BEAMFORMER_CAPABLE)))
1322                 cap &= ~IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE;
1323
1324         /*
1325          * If some other vif is using the MU-MIMO capability we cannot associate
1326          * using MU-MIMO - this will lead to contradictions in the group-id
1327          * mechanism.
1328          * Ownership is defined since association request, in order to avoid
1329          * simultaneous associations with MU-MIMO.
1330          */
1331         if (cap & IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE) {
1332                 bool disable_mu_mimo = false;
1333                 struct ieee80211_sub_if_data *other;
1334
1335                 list_for_each_entry(other, &local->interfaces, list) {
1336                         if (other->vif.bss_conf.mu_mimo_owner) {
1337                                 disable_mu_mimo = true;
1338                                 break;
1339                         }
1340                 }
1341                 if (disable_mu_mimo)
1342                         cap &= ~IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE;
1343                 else
1344                         mu_mimo_owner = true;
1345         }
1346
1347         mask = IEEE80211_VHT_CAP_BEAMFORMEE_STS_MASK;
1348
1349         ap_bf_sts = le32_to_cpu(ap_vht_cap->vht_cap_info) & mask;
1350         our_bf_sts = cap & mask;
1351
1352         if (ap_bf_sts < our_bf_sts) {
1353                 cap &= ~mask;
1354                 cap |= ap_bf_sts;
1355         }
1356
1357         /* reserve and fill IE */
1358         pos = skb_put(skb, sizeof(struct ieee80211_vht_cap) + 2);
1359         ieee80211_ie_build_vht_cap(pos, &vht_cap, cap);
1360
1361         return mu_mimo_owner;
1362 }
1363
1364 static void ieee80211_assoc_add_rates(struct sk_buff *skb,
1365                                       enum nl80211_chan_width width,
1366                                       struct ieee80211_supported_band *sband,
1367                                       struct ieee80211_mgd_assoc_data *assoc_data)
1368 {
1369         u32 rates;
1370
1371         if (assoc_data->supp_rates_len) {
1372                 /*
1373                  * Get all rates supported by the device and the AP as
1374                  * some APs don't like getting a superset of their rates
1375                  * in the association request (e.g. D-Link DAP 1353 in
1376                  * b-only mode)...
1377                  */
1378                 ieee80211_parse_bitrates(width, sband,
1379                                          assoc_data->supp_rates,
1380                                          assoc_data->supp_rates_len,
1381                                          &rates);
1382         } else {
1383                 /*
1384                  * In case AP not provide any supported rates information
1385                  * before association, we send information element(s) with
1386                  * all rates that we support.
1387                  */
1388                 rates = ~0;
1389         }
1390
1391         ieee80211_put_srates_elem(skb, sband, 0, 0, ~rates,
1392                                   WLAN_EID_SUPP_RATES);
1393         ieee80211_put_srates_elem(skb, sband, 0, 0, ~rates,
1394                                   WLAN_EID_EXT_SUPP_RATES);
1395 }
1396
1397 static size_t ieee80211_add_before_ht_elems(struct sk_buff *skb,
1398                                             const u8 *elems,
1399                                             size_t elems_len,
1400                                             size_t offset)
1401 {
1402         size_t noffset;
1403
1404         static const u8 before_ht[] = {
1405                 WLAN_EID_SSID,
1406                 WLAN_EID_SUPP_RATES,
1407                 WLAN_EID_EXT_SUPP_RATES,
1408                 WLAN_EID_PWR_CAPABILITY,
1409                 WLAN_EID_SUPPORTED_CHANNELS,
1410                 WLAN_EID_RSN,
1411                 WLAN_EID_QOS_CAPA,
1412                 WLAN_EID_RRM_ENABLED_CAPABILITIES,
1413                 WLAN_EID_MOBILITY_DOMAIN,
1414                 WLAN_EID_FAST_BSS_TRANSITION,   /* reassoc only */
1415                 WLAN_EID_RIC_DATA,              /* reassoc only */
1416                 WLAN_EID_SUPPORTED_REGULATORY_CLASSES,
1417         };
1418         static const u8 after_ric[] = {
1419                 WLAN_EID_SUPPORTED_REGULATORY_CLASSES,
1420                 WLAN_EID_HT_CAPABILITY,
1421                 WLAN_EID_BSS_COEX_2040,
1422                 /* luckily this is almost always there */
1423                 WLAN_EID_EXT_CAPABILITY,
1424                 WLAN_EID_QOS_TRAFFIC_CAPA,
1425                 WLAN_EID_TIM_BCAST_REQ,
1426                 WLAN_EID_INTERWORKING,
1427                 /* 60 GHz (Multi-band, DMG, MMS) can't happen */
1428                 WLAN_EID_VHT_CAPABILITY,
1429                 WLAN_EID_OPMODE_NOTIF,
1430         };
1431
1432         if (!elems_len)
1433                 return offset;
1434
1435         noffset = ieee80211_ie_split_ric(elems, elems_len,
1436                                          before_ht,
1437                                          ARRAY_SIZE(before_ht),
1438                                          after_ric,
1439                                          ARRAY_SIZE(after_ric),
1440                                          offset);
1441         skb_put_data(skb, elems + offset, noffset - offset);
1442
1443         return noffset;
1444 }
1445
1446 static size_t ieee80211_add_before_vht_elems(struct sk_buff *skb,
1447                                              const u8 *elems,
1448                                              size_t elems_len,
1449                                              size_t offset)
1450 {
1451         static const u8 before_vht[] = {
1452                 /*
1453                  * no need to list the ones split off before HT
1454                  * or generated here
1455                  */
1456                 WLAN_EID_BSS_COEX_2040,
1457                 WLAN_EID_EXT_CAPABILITY,
1458                 WLAN_EID_QOS_TRAFFIC_CAPA,
1459                 WLAN_EID_TIM_BCAST_REQ,
1460                 WLAN_EID_INTERWORKING,
1461                 /* 60 GHz (Multi-band, DMG, MMS) can't happen */
1462         };
1463         size_t noffset;
1464
1465         if (!elems_len)
1466                 return offset;
1467
1468         /* RIC already taken care of in ieee80211_add_before_ht_elems() */
1469         noffset = ieee80211_ie_split(elems, elems_len,
1470                                      before_vht, ARRAY_SIZE(before_vht),
1471                                      offset);
1472         skb_put_data(skb, elems + offset, noffset - offset);
1473
1474         return noffset;
1475 }
1476
1477 static size_t ieee80211_add_before_he_elems(struct sk_buff *skb,
1478                                             const u8 *elems,
1479                                             size_t elems_len,
1480                                             size_t offset)
1481 {
1482         static const u8 before_he[] = {
1483                 /*
1484                  * no need to list the ones split off before VHT
1485                  * or generated here
1486                  */
1487                 WLAN_EID_OPMODE_NOTIF,
1488                 WLAN_EID_EXTENSION, WLAN_EID_EXT_FUTURE_CHAN_GUIDANCE,
1489                 /* 11ai elements */
1490                 WLAN_EID_EXTENSION, WLAN_EID_EXT_FILS_SESSION,
1491                 WLAN_EID_EXTENSION, WLAN_EID_EXT_FILS_PUBLIC_KEY,
1492                 WLAN_EID_EXTENSION, WLAN_EID_EXT_FILS_KEY_CONFIRM,
1493                 WLAN_EID_EXTENSION, WLAN_EID_EXT_FILS_HLP_CONTAINER,
1494                 WLAN_EID_EXTENSION, WLAN_EID_EXT_FILS_IP_ADDR_ASSIGN,
1495                 /* TODO: add 11ah/11aj/11ak elements */
1496         };
1497         size_t noffset;
1498
1499         if (!elems_len)
1500                 return offset;
1501
1502         /* RIC already taken care of in ieee80211_add_before_ht_elems() */
1503         noffset = ieee80211_ie_split(elems, elems_len,
1504                                      before_he, ARRAY_SIZE(before_he),
1505                                      offset);
1506         skb_put_data(skb, elems + offset, noffset - offset);
1507
1508         return noffset;
1509 }
1510
1511 #define PRESENT_ELEMS_MAX       8
1512 #define PRESENT_ELEM_EXT_OFFS   0x100
1513
1514 static void
1515 ieee80211_assoc_add_ml_elem(struct ieee80211_sub_if_data *sdata,
1516                             struct sk_buff *skb, u16 capab,
1517                             const struct element *ext_capa,
1518                             const u16 *present_elems,
1519                             struct ieee80211_mgd_assoc_data *assoc_data);
1520
1521 static size_t
1522 ieee80211_add_link_elems(struct ieee80211_sub_if_data *sdata,
1523                          struct sk_buff *skb, u16 *capab,
1524                          const struct element *ext_capa,
1525                          const u8 *extra_elems,
1526                          size_t extra_elems_len,
1527                          unsigned int link_id,
1528                          struct ieee80211_link_data *link,
1529                          u16 *present_elems,
1530                          struct ieee80211_mgd_assoc_data *assoc_data)
1531 {
1532         enum nl80211_iftype iftype = ieee80211_vif_type_p2p(&sdata->vif);
1533         struct cfg80211_bss *cbss = assoc_data->link[link_id].bss;
1534         struct ieee80211_channel *chan = cbss->channel;
1535         const struct ieee80211_sband_iftype_data *iftd;
1536         struct ieee80211_local *local = sdata->local;
1537         struct ieee80211_supported_band *sband;
1538         enum nl80211_chan_width width = NL80211_CHAN_WIDTH_20;
1539         struct ieee80211_chanctx_conf *chanctx_conf;
1540         enum ieee80211_smps_mode smps_mode;
1541         u16 orig_capab = *capab;
1542         size_t offset = 0;
1543         int present_elems_len = 0;
1544         u8 *pos;
1545         int i;
1546
1547 #define ADD_PRESENT_ELEM(id) do {                                       \
1548         /* need a last for termination - we use 0 == SSID */            \
1549         if (!WARN_ON(present_elems_len >= PRESENT_ELEMS_MAX - 1))       \
1550                 present_elems[present_elems_len++] = (id);              \
1551 } while (0)
1552 #define ADD_PRESENT_EXT_ELEM(id) ADD_PRESENT_ELEM(PRESENT_ELEM_EXT_OFFS | (id))
1553
1554         if (link)
1555                 smps_mode = link->smps_mode;
1556         else if (sdata->u.mgd.powersave)
1557                 smps_mode = IEEE80211_SMPS_DYNAMIC;
1558         else
1559                 smps_mode = IEEE80211_SMPS_OFF;
1560
1561         if (link) {
1562                 /*
1563                  * 5/10 MHz scenarios are only viable without MLO, in which
1564                  * case this pointer should be used ... All of this is a bit
1565                  * unclear though, not sure this even works at all.
1566                  */
1567                 rcu_read_lock();
1568                 chanctx_conf = rcu_dereference(link->conf->chanctx_conf);
1569                 if (chanctx_conf)
1570                         width = chanctx_conf->def.width;
1571                 rcu_read_unlock();
1572         }
1573
1574         sband = local->hw.wiphy->bands[chan->band];
1575         iftd = ieee80211_get_sband_iftype_data(sband, iftype);
1576
1577         if (sband->band == NL80211_BAND_2GHZ) {
1578                 *capab |= WLAN_CAPABILITY_SHORT_SLOT_TIME;
1579                 *capab |= WLAN_CAPABILITY_SHORT_PREAMBLE;
1580         }
1581
1582         if ((cbss->capability & WLAN_CAPABILITY_SPECTRUM_MGMT) &&
1583             ieee80211_hw_check(&local->hw, SPECTRUM_MGMT))
1584                 *capab |= WLAN_CAPABILITY_SPECTRUM_MGMT;
1585
1586         if (sband->band != NL80211_BAND_S1GHZ)
1587                 ieee80211_assoc_add_rates(skb, width, sband, assoc_data);
1588
1589         if (*capab & WLAN_CAPABILITY_SPECTRUM_MGMT ||
1590             *capab & WLAN_CAPABILITY_RADIO_MEASURE) {
1591                 struct cfg80211_chan_def chandef = {
1592                         .width = width,
1593                         .chan = chan,
1594                 };
1595
1596                 pos = skb_put(skb, 4);
1597                 *pos++ = WLAN_EID_PWR_CAPABILITY;
1598                 *pos++ = 2;
1599                 *pos++ = 0; /* min tx power */
1600                  /* max tx power */
1601                 *pos++ = ieee80211_chandef_max_power(&chandef);
1602                 ADD_PRESENT_ELEM(WLAN_EID_PWR_CAPABILITY);
1603         }
1604
1605         /*
1606          * Per spec, we shouldn't include the list of channels if we advertise
1607          * support for extended channel switching, but we've always done that;
1608          * (for now?) apply this restriction only on the (new) 6 GHz band.
1609          */
1610         if (*capab & WLAN_CAPABILITY_SPECTRUM_MGMT &&
1611             (sband->band != NL80211_BAND_6GHZ ||
1612              !ext_capa || ext_capa->datalen < 1 ||
1613              !(ext_capa->data[0] & WLAN_EXT_CAPA1_EXT_CHANNEL_SWITCHING))) {
1614                 /* TODO: get this in reg domain format */
1615                 pos = skb_put(skb, 2 * sband->n_channels + 2);
1616                 *pos++ = WLAN_EID_SUPPORTED_CHANNELS;
1617                 *pos++ = 2 * sband->n_channels;
1618                 for (i = 0; i < sband->n_channels; i++) {
1619                         int cf = sband->channels[i].center_freq;
1620
1621                         *pos++ = ieee80211_frequency_to_channel(cf);
1622                         *pos++ = 1; /* one channel in the subband*/
1623                 }
1624                 ADD_PRESENT_ELEM(WLAN_EID_SUPPORTED_CHANNELS);
1625         }
1626
1627         /* if present, add any custom IEs that go before HT */
1628         offset = ieee80211_add_before_ht_elems(skb, extra_elems,
1629                                                extra_elems_len,
1630                                                offset);
1631
1632         if (sband->band != NL80211_BAND_6GHZ &&
1633             assoc_data->link[link_id].conn.mode >= IEEE80211_CONN_MODE_HT) {
1634                 ieee80211_add_ht_ie(sdata, skb,
1635                                     assoc_data->link[link_id].ap_ht_param,
1636                                     sband, chan, smps_mode,
1637                                     &assoc_data->link[link_id].conn);
1638                 ADD_PRESENT_ELEM(WLAN_EID_HT_CAPABILITY);
1639         }
1640
1641         /* if present, add any custom IEs that go before VHT */
1642         offset = ieee80211_add_before_vht_elems(skb, extra_elems,
1643                                                 extra_elems_len,
1644                                                 offset);
1645
1646         if (sband->band != NL80211_BAND_6GHZ &&
1647             assoc_data->link[link_id].conn.mode >= IEEE80211_CONN_MODE_VHT &&
1648             sband->vht_cap.vht_supported) {
1649                 bool mu_mimo_owner =
1650                         ieee80211_add_vht_ie(sdata, skb, sband,
1651                                              &assoc_data->link[link_id].ap_vht_cap,
1652                                              &assoc_data->link[link_id].conn);
1653
1654                 if (link)
1655                         link->conf->mu_mimo_owner = mu_mimo_owner;
1656                 ADD_PRESENT_ELEM(WLAN_EID_VHT_CAPABILITY);
1657         }
1658
1659         /* if present, add any custom IEs that go before HE */
1660         offset = ieee80211_add_before_he_elems(skb, extra_elems,
1661                                                extra_elems_len,
1662                                                offset);
1663
1664         if (assoc_data->link[link_id].conn.mode >= IEEE80211_CONN_MODE_HE) {
1665                 ieee80211_put_he_cap(skb, sdata, sband,
1666                                      &assoc_data->link[link_id].conn);
1667                 ADD_PRESENT_EXT_ELEM(WLAN_EID_EXT_HE_CAPABILITY);
1668                 ieee80211_put_he_6ghz_cap(skb, sdata, smps_mode);
1669         }
1670
1671         /*
1672          * careful - need to know about all the present elems before
1673          * calling ieee80211_assoc_add_ml_elem(), so add this one if
1674          * we're going to put it after the ML element
1675          */
1676         if (assoc_data->link[link_id].conn.mode >= IEEE80211_CONN_MODE_EHT)
1677                 ADD_PRESENT_EXT_ELEM(WLAN_EID_EXT_EHT_CAPABILITY);
1678
1679         if (link_id == assoc_data->assoc_link_id)
1680                 ieee80211_assoc_add_ml_elem(sdata, skb, orig_capab, ext_capa,
1681                                             present_elems, assoc_data);
1682
1683         /* crash if somebody gets it wrong */
1684         present_elems = NULL;
1685
1686         if (assoc_data->link[link_id].conn.mode >= IEEE80211_CONN_MODE_EHT)
1687                 ieee80211_put_eht_cap(skb, sdata, sband,
1688                                       &assoc_data->link[link_id].conn);
1689
1690         if (sband->band == NL80211_BAND_S1GHZ) {
1691                 ieee80211_add_aid_request_ie(sdata, skb);
1692                 ieee80211_add_s1g_capab_ie(sdata, &sband->s1g_cap, skb);
1693         }
1694
1695         if (iftd && iftd->vendor_elems.data && iftd->vendor_elems.len)
1696                 skb_put_data(skb, iftd->vendor_elems.data, iftd->vendor_elems.len);
1697
1698         return offset;
1699 }
1700
1701 static void ieee80211_add_non_inheritance_elem(struct sk_buff *skb,
1702                                                const u16 *outer,
1703                                                const u16 *inner)
1704 {
1705         unsigned int skb_len = skb->len;
1706         bool at_extension = false;
1707         bool added = false;
1708         int i, j;
1709         u8 *len, *list_len = NULL;
1710
1711         skb_put_u8(skb, WLAN_EID_EXTENSION);
1712         len = skb_put(skb, 1);
1713         skb_put_u8(skb, WLAN_EID_EXT_NON_INHERITANCE);
1714
1715         for (i = 0; i < PRESENT_ELEMS_MAX && outer[i]; i++) {
1716                 u16 elem = outer[i];
1717                 bool have_inner = false;
1718
1719                 /* should at least be sorted in the sense of normal -> ext */
1720                 WARN_ON(at_extension && elem < PRESENT_ELEM_EXT_OFFS);
1721
1722                 /* switch to extension list */
1723                 if (!at_extension && elem >= PRESENT_ELEM_EXT_OFFS) {
1724                         at_extension = true;
1725                         if (!list_len)
1726                                 skb_put_u8(skb, 0);
1727                         list_len = NULL;
1728                 }
1729
1730                 for (j = 0; j < PRESENT_ELEMS_MAX && inner[j]; j++) {
1731                         if (elem == inner[j]) {
1732                                 have_inner = true;
1733                                 break;
1734                         }
1735                 }
1736
1737                 if (have_inner)
1738                         continue;
1739
1740                 if (!list_len) {
1741                         list_len = skb_put(skb, 1);
1742                         *list_len = 0;
1743                 }
1744                 *list_len += 1;
1745                 skb_put_u8(skb, (u8)elem);
1746                 added = true;
1747         }
1748
1749         /* if we added a list but no extension list, make a zero-len one */
1750         if (added && (!at_extension || !list_len))
1751                 skb_put_u8(skb, 0);
1752
1753         /* if nothing added remove extension element completely */
1754         if (!added)
1755                 skb_trim(skb, skb_len);
1756         else
1757                 *len = skb->len - skb_len - 2;
1758 }
1759
1760 static void
1761 ieee80211_assoc_add_ml_elem(struct ieee80211_sub_if_data *sdata,
1762                             struct sk_buff *skb, u16 capab,
1763                             const struct element *ext_capa,
1764                             const u16 *outer_present_elems,
1765                             struct ieee80211_mgd_assoc_data *assoc_data)
1766 {
1767         struct ieee80211_local *local = sdata->local;
1768         struct ieee80211_multi_link_elem *ml_elem;
1769         struct ieee80211_mle_basic_common_info *common;
1770         const struct wiphy_iftype_ext_capab *ift_ext_capa;
1771         __le16 eml_capa = 0, mld_capa_ops = 0;
1772         unsigned int link_id;
1773         u8 *ml_elem_len;
1774         void *capab_pos;
1775
1776         if (!ieee80211_vif_is_mld(&sdata->vif))
1777                 return;
1778
1779         ift_ext_capa = cfg80211_get_iftype_ext_capa(local->hw.wiphy,
1780                                                     ieee80211_vif_type_p2p(&sdata->vif));
1781         if (ift_ext_capa) {
1782                 eml_capa = cpu_to_le16(ift_ext_capa->eml_capabilities);
1783                 mld_capa_ops = cpu_to_le16(ift_ext_capa->mld_capa_and_ops);
1784         }
1785
1786         skb_put_u8(skb, WLAN_EID_EXTENSION);
1787         ml_elem_len = skb_put(skb, 1);
1788         skb_put_u8(skb, WLAN_EID_EXT_EHT_MULTI_LINK);
1789         ml_elem = skb_put(skb, sizeof(*ml_elem));
1790         ml_elem->control =
1791                 cpu_to_le16(IEEE80211_ML_CONTROL_TYPE_BASIC |
1792                             IEEE80211_MLC_BASIC_PRES_MLD_CAPA_OP);
1793         common = skb_put(skb, sizeof(*common));
1794         common->len = sizeof(*common) +
1795                       2;  /* MLD capa/ops */
1796         memcpy(common->mld_mac_addr, sdata->vif.addr, ETH_ALEN);
1797
1798         /* add EML_CAPA only if needed, see Draft P802.11be_D2.1, 35.3.17 */
1799         if (eml_capa &
1800             cpu_to_le16((IEEE80211_EML_CAP_EMLSR_SUPP |
1801                          IEEE80211_EML_CAP_EMLMR_SUPPORT))) {
1802                 common->len += 2; /* EML capabilities */
1803                 ml_elem->control |=
1804                         cpu_to_le16(IEEE80211_MLC_BASIC_PRES_EML_CAPA);
1805                 skb_put_data(skb, &eml_capa, sizeof(eml_capa));
1806         }
1807         skb_put_data(skb, &mld_capa_ops, sizeof(mld_capa_ops));
1808
1809         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
1810                 u16 link_present_elems[PRESENT_ELEMS_MAX] = {};
1811                 const u8 *extra_elems;
1812                 size_t extra_elems_len;
1813                 size_t extra_used;
1814                 u8 *subelem_len = NULL;
1815                 __le16 ctrl;
1816
1817                 if (!assoc_data->link[link_id].bss ||
1818                     link_id == assoc_data->assoc_link_id)
1819                         continue;
1820
1821                 extra_elems = assoc_data->link[link_id].elems;
1822                 extra_elems_len = assoc_data->link[link_id].elems_len;
1823
1824                 skb_put_u8(skb, IEEE80211_MLE_SUBELEM_PER_STA_PROFILE);
1825                 subelem_len = skb_put(skb, 1);
1826
1827                 ctrl = cpu_to_le16(link_id |
1828                                    IEEE80211_MLE_STA_CONTROL_COMPLETE_PROFILE |
1829                                    IEEE80211_MLE_STA_CONTROL_STA_MAC_ADDR_PRESENT);
1830                 skb_put_data(skb, &ctrl, sizeof(ctrl));
1831                 skb_put_u8(skb, 1 + ETH_ALEN); /* STA Info Length */
1832                 skb_put_data(skb, assoc_data->link[link_id].addr,
1833                              ETH_ALEN);
1834                 /*
1835                  * Now add the contents of the (re)association request,
1836                  * but the "listen interval" and "current AP address"
1837                  * (if applicable) are skipped. So we only have
1838                  * the capability field (remember the position and fill
1839                  * later), followed by the elements added below by
1840                  * calling ieee80211_add_link_elems().
1841                  */
1842                 capab_pos = skb_put(skb, 2);
1843
1844                 extra_used = ieee80211_add_link_elems(sdata, skb, &capab,
1845                                                       ext_capa,
1846                                                       extra_elems,
1847                                                       extra_elems_len,
1848                                                       link_id, NULL,
1849                                                       link_present_elems,
1850                                                       assoc_data);
1851                 if (extra_elems)
1852                         skb_put_data(skb, extra_elems + extra_used,
1853                                      extra_elems_len - extra_used);
1854
1855                 put_unaligned_le16(capab, capab_pos);
1856
1857                 ieee80211_add_non_inheritance_elem(skb, outer_present_elems,
1858                                                    link_present_elems);
1859
1860                 ieee80211_fragment_element(skb, subelem_len,
1861                                            IEEE80211_MLE_SUBELEM_FRAGMENT);
1862         }
1863
1864         ieee80211_fragment_element(skb, ml_elem_len, WLAN_EID_FRAGMENT);
1865 }
1866
1867 static int
1868 ieee80211_link_common_elems_size(struct ieee80211_sub_if_data *sdata,
1869                                  enum nl80211_iftype iftype,
1870                                  struct cfg80211_bss *cbss,
1871                                  size_t elems_len)
1872 {
1873         struct ieee80211_local *local = sdata->local;
1874         const struct ieee80211_sband_iftype_data *iftd;
1875         struct ieee80211_supported_band *sband;
1876         size_t size = 0;
1877
1878         if (!cbss)
1879                 return size;
1880
1881         sband = local->hw.wiphy->bands[cbss->channel->band];
1882
1883         /* add STA profile elements length */
1884         size += elems_len;
1885
1886         /* and supported rates length */
1887         size += 4 + sband->n_bitrates;
1888
1889         /* supported channels */
1890         size += 2 + 2 * sband->n_channels;
1891
1892         iftd = ieee80211_get_sband_iftype_data(sband, iftype);
1893         if (iftd)
1894                 size += iftd->vendor_elems.len;
1895
1896         /* power capability */
1897         size += 4;
1898
1899         /* HT, VHT, HE, EHT */
1900         size += 2 + sizeof(struct ieee80211_ht_cap);
1901         size += 2 + sizeof(struct ieee80211_vht_cap);
1902         size += 2 + 1 + sizeof(struct ieee80211_he_cap_elem) +
1903                 sizeof(struct ieee80211_he_mcs_nss_supp) +
1904                 IEEE80211_HE_PPE_THRES_MAX_LEN;
1905
1906         if (sband->band == NL80211_BAND_6GHZ)
1907                 size += 2 + 1 + sizeof(struct ieee80211_he_6ghz_capa);
1908
1909         size += 2 + 1 + sizeof(struct ieee80211_eht_cap_elem) +
1910                 sizeof(struct ieee80211_eht_mcs_nss_supp) +
1911                 IEEE80211_EHT_PPE_THRES_MAX_LEN;
1912
1913         return size;
1914 }
1915
1916 static int ieee80211_send_assoc(struct ieee80211_sub_if_data *sdata)
1917 {
1918         struct ieee80211_local *local = sdata->local;
1919         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
1920         struct ieee80211_mgd_assoc_data *assoc_data = ifmgd->assoc_data;
1921         struct ieee80211_link_data *link;
1922         struct sk_buff *skb;
1923         struct ieee80211_mgmt *mgmt;
1924         u8 *pos, qos_info, *ie_start;
1925         size_t offset, noffset;
1926         u16 capab = 0, link_capab;
1927         __le16 listen_int;
1928         struct element *ext_capa = NULL;
1929         enum nl80211_iftype iftype = ieee80211_vif_type_p2p(&sdata->vif);
1930         struct ieee80211_prep_tx_info info = {};
1931         unsigned int link_id, n_links = 0;
1932         u16 present_elems[PRESENT_ELEMS_MAX] = {};
1933         void *capab_pos;
1934         size_t size;
1935         int ret;
1936
1937         /* we know it's writable, cast away the const */
1938         if (assoc_data->ie_len)
1939                 ext_capa = (void *)cfg80211_find_elem(WLAN_EID_EXT_CAPABILITY,
1940                                                       assoc_data->ie,
1941                                                       assoc_data->ie_len);
1942
1943         lockdep_assert_wiphy(sdata->local->hw.wiphy);
1944
1945         size = local->hw.extra_tx_headroom +
1946                sizeof(*mgmt) + /* bit too much but doesn't matter */
1947                2 + assoc_data->ssid_len + /* SSID */
1948                assoc_data->ie_len + /* extra IEs */
1949                (assoc_data->fils_kek_len ? 16 /* AES-SIV */ : 0) +
1950                9; /* WMM */
1951
1952         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
1953                 struct cfg80211_bss *cbss = assoc_data->link[link_id].bss;
1954                 size_t elems_len = assoc_data->link[link_id].elems_len;
1955
1956                 if (!cbss)
1957                         continue;
1958
1959                 n_links++;
1960
1961                 size += ieee80211_link_common_elems_size(sdata, iftype, cbss,
1962                                                          elems_len);
1963
1964                 /* non-inheritance element */
1965                 size += 2 + 2 + PRESENT_ELEMS_MAX;
1966
1967                 /* should be the same across all BSSes */
1968                 if (cbss->capability & WLAN_CAPABILITY_PRIVACY)
1969                         capab |= WLAN_CAPABILITY_PRIVACY;
1970         }
1971
1972         if (ieee80211_vif_is_mld(&sdata->vif)) {
1973                 /* consider the multi-link element with STA profile */
1974                 size += sizeof(struct ieee80211_multi_link_elem);
1975                 /* max common info field in basic multi-link element */
1976                 size += sizeof(struct ieee80211_mle_basic_common_info) +
1977                         2 + /* capa & op */
1978                         2; /* EML capa */
1979
1980                 /*
1981                  * The capability elements were already considered above;
1982                  * note this over-estimates a bit because there's no
1983                  * STA profile for the assoc link.
1984                  */
1985                 size += (n_links - 1) *
1986                         (1 + 1 + /* subelement ID/length */
1987                          2 + /* STA control */
1988                          1 + ETH_ALEN + 2 /* STA Info field */);
1989         }
1990
1991         link = sdata_dereference(sdata->link[assoc_data->assoc_link_id], sdata);
1992         if (WARN_ON(!link))
1993                 return -EINVAL;
1994
1995         if (WARN_ON(!assoc_data->link[assoc_data->assoc_link_id].bss))
1996                 return -EINVAL;
1997
1998         skb = alloc_skb(size, GFP_KERNEL);
1999         if (!skb)
2000                 return -ENOMEM;
2001
2002         skb_reserve(skb, local->hw.extra_tx_headroom);
2003
2004         if (ifmgd->flags & IEEE80211_STA_ENABLE_RRM)
2005                 capab |= WLAN_CAPABILITY_RADIO_MEASURE;
2006
2007         /* Set MBSSID support for HE AP if needed */
2008         if (ieee80211_hw_check(&local->hw, SUPPORTS_ONLY_HE_MULTI_BSSID) &&
2009             link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_HE &&
2010             ext_capa && ext_capa->datalen >= 3)
2011                 ext_capa->data[2] |= WLAN_EXT_CAPA3_MULTI_BSSID_SUPPORT;
2012
2013         mgmt = skb_put_zero(skb, 24);
2014         memcpy(mgmt->da, sdata->vif.cfg.ap_addr, ETH_ALEN);
2015         memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
2016         memcpy(mgmt->bssid, sdata->vif.cfg.ap_addr, ETH_ALEN);
2017
2018         listen_int = cpu_to_le16(assoc_data->s1g ?
2019                         ieee80211_encode_usf(local->hw.conf.listen_interval) :
2020                         local->hw.conf.listen_interval);
2021         if (!is_zero_ether_addr(assoc_data->prev_ap_addr)) {
2022                 skb_put(skb, 10);
2023                 mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
2024                                                   IEEE80211_STYPE_REASSOC_REQ);
2025                 capab_pos = &mgmt->u.reassoc_req.capab_info;
2026                 mgmt->u.reassoc_req.listen_interval = listen_int;
2027                 memcpy(mgmt->u.reassoc_req.current_ap,
2028                        assoc_data->prev_ap_addr, ETH_ALEN);
2029                 info.subtype = IEEE80211_STYPE_REASSOC_REQ;
2030         } else {
2031                 skb_put(skb, 4);
2032                 mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
2033                                                   IEEE80211_STYPE_ASSOC_REQ);
2034                 capab_pos = &mgmt->u.assoc_req.capab_info;
2035                 mgmt->u.assoc_req.listen_interval = listen_int;
2036                 info.subtype = IEEE80211_STYPE_ASSOC_REQ;
2037         }
2038
2039         /* SSID */
2040         pos = skb_put(skb, 2 + assoc_data->ssid_len);
2041         ie_start = pos;
2042         *pos++ = WLAN_EID_SSID;
2043         *pos++ = assoc_data->ssid_len;
2044         memcpy(pos, assoc_data->ssid, assoc_data->ssid_len);
2045
2046         /*
2047          * This bit is technically reserved, so it shouldn't matter for either
2048          * the AP or us, but it also means we shouldn't set it. However, we've
2049          * always set it in the past, and apparently some EHT APs check that
2050          * we don't set it. To avoid interoperability issues with old APs that
2051          * for some reason check it and want it to be set, set the bit for all
2052          * pre-EHT connections as we used to do.
2053          */
2054         if (link->u.mgd.conn.mode < IEEE80211_CONN_MODE_EHT)
2055                 capab |= WLAN_CAPABILITY_ESS;
2056
2057         /* add the elements for the assoc (main) link */
2058         link_capab = capab;
2059         offset = ieee80211_add_link_elems(sdata, skb, &link_capab,
2060                                           ext_capa,
2061                                           assoc_data->ie,
2062                                           assoc_data->ie_len,
2063                                           assoc_data->assoc_link_id, link,
2064                                           present_elems, assoc_data);
2065         put_unaligned_le16(link_capab, capab_pos);
2066
2067         /* if present, add any custom non-vendor IEs */
2068         if (assoc_data->ie_len) {
2069                 noffset = ieee80211_ie_split_vendor(assoc_data->ie,
2070                                                     assoc_data->ie_len,
2071                                                     offset);
2072                 skb_put_data(skb, assoc_data->ie + offset, noffset - offset);
2073                 offset = noffset;
2074         }
2075
2076         if (assoc_data->wmm) {
2077                 if (assoc_data->uapsd) {
2078                         qos_info = ifmgd->uapsd_queues;
2079                         qos_info |= (ifmgd->uapsd_max_sp_len <<
2080                                      IEEE80211_WMM_IE_STA_QOSINFO_SP_SHIFT);
2081                 } else {
2082                         qos_info = 0;
2083                 }
2084
2085                 pos = ieee80211_add_wmm_info_ie(skb_put(skb, 9), qos_info);
2086         }
2087
2088         /* add any remaining custom (i.e. vendor specific here) IEs */
2089         if (assoc_data->ie_len) {
2090                 noffset = assoc_data->ie_len;
2091                 skb_put_data(skb, assoc_data->ie + offset, noffset - offset);
2092         }
2093
2094         if (assoc_data->fils_kek_len) {
2095                 ret = fils_encrypt_assoc_req(skb, assoc_data);
2096                 if (ret < 0) {
2097                         dev_kfree_skb(skb);
2098                         return ret;
2099                 }
2100         }
2101
2102         pos = skb_tail_pointer(skb);
2103         kfree(ifmgd->assoc_req_ies);
2104         ifmgd->assoc_req_ies = kmemdup(ie_start, pos - ie_start, GFP_ATOMIC);
2105         if (!ifmgd->assoc_req_ies) {
2106                 dev_kfree_skb(skb);
2107                 return -ENOMEM;
2108         }
2109
2110         ifmgd->assoc_req_ies_len = pos - ie_start;
2111
2112         info.link_id = assoc_data->assoc_link_id;
2113         drv_mgd_prepare_tx(local, sdata, &info);
2114
2115         IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT;
2116         if (ieee80211_hw_check(&local->hw, REPORTS_TX_ACK_STATUS))
2117                 IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS |
2118                                                 IEEE80211_TX_INTFL_MLME_CONN_TX;
2119         ieee80211_tx_skb(sdata, skb);
2120
2121         return 0;
2122 }
2123
2124 void ieee80211_send_pspoll(struct ieee80211_local *local,
2125                            struct ieee80211_sub_if_data *sdata)
2126 {
2127         struct ieee80211_pspoll *pspoll;
2128         struct sk_buff *skb;
2129
2130         skb = ieee80211_pspoll_get(&local->hw, &sdata->vif);
2131         if (!skb)
2132                 return;
2133
2134         pspoll = (struct ieee80211_pspoll *) skb->data;
2135         pspoll->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
2136
2137         IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT;
2138         ieee80211_tx_skb(sdata, skb);
2139 }
2140
2141 void ieee80211_send_nullfunc(struct ieee80211_local *local,
2142                              struct ieee80211_sub_if_data *sdata,
2143                              bool powersave)
2144 {
2145         struct sk_buff *skb;
2146         struct ieee80211_hdr_3addr *nullfunc;
2147         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
2148
2149         skb = ieee80211_nullfunc_get(&local->hw, &sdata->vif, -1,
2150                                      !ieee80211_hw_check(&local->hw,
2151                                                          DOESNT_SUPPORT_QOS_NDP));
2152         if (!skb)
2153                 return;
2154
2155         nullfunc = (struct ieee80211_hdr_3addr *) skb->data;
2156         if (powersave)
2157                 nullfunc->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
2158
2159         IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
2160                                         IEEE80211_TX_INTFL_OFFCHAN_TX_OK;
2161
2162         if (ieee80211_hw_check(&local->hw, REPORTS_TX_ACK_STATUS))
2163                 IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
2164
2165         if (ifmgd->flags & IEEE80211_STA_CONNECTION_POLL)
2166                 IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_CTL_USE_MINRATE;
2167
2168         ieee80211_tx_skb(sdata, skb);
2169 }
2170
2171 void ieee80211_send_4addr_nullfunc(struct ieee80211_local *local,
2172                                    struct ieee80211_sub_if_data *sdata)
2173 {
2174         struct sk_buff *skb;
2175         struct ieee80211_hdr *nullfunc;
2176         __le16 fc;
2177
2178         if (WARN_ON(sdata->vif.type != NL80211_IFTYPE_STATION))
2179                 return;
2180
2181         skb = dev_alloc_skb(local->hw.extra_tx_headroom + 30);
2182         if (!skb)
2183                 return;
2184
2185         skb_reserve(skb, local->hw.extra_tx_headroom);
2186
2187         nullfunc = skb_put_zero(skb, 30);
2188         fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_NULLFUNC |
2189                          IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS);
2190         nullfunc->frame_control = fc;
2191         memcpy(nullfunc->addr1, sdata->deflink.u.mgd.bssid, ETH_ALEN);
2192         memcpy(nullfunc->addr2, sdata->vif.addr, ETH_ALEN);
2193         memcpy(nullfunc->addr3, sdata->deflink.u.mgd.bssid, ETH_ALEN);
2194         memcpy(nullfunc->addr4, sdata->vif.addr, ETH_ALEN);
2195
2196         IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT;
2197         IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_CTL_USE_MINRATE;
2198         ieee80211_tx_skb(sdata, skb);
2199 }
2200
2201 /* spectrum management related things */
2202 static void ieee80211_csa_switch_work(struct wiphy *wiphy,
2203                                       struct wiphy_work *work)
2204 {
2205         struct ieee80211_link_data *link =
2206                 container_of(work, struct ieee80211_link_data,
2207                              u.mgd.csa.switch_work.work);
2208         struct ieee80211_sub_if_data *sdata = link->sdata;
2209         struct ieee80211_local *local = sdata->local;
2210         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
2211         int ret;
2212
2213         if (!ieee80211_sdata_running(sdata))
2214                 return;
2215
2216         lockdep_assert_wiphy(local->hw.wiphy);
2217
2218         if (!ifmgd->associated)
2219                 return;
2220
2221         if (!link->conf->csa_active)
2222                 return;
2223
2224         /*
2225          * If the link isn't active (now), we cannot wait for beacons, won't
2226          * have a reserved chanctx, etc. Just switch over the chandef and
2227          * update cfg80211 directly.
2228          */
2229         if (!ieee80211_vif_link_active(&sdata->vif, link->link_id)) {
2230                 link->conf->chanreq = link->csa.chanreq;
2231                 cfg80211_ch_switch_notify(sdata->dev, &link->csa.chanreq.oper,
2232                                           link->link_id);
2233                 return;
2234         }
2235
2236         /*
2237          * using reservation isn't immediate as it may be deferred until later
2238          * with multi-vif. once reservation is complete it will re-schedule the
2239          * work with no reserved_chanctx so verify chandef to check if it
2240          * completed successfully
2241          */
2242
2243         if (link->reserved_chanctx) {
2244                 /*
2245                  * with multi-vif csa driver may call ieee80211_csa_finish()
2246                  * many times while waiting for other interfaces to use their
2247                  * reservations
2248                  */
2249                 if (link->reserved_ready)
2250                         return;
2251
2252                 ret = ieee80211_link_use_reserved_context(link);
2253                 if (ret) {
2254                         link_info(link,
2255                                   "failed to use reserved channel context, disconnecting (err=%d)\n",
2256                                   ret);
2257                         wiphy_work_queue(sdata->local->hw.wiphy,
2258                                          &ifmgd->csa_connection_drop_work);
2259                 }
2260                 return;
2261         }
2262
2263         if (!ieee80211_chanreq_identical(&link->conf->chanreq,
2264                                          &link->csa.chanreq)) {
2265                 link_info(link,
2266                           "failed to finalize channel switch, disconnecting\n");
2267                 wiphy_work_queue(sdata->local->hw.wiphy,
2268                                  &ifmgd->csa_connection_drop_work);
2269                 return;
2270         }
2271
2272         link->u.mgd.csa.waiting_bcn = true;
2273
2274         /* apply new TPE restrictions immediately on the new channel */
2275         if (link->u.mgd.csa.ap_chandef.chan->band == NL80211_BAND_6GHZ &&
2276             link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_HE) {
2277                 ieee80211_rearrange_tpe(&link->u.mgd.csa.tpe,
2278                                         &link->u.mgd.csa.ap_chandef,
2279                                         &link->conf->chanreq.oper);
2280                 if (memcmp(&link->conf->tpe, &link->u.mgd.csa.tpe,
2281                            sizeof(link->u.mgd.csa.tpe))) {
2282                         link->conf->tpe = link->u.mgd.csa.tpe;
2283                         ieee80211_link_info_change_notify(sdata, link,
2284                                                           BSS_CHANGED_TPE);
2285                 }
2286         }
2287
2288         ieee80211_sta_reset_beacon_monitor(sdata);
2289         ieee80211_sta_reset_conn_monitor(sdata);
2290 }
2291
2292 static void ieee80211_chswitch_post_beacon(struct ieee80211_link_data *link)
2293 {
2294         struct ieee80211_sub_if_data *sdata = link->sdata;
2295         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
2296         int ret;
2297
2298         lockdep_assert_wiphy(sdata->local->hw.wiphy);
2299
2300         WARN_ON(!link->conf->csa_active);
2301
2302         ieee80211_vif_unblock_queues_csa(sdata);
2303
2304         link->conf->csa_active = false;
2305         link->u.mgd.csa.blocked_tx = false;
2306         link->u.mgd.csa.waiting_bcn = false;
2307
2308         ret = drv_post_channel_switch(link);
2309         if (ret) {
2310                 link_info(link,
2311                           "driver post channel switch failed, disconnecting\n");
2312                 wiphy_work_queue(sdata->local->hw.wiphy,
2313                                  &ifmgd->csa_connection_drop_work);
2314                 return;
2315         }
2316
2317         cfg80211_ch_switch_notify(sdata->dev, &link->conf->chanreq.oper,
2318                                   link->link_id);
2319 }
2320
2321 void ieee80211_chswitch_done(struct ieee80211_vif *vif, bool success,
2322                              unsigned int link_id)
2323 {
2324         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
2325
2326         trace_api_chswitch_done(sdata, success, link_id);
2327
2328         rcu_read_lock();
2329
2330         if (!success) {
2331                 sdata_info(sdata,
2332                            "driver channel switch failed (link %d), disconnecting\n",
2333                            link_id);
2334                 wiphy_work_queue(sdata->local->hw.wiphy,
2335                                  &sdata->u.mgd.csa_connection_drop_work);
2336         } else {
2337                 struct ieee80211_link_data *link =
2338                         rcu_dereference(sdata->link[link_id]);
2339
2340                 if (WARN_ON(!link)) {
2341                         rcu_read_unlock();
2342                         return;
2343                 }
2344
2345                 wiphy_delayed_work_queue(sdata->local->hw.wiphy,
2346                                          &link->u.mgd.csa.switch_work, 0);
2347         }
2348
2349         rcu_read_unlock();
2350 }
2351 EXPORT_SYMBOL(ieee80211_chswitch_done);
2352
2353 static void
2354 ieee80211_sta_abort_chanswitch(struct ieee80211_link_data *link)
2355 {
2356         struct ieee80211_sub_if_data *sdata = link->sdata;
2357         struct ieee80211_local *local = sdata->local;
2358
2359         lockdep_assert_wiphy(local->hw.wiphy);
2360
2361         if (!local->ops->abort_channel_switch)
2362                 return;
2363
2364         ieee80211_link_unreserve_chanctx(link);
2365
2366         ieee80211_vif_unblock_queues_csa(sdata);
2367
2368         link->conf->csa_active = false;
2369         link->u.mgd.csa.blocked_tx = false;
2370
2371         drv_abort_channel_switch(link);
2372 }
2373
2374 struct sta_csa_rnr_iter_data {
2375         struct ieee80211_link_data *link;
2376         struct ieee80211_channel *chan;
2377         u8 mld_id;
2378 };
2379
2380 static enum cfg80211_rnr_iter_ret
2381 ieee80211_sta_csa_rnr_iter(void *_data, u8 type,
2382                            const struct ieee80211_neighbor_ap_info *info,
2383                            const u8 *tbtt_info, u8 tbtt_info_len)
2384 {
2385         struct sta_csa_rnr_iter_data *data = _data;
2386         struct ieee80211_link_data *link = data->link;
2387         struct ieee80211_sub_if_data *sdata = link->sdata;
2388         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
2389         const struct ieee80211_tbtt_info_ge_11 *ti;
2390         enum nl80211_band band;
2391         unsigned int center_freq;
2392         int link_id;
2393
2394         if (type != IEEE80211_TBTT_INFO_TYPE_TBTT)
2395                 return RNR_ITER_CONTINUE;
2396
2397         if (tbtt_info_len < sizeof(*ti))
2398                 return RNR_ITER_CONTINUE;
2399
2400         ti = (const void *)tbtt_info;
2401
2402         if (ti->mld_params.mld_id != data->mld_id)
2403                 return RNR_ITER_CONTINUE;
2404
2405         link_id = le16_get_bits(ti->mld_params.params,
2406                                 IEEE80211_RNR_MLD_PARAMS_LINK_ID);
2407         if (link_id != data->link->link_id)
2408                 return RNR_ITER_CONTINUE;
2409
2410         /* we found the entry for our link! */
2411
2412         /* this AP is confused, it had this right before ... just disconnect */
2413         if (!ieee80211_operating_class_to_band(info->op_class, &band)) {
2414                 link_info(link,
2415                           "AP now has invalid operating class in RNR, disconnect\n");
2416                 wiphy_work_queue(sdata->local->hw.wiphy,
2417                                  &ifmgd->csa_connection_drop_work);
2418                 return RNR_ITER_BREAK;
2419         }
2420
2421         center_freq = ieee80211_channel_to_frequency(info->channel, band);
2422         data->chan = ieee80211_get_channel(sdata->local->hw.wiphy, center_freq);
2423
2424         return RNR_ITER_BREAK;
2425 }
2426
2427 static void
2428 ieee80211_sta_other_link_csa_disappeared(struct ieee80211_link_data *link,
2429                                          struct ieee802_11_elems *elems)
2430 {
2431         struct ieee80211_sub_if_data *sdata = link->sdata;
2432         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
2433         struct sta_csa_rnr_iter_data data = {
2434                 .link = link,
2435         };
2436
2437         /*
2438          * If we get here, we see a beacon from another link without
2439          * CSA still being reported for it, so now we have to check
2440          * if the CSA was aborted or completed. This may not even be
2441          * perfectly possible if the CSA was only done for changing
2442          * the puncturing, but in that case if the link in inactive
2443          * we don't really care, and if it's an active link (or when
2444          * it's activated later) we'll get a beacon and adjust.
2445          */
2446
2447         if (WARN_ON(!elems->ml_basic))
2448                 return;
2449
2450         data.mld_id = ieee80211_mle_get_mld_id((const void *)elems->ml_basic);
2451
2452         /*
2453          * So in order to do this, iterate the RNR element(s) and see
2454          * what channel is reported now.
2455          */
2456         cfg80211_iter_rnr(elems->ie_start, elems->total_len,
2457                           ieee80211_sta_csa_rnr_iter, &data);
2458
2459         if (!data.chan) {
2460                 link_info(link,
2461                           "couldn't find (valid) channel in RNR for CSA, disconnect\n");
2462                 wiphy_work_queue(sdata->local->hw.wiphy,
2463                                  &ifmgd->csa_connection_drop_work);
2464                 return;
2465         }
2466
2467         /*
2468          * If it doesn't match the CSA, then assume it aborted. This
2469          * may erroneously detect that it was _not_ aborted when it
2470          * was in fact aborted, but only changed the bandwidth or the
2471          * puncturing configuration, but we don't have enough data to
2472          * detect that.
2473          */
2474         if (data.chan != link->csa.chanreq.oper.chan)
2475                 ieee80211_sta_abort_chanswitch(link);
2476 }
2477
2478 enum ieee80211_csa_source {
2479         IEEE80211_CSA_SOURCE_BEACON,
2480         IEEE80211_CSA_SOURCE_OTHER_LINK,
2481         IEEE80211_CSA_SOURCE_PROT_ACTION,
2482         IEEE80211_CSA_SOURCE_UNPROT_ACTION,
2483 };
2484
2485 static void
2486 ieee80211_sta_process_chanswitch(struct ieee80211_link_data *link,
2487                                  u64 timestamp, u32 device_timestamp,
2488                                  struct ieee802_11_elems *full_elems,
2489                                  struct ieee802_11_elems *csa_elems,
2490                                  enum ieee80211_csa_source source)
2491 {
2492         struct ieee80211_sub_if_data *sdata = link->sdata;
2493         struct ieee80211_local *local = sdata->local;
2494         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
2495         struct ieee80211_chanctx *chanctx = NULL;
2496         struct ieee80211_chanctx_conf *conf;
2497         struct ieee80211_csa_ie csa_ie = {};
2498         struct ieee80211_channel_switch ch_switch = {
2499                 .link_id = link->link_id,
2500                 .timestamp = timestamp,
2501                 .device_timestamp = device_timestamp,
2502         };
2503         unsigned long now;
2504         int res;
2505
2506         lockdep_assert_wiphy(local->hw.wiphy);
2507
2508         if (csa_elems) {
2509                 struct cfg80211_bss *cbss = link->conf->bss;
2510                 enum nl80211_band current_band;
2511                 struct ieee80211_bss *bss;
2512
2513                 if (WARN_ON(!cbss))
2514                         return;
2515
2516                 current_band = cbss->channel->band;
2517                 bss = (void *)cbss->priv;
2518
2519                 res = ieee80211_parse_ch_switch_ie(sdata, csa_elems,
2520                                                    current_band,
2521                                                    bss->vht_cap_info,
2522                                                    &link->u.mgd.conn,
2523                                                    link->u.mgd.bssid,
2524                                                    source == IEEE80211_CSA_SOURCE_UNPROT_ACTION,
2525                                                    &csa_ie);
2526                 if (res == 0) {
2527                         ch_switch.block_tx = csa_ie.mode;
2528                         ch_switch.chandef = csa_ie.chanreq.oper;
2529                         ch_switch.count = csa_ie.count;
2530                         ch_switch.delay = csa_ie.max_switch_time;
2531                 }
2532
2533                 link->u.mgd.csa.tpe = csa_elems->csa_tpe;
2534         } else {
2535                 /*
2536                  * If there was no per-STA profile for this link, we
2537                  * get called with csa_elems == NULL. This of course means
2538                  * there are no CSA elements, so set res=1 indicating
2539                  * no more CSA.
2540                  */
2541                 res = 1;
2542         }
2543
2544         if (res < 0) {
2545                 /* ignore this case, not a protected frame */
2546                 if (source == IEEE80211_CSA_SOURCE_UNPROT_ACTION)
2547                         return;
2548                 goto drop_connection;
2549         }
2550
2551         if (link->conf->csa_active) {
2552                 switch (source) {
2553                 case IEEE80211_CSA_SOURCE_PROT_ACTION:
2554                 case IEEE80211_CSA_SOURCE_UNPROT_ACTION:
2555                         /* already processing - disregard action frames */
2556                         return;
2557                 case IEEE80211_CSA_SOURCE_BEACON:
2558                         if (link->u.mgd.csa.waiting_bcn) {
2559                                 ieee80211_chswitch_post_beacon(link);
2560                                 /*
2561                                  * If the CSA is still present after the switch
2562                                  * we need to consider it as a new CSA (possibly
2563                                  * to self). This happens by not returning here
2564                                  * so we'll get to the check below.
2565                                  */
2566                         } else if (res) {
2567                                 ieee80211_sta_abort_chanswitch(link);
2568                                 return;
2569                         } else {
2570                                 drv_channel_switch_rx_beacon(sdata, &ch_switch);
2571                                 return;
2572                         }
2573                         break;
2574                 case IEEE80211_CSA_SOURCE_OTHER_LINK:
2575                         /* active link: we want to see the beacon to continue */
2576                         if (ieee80211_vif_link_active(&sdata->vif,
2577                                                       link->link_id))
2578                                 return;
2579
2580                         /* switch work ran, so just complete the process */
2581                         if (link->u.mgd.csa.waiting_bcn) {
2582                                 ieee80211_chswitch_post_beacon(link);
2583                                 /*
2584                                  * If the CSA is still present after the switch
2585                                  * we need to consider it as a new CSA (possibly
2586                                  * to self). This happens by not returning here
2587                                  * so we'll get to the check below.
2588                                  */
2589                                 break;
2590                         }
2591
2592                         /* link still has CSA but we already know, do nothing */
2593                         if (!res)
2594                                 return;
2595
2596                         /* check in the RNR if the CSA aborted */
2597                         ieee80211_sta_other_link_csa_disappeared(link,
2598                                                                  full_elems);
2599                         return;
2600                 }
2601         }
2602
2603         /* no active CSA nor a new one */
2604         if (res) {
2605                 /*
2606                  * However, we may have stopped queues when receiving a public
2607                  * action frame that couldn't be protected, if it had the quiet
2608                  * bit set. This is a trade-off, we want to be quiet as soon as
2609                  * possible, but also don't trust the public action frame much,
2610                  * as it can't be protected.
2611                  */
2612                 if (unlikely(link->u.mgd.csa.blocked_tx)) {
2613                         link->u.mgd.csa.blocked_tx = false;
2614                         ieee80211_vif_unblock_queues_csa(sdata);
2615                 }
2616                 return;
2617         }
2618
2619         /*
2620          * We don't really trust public action frames, but block queues (go to
2621          * quiet mode) for them anyway, we should get a beacon soon to either
2622          * know what the CSA really is, or figure out the public action frame
2623          * was actually an attack.
2624          */
2625         if (source == IEEE80211_CSA_SOURCE_UNPROT_ACTION) {
2626                 if (csa_ie.mode) {
2627                         link->u.mgd.csa.blocked_tx = true;
2628                         ieee80211_vif_block_queues_csa(sdata);
2629                 }
2630                 return;
2631         }
2632
2633         if (link->conf->chanreq.oper.chan->band !=
2634             csa_ie.chanreq.oper.chan->band) {
2635                 link_info(link,
2636                           "AP %pM switches to different band (%d MHz, width:%d, CF1/2: %d/%d MHz), disconnecting\n",
2637                           link->u.mgd.bssid,
2638                           csa_ie.chanreq.oper.chan->center_freq,
2639                           csa_ie.chanreq.oper.width,
2640                           csa_ie.chanreq.oper.center_freq1,
2641                           csa_ie.chanreq.oper.center_freq2);
2642                 goto drop_connection;
2643         }
2644
2645         if (!cfg80211_chandef_usable(local->hw.wiphy, &csa_ie.chanreq.oper,
2646                                      IEEE80211_CHAN_DISABLED)) {
2647                 link_info(link,
2648                           "AP %pM switches to unsupported channel (%d.%03d MHz, width:%d, CF1/2: %d.%03d/%d MHz), disconnecting\n",
2649                           link->u.mgd.bssid,
2650                           csa_ie.chanreq.oper.chan->center_freq,
2651                           csa_ie.chanreq.oper.chan->freq_offset,
2652                           csa_ie.chanreq.oper.width,
2653                           csa_ie.chanreq.oper.center_freq1,
2654                           csa_ie.chanreq.oper.freq1_offset,
2655                           csa_ie.chanreq.oper.center_freq2);
2656                 goto drop_connection;
2657         }
2658
2659         if (cfg80211_chandef_identical(&csa_ie.chanreq.oper,
2660                                        &link->conf->chanreq.oper) &&
2661             (!csa_ie.mode || source != IEEE80211_CSA_SOURCE_BEACON)) {
2662                 if (link->u.mgd.csa.ignored_same_chan)
2663                         return;
2664                 link_info(link,
2665                           "AP %pM tries to chanswitch to same channel, ignore\n",
2666                           link->u.mgd.bssid);
2667                 link->u.mgd.csa.ignored_same_chan = true;
2668                 return;
2669         }
2670
2671         /*
2672          * Drop all TDLS peers on the affected link - either we disconnect or
2673          * move to a different channel from this point on. There's no telling
2674          * what our peer will do.
2675          * The TDLS WIDER_BW scenario is also problematic, as peers might now
2676          * have an incompatible wider chandef.
2677          */
2678         ieee80211_teardown_tdls_peers(link);
2679
2680         conf = rcu_dereference_protected(link->conf->chanctx_conf,
2681                                          lockdep_is_held(&local->hw.wiphy->mtx));
2682         if (ieee80211_vif_link_active(&sdata->vif, link->link_id) && !conf) {
2683                 link_info(link,
2684                           "no channel context assigned to vif?, disconnecting\n");
2685                 goto drop_connection;
2686         }
2687
2688         if (conf)
2689                 chanctx = container_of(conf, struct ieee80211_chanctx, conf);
2690
2691         if (!ieee80211_hw_check(&local->hw, CHANCTX_STA_CSA)) {
2692                 link_info(link,
2693                           "driver doesn't support chan-switch with channel contexts\n");
2694                 goto drop_connection;
2695         }
2696
2697         if (drv_pre_channel_switch(sdata, &ch_switch)) {
2698                 link_info(link,
2699                           "preparing for channel switch failed, disconnecting\n");
2700                 goto drop_connection;
2701         }
2702
2703         link->u.mgd.csa.ap_chandef = csa_ie.chanreq.ap;
2704
2705         link->csa.chanreq.oper = csa_ie.chanreq.oper;
2706         ieee80211_set_chanreq_ap(sdata, &link->csa.chanreq, &link->u.mgd.conn,
2707                                  &csa_ie.chanreq.ap);
2708
2709         if (chanctx) {
2710                 res = ieee80211_link_reserve_chanctx(link, &link->csa.chanreq,
2711                                                      chanctx->mode, false);
2712                 if (res) {
2713                         link_info(link,
2714                                   "failed to reserve channel context for channel switch, disconnecting (err=%d)\n",
2715                                   res);
2716                         goto drop_connection;
2717                 }
2718         }
2719
2720         link->conf->csa_active = true;
2721         link->u.mgd.csa.ignored_same_chan = false;
2722         link->u.mgd.beacon_crc_valid = false;
2723         link->u.mgd.csa.blocked_tx = csa_ie.mode;
2724
2725         if (csa_ie.mode)
2726                 ieee80211_vif_block_queues_csa(sdata);
2727
2728         cfg80211_ch_switch_started_notify(sdata->dev, &csa_ie.chanreq.oper,
2729                                           link->link_id, csa_ie.count,
2730                                           csa_ie.mode);
2731
2732         /* we may have to handle timeout for deactivated link in software */
2733         now = jiffies;
2734         link->u.mgd.csa.time = now +
2735                                TU_TO_JIFFIES((max_t(int, csa_ie.count, 1) - 1) *
2736                                              link->conf->beacon_int);
2737
2738         if (ieee80211_vif_link_active(&sdata->vif, link->link_id) &&
2739             local->ops->channel_switch) {
2740                 /*
2741                  * Use driver's channel switch callback, the driver will
2742                  * later call ieee80211_chswitch_done(). It may deactivate
2743                  * the link as well, we handle that elsewhere and queue
2744                  * the csa.switch_work for the calculated time then.
2745                  */
2746                 drv_channel_switch(local, sdata, &ch_switch);
2747                 return;
2748         }
2749
2750         /* channel switch handled in software */
2751         wiphy_delayed_work_queue(local->hw.wiphy,
2752                                  &link->u.mgd.csa.switch_work,
2753                                  link->u.mgd.csa.time - now);
2754         return;
2755  drop_connection:
2756         /*
2757          * This is just so that the disconnect flow will know that
2758          * we were trying to switch channel and failed. In case the
2759          * mode is 1 (we are not allowed to Tx), we will know not to
2760          * send a deauthentication frame. Those two fields will be
2761          * reset when the disconnection worker runs.
2762          */
2763         link->conf->csa_active = true;
2764         link->u.mgd.csa.blocked_tx = csa_ie.mode;
2765
2766         wiphy_work_queue(sdata->local->hw.wiphy,
2767                          &ifmgd->csa_connection_drop_work);
2768 }
2769
2770 struct sta_bss_param_ch_cnt_data {
2771         struct ieee80211_sub_if_data *sdata;
2772         u8 reporting_link_id;
2773         u8 mld_id;
2774 };
2775
2776 static enum cfg80211_rnr_iter_ret
2777 ieee80211_sta_bss_param_ch_cnt_iter(void *_data, u8 type,
2778                                     const struct ieee80211_neighbor_ap_info *info,
2779                                     const u8 *tbtt_info, u8 tbtt_info_len)
2780 {
2781         struct sta_bss_param_ch_cnt_data *data = _data;
2782         struct ieee80211_sub_if_data *sdata = data->sdata;
2783         const struct ieee80211_tbtt_info_ge_11 *ti;
2784         u8 bss_param_ch_cnt;
2785         int link_id;
2786
2787         if (type != IEEE80211_TBTT_INFO_TYPE_TBTT)
2788                 return RNR_ITER_CONTINUE;
2789
2790         if (tbtt_info_len < sizeof(*ti))
2791                 return RNR_ITER_CONTINUE;
2792
2793         ti = (const void *)tbtt_info;
2794
2795         if (ti->mld_params.mld_id != data->mld_id)
2796                 return RNR_ITER_CONTINUE;
2797
2798         link_id = le16_get_bits(ti->mld_params.params,
2799                                 IEEE80211_RNR_MLD_PARAMS_LINK_ID);
2800         bss_param_ch_cnt =
2801                 le16_get_bits(ti->mld_params.params,
2802                               IEEE80211_RNR_MLD_PARAMS_BSS_CHANGE_COUNT);
2803
2804         if (bss_param_ch_cnt != 255 &&
2805             link_id < ARRAY_SIZE(sdata->link)) {
2806                 struct ieee80211_link_data *link =
2807                         sdata_dereference(sdata->link[link_id], sdata);
2808
2809                 if (link && link->conf->bss_param_ch_cnt != bss_param_ch_cnt) {
2810                         link->conf->bss_param_ch_cnt = bss_param_ch_cnt;
2811                         link->conf->bss_param_ch_cnt_link_id =
2812                                 data->reporting_link_id;
2813                 }
2814         }
2815
2816         return RNR_ITER_CONTINUE;
2817 }
2818
2819 static void
2820 ieee80211_mgd_update_bss_param_ch_cnt(struct ieee80211_sub_if_data *sdata,
2821                                       struct ieee80211_bss_conf *bss_conf,
2822                                       struct ieee802_11_elems *elems)
2823 {
2824         struct sta_bss_param_ch_cnt_data data = {
2825                 .reporting_link_id = bss_conf->link_id,
2826                 .sdata = sdata,
2827         };
2828         int bss_param_ch_cnt;
2829
2830         if (!elems->ml_basic)
2831                 return;
2832
2833         data.mld_id = ieee80211_mle_get_mld_id((const void *)elems->ml_basic);
2834
2835         cfg80211_iter_rnr(elems->ie_start, elems->total_len,
2836                           ieee80211_sta_bss_param_ch_cnt_iter, &data);
2837
2838         bss_param_ch_cnt =
2839                 ieee80211_mle_get_bss_param_ch_cnt((const void *)elems->ml_basic);
2840
2841         /*
2842          * Update bss_param_ch_cnt_link_id even if bss_param_ch_cnt
2843          * didn't change to indicate that we got a beacon on our own
2844          * link.
2845          */
2846         if (bss_param_ch_cnt >= 0 && bss_param_ch_cnt != 255) {
2847                 bss_conf->bss_param_ch_cnt = bss_param_ch_cnt;
2848                 bss_conf->bss_param_ch_cnt_link_id =
2849                         bss_conf->link_id;
2850         }
2851 }
2852
2853 static bool
2854 ieee80211_find_80211h_pwr_constr(struct ieee80211_channel *channel,
2855                                  const u8 *country_ie, u8 country_ie_len,
2856                                  const u8 *pwr_constr_elem,
2857                                  int *chan_pwr, int *pwr_reduction)
2858 {
2859         struct ieee80211_country_ie_triplet *triplet;
2860         int chan = ieee80211_frequency_to_channel(channel->center_freq);
2861         int i, chan_increment;
2862         bool have_chan_pwr = false;
2863
2864         /* Invalid IE */
2865         if (country_ie_len % 2 || country_ie_len < IEEE80211_COUNTRY_IE_MIN_LEN)
2866                 return false;
2867
2868         triplet = (void *)(country_ie + 3);
2869         country_ie_len -= 3;
2870
2871         switch (channel->band) {
2872         default:
2873                 WARN_ON_ONCE(1);
2874                 fallthrough;
2875         case NL80211_BAND_2GHZ:
2876         case NL80211_BAND_60GHZ:
2877         case NL80211_BAND_LC:
2878                 chan_increment = 1;
2879                 break;
2880         case NL80211_BAND_5GHZ:
2881                 chan_increment = 4;
2882                 break;
2883         case NL80211_BAND_6GHZ:
2884                 /*
2885                  * In the 6 GHz band, the "maximum transmit power level"
2886                  * field in the triplets is reserved, and thus will be
2887                  * zero and we shouldn't use it to control TX power.
2888                  * The actual TX power will be given in the transmit
2889                  * power envelope element instead.
2890                  */
2891                 return false;
2892         }
2893
2894         /* find channel */
2895         while (country_ie_len >= 3) {
2896                 u8 first_channel = triplet->chans.first_channel;
2897
2898                 if (first_channel >= IEEE80211_COUNTRY_EXTENSION_ID)
2899                         goto next;
2900
2901                 for (i = 0; i < triplet->chans.num_channels; i++) {
2902                         if (first_channel + i * chan_increment == chan) {
2903                                 have_chan_pwr = true;
2904                                 *chan_pwr = triplet->chans.max_power;
2905                                 break;
2906                         }
2907                 }
2908                 if (have_chan_pwr)
2909                         break;
2910
2911  next:
2912                 triplet++;
2913                 country_ie_len -= 3;
2914         }
2915
2916         if (have_chan_pwr && pwr_constr_elem)
2917                 *pwr_reduction = *pwr_constr_elem;
2918         else
2919                 *pwr_reduction = 0;
2920
2921         return have_chan_pwr;
2922 }
2923
2924 static void ieee80211_find_cisco_dtpc(struct ieee80211_channel *channel,
2925                                       const u8 *cisco_dtpc_ie,
2926                                       int *pwr_level)
2927 {
2928         /* From practical testing, the first data byte of the DTPC element
2929          * seems to contain the requested dBm level, and the CLI on Cisco
2930          * APs clearly state the range is -127 to 127 dBm, which indicates
2931          * a signed byte, although it seemingly never actually goes negative.
2932          * The other byte seems to always be zero.
2933          */
2934         *pwr_level = (__s8)cisco_dtpc_ie[4];
2935 }
2936
2937 static u64 ieee80211_handle_pwr_constr(struct ieee80211_link_data *link,
2938                                        struct ieee80211_channel *channel,
2939                                        struct ieee80211_mgmt *mgmt,
2940                                        const u8 *country_ie, u8 country_ie_len,
2941                                        const u8 *pwr_constr_ie,
2942                                        const u8 *cisco_dtpc_ie)
2943 {
2944         struct ieee80211_sub_if_data *sdata = link->sdata;
2945         bool has_80211h_pwr = false, has_cisco_pwr = false;
2946         int chan_pwr = 0, pwr_reduction_80211h = 0;
2947         int pwr_level_cisco, pwr_level_80211h;
2948         int new_ap_level;
2949         __le16 capab = mgmt->u.probe_resp.capab_info;
2950
2951         if (ieee80211_is_s1g_beacon(mgmt->frame_control))
2952                 return 0;       /* TODO */
2953
2954         if (country_ie &&
2955             (capab & cpu_to_le16(WLAN_CAPABILITY_SPECTRUM_MGMT) ||
2956              capab & cpu_to_le16(WLAN_CAPABILITY_RADIO_MEASURE))) {
2957                 has_80211h_pwr = ieee80211_find_80211h_pwr_constr(
2958                         channel, country_ie, country_ie_len,
2959                         pwr_constr_ie, &chan_pwr, &pwr_reduction_80211h);
2960                 pwr_level_80211h =
2961                         max_t(int, 0, chan_pwr - pwr_reduction_80211h);
2962         }
2963
2964         if (cisco_dtpc_ie) {
2965                 ieee80211_find_cisco_dtpc(
2966                         channel, cisco_dtpc_ie, &pwr_level_cisco);
2967                 has_cisco_pwr = true;
2968         }
2969
2970         if (!has_80211h_pwr && !has_cisco_pwr)
2971                 return 0;
2972
2973         /* If we have both 802.11h and Cisco DTPC, apply both limits
2974          * by picking the smallest of the two power levels advertised.
2975          */
2976         if (has_80211h_pwr &&
2977             (!has_cisco_pwr || pwr_level_80211h <= pwr_level_cisco)) {
2978                 new_ap_level = pwr_level_80211h;
2979
2980                 if (link->ap_power_level == new_ap_level)
2981                         return 0;
2982
2983                 sdata_dbg(sdata,
2984                           "Limiting TX power to %d (%d - %d) dBm as advertised by %pM\n",
2985                           pwr_level_80211h, chan_pwr, pwr_reduction_80211h,
2986                           link->u.mgd.bssid);
2987         } else {  /* has_cisco_pwr is always true here. */
2988                 new_ap_level = pwr_level_cisco;
2989
2990                 if (link->ap_power_level == new_ap_level)
2991                         return 0;
2992
2993                 sdata_dbg(sdata,
2994                           "Limiting TX power to %d dBm as advertised by %pM\n",
2995                           pwr_level_cisco, link->u.mgd.bssid);
2996         }
2997
2998         link->ap_power_level = new_ap_level;
2999         if (__ieee80211_recalc_txpower(link))
3000                 return BSS_CHANGED_TXPOWER;
3001         return 0;
3002 }
3003
3004 /* powersave */
3005 static void ieee80211_enable_ps(struct ieee80211_local *local,
3006                                 struct ieee80211_sub_if_data *sdata)
3007 {
3008         struct ieee80211_conf *conf = &local->hw.conf;
3009
3010         /*
3011          * If we are scanning right now then the parameters will
3012          * take effect when scan finishes.
3013          */
3014         if (local->scanning)
3015                 return;
3016
3017         if (conf->dynamic_ps_timeout > 0 &&
3018             !ieee80211_hw_check(&local->hw, SUPPORTS_DYNAMIC_PS)) {
3019                 mod_timer(&local->dynamic_ps_timer, jiffies +
3020                           msecs_to_jiffies(conf->dynamic_ps_timeout));
3021         } else {
3022                 if (ieee80211_hw_check(&local->hw, PS_NULLFUNC_STACK))
3023                         ieee80211_send_nullfunc(local, sdata, true);
3024
3025                 if (ieee80211_hw_check(&local->hw, PS_NULLFUNC_STACK) &&
3026                     ieee80211_hw_check(&local->hw, REPORTS_TX_ACK_STATUS))
3027                         return;
3028
3029                 conf->flags |= IEEE80211_CONF_PS;
3030                 ieee80211_hw_config(local, IEEE80211_CONF_CHANGE_PS);
3031         }
3032 }
3033
3034 static void ieee80211_change_ps(struct ieee80211_local *local)
3035 {
3036         struct ieee80211_conf *conf = &local->hw.conf;
3037
3038         if (local->ps_sdata) {
3039                 ieee80211_enable_ps(local, local->ps_sdata);
3040         } else if (conf->flags & IEEE80211_CONF_PS) {
3041                 conf->flags &= ~IEEE80211_CONF_PS;
3042                 ieee80211_hw_config(local, IEEE80211_CONF_CHANGE_PS);
3043                 del_timer_sync(&local->dynamic_ps_timer);
3044                 wiphy_work_cancel(local->hw.wiphy,
3045                                   &local->dynamic_ps_enable_work);
3046         }
3047 }
3048
3049 static bool ieee80211_powersave_allowed(struct ieee80211_sub_if_data *sdata)
3050 {
3051         struct ieee80211_local *local = sdata->local;
3052         struct ieee80211_if_managed *mgd = &sdata->u.mgd;
3053         struct sta_info *sta = NULL;
3054         bool authorized = false;
3055
3056         if (!mgd->powersave)
3057                 return false;
3058
3059         if (mgd->broken_ap)
3060                 return false;
3061
3062         if (!mgd->associated)
3063                 return false;
3064
3065         if (mgd->flags & IEEE80211_STA_CONNECTION_POLL)
3066                 return false;
3067
3068         if (!(local->hw.wiphy->flags & WIPHY_FLAG_SUPPORTS_MLO) &&
3069             !sdata->deflink.u.mgd.have_beacon)
3070                 return false;
3071
3072         rcu_read_lock();
3073         sta = sta_info_get(sdata, sdata->vif.cfg.ap_addr);
3074         if (sta)
3075                 authorized = test_sta_flag(sta, WLAN_STA_AUTHORIZED);
3076         rcu_read_unlock();
3077
3078         return authorized;
3079 }
3080
3081 /* need to hold RTNL or interface lock */
3082 void ieee80211_recalc_ps(struct ieee80211_local *local)
3083 {
3084         struct ieee80211_sub_if_data *sdata, *found = NULL;
3085         int count = 0;
3086         int timeout;
3087
3088         if (!ieee80211_hw_check(&local->hw, SUPPORTS_PS) ||
3089             ieee80211_hw_check(&local->hw, SUPPORTS_DYNAMIC_PS)) {
3090                 local->ps_sdata = NULL;
3091                 return;
3092         }
3093
3094         list_for_each_entry(sdata, &local->interfaces, list) {
3095                 if (!ieee80211_sdata_running(sdata))
3096                         continue;
3097                 if (sdata->vif.type == NL80211_IFTYPE_AP) {
3098                         /* If an AP vif is found, then disable PS
3099                          * by setting the count to zero thereby setting
3100                          * ps_sdata to NULL.
3101                          */
3102                         count = 0;
3103                         break;
3104                 }
3105                 if (sdata->vif.type != NL80211_IFTYPE_STATION)
3106                         continue;
3107                 found = sdata;
3108                 count++;
3109         }
3110
3111         if (count == 1 && ieee80211_powersave_allowed(found)) {
3112                 u8 dtimper = found->deflink.u.mgd.dtim_period;
3113
3114                 timeout = local->dynamic_ps_forced_timeout;
3115                 if (timeout < 0)
3116                         timeout = 100;
3117                 local->hw.conf.dynamic_ps_timeout = timeout;
3118
3119                 /* If the TIM IE is invalid, pretend the value is 1 */
3120                 if (!dtimper)
3121                         dtimper = 1;
3122
3123                 local->hw.conf.ps_dtim_period = dtimper;
3124                 local->ps_sdata = found;
3125         } else {
3126                 local->ps_sdata = NULL;
3127         }
3128
3129         ieee80211_change_ps(local);
3130 }
3131
3132 void ieee80211_recalc_ps_vif(struct ieee80211_sub_if_data *sdata)
3133 {
3134         bool ps_allowed = ieee80211_powersave_allowed(sdata);
3135
3136         if (sdata->vif.cfg.ps != ps_allowed) {
3137                 sdata->vif.cfg.ps = ps_allowed;
3138                 ieee80211_vif_cfg_change_notify(sdata, BSS_CHANGED_PS);
3139         }
3140 }
3141
3142 void ieee80211_dynamic_ps_disable_work(struct wiphy *wiphy,
3143                                        struct wiphy_work *work)
3144 {
3145         struct ieee80211_local *local =
3146                 container_of(work, struct ieee80211_local,
3147                              dynamic_ps_disable_work);
3148
3149         if (local->hw.conf.flags & IEEE80211_CONF_PS) {
3150                 local->hw.conf.flags &= ~IEEE80211_CONF_PS;
3151                 ieee80211_hw_config(local, IEEE80211_CONF_CHANGE_PS);
3152         }
3153
3154         ieee80211_wake_queues_by_reason(&local->hw,
3155                                         IEEE80211_MAX_QUEUE_MAP,
3156                                         IEEE80211_QUEUE_STOP_REASON_PS,
3157                                         false);
3158 }
3159
3160 void ieee80211_dynamic_ps_enable_work(struct wiphy *wiphy,
3161                                       struct wiphy_work *work)
3162 {
3163         struct ieee80211_local *local =
3164                 container_of(work, struct ieee80211_local,
3165                              dynamic_ps_enable_work);
3166         struct ieee80211_sub_if_data *sdata = local->ps_sdata;
3167         struct ieee80211_if_managed *ifmgd;
3168         unsigned long flags;
3169         int q;
3170
3171         /* can only happen when PS was just disabled anyway */
3172         if (!sdata)
3173                 return;
3174
3175         ifmgd = &sdata->u.mgd;
3176
3177         if (local->hw.conf.flags & IEEE80211_CONF_PS)
3178                 return;
3179
3180         if (local->hw.conf.dynamic_ps_timeout > 0) {
3181                 /* don't enter PS if TX frames are pending */
3182                 if (drv_tx_frames_pending(local)) {
3183                         mod_timer(&local->dynamic_ps_timer, jiffies +
3184                                   msecs_to_jiffies(
3185                                   local->hw.conf.dynamic_ps_timeout));
3186                         return;
3187                 }
3188
3189                 /*
3190                  * transmission can be stopped by others which leads to
3191                  * dynamic_ps_timer expiry. Postpone the ps timer if it
3192                  * is not the actual idle state.
3193                  */
3194                 spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
3195                 for (q = 0; q < local->hw.queues; q++) {
3196                         if (local->queue_stop_reasons[q]) {
3197                                 spin_unlock_irqrestore(&local->queue_stop_reason_lock,
3198                                                        flags);
3199                                 mod_timer(&local->dynamic_ps_timer, jiffies +
3200                                           msecs_to_jiffies(
3201                                           local->hw.conf.dynamic_ps_timeout));
3202                                 return;
3203                         }
3204                 }
3205                 spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
3206         }
3207
3208         if (ieee80211_hw_check(&local->hw, PS_NULLFUNC_STACK) &&
3209             !(ifmgd->flags & IEEE80211_STA_NULLFUNC_ACKED)) {
3210                 if (drv_tx_frames_pending(local)) {
3211                         mod_timer(&local->dynamic_ps_timer, jiffies +
3212                                   msecs_to_jiffies(
3213                                   local->hw.conf.dynamic_ps_timeout));
3214                 } else {
3215                         ieee80211_send_nullfunc(local, sdata, true);
3216                         /* Flush to get the tx status of nullfunc frame */
3217                         ieee80211_flush_queues(local, sdata, false);
3218                 }
3219         }
3220
3221         if (!(ieee80211_hw_check(&local->hw, REPORTS_TX_ACK_STATUS) &&
3222               ieee80211_hw_check(&local->hw, PS_NULLFUNC_STACK)) ||
3223             (ifmgd->flags & IEEE80211_STA_NULLFUNC_ACKED)) {
3224                 ifmgd->flags &= ~IEEE80211_STA_NULLFUNC_ACKED;
3225                 local->hw.conf.flags |= IEEE80211_CONF_PS;
3226                 ieee80211_hw_config(local, IEEE80211_CONF_CHANGE_PS);
3227         }
3228 }
3229
3230 void ieee80211_dynamic_ps_timer(struct timer_list *t)
3231 {
3232         struct ieee80211_local *local = from_timer(local, t, dynamic_ps_timer);
3233
3234         wiphy_work_queue(local->hw.wiphy, &local->dynamic_ps_enable_work);
3235 }
3236
3237 void ieee80211_dfs_cac_timer_work(struct wiphy *wiphy, struct wiphy_work *work)
3238 {
3239         struct ieee80211_link_data *link =
3240                 container_of(work, struct ieee80211_link_data,
3241                              dfs_cac_timer_work.work);
3242         struct cfg80211_chan_def chandef = link->conf->chanreq.oper;
3243         struct ieee80211_sub_if_data *sdata = link->sdata;
3244
3245         lockdep_assert_wiphy(sdata->local->hw.wiphy);
3246
3247         if (sdata->wdev.links[link->link_id].cac_started) {
3248                 ieee80211_link_release_channel(link);
3249                 cfg80211_cac_event(sdata->dev, &chandef,
3250                                    NL80211_RADAR_CAC_FINISHED,
3251                                    GFP_KERNEL, link->link_id);
3252         }
3253 }
3254
3255 static bool
3256 __ieee80211_sta_handle_tspec_ac_params(struct ieee80211_sub_if_data *sdata)
3257 {
3258         struct ieee80211_local *local = sdata->local;
3259         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
3260         bool ret = false;
3261         int ac;
3262
3263         if (local->hw.queues < IEEE80211_NUM_ACS)
3264                 return false;
3265
3266         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
3267                 struct ieee80211_sta_tx_tspec *tx_tspec = &ifmgd->tx_tspec[ac];
3268                 int non_acm_ac;
3269                 unsigned long now = jiffies;
3270
3271                 if (tx_tspec->action == TX_TSPEC_ACTION_NONE &&
3272                     tx_tspec->admitted_time &&
3273                     time_after(now, tx_tspec->time_slice_start + HZ)) {
3274                         tx_tspec->consumed_tx_time = 0;
3275                         tx_tspec->time_slice_start = now;
3276
3277                         if (tx_tspec->downgraded)
3278                                 tx_tspec->action =
3279                                         TX_TSPEC_ACTION_STOP_DOWNGRADE;
3280                 }
3281
3282                 switch (tx_tspec->action) {
3283                 case TX_TSPEC_ACTION_STOP_DOWNGRADE:
3284                         /* take the original parameters */
3285                         if (drv_conf_tx(local, &sdata->deflink, ac,
3286                                         &sdata->deflink.tx_conf[ac]))
3287                                 link_err(&sdata->deflink,
3288                                          "failed to set TX queue parameters for queue %d\n",
3289                                          ac);
3290                         tx_tspec->action = TX_TSPEC_ACTION_NONE;
3291                         tx_tspec->downgraded = false;
3292                         ret = true;
3293                         break;
3294                 case TX_TSPEC_ACTION_DOWNGRADE:
3295                         if (time_after(now, tx_tspec->time_slice_start + HZ)) {
3296                                 tx_tspec->action = TX_TSPEC_ACTION_NONE;
3297                                 ret = true;
3298                                 break;
3299                         }
3300                         /* downgrade next lower non-ACM AC */
3301                         for (non_acm_ac = ac + 1;
3302                              non_acm_ac < IEEE80211_NUM_ACS;
3303                              non_acm_ac++)
3304                                 if (!(sdata->wmm_acm & BIT(7 - 2 * non_acm_ac)))
3305                                         break;
3306                         /* Usually the loop will result in using BK even if it
3307                          * requires admission control, but such a configuration
3308                          * makes no sense and we have to transmit somehow - the
3309                          * AC selection does the same thing.
3310                          * If we started out trying to downgrade from BK, then
3311                          * the extra condition here might be needed.
3312                          */
3313                         if (non_acm_ac >= IEEE80211_NUM_ACS)
3314                                 non_acm_ac = IEEE80211_AC_BK;
3315                         if (drv_conf_tx(local, &sdata->deflink, ac,
3316                                         &sdata->deflink.tx_conf[non_acm_ac]))
3317                                 link_err(&sdata->deflink,
3318                                          "failed to set TX queue parameters for queue %d\n",
3319                                          ac);
3320                         tx_tspec->action = TX_TSPEC_ACTION_NONE;
3321                         ret = true;
3322                         wiphy_delayed_work_queue(local->hw.wiphy,
3323                                                  &ifmgd->tx_tspec_wk,
3324                                                  tx_tspec->time_slice_start +
3325                                                  HZ - now + 1);
3326                         break;
3327                 case TX_TSPEC_ACTION_NONE:
3328                         /* nothing now */
3329                         break;
3330                 }
3331         }
3332
3333         return ret;
3334 }
3335
3336 void ieee80211_sta_handle_tspec_ac_params(struct ieee80211_sub_if_data *sdata)
3337 {
3338         if (__ieee80211_sta_handle_tspec_ac_params(sdata))
3339                 ieee80211_link_info_change_notify(sdata, &sdata->deflink,
3340                                                   BSS_CHANGED_QOS);
3341 }
3342
3343 static void ieee80211_sta_handle_tspec_ac_params_wk(struct wiphy *wiphy,
3344                                                     struct wiphy_work *work)
3345 {
3346         struct ieee80211_sub_if_data *sdata;
3347
3348         sdata = container_of(work, struct ieee80211_sub_if_data,
3349                              u.mgd.tx_tspec_wk.work);
3350         ieee80211_sta_handle_tspec_ac_params(sdata);
3351 }
3352
3353 void ieee80211_mgd_set_link_qos_params(struct ieee80211_link_data *link)
3354 {
3355         struct ieee80211_sub_if_data *sdata = link->sdata;
3356         struct ieee80211_local *local = sdata->local;
3357         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
3358         struct ieee80211_tx_queue_params *params = link->tx_conf;
3359         u8 ac;
3360
3361         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
3362                 mlme_dbg(sdata,
3363                          "WMM AC=%d acm=%d aifs=%d cWmin=%d cWmax=%d txop=%d uapsd=%d, downgraded=%d\n",
3364                          ac, params[ac].acm,
3365                          params[ac].aifs, params[ac].cw_min, params[ac].cw_max,
3366                          params[ac].txop, params[ac].uapsd,
3367                          ifmgd->tx_tspec[ac].downgraded);
3368                 if (!ifmgd->tx_tspec[ac].downgraded &&
3369                     drv_conf_tx(local, link, ac, &params[ac]))
3370                         link_err(link,
3371                                  "failed to set TX queue parameters for AC %d\n",
3372                                  ac);
3373         }
3374 }
3375
3376 /* MLME */
3377 static bool
3378 ieee80211_sta_wmm_params(struct ieee80211_local *local,
3379                          struct ieee80211_link_data *link,
3380                          const u8 *wmm_param, size_t wmm_param_len,
3381                          const struct ieee80211_mu_edca_param_set *mu_edca)
3382 {
3383         struct ieee80211_sub_if_data *sdata = link->sdata;
3384         struct ieee80211_tx_queue_params params[IEEE80211_NUM_ACS];
3385         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
3386         size_t left;
3387         int count, mu_edca_count, ac;
3388         const u8 *pos;
3389         u8 uapsd_queues = 0;
3390
3391         if (!local->ops->conf_tx)
3392                 return false;
3393
3394         if (local->hw.queues < IEEE80211_NUM_ACS)
3395                 return false;
3396
3397         if (!wmm_param)
3398                 return false;
3399
3400         if (wmm_param_len < 8 || wmm_param[5] /* version */ != 1)
3401                 return false;
3402
3403         if (ifmgd->flags & IEEE80211_STA_UAPSD_ENABLED)
3404                 uapsd_queues = ifmgd->uapsd_queues;
3405
3406         count = wmm_param[6] & 0x0f;
3407         /* -1 is the initial value of ifmgd->mu_edca_last_param_set.
3408          * if mu_edca was preset before and now it disappeared tell
3409          * the driver about it.
3410          */
3411         mu_edca_count = mu_edca ? mu_edca->mu_qos_info & 0x0f : -1;
3412         if (count == link->u.mgd.wmm_last_param_set &&
3413             mu_edca_count == link->u.mgd.mu_edca_last_param_set)
3414                 return false;
3415         link->u.mgd.wmm_last_param_set = count;
3416         link->u.mgd.mu_edca_last_param_set = mu_edca_count;
3417
3418         pos = wmm_param + 8;
3419         left = wmm_param_len - 8;
3420
3421         memset(&params, 0, sizeof(params));
3422
3423         sdata->wmm_acm = 0;
3424         for (; left >= 4; left -= 4, pos += 4) {
3425                 int aci = (pos[0] >> 5) & 0x03;
3426                 int acm = (pos[0] >> 4) & 0x01;
3427                 bool uapsd = false;
3428
3429                 switch (aci) {
3430                 case 1: /* AC_BK */
3431                         ac = IEEE80211_AC_BK;
3432                         if (acm)
3433                                 sdata->wmm_acm |= BIT(1) | BIT(2); /* BK/- */
3434                         if (uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_BK)
3435                                 uapsd = true;
3436                         params[ac].mu_edca = !!mu_edca;
3437                         if (mu_edca)
3438                                 params[ac].mu_edca_param_rec = mu_edca->ac_bk;
3439                         break;
3440                 case 2: /* AC_VI */
3441                         ac = IEEE80211_AC_VI;
3442                         if (acm)
3443                                 sdata->wmm_acm |= BIT(4) | BIT(5); /* CL/VI */
3444                         if (uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VI)
3445                                 uapsd = true;
3446                         params[ac].mu_edca = !!mu_edca;
3447                         if (mu_edca)
3448                                 params[ac].mu_edca_param_rec = mu_edca->ac_vi;
3449                         break;
3450                 case 3: /* AC_VO */
3451                         ac = IEEE80211_AC_VO;
3452                         if (acm)
3453                                 sdata->wmm_acm |= BIT(6) | BIT(7); /* VO/NC */
3454                         if (uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VO)
3455                                 uapsd = true;
3456                         params[ac].mu_edca = !!mu_edca;
3457                         if (mu_edca)
3458                                 params[ac].mu_edca_param_rec = mu_edca->ac_vo;
3459                         break;
3460                 case 0: /* AC_BE */
3461                 default:
3462                         ac = IEEE80211_AC_BE;
3463                         if (acm)
3464                                 sdata->wmm_acm |= BIT(0) | BIT(3); /* BE/EE */
3465                         if (uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_BE)
3466                                 uapsd = true;
3467                         params[ac].mu_edca = !!mu_edca;
3468                         if (mu_edca)
3469                                 params[ac].mu_edca_param_rec = mu_edca->ac_be;
3470                         break;
3471                 }
3472
3473                 params[ac].aifs = pos[0] & 0x0f;
3474
3475                 if (params[ac].aifs < 2) {
3476                         link_info(link,
3477                                   "AP has invalid WMM params (AIFSN=%d for ACI %d), will use 2\n",
3478                                   params[ac].aifs, aci);
3479                         params[ac].aifs = 2;
3480                 }
3481                 params[ac].cw_max = ecw2cw((pos[1] & 0xf0) >> 4);
3482                 params[ac].cw_min = ecw2cw(pos[1] & 0x0f);
3483                 params[ac].txop = get_unaligned_le16(pos + 2);
3484                 params[ac].acm = acm;
3485                 params[ac].uapsd = uapsd;
3486
3487                 if (params[ac].cw_min == 0 ||
3488                     params[ac].cw_min > params[ac].cw_max) {
3489                         link_info(link,
3490                                   "AP has invalid WMM params (CWmin/max=%d/%d for ACI %d), using defaults\n",
3491                                   params[ac].cw_min, params[ac].cw_max, aci);
3492                         return false;
3493                 }
3494                 ieee80211_regulatory_limit_wmm_params(sdata, &params[ac], ac);
3495         }
3496
3497         /* WMM specification requires all 4 ACIs. */
3498         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
3499                 if (params[ac].cw_min == 0) {
3500                         link_info(link,
3501                                   "AP has invalid WMM params (missing AC %d), using defaults\n",
3502                                   ac);
3503                         return false;
3504                 }
3505         }
3506
3507         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
3508                 link->tx_conf[ac] = params[ac];
3509
3510         ieee80211_mgd_set_link_qos_params(link);
3511
3512         /* enable WMM or activate new settings */
3513         link->conf->qos = true;
3514         return true;
3515 }
3516
3517 static void __ieee80211_stop_poll(struct ieee80211_sub_if_data *sdata)
3518 {
3519         lockdep_assert_wiphy(sdata->local->hw.wiphy);
3520
3521         sdata->u.mgd.flags &= ~IEEE80211_STA_CONNECTION_POLL;
3522         ieee80211_run_deferred_scan(sdata->local);
3523 }
3524
3525 static void ieee80211_stop_poll(struct ieee80211_sub_if_data *sdata)
3526 {
3527         lockdep_assert_wiphy(sdata->local->hw.wiphy);
3528
3529         __ieee80211_stop_poll(sdata);
3530 }
3531
3532 static u64 ieee80211_handle_bss_capability(struct ieee80211_link_data *link,
3533                                            u16 capab, bool erp_valid, u8 erp)
3534 {
3535         struct ieee80211_bss_conf *bss_conf = link->conf;
3536         struct ieee80211_supported_band *sband;
3537         u64 changed = 0;
3538         bool use_protection;
3539         bool use_short_preamble;
3540         bool use_short_slot;
3541
3542         sband = ieee80211_get_link_sband(link);
3543         if (!sband)
3544                 return changed;
3545
3546         if (erp_valid) {
3547                 use_protection = (erp & WLAN_ERP_USE_PROTECTION) != 0;
3548                 use_short_preamble = (erp & WLAN_ERP_BARKER_PREAMBLE) == 0;
3549         } else {
3550                 use_protection = false;
3551                 use_short_preamble = !!(capab & WLAN_CAPABILITY_SHORT_PREAMBLE);
3552         }
3553
3554         use_short_slot = !!(capab & WLAN_CAPABILITY_SHORT_SLOT_TIME);
3555         if (sband->band == NL80211_BAND_5GHZ ||
3556             sband->band == NL80211_BAND_6GHZ)
3557                 use_short_slot = true;
3558
3559         if (use_protection != bss_conf->use_cts_prot) {
3560                 bss_conf->use_cts_prot = use_protection;
3561                 changed |= BSS_CHANGED_ERP_CTS_PROT;
3562         }
3563
3564         if (use_short_preamble != bss_conf->use_short_preamble) {
3565                 bss_conf->use_short_preamble = use_short_preamble;
3566                 changed |= BSS_CHANGED_ERP_PREAMBLE;
3567         }
3568
3569         if (use_short_slot != bss_conf->use_short_slot) {
3570                 bss_conf->use_short_slot = use_short_slot;
3571                 changed |= BSS_CHANGED_ERP_SLOT;
3572         }
3573
3574         return changed;
3575 }
3576
3577 static u64 ieee80211_link_set_associated(struct ieee80211_link_data *link,
3578                                          struct cfg80211_bss *cbss)
3579 {
3580         struct ieee80211_sub_if_data *sdata = link->sdata;
3581         struct ieee80211_bss_conf *bss_conf = link->conf;
3582         struct ieee80211_bss *bss = (void *)cbss->priv;
3583         u64 changed = BSS_CHANGED_QOS;
3584
3585         /* not really used in MLO */
3586         sdata->u.mgd.beacon_timeout =
3587                 usecs_to_jiffies(ieee80211_tu_to_usec(beacon_loss_count *
3588                                                       bss_conf->beacon_int));
3589
3590         changed |= ieee80211_handle_bss_capability(link,
3591                                                    bss_conf->assoc_capability,
3592                                                    bss->has_erp_value,
3593                                                    bss->erp_value);
3594
3595         ieee80211_check_rate_mask(link);
3596
3597         link->conf->bss = cbss;
3598         memcpy(link->u.mgd.bssid, cbss->bssid, ETH_ALEN);
3599
3600         if (sdata->vif.p2p ||
3601             sdata->vif.driver_flags & IEEE80211_VIF_GET_NOA_UPDATE) {
3602                 const struct cfg80211_bss_ies *ies;
3603
3604                 rcu_read_lock();
3605                 ies = rcu_dereference(cbss->ies);
3606                 if (ies) {
3607                         int ret;
3608
3609                         ret = cfg80211_get_p2p_attr(
3610                                         ies->data, ies->len,
3611                                         IEEE80211_P2P_ATTR_ABSENCE_NOTICE,
3612                                         (u8 *) &bss_conf->p2p_noa_attr,
3613                                         sizeof(bss_conf->p2p_noa_attr));
3614                         if (ret >= 2) {
3615                                 link->u.mgd.p2p_noa_index =
3616                                         bss_conf->p2p_noa_attr.index;
3617                                 changed |= BSS_CHANGED_P2P_PS;
3618                         }
3619                 }
3620                 rcu_read_unlock();
3621         }
3622
3623         if (link->u.mgd.have_beacon) {
3624                 bss_conf->beacon_rate = bss->beacon_rate;
3625                 changed |= BSS_CHANGED_BEACON_INFO;
3626         } else {
3627                 bss_conf->beacon_rate = NULL;
3628         }
3629
3630         /* Tell the driver to monitor connection quality (if supported) */
3631         if (sdata->vif.driver_flags & IEEE80211_VIF_SUPPORTS_CQM_RSSI &&
3632             bss_conf->cqm_rssi_thold)
3633                 changed |= BSS_CHANGED_CQM;
3634
3635         return changed;
3636 }
3637
3638 static void ieee80211_set_associated(struct ieee80211_sub_if_data *sdata,
3639                                      struct ieee80211_mgd_assoc_data *assoc_data,
3640                                      u64 changed[IEEE80211_MLD_MAX_NUM_LINKS])
3641 {
3642         struct ieee80211_local *local = sdata->local;
3643         struct ieee80211_vif_cfg *vif_cfg = &sdata->vif.cfg;
3644         u64 vif_changed = BSS_CHANGED_ASSOC;
3645         unsigned int link_id;
3646
3647         lockdep_assert_wiphy(local->hw.wiphy);
3648
3649         sdata->u.mgd.associated = true;
3650
3651         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
3652                 struct cfg80211_bss *cbss = assoc_data->link[link_id].bss;
3653                 struct ieee80211_link_data *link;
3654
3655                 if (!cbss ||
3656                     assoc_data->link[link_id].status != WLAN_STATUS_SUCCESS)
3657                         continue;
3658
3659                 if (ieee80211_vif_is_mld(&sdata->vif) &&
3660                     !(ieee80211_vif_usable_links(&sdata->vif) & BIT(link_id)))
3661                         continue;
3662
3663                 link = sdata_dereference(sdata->link[link_id], sdata);
3664                 if (WARN_ON(!link))
3665                         return;
3666
3667                 changed[link_id] |= ieee80211_link_set_associated(link, cbss);
3668         }
3669
3670         /* just to be sure */
3671         ieee80211_stop_poll(sdata);
3672
3673         ieee80211_led_assoc(local, 1);
3674
3675         vif_cfg->assoc = 1;
3676
3677         /* Enable ARP filtering */
3678         if (vif_cfg->arp_addr_cnt)
3679                 vif_changed |= BSS_CHANGED_ARP_FILTER;
3680
3681         if (ieee80211_vif_is_mld(&sdata->vif)) {
3682                 for (link_id = 0;
3683                      link_id < IEEE80211_MLD_MAX_NUM_LINKS;
3684                      link_id++) {
3685                         struct ieee80211_link_data *link;
3686                         struct cfg80211_bss *cbss = assoc_data->link[link_id].bss;
3687
3688                         if (!cbss ||
3689                             !(BIT(link_id) &
3690                               ieee80211_vif_usable_links(&sdata->vif)) ||
3691                             assoc_data->link[link_id].status != WLAN_STATUS_SUCCESS)
3692                                 continue;
3693
3694                         link = sdata_dereference(sdata->link[link_id], sdata);
3695                         if (WARN_ON(!link))
3696                                 return;
3697
3698                         ieee80211_link_info_change_notify(sdata, link,
3699                                                           changed[link_id]);
3700
3701                         ieee80211_recalc_smps(sdata, link);
3702                 }
3703
3704                 ieee80211_vif_cfg_change_notify(sdata, vif_changed);
3705         } else {
3706                 ieee80211_bss_info_change_notify(sdata,
3707                                                  vif_changed | changed[0]);
3708         }
3709
3710         ieee80211_recalc_ps(local);
3711
3712         /* leave this here to not change ordering in non-MLO cases */
3713         if (!ieee80211_vif_is_mld(&sdata->vif))
3714                 ieee80211_recalc_smps(sdata, &sdata->deflink);
3715         ieee80211_recalc_ps_vif(sdata);
3716
3717         netif_carrier_on(sdata->dev);
3718 }
3719
3720 static void ieee80211_ml_reconf_reset(struct ieee80211_sub_if_data *sdata)
3721 {
3722         struct ieee80211_mgd_assoc_data *add_links_data =
3723                 sdata->u.mgd.reconf.add_links_data;
3724
3725         if (!ieee80211_vif_is_mld(&sdata->vif) ||
3726             !(sdata->u.mgd.reconf.added_links |
3727               sdata->u.mgd.reconf.removed_links))
3728                 return;
3729
3730         wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
3731                                   &sdata->u.mgd.reconf.wk);
3732         sdata->u.mgd.reconf.added_links = 0;
3733         sdata->u.mgd.reconf.removed_links = 0;
3734         sdata->u.mgd.reconf.dialog_token = 0;
3735
3736         if (add_links_data) {
3737                 struct cfg80211_mlo_reconf_done_data done_data = {};
3738                 u8 link_id;
3739
3740                 for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS;
3741                      link_id++)
3742                         done_data.links[link_id].bss =
3743                                 add_links_data->link[link_id].bss;
3744
3745                 cfg80211_mlo_reconf_add_done(sdata->dev, &done_data);
3746
3747                 kfree(sdata->u.mgd.reconf.add_links_data);
3748                 sdata->u.mgd.reconf.add_links_data = NULL;
3749         }
3750 }
3751
3752 static void ieee80211_set_disassoc(struct ieee80211_sub_if_data *sdata,
3753                                    u16 stype, u16 reason, bool tx,
3754                                    u8 *frame_buf)
3755 {
3756         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
3757         struct ieee80211_local *local = sdata->local;
3758         struct sta_info *ap_sta = sta_info_get(sdata, sdata->vif.cfg.ap_addr);
3759         unsigned int link_id;
3760         u64 changed = 0;
3761         struct ieee80211_prep_tx_info info = {
3762                 .subtype = stype,
3763                 .was_assoc = true,
3764                 .link_id = ffs(sdata->vif.active_links) - 1,
3765         };
3766
3767         lockdep_assert_wiphy(local->hw.wiphy);
3768
3769         if (WARN_ON(!ap_sta))
3770                 return;
3771
3772         if (WARN_ON_ONCE(tx && !frame_buf))
3773                 return;
3774
3775         if (WARN_ON(!ifmgd->associated))
3776                 return;
3777
3778         ieee80211_stop_poll(sdata);
3779
3780         ifmgd->associated = false;
3781
3782         /* other links will be destroyed */
3783         sdata->deflink.conf->bss = NULL;
3784         sdata->deflink.smps_mode = IEEE80211_SMPS_OFF;
3785
3786         netif_carrier_off(sdata->dev);
3787
3788         /*
3789          * if we want to get out of ps before disassoc (why?) we have
3790          * to do it before sending disassoc, as otherwise the null-packet
3791          * won't be valid.
3792          */
3793         if (local->hw.conf.flags & IEEE80211_CONF_PS) {
3794                 local->hw.conf.flags &= ~IEEE80211_CONF_PS;
3795                 ieee80211_hw_config(local, IEEE80211_CONF_CHANGE_PS);
3796         }
3797         local->ps_sdata = NULL;
3798
3799         /* disable per-vif ps */
3800         ieee80211_recalc_ps_vif(sdata);
3801
3802         /* make sure ongoing transmission finishes */
3803         synchronize_net();
3804
3805         /*
3806          * drop any frame before deauth/disassoc, this can be data or
3807          * management frame. Since we are disconnecting, we should not
3808          * insist sending these frames which can take time and delay
3809          * the disconnection and possible the roaming.
3810          */
3811         if (tx)
3812                 ieee80211_flush_queues(local, sdata, true);
3813
3814         /* deauthenticate/disassociate now */
3815         if (tx || frame_buf) {
3816                 drv_mgd_prepare_tx(sdata->local, sdata, &info);
3817
3818                 ieee80211_send_deauth_disassoc(sdata, sdata->vif.cfg.ap_addr,
3819                                                sdata->vif.cfg.ap_addr, stype,
3820                                                reason, tx, frame_buf);
3821         }
3822
3823         /* flush out frame - make sure the deauth was actually sent */
3824         if (tx)
3825                 ieee80211_flush_queues(local, sdata, false);
3826
3827         drv_mgd_complete_tx(sdata->local, sdata, &info);
3828
3829         /* clear AP addr only after building the needed mgmt frames */
3830         eth_zero_addr(sdata->deflink.u.mgd.bssid);
3831         eth_zero_addr(sdata->vif.cfg.ap_addr);
3832
3833         sdata->vif.cfg.ssid_len = 0;
3834
3835         /* Remove TDLS peers */
3836         __sta_info_flush(sdata, false, -1, ap_sta);
3837
3838         if (sdata->vif.driver_flags & IEEE80211_VIF_REMOVE_AP_AFTER_DISASSOC) {
3839                 /* Only move the AP state */
3840                 sta_info_move_state(ap_sta, IEEE80211_STA_NONE);
3841         } else {
3842                 /* Remove AP peer */
3843                 sta_info_flush(sdata, -1);
3844         }
3845
3846         /* finally reset all BSS / config parameters */
3847         if (!ieee80211_vif_is_mld(&sdata->vif))
3848                 changed |= ieee80211_reset_erp_info(sdata);
3849
3850         ieee80211_led_assoc(local, 0);
3851         changed |= BSS_CHANGED_ASSOC;
3852         sdata->vif.cfg.assoc = false;
3853
3854         sdata->deflink.u.mgd.p2p_noa_index = -1;
3855         memset(&sdata->vif.bss_conf.p2p_noa_attr, 0,
3856                sizeof(sdata->vif.bss_conf.p2p_noa_attr));
3857
3858         /* on the next assoc, re-program HT/VHT parameters */
3859         memset(&ifmgd->ht_capa, 0, sizeof(ifmgd->ht_capa));
3860         memset(&ifmgd->ht_capa_mask, 0, sizeof(ifmgd->ht_capa_mask));
3861         memset(&ifmgd->vht_capa, 0, sizeof(ifmgd->vht_capa));
3862         memset(&ifmgd->vht_capa_mask, 0, sizeof(ifmgd->vht_capa_mask));
3863
3864         /*
3865          * reset MU-MIMO ownership and group data in default link,
3866          * if used, other links are destroyed
3867          */
3868         memset(sdata->vif.bss_conf.mu_group.membership, 0,
3869                sizeof(sdata->vif.bss_conf.mu_group.membership));
3870         memset(sdata->vif.bss_conf.mu_group.position, 0,
3871                sizeof(sdata->vif.bss_conf.mu_group.position));
3872         if (!ieee80211_vif_is_mld(&sdata->vif))
3873                 changed |= BSS_CHANGED_MU_GROUPS;
3874         sdata->vif.bss_conf.mu_mimo_owner = false;
3875
3876         sdata->deflink.ap_power_level = IEEE80211_UNSET_POWER_LEVEL;
3877
3878         del_timer_sync(&local->dynamic_ps_timer);
3879         wiphy_work_cancel(local->hw.wiphy, &local->dynamic_ps_enable_work);
3880
3881         /* Disable ARP filtering */
3882         if (sdata->vif.cfg.arp_addr_cnt)
3883                 changed |= BSS_CHANGED_ARP_FILTER;
3884
3885         sdata->vif.bss_conf.qos = false;
3886         if (!ieee80211_vif_is_mld(&sdata->vif)) {
3887                 changed |= BSS_CHANGED_QOS;
3888                 /* The BSSID (not really interesting) and HT changed */
3889                 changed |= BSS_CHANGED_BSSID | BSS_CHANGED_HT;
3890                 ieee80211_bss_info_change_notify(sdata, changed);
3891         } else {
3892                 ieee80211_vif_cfg_change_notify(sdata, changed);
3893         }
3894
3895         if (sdata->vif.driver_flags & IEEE80211_VIF_REMOVE_AP_AFTER_DISASSOC) {
3896                 /*
3897                  * After notifying the driver about the disassoc,
3898                  * remove the ap sta.
3899                  */
3900                 sta_info_flush(sdata, -1);
3901         }
3902
3903         /* disassociated - set to defaults now */
3904         ieee80211_set_wmm_default(&sdata->deflink, false, false);
3905
3906         del_timer_sync(&sdata->u.mgd.conn_mon_timer);
3907         del_timer_sync(&sdata->u.mgd.bcn_mon_timer);
3908         del_timer_sync(&sdata->u.mgd.timer);
3909
3910         sdata->vif.bss_conf.dtim_period = 0;
3911         sdata->vif.bss_conf.beacon_rate = NULL;
3912
3913         sdata->deflink.u.mgd.have_beacon = false;
3914         sdata->deflink.u.mgd.tracking_signal_avg = false;
3915         sdata->deflink.u.mgd.disable_wmm_tracking = false;
3916
3917         ifmgd->flags = 0;
3918
3919         for (link_id = 0; link_id < ARRAY_SIZE(sdata->link); link_id++) {
3920                 struct ieee80211_link_data *link;
3921
3922                 link = sdata_dereference(sdata->link[link_id], sdata);
3923                 if (!link)
3924                         continue;
3925                 ieee80211_link_release_channel(link);
3926         }
3927
3928         sdata->vif.bss_conf.csa_active = false;
3929         sdata->deflink.u.mgd.csa.blocked_tx = false;
3930         sdata->deflink.u.mgd.csa.waiting_bcn = false;
3931         sdata->deflink.u.mgd.csa.ignored_same_chan = false;
3932         ieee80211_vif_unblock_queues_csa(sdata);
3933
3934         /* existing TX TSPEC sessions no longer exist */
3935         memset(ifmgd->tx_tspec, 0, sizeof(ifmgd->tx_tspec));
3936         wiphy_delayed_work_cancel(local->hw.wiphy, &ifmgd->tx_tspec_wk);
3937
3938         sdata->vif.bss_conf.power_type = IEEE80211_REG_UNSET_AP;
3939         sdata->vif.bss_conf.pwr_reduction = 0;
3940         ieee80211_clear_tpe(&sdata->vif.bss_conf.tpe);
3941
3942         sdata->vif.cfg.eml_cap = 0;
3943         sdata->vif.cfg.eml_med_sync_delay = 0;
3944         sdata->vif.cfg.mld_capa_op = 0;
3945
3946         memset(&sdata->u.mgd.ttlm_info, 0,
3947                sizeof(sdata->u.mgd.ttlm_info));
3948         wiphy_delayed_work_cancel(sdata->local->hw.wiphy, &ifmgd->ttlm_work);
3949
3950         memset(&sdata->vif.neg_ttlm, 0, sizeof(sdata->vif.neg_ttlm));
3951         wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
3952                                   &ifmgd->neg_ttlm_timeout_work);
3953
3954         sdata->u.mgd.removed_links = 0;
3955         wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
3956                                   &sdata->u.mgd.ml_reconf_work);
3957
3958         wiphy_work_cancel(sdata->local->hw.wiphy,
3959                           &ifmgd->teardown_ttlm_work);
3960
3961         ieee80211_vif_set_links(sdata, 0, 0);
3962
3963         ifmgd->mcast_seq_last = IEEE80211_SN_MODULO;
3964
3965         /* if disconnection happens in the middle of the ML reconfiguration
3966          * flow, cfg80211 must called to release the BSS references obtained
3967          * when the flow started.
3968          */
3969         ieee80211_ml_reconf_reset(sdata);
3970 }
3971
3972 static void ieee80211_reset_ap_probe(struct ieee80211_sub_if_data *sdata)
3973 {
3974         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
3975         struct ieee80211_local *local = sdata->local;
3976
3977         lockdep_assert_wiphy(local->hw.wiphy);
3978
3979         if (!(ifmgd->flags & IEEE80211_STA_CONNECTION_POLL))
3980                 return;
3981
3982         __ieee80211_stop_poll(sdata);
3983
3984         ieee80211_recalc_ps(local);
3985
3986         if (ieee80211_hw_check(&sdata->local->hw, CONNECTION_MONITOR))
3987                 return;
3988
3989         /*
3990          * We've received a probe response, but are not sure whether
3991          * we have or will be receiving any beacons or data, so let's
3992          * schedule the timers again, just in case.
3993          */
3994         ieee80211_sta_reset_beacon_monitor(sdata);
3995
3996         mod_timer(&ifmgd->conn_mon_timer,
3997                   round_jiffies_up(jiffies +
3998                                    IEEE80211_CONNECTION_IDLE_TIME));
3999 }
4000
4001 static void ieee80211_sta_tx_wmm_ac_notify(struct ieee80211_sub_if_data *sdata,
4002                                            struct ieee80211_hdr *hdr,
4003                                            u16 tx_time)
4004 {
4005         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4006         u16 tid;
4007         int ac;
4008         struct ieee80211_sta_tx_tspec *tx_tspec;
4009         unsigned long now = jiffies;
4010
4011         if (!ieee80211_is_data_qos(hdr->frame_control))
4012                 return;
4013
4014         tid = ieee80211_get_tid(hdr);
4015         ac = ieee80211_ac_from_tid(tid);
4016         tx_tspec = &ifmgd->tx_tspec[ac];
4017
4018         if (likely(!tx_tspec->admitted_time))
4019                 return;
4020
4021         if (time_after(now, tx_tspec->time_slice_start + HZ)) {
4022                 tx_tspec->consumed_tx_time = 0;
4023                 tx_tspec->time_slice_start = now;
4024
4025                 if (tx_tspec->downgraded) {
4026                         tx_tspec->action = TX_TSPEC_ACTION_STOP_DOWNGRADE;
4027                         wiphy_delayed_work_queue(sdata->local->hw.wiphy,
4028                                                  &ifmgd->tx_tspec_wk, 0);
4029                 }
4030         }
4031
4032         if (tx_tspec->downgraded)
4033                 return;
4034
4035         tx_tspec->consumed_tx_time += tx_time;
4036
4037         if (tx_tspec->consumed_tx_time >= tx_tspec->admitted_time) {
4038                 tx_tspec->downgraded = true;
4039                 tx_tspec->action = TX_TSPEC_ACTION_DOWNGRADE;
4040                 wiphy_delayed_work_queue(sdata->local->hw.wiphy,
4041                                          &ifmgd->tx_tspec_wk, 0);
4042         }
4043 }
4044
4045 void ieee80211_sta_tx_notify(struct ieee80211_sub_if_data *sdata,
4046                              struct ieee80211_hdr *hdr, bool ack, u16 tx_time)
4047 {
4048         ieee80211_sta_tx_wmm_ac_notify(sdata, hdr, tx_time);
4049
4050         if (!ieee80211_is_any_nullfunc(hdr->frame_control) ||
4051             !sdata->u.mgd.probe_send_count)
4052                 return;
4053
4054         if (ack)
4055                 sdata->u.mgd.probe_send_count = 0;
4056         else
4057                 sdata->u.mgd.nullfunc_failed = true;
4058         wiphy_work_queue(sdata->local->hw.wiphy, &sdata->work);
4059 }
4060
4061 static void ieee80211_mlme_send_probe_req(struct ieee80211_sub_if_data *sdata,
4062                                           const u8 *src, const u8 *dst,
4063                                           const u8 *ssid, size_t ssid_len,
4064                                           struct ieee80211_channel *channel)
4065 {
4066         struct sk_buff *skb;
4067
4068         skb = ieee80211_build_probe_req(sdata, src, dst, (u32)-1, channel,
4069                                         ssid, ssid_len, NULL, 0,
4070                                         IEEE80211_PROBE_FLAG_DIRECTED);
4071         if (skb)
4072                 ieee80211_tx_skb(sdata, skb);
4073 }
4074
4075 static void ieee80211_mgd_probe_ap_send(struct ieee80211_sub_if_data *sdata)
4076 {
4077         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4078         u8 *dst = sdata->vif.cfg.ap_addr;
4079         u8 unicast_limit = max(1, max_probe_tries - 3);
4080         struct sta_info *sta;
4081
4082         lockdep_assert_wiphy(sdata->local->hw.wiphy);
4083
4084         if (WARN_ON(ieee80211_vif_is_mld(&sdata->vif)))
4085                 return;
4086
4087         /*
4088          * Try sending broadcast probe requests for the last three
4089          * probe requests after the first ones failed since some
4090          * buggy APs only support broadcast probe requests.
4091          */
4092         if (ifmgd->probe_send_count >= unicast_limit)
4093                 dst = NULL;
4094
4095         /*
4096          * When the hardware reports an accurate Tx ACK status, it's
4097          * better to send a nullfunc frame instead of a probe request,
4098          * as it will kick us off the AP quickly if we aren't associated
4099          * anymore. The timeout will be reset if the frame is ACKed by
4100          * the AP.
4101          */
4102         ifmgd->probe_send_count++;
4103
4104         if (dst) {
4105                 sta = sta_info_get(sdata, dst);
4106                 if (!WARN_ON(!sta))
4107                         ieee80211_check_fast_rx(sta);
4108         }
4109
4110         if (ieee80211_hw_check(&sdata->local->hw, REPORTS_TX_ACK_STATUS)) {
4111                 ifmgd->nullfunc_failed = false;
4112                 ieee80211_send_nullfunc(sdata->local, sdata, false);
4113         } else {
4114                 ieee80211_mlme_send_probe_req(sdata, sdata->vif.addr, dst,
4115                                               sdata->vif.cfg.ssid,
4116                                               sdata->vif.cfg.ssid_len,
4117                                               sdata->deflink.conf->bss->channel);
4118         }
4119
4120         ifmgd->probe_timeout = jiffies + msecs_to_jiffies(probe_wait_ms);
4121         run_again(sdata, ifmgd->probe_timeout);
4122 }
4123
4124 static void ieee80211_mgd_probe_ap(struct ieee80211_sub_if_data *sdata,
4125                                    bool beacon)
4126 {
4127         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4128         bool already = false;
4129
4130         lockdep_assert_wiphy(sdata->local->hw.wiphy);
4131
4132         if (WARN_ON_ONCE(ieee80211_vif_is_mld(&sdata->vif)))
4133                 return;
4134
4135         if (!ieee80211_sdata_running(sdata))
4136                 return;
4137
4138         if (!ifmgd->associated)
4139                 return;
4140
4141         if (sdata->local->tmp_channel || sdata->local->scanning)
4142                 return;
4143
4144         if (sdata->local->suspending) {
4145                 /* reschedule after resume */
4146                 ieee80211_reset_ap_probe(sdata);
4147                 return;
4148         }
4149
4150         if (beacon) {
4151                 mlme_dbg_ratelimited(sdata,
4152                                      "detected beacon loss from AP (missed %d beacons) - probing\n",
4153                                      beacon_loss_count);
4154
4155                 ieee80211_cqm_beacon_loss_notify(&sdata->vif, GFP_KERNEL);
4156         }
4157
4158         /*
4159          * The driver/our work has already reported this event or the
4160          * connection monitoring has kicked in and we have already sent
4161          * a probe request. Or maybe the AP died and the driver keeps
4162          * reporting until we disassociate...
4163          *
4164          * In either case we have to ignore the current call to this
4165          * function (except for setting the correct probe reason bit)
4166          * because otherwise we would reset the timer every time and
4167          * never check whether we received a probe response!
4168          */
4169         if (ifmgd->flags & IEEE80211_STA_CONNECTION_POLL)
4170                 already = true;
4171
4172         ifmgd->flags |= IEEE80211_STA_CONNECTION_POLL;
4173
4174         if (already)
4175                 return;
4176
4177         ieee80211_recalc_ps(sdata->local);
4178
4179         ifmgd->probe_send_count = 0;
4180         ieee80211_mgd_probe_ap_send(sdata);
4181 }
4182
4183 struct sk_buff *ieee80211_ap_probereq_get(struct ieee80211_hw *hw,
4184                                           struct ieee80211_vif *vif)
4185 {
4186         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
4187         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4188         struct cfg80211_bss *cbss;
4189         struct sk_buff *skb;
4190         const struct element *ssid;
4191         int ssid_len;
4192
4193         lockdep_assert_wiphy(sdata->local->hw.wiphy);
4194
4195         if (WARN_ON(sdata->vif.type != NL80211_IFTYPE_STATION ||
4196                     ieee80211_vif_is_mld(&sdata->vif)))
4197                 return NULL;
4198
4199         if (ifmgd->associated)
4200                 cbss = sdata->deflink.conf->bss;
4201         else if (ifmgd->auth_data)
4202                 cbss = ifmgd->auth_data->bss;
4203         else if (ifmgd->assoc_data && ifmgd->assoc_data->link[0].bss)
4204                 cbss = ifmgd->assoc_data->link[0].bss;
4205         else
4206                 return NULL;
4207
4208         rcu_read_lock();
4209         ssid = ieee80211_bss_get_elem(cbss, WLAN_EID_SSID);
4210         if (WARN_ONCE(!ssid || ssid->datalen > IEEE80211_MAX_SSID_LEN,
4211                       "invalid SSID element (len=%d)",
4212                       ssid ? ssid->datalen : -1))
4213                 ssid_len = 0;
4214         else
4215                 ssid_len = ssid->datalen;
4216
4217         skb = ieee80211_build_probe_req(sdata, sdata->vif.addr, cbss->bssid,
4218                                         (u32) -1, cbss->channel,
4219                                         ssid->data, ssid_len,
4220                                         NULL, 0, IEEE80211_PROBE_FLAG_DIRECTED);
4221         rcu_read_unlock();
4222
4223         return skb;
4224 }
4225 EXPORT_SYMBOL(ieee80211_ap_probereq_get);
4226
4227 static void ieee80211_report_disconnect(struct ieee80211_sub_if_data *sdata,
4228                                         const u8 *buf, size_t len, bool tx,
4229                                         u16 reason, bool reconnect)
4230 {
4231         struct ieee80211_event event = {
4232                 .type = MLME_EVENT,
4233                 .u.mlme.data = tx ? DEAUTH_TX_EVENT : DEAUTH_RX_EVENT,
4234                 .u.mlme.reason = reason,
4235         };
4236
4237         if (tx)
4238                 cfg80211_tx_mlme_mgmt(sdata->dev, buf, len, reconnect);
4239         else
4240                 cfg80211_rx_mlme_mgmt(sdata->dev, buf, len);
4241
4242         drv_event_callback(sdata->local, sdata, &event);
4243 }
4244
4245 static void __ieee80211_disconnect(struct ieee80211_sub_if_data *sdata)
4246 {
4247         struct ieee80211_local *local = sdata->local;
4248         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4249         u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
4250         bool tx = false;
4251
4252         lockdep_assert_wiphy(local->hw.wiphy);
4253
4254         if (!ifmgd->associated)
4255                 return;
4256
4257         /* only transmit if we have a link that makes that worthwhile */
4258         for (unsigned int link_id = 0;
4259              link_id < ARRAY_SIZE(sdata->link);
4260              link_id++) {
4261                 struct ieee80211_link_data *link;
4262
4263                 if (!ieee80211_vif_link_active(&sdata->vif, link_id))
4264                         continue;
4265
4266                 link = sdata_dereference(sdata->link[link_id], sdata);
4267                 if (WARN_ON_ONCE(!link))
4268                         continue;
4269
4270                 if (link->u.mgd.csa.blocked_tx)
4271                         continue;
4272
4273                 tx = true;
4274                 break;
4275         }
4276
4277         if (!ifmgd->driver_disconnect) {
4278                 unsigned int link_id;
4279
4280                 /*
4281                  * AP is probably out of range (or not reachable for another
4282                  * reason) so remove the bss structs for that AP. In the case
4283                  * of multi-link, it's not clear that all of them really are
4284                  * out of range, but if they weren't the driver likely would
4285                  * have switched to just have a single link active?
4286                  */
4287                 for (link_id = 0;
4288                      link_id < ARRAY_SIZE(sdata->link);
4289                      link_id++) {
4290                         struct ieee80211_link_data *link;
4291
4292                         link = sdata_dereference(sdata->link[link_id], sdata);
4293                         if (!link)
4294                                 continue;
4295                         cfg80211_unlink_bss(local->hw.wiphy, link->conf->bss);
4296                         link->conf->bss = NULL;
4297                 }
4298         }
4299
4300         ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH,
4301                                ifmgd->driver_disconnect ?
4302                                         WLAN_REASON_DEAUTH_LEAVING :
4303                                         WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY,
4304                                tx, frame_buf);
4305         /* the other links will be destroyed */
4306         sdata->vif.bss_conf.csa_active = false;
4307         sdata->deflink.u.mgd.csa.waiting_bcn = false;
4308         sdata->deflink.u.mgd.csa.blocked_tx = false;
4309         ieee80211_vif_unblock_queues_csa(sdata);
4310
4311         ieee80211_report_disconnect(sdata, frame_buf, sizeof(frame_buf), tx,
4312                                     WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY,
4313                                     ifmgd->reconnect);
4314         ifmgd->reconnect = false;
4315 }
4316
4317 static void ieee80211_beacon_connection_loss_work(struct wiphy *wiphy,
4318                                                   struct wiphy_work *work)
4319 {
4320         struct ieee80211_sub_if_data *sdata =
4321                 container_of(work, struct ieee80211_sub_if_data,
4322                              u.mgd.beacon_connection_loss_work);
4323         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4324
4325         if (ifmgd->connection_loss) {
4326                 sdata_info(sdata, "Connection to AP %pM lost\n",
4327                            sdata->vif.cfg.ap_addr);
4328                 __ieee80211_disconnect(sdata);
4329                 ifmgd->connection_loss = false;
4330         } else if (ifmgd->driver_disconnect) {
4331                 sdata_info(sdata,
4332                            "Driver requested disconnection from AP %pM\n",
4333                            sdata->vif.cfg.ap_addr);
4334                 __ieee80211_disconnect(sdata);
4335                 ifmgd->driver_disconnect = false;
4336         } else {
4337                 if (ifmgd->associated)
4338                         sdata->deflink.u.mgd.beacon_loss_count++;
4339                 ieee80211_mgd_probe_ap(sdata, true);
4340         }
4341 }
4342
4343 static void ieee80211_csa_connection_drop_work(struct wiphy *wiphy,
4344                                                struct wiphy_work *work)
4345 {
4346         struct ieee80211_sub_if_data *sdata =
4347                 container_of(work, struct ieee80211_sub_if_data,
4348                              u.mgd.csa_connection_drop_work);
4349
4350         __ieee80211_disconnect(sdata);
4351 }
4352
4353 void ieee80211_beacon_loss(struct ieee80211_vif *vif)
4354 {
4355         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
4356         struct ieee80211_hw *hw = &sdata->local->hw;
4357
4358         trace_api_beacon_loss(sdata);
4359
4360         sdata->u.mgd.connection_loss = false;
4361         wiphy_work_queue(hw->wiphy, &sdata->u.mgd.beacon_connection_loss_work);
4362 }
4363 EXPORT_SYMBOL(ieee80211_beacon_loss);
4364
4365 void ieee80211_connection_loss(struct ieee80211_vif *vif)
4366 {
4367         struct ieee80211_sub_if_data *sdata;
4368         struct ieee80211_hw *hw;
4369
4370         KUNIT_STATIC_STUB_REDIRECT(ieee80211_connection_loss, vif);
4371
4372         sdata = vif_to_sdata(vif);
4373         hw = &sdata->local->hw;
4374
4375         trace_api_connection_loss(sdata);
4376
4377         sdata->u.mgd.connection_loss = true;
4378         wiphy_work_queue(hw->wiphy, &sdata->u.mgd.beacon_connection_loss_work);
4379 }
4380 EXPORT_SYMBOL(ieee80211_connection_loss);
4381
4382 void ieee80211_disconnect(struct ieee80211_vif *vif, bool reconnect)
4383 {
4384         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
4385         struct ieee80211_hw *hw = &sdata->local->hw;
4386
4387         trace_api_disconnect(sdata, reconnect);
4388
4389         if (WARN_ON(sdata->vif.type != NL80211_IFTYPE_STATION))
4390                 return;
4391
4392         sdata->u.mgd.driver_disconnect = true;
4393         sdata->u.mgd.reconnect = reconnect;
4394         wiphy_work_queue(hw->wiphy, &sdata->u.mgd.beacon_connection_loss_work);
4395 }
4396 EXPORT_SYMBOL(ieee80211_disconnect);
4397
4398 static void ieee80211_destroy_auth_data(struct ieee80211_sub_if_data *sdata,
4399                                         bool assoc)
4400 {
4401         struct ieee80211_mgd_auth_data *auth_data = sdata->u.mgd.auth_data;
4402
4403         lockdep_assert_wiphy(sdata->local->hw.wiphy);
4404
4405         sdata->u.mgd.auth_data = NULL;
4406
4407         if (!assoc) {
4408                 /*
4409                  * we are not authenticated yet, the only timer that could be
4410                  * running is the timeout for the authentication response which
4411                  * which is not relevant anymore.
4412                  */
4413                 del_timer_sync(&sdata->u.mgd.timer);
4414                 sta_info_destroy_addr(sdata, auth_data->ap_addr);
4415
4416                 /* other links are destroyed */
4417                 eth_zero_addr(sdata->deflink.u.mgd.bssid);
4418                 ieee80211_link_info_change_notify(sdata, &sdata->deflink,
4419                                                   BSS_CHANGED_BSSID);
4420                 sdata->u.mgd.flags = 0;
4421
4422                 ieee80211_link_release_channel(&sdata->deflink);
4423                 ieee80211_vif_set_links(sdata, 0, 0);
4424         }
4425
4426         cfg80211_put_bss(sdata->local->hw.wiphy, auth_data->bss);
4427         kfree(auth_data);
4428 }
4429
4430 enum assoc_status {
4431         ASSOC_SUCCESS,
4432         ASSOC_REJECTED,
4433         ASSOC_TIMEOUT,
4434         ASSOC_ABANDON,
4435 };
4436
4437 static void ieee80211_destroy_assoc_data(struct ieee80211_sub_if_data *sdata,
4438                                          enum assoc_status status)
4439 {
4440         struct ieee80211_mgd_assoc_data *assoc_data = sdata->u.mgd.assoc_data;
4441
4442         lockdep_assert_wiphy(sdata->local->hw.wiphy);
4443
4444         sdata->u.mgd.assoc_data = NULL;
4445
4446         if (status != ASSOC_SUCCESS) {
4447                 /*
4448                  * we are not associated yet, the only timer that could be
4449                  * running is the timeout for the association response which
4450                  * which is not relevant anymore.
4451                  */
4452                 del_timer_sync(&sdata->u.mgd.timer);
4453                 sta_info_destroy_addr(sdata, assoc_data->ap_addr);
4454
4455                 eth_zero_addr(sdata->deflink.u.mgd.bssid);
4456                 ieee80211_link_info_change_notify(sdata, &sdata->deflink,
4457                                                   BSS_CHANGED_BSSID);
4458                 sdata->u.mgd.flags = 0;
4459                 sdata->vif.bss_conf.mu_mimo_owner = false;
4460
4461                 if (status != ASSOC_REJECTED) {
4462                         struct cfg80211_assoc_failure data = {
4463                                 .timeout = status == ASSOC_TIMEOUT,
4464                         };
4465                         int i;
4466
4467                         BUILD_BUG_ON(ARRAY_SIZE(data.bss) !=
4468                                      ARRAY_SIZE(assoc_data->link));
4469
4470                         for (i = 0; i < ARRAY_SIZE(data.bss); i++)
4471                                 data.bss[i] = assoc_data->link[i].bss;
4472
4473                         if (ieee80211_vif_is_mld(&sdata->vif))
4474                                 data.ap_mld_addr = assoc_data->ap_addr;
4475
4476                         cfg80211_assoc_failure(sdata->dev, &data);
4477                 }
4478
4479                 ieee80211_link_release_channel(&sdata->deflink);
4480                 ieee80211_vif_set_links(sdata, 0, 0);
4481         }
4482
4483         kfree(assoc_data);
4484 }
4485
4486 static void ieee80211_auth_challenge(struct ieee80211_sub_if_data *sdata,
4487                                      struct ieee80211_mgmt *mgmt, size_t len)
4488 {
4489         struct ieee80211_local *local = sdata->local;
4490         struct ieee80211_mgd_auth_data *auth_data = sdata->u.mgd.auth_data;
4491         const struct element *challenge;
4492         u8 *pos;
4493         u32 tx_flags = 0;
4494         struct ieee80211_prep_tx_info info = {
4495                 .subtype = IEEE80211_STYPE_AUTH,
4496                 .link_id = auth_data->link_id,
4497         };
4498
4499         pos = mgmt->u.auth.variable;
4500         challenge = cfg80211_find_elem(WLAN_EID_CHALLENGE, pos,
4501                                        len - (pos - (u8 *)mgmt));
4502         if (!challenge)
4503                 return;
4504         auth_data->expected_transaction = 4;
4505         drv_mgd_prepare_tx(sdata->local, sdata, &info);
4506         if (ieee80211_hw_check(&local->hw, REPORTS_TX_ACK_STATUS))
4507                 tx_flags = IEEE80211_TX_CTL_REQ_TX_STATUS |
4508                            IEEE80211_TX_INTFL_MLME_CONN_TX;
4509         ieee80211_send_auth(sdata, 3, auth_data->algorithm, 0,
4510                             (void *)challenge,
4511                             challenge->datalen + sizeof(*challenge),
4512                             auth_data->ap_addr, auth_data->ap_addr,
4513                             auth_data->key, auth_data->key_len,
4514                             auth_data->key_idx, tx_flags);
4515 }
4516
4517 static bool ieee80211_mark_sta_auth(struct ieee80211_sub_if_data *sdata)
4518 {
4519         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4520         const u8 *ap_addr = ifmgd->auth_data->ap_addr;
4521         struct sta_info *sta;
4522
4523         lockdep_assert_wiphy(sdata->local->hw.wiphy);
4524
4525         sdata_info(sdata, "authenticated\n");
4526         ifmgd->auth_data->done = true;
4527         ifmgd->auth_data->timeout = jiffies + IEEE80211_AUTH_WAIT_ASSOC;
4528         ifmgd->auth_data->timeout_started = true;
4529         run_again(sdata, ifmgd->auth_data->timeout);
4530
4531         /* move station state to auth */
4532         sta = sta_info_get(sdata, ap_addr);
4533         if (!sta) {
4534                 WARN_ONCE(1, "%s: STA %pM not found", sdata->name, ap_addr);
4535                 return false;
4536         }
4537         if (sta_info_move_state(sta, IEEE80211_STA_AUTH)) {
4538                 sdata_info(sdata, "failed moving %pM to auth\n", ap_addr);
4539                 return false;
4540         }
4541
4542         return true;
4543 }
4544
4545 static void ieee80211_rx_mgmt_auth(struct ieee80211_sub_if_data *sdata,
4546                                    struct ieee80211_mgmt *mgmt, size_t len)
4547 {
4548         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4549         u16 auth_alg, auth_transaction, status_code;
4550         struct ieee80211_event event = {
4551                 .type = MLME_EVENT,
4552                 .u.mlme.data = AUTH_EVENT,
4553         };
4554         struct ieee80211_prep_tx_info info = {
4555                 .subtype = IEEE80211_STYPE_AUTH,
4556         };
4557
4558         lockdep_assert_wiphy(sdata->local->hw.wiphy);
4559
4560         if (len < 24 + 6)
4561                 return;
4562
4563         if (!ifmgd->auth_data || ifmgd->auth_data->done)
4564                 return;
4565
4566         if (!ether_addr_equal(ifmgd->auth_data->ap_addr, mgmt->bssid))
4567                 return;
4568
4569         auth_alg = le16_to_cpu(mgmt->u.auth.auth_alg);
4570         auth_transaction = le16_to_cpu(mgmt->u.auth.auth_transaction);
4571         status_code = le16_to_cpu(mgmt->u.auth.status_code);
4572
4573         if (auth_alg != ifmgd->auth_data->algorithm ||
4574             (auth_alg != WLAN_AUTH_SAE &&
4575              auth_transaction != ifmgd->auth_data->expected_transaction) ||
4576             (auth_alg == WLAN_AUTH_SAE &&
4577              (auth_transaction < ifmgd->auth_data->expected_transaction ||
4578               auth_transaction > 2))) {
4579                 sdata_info(sdata, "%pM unexpected authentication state: alg %d (expected %d) transact %d (expected %d)\n",
4580                            mgmt->sa, auth_alg, ifmgd->auth_data->algorithm,
4581                            auth_transaction,
4582                            ifmgd->auth_data->expected_transaction);
4583                 goto notify_driver;
4584         }
4585
4586         if (status_code != WLAN_STATUS_SUCCESS) {
4587                 cfg80211_rx_mlme_mgmt(sdata->dev, (u8 *)mgmt, len);
4588
4589                 if (auth_alg == WLAN_AUTH_SAE &&
4590                     (status_code == WLAN_STATUS_ANTI_CLOG_REQUIRED ||
4591                      (auth_transaction == 1 &&
4592                       (status_code == WLAN_STATUS_SAE_HASH_TO_ELEMENT ||
4593                        status_code == WLAN_STATUS_SAE_PK)))) {
4594                         /* waiting for userspace now */
4595                         ifmgd->auth_data->waiting = true;
4596                         ifmgd->auth_data->timeout =
4597                                 jiffies + IEEE80211_AUTH_WAIT_SAE_RETRY;
4598                         ifmgd->auth_data->timeout_started = true;
4599                         run_again(sdata, ifmgd->auth_data->timeout);
4600                         goto notify_driver;
4601                 }
4602
4603                 sdata_info(sdata, "%pM denied authentication (status %d)\n",
4604                            mgmt->sa, status_code);
4605                 ieee80211_destroy_auth_data(sdata, false);
4606                 event.u.mlme.status = MLME_DENIED;
4607                 event.u.mlme.reason = status_code;
4608                 drv_event_callback(sdata->local, sdata, &event);
4609                 goto notify_driver;
4610         }
4611
4612         switch (ifmgd->auth_data->algorithm) {
4613         case WLAN_AUTH_OPEN:
4614         case WLAN_AUTH_LEAP:
4615         case WLAN_AUTH_FT:
4616         case WLAN_AUTH_SAE:
4617         case WLAN_AUTH_FILS_SK:
4618         case WLAN_AUTH_FILS_SK_PFS:
4619         case WLAN_AUTH_FILS_PK:
4620                 break;
4621         case WLAN_AUTH_SHARED_KEY:
4622                 if (ifmgd->auth_data->expected_transaction != 4) {
4623                         ieee80211_auth_challenge(sdata, mgmt, len);
4624                         /* need another frame */
4625                         return;
4626                 }
4627                 break;
4628         default:
4629                 WARN_ONCE(1, "invalid auth alg %d",
4630                           ifmgd->auth_data->algorithm);
4631                 goto notify_driver;
4632         }
4633
4634         event.u.mlme.status = MLME_SUCCESS;
4635         info.success = 1;
4636         drv_event_callback(sdata->local, sdata, &event);
4637         if (ifmgd->auth_data->algorithm != WLAN_AUTH_SAE ||
4638             (auth_transaction == 2 &&
4639              ifmgd->auth_data->expected_transaction == 2)) {
4640                 if (!ieee80211_mark_sta_auth(sdata))
4641                         return; /* ignore frame -- wait for timeout */
4642         } else if (ifmgd->auth_data->algorithm == WLAN_AUTH_SAE &&
4643                    auth_transaction == 2) {
4644                 sdata_info(sdata, "SAE peer confirmed\n");
4645                 ifmgd->auth_data->peer_confirmed = true;
4646         }
4647
4648         cfg80211_rx_mlme_mgmt(sdata->dev, (u8 *)mgmt, len);
4649 notify_driver:
4650         drv_mgd_complete_tx(sdata->local, sdata, &info);
4651 }
4652
4653 #define case_WLAN(type) \
4654         case WLAN_REASON_##type: return #type
4655
4656 const char *ieee80211_get_reason_code_string(u16 reason_code)
4657 {
4658         switch (reason_code) {
4659         case_WLAN(UNSPECIFIED);
4660         case_WLAN(PREV_AUTH_NOT_VALID);
4661         case_WLAN(DEAUTH_LEAVING);
4662         case_WLAN(DISASSOC_DUE_TO_INACTIVITY);
4663         case_WLAN(DISASSOC_AP_BUSY);
4664         case_WLAN(CLASS2_FRAME_FROM_NONAUTH_STA);
4665         case_WLAN(CLASS3_FRAME_FROM_NONASSOC_STA);
4666         case_WLAN(DISASSOC_STA_HAS_LEFT);
4667         case_WLAN(STA_REQ_ASSOC_WITHOUT_AUTH);
4668         case_WLAN(DISASSOC_BAD_POWER);
4669         case_WLAN(DISASSOC_BAD_SUPP_CHAN);
4670         case_WLAN(INVALID_IE);
4671         case_WLAN(MIC_FAILURE);
4672         case_WLAN(4WAY_HANDSHAKE_TIMEOUT);
4673         case_WLAN(GROUP_KEY_HANDSHAKE_TIMEOUT);
4674         case_WLAN(IE_DIFFERENT);
4675         case_WLAN(INVALID_GROUP_CIPHER);
4676         case_WLAN(INVALID_PAIRWISE_CIPHER);
4677         case_WLAN(INVALID_AKMP);
4678         case_WLAN(UNSUPP_RSN_VERSION);
4679         case_WLAN(INVALID_RSN_IE_CAP);
4680         case_WLAN(IEEE8021X_FAILED);
4681         case_WLAN(CIPHER_SUITE_REJECTED);
4682         case_WLAN(DISASSOC_UNSPECIFIED_QOS);
4683         case_WLAN(DISASSOC_QAP_NO_BANDWIDTH);
4684         case_WLAN(DISASSOC_LOW_ACK);
4685         case_WLAN(DISASSOC_QAP_EXCEED_TXOP);
4686         case_WLAN(QSTA_LEAVE_QBSS);
4687         case_WLAN(QSTA_NOT_USE);
4688         case_WLAN(QSTA_REQUIRE_SETUP);
4689         case_WLAN(QSTA_TIMEOUT);
4690         case_WLAN(QSTA_CIPHER_NOT_SUPP);
4691         case_WLAN(MESH_PEER_CANCELED);
4692         case_WLAN(MESH_MAX_PEERS);
4693         case_WLAN(MESH_CONFIG);
4694         case_WLAN(MESH_CLOSE);
4695         case_WLAN(MESH_MAX_RETRIES);
4696         case_WLAN(MESH_CONFIRM_TIMEOUT);
4697         case_WLAN(MESH_INVALID_GTK);
4698         case_WLAN(MESH_INCONSISTENT_PARAM);
4699         case_WLAN(MESH_INVALID_SECURITY);
4700         case_WLAN(MESH_PATH_ERROR);
4701         case_WLAN(MESH_PATH_NOFORWARD);
4702         case_WLAN(MESH_PATH_DEST_UNREACHABLE);
4703         case_WLAN(MAC_EXISTS_IN_MBSS);
4704         case_WLAN(MESH_CHAN_REGULATORY);
4705         case_WLAN(MESH_CHAN);
4706         default: return "<unknown>";
4707         }
4708 }
4709
4710 static void ieee80211_rx_mgmt_deauth(struct ieee80211_sub_if_data *sdata,
4711                                      struct ieee80211_mgmt *mgmt, size_t len)
4712 {
4713         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4714         u16 reason_code = le16_to_cpu(mgmt->u.deauth.reason_code);
4715
4716         lockdep_assert_wiphy(sdata->local->hw.wiphy);
4717
4718         if (len < 24 + 2)
4719                 return;
4720
4721         if (!ether_addr_equal(mgmt->bssid, mgmt->sa)) {
4722                 ieee80211_tdls_handle_disconnect(sdata, mgmt->sa, reason_code);
4723                 return;
4724         }
4725
4726         if (ifmgd->associated &&
4727             ether_addr_equal(mgmt->bssid, sdata->vif.cfg.ap_addr)) {
4728                 sdata_info(sdata, "deauthenticated from %pM (Reason: %u=%s)\n",
4729                            sdata->vif.cfg.ap_addr, reason_code,
4730                            ieee80211_get_reason_code_string(reason_code));
4731
4732                 ieee80211_set_disassoc(sdata, 0, 0, false, NULL);
4733
4734                 ieee80211_report_disconnect(sdata, (u8 *)mgmt, len, false,
4735                                             reason_code, false);
4736                 return;
4737         }
4738
4739         if (ifmgd->assoc_data &&
4740             ether_addr_equal(mgmt->bssid, ifmgd->assoc_data->ap_addr)) {
4741                 sdata_info(sdata,
4742                            "deauthenticated from %pM while associating (Reason: %u=%s)\n",
4743                            ifmgd->assoc_data->ap_addr, reason_code,
4744                            ieee80211_get_reason_code_string(reason_code));
4745
4746                 ieee80211_destroy_assoc_data(sdata, ASSOC_ABANDON);
4747
4748                 cfg80211_rx_mlme_mgmt(sdata->dev, (u8 *)mgmt, len);
4749                 return;
4750         }
4751 }
4752
4753
4754 static void ieee80211_rx_mgmt_disassoc(struct ieee80211_sub_if_data *sdata,
4755                                        struct ieee80211_mgmt *mgmt, size_t len)
4756 {
4757         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
4758         u16 reason_code;
4759
4760         lockdep_assert_wiphy(sdata->local->hw.wiphy);
4761
4762         if (len < 24 + 2)
4763                 return;
4764
4765         if (!ifmgd->associated ||
4766             !ether_addr_equal(mgmt->bssid, sdata->vif.cfg.ap_addr))
4767                 return;
4768
4769         reason_code = le16_to_cpu(mgmt->u.disassoc.reason_code);
4770
4771         if (!ether_addr_equal(mgmt->bssid, mgmt->sa)) {
4772                 ieee80211_tdls_handle_disconnect(sdata, mgmt->sa, reason_code);
4773                 return;
4774         }
4775
4776         sdata_info(sdata, "disassociated from %pM (Reason: %u=%s)\n",
4777                    sdata->vif.cfg.ap_addr, reason_code,
4778                    ieee80211_get_reason_code_string(reason_code));
4779
4780         ieee80211_set_disassoc(sdata, 0, 0, false, NULL);
4781
4782         ieee80211_report_disconnect(sdata, (u8 *)mgmt, len, false, reason_code,
4783                                     false);
4784 }
4785
4786 static bool ieee80211_twt_req_supported(struct ieee80211_sub_if_data *sdata,
4787                                         struct ieee80211_supported_band *sband,
4788                                         const struct link_sta_info *link_sta,
4789                                         const struct ieee802_11_elems *elems)
4790 {
4791         const struct ieee80211_sta_he_cap *own_he_cap =
4792                 ieee80211_get_he_iftype_cap_vif(sband, &sdata->vif);
4793
4794         if (elems->ext_capab_len < 10)
4795                 return false;
4796
4797         if (!(elems->ext_capab[9] & WLAN_EXT_CAPA10_TWT_RESPONDER_SUPPORT))
4798                 return false;
4799
4800         return link_sta->pub->he_cap.he_cap_elem.mac_cap_info[0] &
4801                 IEEE80211_HE_MAC_CAP0_TWT_RES &&
4802                 own_he_cap &&
4803                 (own_he_cap->he_cap_elem.mac_cap_info[0] &
4804                         IEEE80211_HE_MAC_CAP0_TWT_REQ);
4805 }
4806
4807 static u64 ieee80211_recalc_twt_req(struct ieee80211_sub_if_data *sdata,
4808                                     struct ieee80211_supported_band *sband,
4809                                     struct ieee80211_link_data *link,
4810                                     struct link_sta_info *link_sta,
4811                                     struct ieee802_11_elems *elems)
4812 {
4813         bool twt = ieee80211_twt_req_supported(sdata, sband, link_sta, elems);
4814
4815         if (link->conf->twt_requester != twt) {
4816                 link->conf->twt_requester = twt;
4817                 return BSS_CHANGED_TWT;
4818         }
4819         return 0;
4820 }
4821
4822 static bool ieee80211_twt_bcast_support(struct ieee80211_sub_if_data *sdata,
4823                                         struct ieee80211_bss_conf *bss_conf,
4824                                         struct ieee80211_supported_band *sband,
4825                                         struct link_sta_info *link_sta)
4826 {
4827         const struct ieee80211_sta_he_cap *own_he_cap =
4828                 ieee80211_get_he_iftype_cap_vif(sband, &sdata->vif);
4829
4830         return bss_conf->he_support &&
4831                 (link_sta->pub->he_cap.he_cap_elem.mac_cap_info[2] &
4832                         IEEE80211_HE_MAC_CAP2_BCAST_TWT) &&
4833                 own_he_cap &&
4834                 (own_he_cap->he_cap_elem.mac_cap_info[2] &
4835                         IEEE80211_HE_MAC_CAP2_BCAST_TWT);
4836 }
4837
4838 static bool ieee80211_assoc_config_link(struct ieee80211_link_data *link,
4839                                         struct link_sta_info *link_sta,
4840                                         struct cfg80211_bss *cbss,
4841                                         struct ieee80211_mgmt *mgmt,
4842                                         const u8 *elem_start,
4843                                         unsigned int elem_len,
4844                                         u64 *changed)
4845 {
4846         struct ieee80211_sub_if_data *sdata = link->sdata;
4847         struct ieee80211_mgd_assoc_data *assoc_data =
4848                 sdata->u.mgd.assoc_data ?: sdata->u.mgd.reconf.add_links_data;
4849         struct ieee80211_bss_conf *bss_conf = link->conf;
4850         struct ieee80211_local *local = sdata->local;
4851         unsigned int link_id = link->link_id;
4852         struct ieee80211_elems_parse_params parse_params = {
4853                 .mode = link->u.mgd.conn.mode,
4854                 .start = elem_start,
4855                 .len = elem_len,
4856                 .link_id = link_id == assoc_data->assoc_link_id ? -1 : link_id,
4857                 .from_ap = true,
4858         };
4859         bool is_5ghz = cbss->channel->band == NL80211_BAND_5GHZ;
4860         bool is_6ghz = cbss->channel->band == NL80211_BAND_6GHZ;
4861         bool is_s1g = cbss->channel->band == NL80211_BAND_S1GHZ;
4862         const struct cfg80211_bss_ies *bss_ies = NULL;
4863         struct ieee80211_supported_band *sband;
4864         struct ieee802_11_elems *elems;
4865         const __le16 prof_bss_param_ch_present =
4866                 cpu_to_le16(IEEE80211_MLE_STA_CONTROL_BSS_PARAM_CHANGE_CNT_PRESENT);
4867         u16 capab_info;
4868         bool ret;
4869
4870         elems = ieee802_11_parse_elems_full(&parse_params);
4871         if (!elems)
4872                 return false;
4873
4874         if (link_id == assoc_data->assoc_link_id) {
4875                 capab_info = le16_to_cpu(mgmt->u.assoc_resp.capab_info);
4876
4877                 /*
4878                  * we should not get to this flow unless the association was
4879                  * successful, so set the status directly to success
4880                  */
4881                 assoc_data->link[link_id].status = WLAN_STATUS_SUCCESS;
4882                 if (elems->ml_basic) {
4883                         int bss_param_ch_cnt =
4884                                 ieee80211_mle_get_bss_param_ch_cnt((const void *)elems->ml_basic);
4885
4886                         if (bss_param_ch_cnt < 0) {
4887                                 ret = false;
4888                                 goto out;
4889                         }
4890                         bss_conf->bss_param_ch_cnt = bss_param_ch_cnt;
4891                         bss_conf->bss_param_ch_cnt_link_id = link_id;
4892                 }
4893         } else if (elems->parse_error & IEEE80211_PARSE_ERR_DUP_NEST_ML_BASIC ||
4894                    !elems->prof ||
4895                    !(elems->prof->control & prof_bss_param_ch_present)) {
4896                 ret = false;
4897                 goto out;
4898         } else {
4899                 const u8 *ptr = elems->prof->variable +
4900                                 elems->prof->sta_info_len - 1;
4901                 int bss_param_ch_cnt;
4902
4903                 /*
4904                  * During parsing, we validated that these fields exist,
4905                  * otherwise elems->prof would have been set to NULL.
4906                  */
4907                 capab_info = get_unaligned_le16(ptr);
4908                 assoc_data->link[link_id].status = get_unaligned_le16(ptr + 2);
4909                 bss_param_ch_cnt =
4910                         ieee80211_mle_basic_sta_prof_bss_param_ch_cnt(elems->prof);
4911                 bss_conf->bss_param_ch_cnt = bss_param_ch_cnt;
4912                 bss_conf->bss_param_ch_cnt_link_id = link_id;
4913
4914                 if (assoc_data->link[link_id].status != WLAN_STATUS_SUCCESS) {
4915                         link_info(link, "association response status code=%u\n",
4916                                   assoc_data->link[link_id].status);
4917                         ret = true;
4918                         goto out;
4919                 }
4920         }
4921
4922         if (!is_s1g && !elems->supp_rates) {
4923                 sdata_info(sdata, "no SuppRates element in AssocResp\n");
4924                 ret = false;
4925                 goto out;
4926         }
4927
4928         link->u.mgd.tdls_chan_switch_prohibited =
4929                 elems->ext_capab && elems->ext_capab_len >= 5 &&
4930                 (elems->ext_capab[4] & WLAN_EXT_CAPA5_TDLS_CH_SW_PROHIBITED);
4931
4932         /*
4933          * Some APs are erroneously not including some information in their
4934          * (re)association response frames. Try to recover by using the data
4935          * from the beacon or probe response. This seems to afflict mobile
4936          * 2G/3G/4G wifi routers, reported models include the "Onda PN51T",
4937          * "Vodafone PocketWiFi 2", "ZTE MF60" and a similar T-Mobile device.
4938          */
4939         if (!is_6ghz &&
4940             ((assoc_data->wmm && !elems->wmm_param) ||
4941              (link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_HT &&
4942               (!elems->ht_cap_elem || !elems->ht_operation)) ||
4943              (is_5ghz && link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_VHT &&
4944               (!elems->vht_cap_elem || !elems->vht_operation)))) {
4945                 const struct cfg80211_bss_ies *ies;
4946                 struct ieee802_11_elems *bss_elems;
4947
4948                 rcu_read_lock();
4949                 ies = rcu_dereference(cbss->ies);
4950                 if (ies)
4951                         bss_ies = kmemdup(ies, sizeof(*ies) + ies->len,
4952                                           GFP_ATOMIC);
4953                 rcu_read_unlock();
4954                 if (!bss_ies) {
4955                         ret = false;
4956                         goto out;
4957                 }
4958
4959                 parse_params.start = bss_ies->data;
4960                 parse_params.len = bss_ies->len;
4961                 parse_params.bss = cbss;
4962                 bss_elems = ieee802_11_parse_elems_full(&parse_params);
4963                 if (!bss_elems) {
4964                         ret = false;
4965                         goto out;
4966                 }
4967
4968                 if (assoc_data->wmm &&
4969                     !elems->wmm_param && bss_elems->wmm_param) {
4970                         elems->wmm_param = bss_elems->wmm_param;
4971                         sdata_info(sdata,
4972                                    "AP bug: WMM param missing from AssocResp\n");
4973                 }
4974
4975                 /*
4976                  * Also check if we requested HT/VHT, otherwise the AP doesn't
4977                  * have to include the IEs in the (re)association response.
4978                  */
4979                 if (!elems->ht_cap_elem && bss_elems->ht_cap_elem &&
4980                     link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_HT) {
4981                         elems->ht_cap_elem = bss_elems->ht_cap_elem;
4982                         sdata_info(sdata,
4983                                    "AP bug: HT capability missing from AssocResp\n");
4984                 }
4985                 if (!elems->ht_operation && bss_elems->ht_operation &&
4986                     link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_HT) {
4987                         elems->ht_operation = bss_elems->ht_operation;
4988                         sdata_info(sdata,
4989                                    "AP bug: HT operation missing from AssocResp\n");
4990                 }
4991
4992                 if (is_5ghz) {
4993                         if (!elems->vht_cap_elem && bss_elems->vht_cap_elem &&
4994                             link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_VHT) {
4995                                 elems->vht_cap_elem = bss_elems->vht_cap_elem;
4996                                 sdata_info(sdata,
4997                                            "AP bug: VHT capa missing from AssocResp\n");
4998                         }
4999
5000                         if (!elems->vht_operation && bss_elems->vht_operation &&
5001                             link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_VHT) {
5002                                 elems->vht_operation = bss_elems->vht_operation;
5003                                 sdata_info(sdata,
5004                                            "AP bug: VHT operation missing from AssocResp\n");
5005                         }
5006                 }
5007                 kfree(bss_elems);
5008         }
5009
5010         /*
5011          * We previously checked these in the beacon/probe response, so
5012          * they should be present here. This is just a safety net.
5013          * Note that the ieee80211_config_bw() below would also check
5014          * for this (and more), but this has better error reporting.
5015          */
5016         if (!is_6ghz && link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_HT &&
5017             (!elems->wmm_param || !elems->ht_cap_elem || !elems->ht_operation)) {
5018                 sdata_info(sdata,
5019                            "HT AP is missing WMM params or HT capability/operation\n");
5020                 ret = false;
5021                 goto out;
5022         }
5023
5024         if (is_5ghz && link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_VHT &&
5025             (!elems->vht_cap_elem || !elems->vht_operation)) {
5026                 sdata_info(sdata,
5027                            "VHT AP is missing VHT capability/operation\n");
5028                 ret = false;
5029                 goto out;
5030         }
5031
5032         /* check/update if AP changed anything in assoc response vs. scan */
5033         if (ieee80211_config_bw(link, elems,
5034                                 link_id == assoc_data->assoc_link_id,
5035                                 changed, "assoc response")) {
5036                 ret = false;
5037                 goto out;
5038         }
5039
5040         if (WARN_ON(!link->conf->chanreq.oper.chan)) {
5041                 ret = false;
5042                 goto out;
5043         }
5044         sband = local->hw.wiphy->bands[link->conf->chanreq.oper.chan->band];
5045
5046         /* Set up internal HT/VHT capabilities */
5047         if (elems->ht_cap_elem && link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_HT)
5048                 ieee80211_ht_cap_ie_to_sta_ht_cap(sdata, sband,
5049                                                   elems->ht_cap_elem,
5050                                                   link_sta);
5051
5052         if (elems->vht_cap_elem &&
5053             link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_VHT) {
5054                 const struct ieee80211_vht_cap *bss_vht_cap = NULL;
5055                 const struct cfg80211_bss_ies *ies;
5056
5057                 /*
5058                  * Cisco AP module 9115 with FW 17.3 has a bug and sends a
5059                  * too large maximum MPDU length in the association response
5060                  * (indicating 12k) that it cannot actually process ...
5061                  * Work around that.
5062                  */
5063                 rcu_read_lock();
5064                 ies = rcu_dereference(cbss->ies);
5065                 if (ies) {
5066                         const struct element *elem;
5067
5068                         elem = cfg80211_find_elem(WLAN_EID_VHT_CAPABILITY,
5069                                                   ies->data, ies->len);
5070                         if (elem && elem->datalen >= sizeof(*bss_vht_cap))
5071                                 bss_vht_cap = (const void *)elem->data;
5072                 }
5073
5074                 ieee80211_vht_cap_ie_to_sta_vht_cap(sdata, sband,
5075                                                     elems->vht_cap_elem,
5076                                                     bss_vht_cap, link_sta);
5077                 rcu_read_unlock();
5078         }
5079
5080         if (elems->he_operation &&
5081             link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_HE &&
5082             elems->he_cap) {
5083                 ieee80211_he_cap_ie_to_sta_he_cap(sdata, sband,
5084                                                   elems->he_cap,
5085                                                   elems->he_cap_len,
5086                                                   elems->he_6ghz_capa,
5087                                                   link_sta);
5088
5089                 bss_conf->he_support = link_sta->pub->he_cap.has_he;
5090                 if (elems->rsnx && elems->rsnx_len &&
5091                     (elems->rsnx[0] & WLAN_RSNX_CAPA_PROTECTED_TWT) &&
5092                     wiphy_ext_feature_isset(local->hw.wiphy,
5093                                             NL80211_EXT_FEATURE_PROTECTED_TWT))
5094                         bss_conf->twt_protected = true;
5095                 else
5096                         bss_conf->twt_protected = false;
5097
5098                 *changed |= ieee80211_recalc_twt_req(sdata, sband, link,
5099                                                      link_sta, elems);
5100
5101                 if (elems->eht_operation && elems->eht_cap &&
5102                     link->u.mgd.conn.mode >= IEEE80211_CONN_MODE_EHT) {
5103                         ieee80211_eht_cap_ie_to_sta_eht_cap(sdata, sband,
5104                                                             elems->he_cap,
5105                                                             elems->he_cap_len,
5106                                                             elems->eht_cap,
5107                                                             elems->eht_cap_len,
5108                                                             link_sta);
5109
5110                         bss_conf->eht_support = link_sta->pub->eht_cap.has_eht;
5111                 } else {
5112                         bss_conf->eht_support = false;
5113                 }
5114         } else {
5115                 bss_conf->he_support = false;
5116                 bss_conf->twt_requester = false;
5117                 bss_conf->twt_protected = false;
5118                 bss_conf->eht_support = false;
5119         }
5120
5121         bss_conf->twt_broadcast =
5122                 ieee80211_twt_bcast_support(sdata, bss_conf, sband, link_sta);
5123
5124         if (bss_conf->he_support) {
5125                 bss_conf->he_bss_color.color =
5126                         le32_get_bits(elems->he_operation->he_oper_params,
5127                                       IEEE80211_HE_OPERATION_BSS_COLOR_MASK);
5128                 bss_conf->he_bss_color.partial =
5129                         le32_get_bits(elems->he_operation->he_oper_params,
5130                                       IEEE80211_HE_OPERATION_PARTIAL_BSS_COLOR);
5131                 bss_conf->he_bss_color.enabled =
5132                         !le32_get_bits(elems->he_operation->he_oper_params,
5133                                        IEEE80211_HE_OPERATION_BSS_COLOR_DISABLED);
5134
5135                 if (bss_conf->he_bss_color.enabled)
5136                         *changed |= BSS_CHANGED_HE_BSS_COLOR;
5137
5138                 bss_conf->htc_trig_based_pkt_ext =
5139                         le32_get_bits(elems->he_operation->he_oper_params,
5140                                       IEEE80211_HE_OPERATION_DFLT_PE_DURATION_MASK);
5141                 bss_conf->frame_time_rts_th =
5142                         le32_get_bits(elems->he_operation->he_oper_params,
5143                                       IEEE80211_HE_OPERATION_RTS_THRESHOLD_MASK);
5144
5145                 bss_conf->uora_exists = !!elems->uora_element;
5146                 if (elems->uora_element)
5147                         bss_conf->uora_ocw_range = elems->uora_element[0];
5148
5149                 ieee80211_he_op_ie_to_bss_conf(&sdata->vif, elems->he_operation);
5150                 ieee80211_he_spr_ie_to_bss_conf(&sdata->vif, elems->he_spr);
5151                 /* TODO: OPEN: what happens if BSS color disable is set? */
5152         }
5153
5154         if (cbss->transmitted_bss) {
5155                 bss_conf->nontransmitted = true;
5156                 ether_addr_copy(bss_conf->transmitter_bssid,
5157                                 cbss->transmitted_bss->bssid);
5158                 bss_conf->bssid_indicator = cbss->max_bssid_indicator;
5159                 bss_conf->bssid_index = cbss->bssid_index;
5160         }
5161
5162         /*
5163          * Some APs, e.g. Netgear WNDR3700, report invalid HT operation data
5164          * in their association response, so ignore that data for our own
5165          * configuration. If it changed since the last beacon, we'll get the
5166          * next beacon and update then.
5167          */
5168
5169         /*
5170          * If an operating mode notification IE is present, override the
5171          * NSS calculation (that would be done in rate_control_rate_init())
5172          * and use the # of streams from that element.
5173          */
5174         if (elems->opmode_notif &&
5175             !(*elems->opmode_notif & IEEE80211_OPMODE_NOTIF_RX_NSS_TYPE_BF)) {
5176                 u8 nss;
5177
5178                 nss = *elems->opmode_notif & IEEE80211_OPMODE_NOTIF_RX_NSS_MASK;
5179                 nss >>= IEEE80211_OPMODE_NOTIF_RX_NSS_SHIFT;
5180                 nss += 1;
5181                 link_sta->pub->rx_nss = nss;
5182         }
5183
5184         /*
5185          * Always handle WMM once after association regardless
5186          * of the first value the AP uses. Setting -1 here has
5187          * that effect because the AP values is an unsigned
5188          * 4-bit value.
5189          */
5190         link->u.mgd.wmm_last_param_set = -1;
5191         link->u.mgd.mu_edca_last_param_set = -1;
5192
5193         if (link->u.mgd.disable_wmm_tracking) {
5194                 ieee80211_set_wmm_default(link, false, false);
5195         } else if (!ieee80211_sta_wmm_params(local, link, elems->wmm_param,
5196                                              elems->wmm_param_len,
5197                                              elems->mu_edca_param_set)) {
5198                 /* still enable QoS since we might have HT/VHT */
5199                 ieee80211_set_wmm_default(link, false, true);
5200                 /* disable WMM tracking in this case to disable
5201                  * tracking WMM parameter changes in the beacon if
5202                  * the parameters weren't actually valid. Doing so
5203                  * avoids changing parameters very strangely when
5204                  * the AP is going back and forth between valid and
5205                  * invalid parameters.
5206                  */
5207                 link->u.mgd.disable_wmm_tracking = true;
5208         }
5209
5210         if (elems->max_idle_period_ie) {
5211                 bss_conf->max_idle_period =
5212                         le16_to_cpu(elems->max_idle_period_ie->max_idle_period);
5213                 bss_conf->protected_keep_alive =
5214                         !!(elems->max_idle_period_ie->idle_options &
5215                            WLAN_IDLE_OPTIONS_PROTECTED_KEEP_ALIVE);
5216                 *changed |= BSS_CHANGED_KEEP_ALIVE;
5217         } else {
5218                 bss_conf->max_idle_period = 0;
5219                 bss_conf->protected_keep_alive = false;
5220         }
5221
5222         /* set assoc capability (AID was already set earlier),
5223          * ieee80211_set_associated() will tell the driver */
5224         bss_conf->assoc_capability = capab_info;
5225
5226         ret = true;
5227 out:
5228         kfree(elems);
5229         kfree(bss_ies);
5230         return ret;
5231 }
5232
5233 static int ieee80211_mgd_setup_link_sta(struct ieee80211_link_data *link,
5234                                         struct sta_info *sta,
5235                                         struct link_sta_info *link_sta,
5236                                         struct cfg80211_bss *cbss)
5237 {
5238         struct ieee80211_sub_if_data *sdata = link->sdata;
5239         struct ieee80211_local *local = sdata->local;
5240         struct ieee80211_bss *bss = (void *)cbss->priv;
5241         u32 rates = 0, basic_rates = 0;
5242         bool have_higher_than_11mbit = false;
5243         int min_rate = INT_MAX, min_rate_index = -1;
5244         struct ieee80211_supported_band *sband;
5245
5246         memcpy(link_sta->addr, cbss->bssid, ETH_ALEN);
5247         memcpy(link_sta->pub->addr, cbss->bssid, ETH_ALEN);
5248
5249         /* TODO: S1G Basic Rate Set is expressed elsewhere */
5250         if (cbss->channel->band == NL80211_BAND_S1GHZ) {
5251                 ieee80211_s1g_sta_rate_init(sta);
5252                 return 0;
5253         }
5254
5255         sband = local->hw.wiphy->bands[cbss->channel->band];
5256
5257         ieee80211_get_rates(sband, bss->supp_rates, bss->supp_rates_len,
5258                             NULL, 0,
5259                             &rates, &basic_rates, NULL,
5260                             &have_higher_than_11mbit,
5261                             &min_rate, &min_rate_index);
5262
5263         /*
5264          * This used to be a workaround for basic rates missing
5265          * in the association response frame. Now that we no
5266          * longer use the basic rates from there, it probably
5267          * doesn't happen any more, but keep the workaround so
5268          * in case some *other* APs are buggy in different ways
5269          * we can connect -- with a warning.
5270          * Allow this workaround only in case the AP provided at least
5271          * one rate.
5272          */
5273         if (min_rate_index < 0) {
5274                 link_info(link, "No legacy rates in association response\n");
5275                 return -EINVAL;
5276         } else if (!basic_rates) {
5277                 link_info(link, "No basic rates, using min rate instead\n");
5278                 basic_rates = BIT(min_rate_index);
5279         }
5280
5281         if (rates)
5282                 link_sta->pub->supp_rates[cbss->channel->band] = rates;
5283         else
5284                 link_info(link, "No rates found, keeping mandatory only\n");
5285
5286         link->conf->basic_rates = basic_rates;
5287
5288         /* cf. IEEE 802.11 9.2.12 */
5289         link->operating_11g_mode = sband->band == NL80211_BAND_2GHZ &&
5290                                    have_higher_than_11mbit;
5291
5292         return 0;
5293 }
5294
5295 static u8 ieee80211_max_rx_chains(struct ieee80211_link_data *link,
5296                                   struct cfg80211_bss *cbss)
5297 {
5298         struct ieee80211_he_mcs_nss_supp *he_mcs_nss_supp;
5299         const struct element *ht_cap_elem, *vht_cap_elem;
5300         const struct cfg80211_bss_ies *ies;
5301         const struct ieee80211_ht_cap *ht_cap;
5302         const struct ieee80211_vht_cap *vht_cap;
5303         const struct ieee80211_he_cap_elem *he_cap;
5304         const struct element *he_cap_elem;
5305         u16 mcs_80_map, mcs_160_map;
5306         int i, mcs_nss_size;
5307         bool support_160;
5308         u8 chains = 1;
5309
5310         if (link->u.mgd.conn.mode < IEEE80211_CONN_MODE_HT)
5311                 return chains;
5312
5313         ht_cap_elem = ieee80211_bss_get_elem(cbss, WLAN_EID_HT_CAPABILITY);
5314         if (ht_cap_elem && ht_cap_elem->datalen >= sizeof(*ht_cap)) {
5315                 ht_cap = (void *)ht_cap_elem->data;
5316                 chains = ieee80211_mcs_to_chains(&ht_cap->mcs);
5317                 /*
5318                  * TODO: use "Tx Maximum Number Spatial Streams Supported" and
5319                  *       "Tx Unequal Modulation Supported" fields.
5320                  */
5321         }
5322
5323         if (link->u.mgd.conn.mode < IEEE80211_CONN_MODE_VHT)
5324                 return chains;
5325
5326         vht_cap_elem = ieee80211_bss_get_elem(cbss, WLAN_EID_VHT_CAPABILITY);
5327         if (vht_cap_elem && vht_cap_elem->datalen >= sizeof(*vht_cap)) {
5328                 u8 nss;
5329                 u16 tx_mcs_map;
5330
5331                 vht_cap = (void *)vht_cap_elem->data;
5332                 tx_mcs_map = le16_to_cpu(vht_cap->supp_mcs.tx_mcs_map);
5333                 for (nss = 8; nss > 0; nss--) {
5334                         if (((tx_mcs_map >> (2 * (nss - 1))) & 3) !=
5335                                         IEEE80211_VHT_MCS_NOT_SUPPORTED)
5336                                 break;
5337                 }
5338                 /* TODO: use "Tx Highest Supported Long GI Data Rate" field? */
5339                 chains = max(chains, nss);
5340         }
5341
5342         if (link->u.mgd.conn.mode < IEEE80211_CONN_MODE_HE)
5343                 return chains;
5344
5345         ies = rcu_dereference(cbss->ies);
5346         he_cap_elem = cfg80211_find_ext_elem(WLAN_EID_EXT_HE_CAPABILITY,
5347                                              ies->data, ies->len);
5348
5349         if (!he_cap_elem || he_cap_elem->datalen < sizeof(*he_cap))
5350                 return chains;
5351
5352         /* skip one byte ext_tag_id */
5353         he_cap = (void *)(he_cap_elem->data + 1);
5354         mcs_nss_size = ieee80211_he_mcs_nss_size(he_cap);
5355
5356         /* invalid HE IE */
5357         if (he_cap_elem->datalen < 1 + mcs_nss_size + sizeof(*he_cap))
5358                 return chains;
5359
5360         /* mcs_nss is right after he_cap info */
5361         he_mcs_nss_supp = (void *)(he_cap + 1);
5362
5363         mcs_80_map = le16_to_cpu(he_mcs_nss_supp->tx_mcs_80);
5364
5365         for (i = 7; i >= 0; i--) {
5366                 u8 mcs_80 = mcs_80_map >> (2 * i) & 3;
5367
5368                 if (mcs_80 != IEEE80211_VHT_MCS_NOT_SUPPORTED) {
5369                         chains = max_t(u8, chains, i + 1);
5370                         break;
5371                 }
5372         }
5373
5374         support_160 = he_cap->phy_cap_info[0] &
5375                       IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G;
5376
5377         if (!support_160)
5378                 return chains;
5379
5380         mcs_160_map = le16_to_cpu(he_mcs_nss_supp->tx_mcs_160);
5381         for (i = 7; i >= 0; i--) {
5382                 u8 mcs_160 = mcs_160_map >> (2 * i) & 3;
5383
5384                 if (mcs_160 != IEEE80211_VHT_MCS_NOT_SUPPORTED) {
5385                         chains = max_t(u8, chains, i + 1);
5386                         break;
5387                 }
5388         }
5389
5390         return chains;
5391 }
5392
5393 static void
5394 ieee80211_determine_our_sta_mode(struct ieee80211_sub_if_data *sdata,
5395                                  struct ieee80211_supported_band *sband,
5396                                  struct cfg80211_assoc_request *req,
5397                                  bool wmm_used, int link_id,
5398                                  struct ieee80211_conn_settings *conn)
5399 {
5400         struct ieee80211_sta_ht_cap sta_ht_cap = sband->ht_cap;
5401         bool is_5ghz = sband->band == NL80211_BAND_5GHZ;
5402         bool is_6ghz = sband->band == NL80211_BAND_6GHZ;
5403         const struct ieee80211_sta_he_cap *he_cap;
5404         const struct ieee80211_sta_eht_cap *eht_cap;
5405         struct ieee80211_sta_vht_cap vht_cap;
5406
5407         if (sband->band == NL80211_BAND_S1GHZ) {
5408                 conn->mode = IEEE80211_CONN_MODE_S1G;
5409                 conn->bw_limit = IEEE80211_CONN_BW_LIMIT_20;
5410                 mlme_dbg(sdata, "operating as S1G STA\n");
5411                 return;
5412         }
5413
5414         conn->mode = IEEE80211_CONN_MODE_LEGACY;
5415         conn->bw_limit = IEEE80211_CONN_BW_LIMIT_20;
5416
5417         ieee80211_apply_htcap_overrides(sdata, &sta_ht_cap);
5418
5419         if (req && req->flags & ASSOC_REQ_DISABLE_HT) {
5420                 mlme_link_id_dbg(sdata, link_id,
5421                                  "HT disabled by flag, limiting to legacy\n");
5422                 goto out;
5423         }
5424
5425         if (!wmm_used) {
5426                 mlme_link_id_dbg(sdata, link_id,
5427                                  "WMM/QoS not supported, limiting to legacy\n");
5428                 goto out;
5429         }
5430
5431         if (req) {
5432                 unsigned int i;
5433
5434                 for (i = 0; i < req->crypto.n_ciphers_pairwise; i++) {
5435                         if (req->crypto.ciphers_pairwise[i] == WLAN_CIPHER_SUITE_WEP40 ||
5436                             req->crypto.ciphers_pairwise[i] == WLAN_CIPHER_SUITE_TKIP ||
5437                             req->crypto.ciphers_pairwise[i] == WLAN_CIPHER_SUITE_WEP104) {
5438                                 netdev_info(sdata->dev,
5439                                             "WEP/TKIP use, limiting to legacy\n");
5440                                 goto out;
5441                         }
5442                 }
5443         }
5444
5445         if (!sta_ht_cap.ht_supported && !is_6ghz) {
5446                 mlme_link_id_dbg(sdata, link_id,
5447                                  "HT not supported (and not on 6 GHz), limiting to legacy\n");
5448                 goto out;
5449         }
5450
5451         /* HT is fine */
5452         conn->mode = IEEE80211_CONN_MODE_HT;
5453         conn->bw_limit = sta_ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40 ?
5454                 IEEE80211_CONN_BW_LIMIT_40 :
5455                 IEEE80211_CONN_BW_LIMIT_20;
5456
5457         memcpy(&vht_cap, &sband->vht_cap, sizeof(vht_cap));
5458         ieee80211_apply_vhtcap_overrides(sdata, &vht_cap);
5459
5460         if (req && req->flags & ASSOC_REQ_DISABLE_VHT) {
5461                 mlme_link_id_dbg(sdata, link_id,
5462                                  "VHT disabled by flag, limiting to HT\n");
5463                 goto out;
5464         }
5465
5466         if (vht_cap.vht_supported && is_5ghz) {
5467                 bool have_80mhz = false;
5468                 unsigned int i;
5469
5470                 if (conn->bw_limit == IEEE80211_CONN_BW_LIMIT_20) {
5471                         mlme_link_id_dbg(sdata, link_id,
5472                                          "no 40 MHz support on 5 GHz, limiting to HT\n");
5473                         goto out;
5474                 }
5475
5476                 /* Allow VHT if at least one channel on the sband supports 80 MHz */
5477                 for (i = 0; i < sband->n_channels; i++) {
5478                         if (sband->channels[i].flags & (IEEE80211_CHAN_DISABLED |
5479                                                         IEEE80211_CHAN_NO_80MHZ))
5480                                 continue;
5481
5482                         have_80mhz = true;
5483                         break;
5484                 }
5485
5486                 if (!have_80mhz) {
5487                         mlme_link_id_dbg(sdata, link_id,
5488                                          "no 80 MHz channel support on 5 GHz, limiting to HT\n");
5489                         goto out;
5490                 }
5491         } else if (is_5ghz) { /* !vht_supported but on 5 GHz */
5492                 mlme_link_id_dbg(sdata, link_id,
5493                                  "no VHT support on 5 GHz, limiting to HT\n");
5494                 goto out;
5495         }
5496
5497         /* VHT - if we have - is fine, including 80 MHz, check 160 below again */
5498         if (sband->band != NL80211_BAND_2GHZ) {
5499                 conn->mode = IEEE80211_CONN_MODE_VHT;
5500                 conn->bw_limit = IEEE80211_CONN_BW_LIMIT_160;
5501         }
5502
5503         if (is_5ghz &&
5504             !(vht_cap.cap & (IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160MHZ |
5505                              IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ))) {
5506                 conn->bw_limit = IEEE80211_CONN_BW_LIMIT_80;
5507                 mlme_link_id_dbg(sdata, link_id,
5508                                  "no VHT 160 MHz capability on 5 GHz, limiting to 80 MHz");
5509         }
5510
5511         if (req && req->flags & ASSOC_REQ_DISABLE_HE) {
5512                 mlme_link_id_dbg(sdata, link_id,
5513                                  "HE disabled by flag, limiting to HT/VHT\n");
5514                 goto out;
5515         }
5516
5517         he_cap = ieee80211_get_he_iftype_cap_vif(sband, &sdata->vif);
5518         if (!he_cap) {
5519                 WARN_ON(is_6ghz);
5520                 mlme_link_id_dbg(sdata, link_id,
5521                                  "no HE support, limiting to HT/VHT\n");
5522                 goto out;
5523         }
5524
5525         /* so we have HE */
5526         conn->mode = IEEE80211_CONN_MODE_HE;
5527
5528         /* check bandwidth */
5529         switch (sband->band) {
5530         default:
5531         case NL80211_BAND_2GHZ:
5532                 if (he_cap->he_cap_elem.phy_cap_info[0] &
5533                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_40MHZ_IN_2G)
5534                         break;
5535                 conn->bw_limit = IEEE80211_CONN_BW_LIMIT_20;
5536                 mlme_link_id_dbg(sdata, link_id,
5537                                  "no 40 MHz HE cap in 2.4 GHz, limiting to 20 MHz\n");
5538                 break;
5539         case NL80211_BAND_5GHZ:
5540                 if (!(he_cap->he_cap_elem.phy_cap_info[0] &
5541                       IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_40MHZ_80MHZ_IN_5G)) {
5542                         conn->bw_limit = IEEE80211_CONN_BW_LIMIT_20;
5543                         mlme_link_id_dbg(sdata, link_id,
5544                                          "no 40/80 MHz HE cap in 5 GHz, limiting to 20 MHz\n");
5545                         break;
5546                 }
5547                 if (!(he_cap->he_cap_elem.phy_cap_info[0] &
5548                       IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G)) {
5549                         conn->bw_limit = min_t(enum ieee80211_conn_bw_limit,
5550                                                conn->bw_limit,
5551                                                IEEE80211_CONN_BW_LIMIT_80);
5552                         mlme_link_id_dbg(sdata, link_id,
5553                                          "no 160 MHz HE cap in 5 GHz, limiting to 80 MHz\n");
5554                 }
5555                 break;
5556         case NL80211_BAND_6GHZ:
5557                 if (he_cap->he_cap_elem.phy_cap_info[0] &
5558                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G)
5559                         break;
5560                 conn->bw_limit = min_t(enum ieee80211_conn_bw_limit,
5561                                        conn->bw_limit,
5562                                        IEEE80211_CONN_BW_LIMIT_80);
5563                 mlme_link_id_dbg(sdata, link_id,
5564                                  "no 160 MHz HE cap in 6 GHz, limiting to 80 MHz\n");
5565                 break;
5566         }
5567
5568         if (req && req->flags & ASSOC_REQ_DISABLE_EHT) {
5569                 mlme_link_id_dbg(sdata, link_id,
5570                                  "EHT disabled by flag, limiting to HE\n");
5571                 goto out;
5572         }
5573
5574         eht_cap = ieee80211_get_eht_iftype_cap_vif(sband, &sdata->vif);
5575         if (!eht_cap) {
5576                 mlme_link_id_dbg(sdata, link_id,
5577                                  "no EHT support, limiting to HE\n");
5578                 goto out;
5579         }
5580
5581         /* we have EHT */
5582
5583         conn->mode = IEEE80211_CONN_MODE_EHT;
5584
5585         /* check bandwidth */
5586         if (is_6ghz &&
5587             eht_cap->eht_cap_elem.phy_cap_info[0] & IEEE80211_EHT_PHY_CAP0_320MHZ_IN_6GHZ)
5588                 conn->bw_limit = IEEE80211_CONN_BW_LIMIT_320;
5589         else if (is_6ghz)
5590                 mlme_link_id_dbg(sdata, link_id,
5591                                  "no EHT 320 MHz cap in 6 GHz, limiting to 160 MHz\n");
5592
5593 out:
5594         mlme_link_id_dbg(sdata, link_id,
5595                          "determined local STA to be %s, BW limited to %d MHz\n",
5596                          ieee80211_conn_mode_str(conn->mode),
5597                          20 * (1 << conn->bw_limit));
5598 }
5599
5600 static void
5601 ieee80211_determine_our_sta_mode_auth(struct ieee80211_sub_if_data *sdata,
5602                                       struct ieee80211_supported_band *sband,
5603                                       struct cfg80211_auth_request *req,
5604                                       bool wmm_used,
5605                                       struct ieee80211_conn_settings *conn)
5606 {
5607         ieee80211_determine_our_sta_mode(sdata, sband, NULL, wmm_used,
5608                                          req->link_id > 0 ? req->link_id : 0,
5609                                          conn);
5610 }
5611
5612 static void
5613 ieee80211_determine_our_sta_mode_assoc(struct ieee80211_sub_if_data *sdata,
5614                                        struct ieee80211_supported_band *sband,
5615                                        struct cfg80211_assoc_request *req,
5616                                        bool wmm_used, int link_id,
5617                                        struct ieee80211_conn_settings *conn)
5618 {
5619         struct ieee80211_conn_settings tmp;
5620
5621         WARN_ON(!req);
5622
5623         ieee80211_determine_our_sta_mode(sdata, sband, req, wmm_used, link_id,
5624                                          &tmp);
5625
5626         conn->mode = min_t(enum ieee80211_conn_mode,
5627                            conn->mode, tmp.mode);
5628         conn->bw_limit = min_t(enum ieee80211_conn_bw_limit,
5629                                conn->bw_limit, tmp.bw_limit);
5630 }
5631
5632 static enum ieee80211_ap_reg_power
5633 ieee80211_ap_power_type(u8 control)
5634 {
5635         switch (u8_get_bits(control, IEEE80211_HE_6GHZ_OPER_CTRL_REG_INFO)) {
5636         case IEEE80211_6GHZ_CTRL_REG_LPI_AP:
5637         case IEEE80211_6GHZ_CTRL_REG_INDOOR_LPI_AP:
5638                 return IEEE80211_REG_LPI_AP;
5639         case IEEE80211_6GHZ_CTRL_REG_SP_AP:
5640         case IEEE80211_6GHZ_CTRL_REG_INDOOR_SP_AP:
5641                 return IEEE80211_REG_SP_AP;
5642         case IEEE80211_6GHZ_CTRL_REG_VLP_AP:
5643                 return IEEE80211_REG_VLP_AP;
5644         default:
5645                 return IEEE80211_REG_UNSET_AP;
5646         }
5647 }
5648
5649 static int ieee80211_prep_channel(struct ieee80211_sub_if_data *sdata,
5650                                   struct ieee80211_link_data *link,
5651                                   int link_id,
5652                                   struct cfg80211_bss *cbss, bool mlo,
5653                                   struct ieee80211_conn_settings *conn,
5654                                   unsigned long *userspace_selectors)
5655 {
5656         struct ieee80211_local *local = sdata->local;
5657         bool is_6ghz = cbss->channel->band == NL80211_BAND_6GHZ;
5658         struct ieee80211_chan_req chanreq = {};
5659         struct cfg80211_chan_def ap_chandef;
5660         struct ieee802_11_elems *elems;
5661         int ret;
5662
5663         lockdep_assert_wiphy(local->hw.wiphy);
5664
5665         rcu_read_lock();
5666         elems = ieee80211_determine_chan_mode(sdata, conn, cbss, link_id,
5667                                               &chanreq, &ap_chandef,
5668                                               userspace_selectors);
5669
5670         if (IS_ERR(elems)) {
5671                 rcu_read_unlock();
5672                 return PTR_ERR(elems);
5673         }
5674
5675         if (mlo && !elems->ml_basic) {
5676                 sdata_info(sdata, "Rejecting MLO as it is not supported by AP\n");
5677                 rcu_read_unlock();
5678                 kfree(elems);
5679                 return -EINVAL;
5680         }
5681
5682         if (link && is_6ghz && conn->mode >= IEEE80211_CONN_MODE_HE) {
5683                 const struct ieee80211_he_6ghz_oper *he_6ghz_oper;
5684
5685                 if (elems->pwr_constr_elem)
5686                         link->conf->pwr_reduction = *elems->pwr_constr_elem;
5687
5688                 he_6ghz_oper = ieee80211_he_6ghz_oper(elems->he_operation);
5689                 if (he_6ghz_oper)
5690                         link->conf->power_type =
5691                                 ieee80211_ap_power_type(he_6ghz_oper->control);
5692                 else
5693                         link_info(link,
5694                                   "HE 6 GHz operation missing (on %d MHz), expect issues\n",
5695                                   cbss->channel->center_freq);
5696
5697                 link->conf->tpe = elems->tpe;
5698                 ieee80211_rearrange_tpe(&link->conf->tpe, &ap_chandef,
5699                                         &chanreq.oper);
5700         }
5701         rcu_read_unlock();
5702         /* the element data was RCU protected so no longer valid anyway */
5703         kfree(elems);
5704         elems = NULL;
5705
5706         if (!link)
5707                 return 0;
5708
5709         rcu_read_lock();
5710         link->needed_rx_chains = min(ieee80211_max_rx_chains(link, cbss),
5711                                      local->rx_chains);
5712         rcu_read_unlock();
5713
5714         /*
5715          * If this fails (possibly due to channel context sharing
5716          * on incompatible channels, e.g. 80+80 and 160 sharing the
5717          * same control channel) try to use a smaller bandwidth.
5718          */
5719         ret = ieee80211_link_use_channel(link, &chanreq,
5720                                          IEEE80211_CHANCTX_SHARED);
5721
5722         /* don't downgrade for 5 and 10 MHz channels, though. */
5723         if (chanreq.oper.width == NL80211_CHAN_WIDTH_5 ||
5724             chanreq.oper.width == NL80211_CHAN_WIDTH_10)
5725                 return ret;
5726
5727         while (ret && chanreq.oper.width != NL80211_CHAN_WIDTH_20_NOHT) {
5728                 ieee80211_chanreq_downgrade(&chanreq, conn);
5729
5730                 ret = ieee80211_link_use_channel(link, &chanreq,
5731                                                  IEEE80211_CHANCTX_SHARED);
5732         }
5733
5734         return ret;
5735 }
5736
5737 static bool ieee80211_get_dtim(const struct cfg80211_bss_ies *ies,
5738                                u8 *dtim_count, u8 *dtim_period)
5739 {
5740         const u8 *tim_ie = cfg80211_find_ie(WLAN_EID_TIM, ies->data, ies->len);
5741         const u8 *idx_ie = cfg80211_find_ie(WLAN_EID_MULTI_BSSID_IDX, ies->data,
5742                                          ies->len);
5743         const struct ieee80211_tim_ie *tim = NULL;
5744         const struct ieee80211_bssid_index *idx;
5745         bool valid = tim_ie && tim_ie[1] >= 2;
5746
5747         if (valid)
5748                 tim = (void *)(tim_ie + 2);
5749
5750         if (dtim_count)
5751                 *dtim_count = valid ? tim->dtim_count : 0;
5752
5753         if (dtim_period)
5754                 *dtim_period = valid ? tim->dtim_period : 0;
5755
5756         /* Check if value is overridden by non-transmitted profile */
5757         if (!idx_ie || idx_ie[1] < 3)
5758                 return valid;
5759
5760         idx = (void *)(idx_ie + 2);
5761
5762         if (dtim_count)
5763                 *dtim_count = idx->dtim_count;
5764
5765         if (dtim_period)
5766                 *dtim_period = idx->dtim_period;
5767
5768         return true;
5769 }
5770
5771 static bool ieee80211_assoc_success(struct ieee80211_sub_if_data *sdata,
5772                                     struct ieee80211_mgmt *mgmt,
5773                                     struct ieee802_11_elems *elems,
5774                                     const u8 *elem_start, unsigned int elem_len)
5775 {
5776         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
5777         struct ieee80211_mgd_assoc_data *assoc_data = ifmgd->assoc_data;
5778         struct ieee80211_local *local = sdata->local;
5779         unsigned int link_id;
5780         struct sta_info *sta;
5781         u64 changed[IEEE80211_MLD_MAX_NUM_LINKS] = {};
5782         u16 valid_links = 0, dormant_links = 0;
5783         int err;
5784
5785         lockdep_assert_wiphy(sdata->local->hw.wiphy);
5786         /*
5787          * station info was already allocated and inserted before
5788          * the association and should be available to us
5789          */
5790         sta = sta_info_get(sdata, assoc_data->ap_addr);
5791         if (WARN_ON(!sta))
5792                 goto out_err;
5793
5794         sta->sta.spp_amsdu = assoc_data->spp_amsdu;
5795
5796         if (ieee80211_vif_is_mld(&sdata->vif)) {
5797                 for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
5798                         if (!assoc_data->link[link_id].bss)
5799                                 continue;
5800
5801                         valid_links |= BIT(link_id);
5802                         if (assoc_data->link[link_id].disabled)
5803                                 dormant_links |= BIT(link_id);
5804
5805                         if (link_id != assoc_data->assoc_link_id) {
5806                                 err = ieee80211_sta_allocate_link(sta, link_id);
5807                                 if (err)
5808                                         goto out_err;
5809                         }
5810                 }
5811
5812                 ieee80211_vif_set_links(sdata, valid_links, dormant_links);
5813         }
5814
5815         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
5816                 struct cfg80211_bss *cbss = assoc_data->link[link_id].bss;
5817                 struct ieee80211_link_data *link;
5818                 struct link_sta_info *link_sta;
5819
5820                 if (!cbss)
5821                         continue;
5822
5823                 link = sdata_dereference(sdata->link[link_id], sdata);
5824                 if (WARN_ON(!link))
5825                         goto out_err;
5826
5827                 if (ieee80211_vif_is_mld(&sdata->vif))
5828                         link_info(link,
5829                                   "local address %pM, AP link address %pM%s\n",
5830                                   link->conf->addr,
5831                                   assoc_data->link[link_id].bss->bssid,
5832                                   link_id == assoc_data->assoc_link_id ?
5833                                         " (assoc)" : "");
5834
5835                 link_sta = rcu_dereference_protected(sta->link[link_id],
5836                                                      lockdep_is_held(&local->hw.wiphy->mtx));
5837                 if (WARN_ON(!link_sta))
5838                         goto out_err;
5839
5840                 if (!link->u.mgd.have_beacon) {
5841                         const struct cfg80211_bss_ies *ies;
5842
5843                         rcu_read_lock();
5844                         ies = rcu_dereference(cbss->beacon_ies);
5845                         if (ies)
5846                                 link->u.mgd.have_beacon = true;
5847                         else
5848                                 ies = rcu_dereference(cbss->ies);
5849                         ieee80211_get_dtim(ies,
5850                                            &link->conf->sync_dtim_count,
5851                                            &link->u.mgd.dtim_period);
5852                         link->conf->beacon_int = cbss->beacon_interval;
5853                         rcu_read_unlock();
5854                 }
5855
5856                 link->conf->dtim_period = link->u.mgd.dtim_period ?: 1;
5857
5858                 if (link_id != assoc_data->assoc_link_id) {
5859                         link->u.mgd.conn = assoc_data->link[link_id].conn;
5860
5861                         err = ieee80211_prep_channel(sdata, link, link_id, cbss,
5862                                                      true, &link->u.mgd.conn,
5863                                                      assoc_data->userspace_selectors);
5864                         if (err) {
5865                                 link_info(link, "prep_channel failed\n");
5866                                 goto out_err;
5867                         }
5868                 }
5869
5870                 err = ieee80211_mgd_setup_link_sta(link, sta, link_sta,
5871                                                    assoc_data->link[link_id].bss);
5872                 if (err)
5873                         goto out_err;
5874
5875                 if (!ieee80211_assoc_config_link(link, link_sta,
5876                                                  assoc_data->link[link_id].bss,
5877                                                  mgmt, elem_start, elem_len,
5878                                                  &changed[link_id]))
5879                         goto out_err;
5880
5881                 if (assoc_data->link[link_id].status != WLAN_STATUS_SUCCESS) {
5882                         valid_links &= ~BIT(link_id);
5883                         ieee80211_sta_remove_link(sta, link_id);
5884                         continue;
5885                 }
5886
5887                 if (link_id != assoc_data->assoc_link_id) {
5888                         err = ieee80211_sta_activate_link(sta, link_id);
5889                         if (err)
5890                                 goto out_err;
5891                 }
5892         }
5893
5894         /* links might have changed due to rejected ones, set them again */
5895         ieee80211_vif_set_links(sdata, valid_links, dormant_links);
5896
5897         rate_control_rate_init_all_links(sta);
5898
5899         if (ifmgd->flags & IEEE80211_STA_MFP_ENABLED) {
5900                 set_sta_flag(sta, WLAN_STA_MFP);
5901                 sta->sta.mfp = true;
5902         } else {
5903                 sta->sta.mfp = false;
5904         }
5905
5906         ieee80211_sta_set_max_amsdu_subframes(sta, elems->ext_capab,
5907                                               elems->ext_capab_len);
5908
5909         sta->sta.wme = (elems->wmm_param || elems->s1g_capab) &&
5910                        local->hw.queues >= IEEE80211_NUM_ACS;
5911
5912         err = sta_info_move_state(sta, IEEE80211_STA_ASSOC);
5913         if (!err && !(ifmgd->flags & IEEE80211_STA_CONTROL_PORT))
5914                 err = sta_info_move_state(sta, IEEE80211_STA_AUTHORIZED);
5915         if (err) {
5916                 sdata_info(sdata,
5917                            "failed to move station %pM to desired state\n",
5918                            sta->sta.addr);
5919                 WARN_ON(__sta_info_destroy(sta));
5920                 goto out_err;
5921         }
5922
5923         if (sdata->wdev.use_4addr)
5924                 drv_sta_set_4addr(local, sdata, &sta->sta, true);
5925
5926         ieee80211_set_associated(sdata, assoc_data, changed);
5927
5928         /*
5929          * If we're using 4-addr mode, let the AP know that we're
5930          * doing so, so that it can create the STA VLAN on its side
5931          */
5932         if (ifmgd->use_4addr)
5933                 ieee80211_send_4addr_nullfunc(local, sdata);
5934
5935         /*
5936          * Start timer to probe the connection to the AP now.
5937          * Also start the timer that will detect beacon loss.
5938          */
5939         ieee80211_sta_reset_beacon_monitor(sdata);
5940         ieee80211_sta_reset_conn_monitor(sdata);
5941
5942         return true;
5943 out_err:
5944         eth_zero_addr(sdata->vif.cfg.ap_addr);
5945         return false;
5946 }
5947
5948 static void ieee80211_rx_mgmt_assoc_resp(struct ieee80211_sub_if_data *sdata,
5949                                          struct ieee80211_mgmt *mgmt,
5950                                          size_t len)
5951 {
5952         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
5953         struct ieee80211_mgd_assoc_data *assoc_data = ifmgd->assoc_data;
5954         u16 capab_info, status_code, aid;
5955         struct ieee80211_elems_parse_params parse_params = {
5956                 .bss = NULL,
5957                 .link_id = -1,
5958                 .from_ap = true,
5959         };
5960         struct ieee802_11_elems *elems;
5961         int ac;
5962         const u8 *elem_start;
5963         unsigned int elem_len;
5964         bool reassoc;
5965         struct ieee80211_event event = {
5966                 .type = MLME_EVENT,
5967                 .u.mlme.data = ASSOC_EVENT,
5968         };
5969         struct ieee80211_prep_tx_info info = {};
5970         struct cfg80211_rx_assoc_resp_data resp = {
5971                 .uapsd_queues = -1,
5972         };
5973         u8 ap_mld_addr[ETH_ALEN] __aligned(2);
5974         unsigned int link_id;
5975
5976         lockdep_assert_wiphy(sdata->local->hw.wiphy);
5977
5978         if (!assoc_data)
5979                 return;
5980
5981         info.link_id = assoc_data->assoc_link_id;
5982
5983         parse_params.mode =
5984                 assoc_data->link[assoc_data->assoc_link_id].conn.mode;
5985
5986         if (!ether_addr_equal(assoc_data->ap_addr, mgmt->bssid) ||
5987             !ether_addr_equal(assoc_data->ap_addr, mgmt->sa))
5988                 return;
5989
5990         /*
5991          * AssocResp and ReassocResp have identical structure, so process both
5992          * of them in this function.
5993          */
5994
5995         if (len < 24 + 6)
5996                 return;
5997
5998         reassoc = ieee80211_is_reassoc_resp(mgmt->frame_control);
5999         capab_info = le16_to_cpu(mgmt->u.assoc_resp.capab_info);
6000         status_code = le16_to_cpu(mgmt->u.assoc_resp.status_code);
6001         if (assoc_data->s1g)
6002                 elem_start = mgmt->u.s1g_assoc_resp.variable;
6003         else
6004                 elem_start = mgmt->u.assoc_resp.variable;
6005
6006         /*
6007          * Note: this may not be perfect, AP might misbehave - if
6008          * anyone needs to rely on perfect complete notification
6009          * with the exact right subtype, then we need to track what
6010          * we actually transmitted.
6011          */
6012         info.subtype = reassoc ? IEEE80211_STYPE_REASSOC_REQ :
6013                                  IEEE80211_STYPE_ASSOC_REQ;
6014
6015         if (assoc_data->fils_kek_len &&
6016             fils_decrypt_assoc_resp(sdata, (u8 *)mgmt, &len, assoc_data) < 0)
6017                 return;
6018
6019         elem_len = len - (elem_start - (u8 *)mgmt);
6020         parse_params.start = elem_start;
6021         parse_params.len = elem_len;
6022         elems = ieee802_11_parse_elems_full(&parse_params);
6023         if (!elems)
6024                 goto notify_driver;
6025
6026         if (elems->aid_resp)
6027                 aid = le16_to_cpu(elems->aid_resp->aid);
6028         else if (assoc_data->s1g)
6029                 aid = 0; /* TODO */
6030         else
6031                 aid = le16_to_cpu(mgmt->u.assoc_resp.aid);
6032
6033         /*
6034          * The 5 MSB of the AID field are reserved
6035          * (802.11-2016 9.4.1.8 AID field)
6036          */
6037         aid &= 0x7ff;
6038
6039         sdata_info(sdata,
6040                    "RX %sssocResp from %pM (capab=0x%x status=%d aid=%d)\n",
6041                    reassoc ? "Rea" : "A", assoc_data->ap_addr,
6042                    capab_info, status_code, (u16)(aid & ~(BIT(15) | BIT(14))));
6043
6044         ifmgd->broken_ap = false;
6045
6046         if (status_code == WLAN_STATUS_ASSOC_REJECTED_TEMPORARILY &&
6047             elems->timeout_int &&
6048             elems->timeout_int->type == WLAN_TIMEOUT_ASSOC_COMEBACK) {
6049                 u32 tu, ms;
6050
6051                 cfg80211_assoc_comeback(sdata->dev, assoc_data->ap_addr,
6052                                         le32_to_cpu(elems->timeout_int->value));
6053
6054                 tu = le32_to_cpu(elems->timeout_int->value);
6055                 ms = tu * 1024 / 1000;
6056                 sdata_info(sdata,
6057                            "%pM rejected association temporarily; comeback duration %u TU (%u ms)\n",
6058                            assoc_data->ap_addr, tu, ms);
6059                 assoc_data->timeout = jiffies + msecs_to_jiffies(ms);
6060                 assoc_data->timeout_started = true;
6061                 assoc_data->comeback = true;
6062                 if (ms > IEEE80211_ASSOC_TIMEOUT)
6063                         run_again(sdata, assoc_data->timeout);
6064                 goto notify_driver;
6065         }
6066
6067         if (status_code != WLAN_STATUS_SUCCESS) {
6068                 sdata_info(sdata, "%pM denied association (code=%d)\n",
6069                            assoc_data->ap_addr, status_code);
6070                 event.u.mlme.status = MLME_DENIED;
6071                 event.u.mlme.reason = status_code;
6072                 drv_event_callback(sdata->local, sdata, &event);
6073         } else {
6074                 if (aid == 0 || aid > IEEE80211_MAX_AID) {
6075                         sdata_info(sdata,
6076                                    "invalid AID value %d (out of range), turn off PS\n",
6077                                    aid);
6078                         aid = 0;
6079                         ifmgd->broken_ap = true;
6080                 }
6081
6082                 if (ieee80211_vif_is_mld(&sdata->vif)) {
6083                         struct ieee80211_mle_basic_common_info *common;
6084
6085                         if (!elems->ml_basic) {
6086                                 sdata_info(sdata,
6087                                            "MLO association with %pM but no (basic) multi-link element in response!\n",
6088                                            assoc_data->ap_addr);
6089                                 goto abandon_assoc;
6090                         }
6091
6092                         common = (void *)elems->ml_basic->variable;
6093
6094                         if (memcmp(assoc_data->ap_addr,
6095                                    common->mld_mac_addr, ETH_ALEN)) {
6096                                 sdata_info(sdata,
6097                                            "AP MLD MAC address mismatch: got %pM expected %pM\n",
6098                                            common->mld_mac_addr,
6099                                            assoc_data->ap_addr);
6100                                 goto abandon_assoc;
6101                         }
6102
6103                         sdata->vif.cfg.eml_cap =
6104                                 ieee80211_mle_get_eml_cap((const void *)elems->ml_basic);
6105                         sdata->vif.cfg.eml_med_sync_delay =
6106                                 ieee80211_mle_get_eml_med_sync_delay((const void *)elems->ml_basic);
6107                         sdata->vif.cfg.mld_capa_op =
6108                                 ieee80211_mle_get_mld_capa_op((const void *)elems->ml_basic);
6109                 }
6110
6111                 sdata->vif.cfg.aid = aid;
6112
6113                 if (!ieee80211_assoc_success(sdata, mgmt, elems,
6114                                              elem_start, elem_len)) {
6115                         /* oops -- internal error -- send timeout for now */
6116                         ieee80211_destroy_assoc_data(sdata, ASSOC_TIMEOUT);
6117                         goto notify_driver;
6118                 }
6119                 event.u.mlme.status = MLME_SUCCESS;
6120                 drv_event_callback(sdata->local, sdata, &event);
6121                 sdata_info(sdata, "associated\n");
6122
6123                 info.success = 1;
6124         }
6125
6126         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
6127                 struct ieee80211_link_data *link;
6128
6129                 if (!assoc_data->link[link_id].bss)
6130                         continue;
6131
6132                 resp.links[link_id].bss = assoc_data->link[link_id].bss;
6133                 ether_addr_copy(resp.links[link_id].addr,
6134                                 assoc_data->link[link_id].addr);
6135                 resp.links[link_id].status = assoc_data->link[link_id].status;
6136
6137                 link = sdata_dereference(sdata->link[link_id], sdata);
6138                 if (!link)
6139                         continue;
6140
6141                 /* get uapsd queues configuration - same for all links */
6142                 resp.uapsd_queues = 0;
6143                 for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
6144                         if (link->tx_conf[ac].uapsd)
6145                                 resp.uapsd_queues |= ieee80211_ac_to_qos_mask[ac];
6146         }
6147
6148         if (ieee80211_vif_is_mld(&sdata->vif)) {
6149                 ether_addr_copy(ap_mld_addr, sdata->vif.cfg.ap_addr);
6150                 resp.ap_mld_addr = ap_mld_addr;
6151         }
6152
6153         ieee80211_destroy_assoc_data(sdata,
6154                                      status_code == WLAN_STATUS_SUCCESS ?
6155                                         ASSOC_SUCCESS :
6156                                         ASSOC_REJECTED);
6157
6158         resp.buf = (u8 *)mgmt;
6159         resp.len = len;
6160         resp.req_ies = ifmgd->assoc_req_ies;
6161         resp.req_ies_len = ifmgd->assoc_req_ies_len;
6162         cfg80211_rx_assoc_resp(sdata->dev, &resp);
6163 notify_driver:
6164         drv_mgd_complete_tx(sdata->local, sdata, &info);
6165         kfree(elems);
6166         return;
6167 abandon_assoc:
6168         ieee80211_destroy_assoc_data(sdata, ASSOC_ABANDON);
6169         goto notify_driver;
6170 }
6171
6172 static void ieee80211_rx_bss_info(struct ieee80211_link_data *link,
6173                                   struct ieee80211_mgmt *mgmt, size_t len,
6174                                   struct ieee80211_rx_status *rx_status)
6175 {
6176         struct ieee80211_sub_if_data *sdata = link->sdata;
6177         struct ieee80211_local *local = sdata->local;
6178         struct ieee80211_bss *bss;
6179         struct ieee80211_channel *channel;
6180
6181         lockdep_assert_wiphy(sdata->local->hw.wiphy);
6182
6183         channel = ieee80211_get_channel_khz(local->hw.wiphy,
6184                                         ieee80211_rx_status_to_khz(rx_status));
6185         if (!channel)
6186                 return;
6187
6188         bss = ieee80211_bss_info_update(local, rx_status, mgmt, len, channel);
6189         if (bss) {
6190                 link->conf->beacon_rate = bss->beacon_rate;
6191                 ieee80211_rx_bss_put(local, bss);
6192         }
6193 }
6194
6195
6196 static void ieee80211_rx_mgmt_probe_resp(struct ieee80211_link_data *link,
6197                                          struct sk_buff *skb)
6198 {
6199         struct ieee80211_sub_if_data *sdata = link->sdata;
6200         struct ieee80211_mgmt *mgmt = (void *)skb->data;
6201         struct ieee80211_if_managed *ifmgd;
6202         struct ieee80211_rx_status *rx_status = (void *) skb->cb;
6203         struct ieee80211_channel *channel;
6204         size_t baselen, len = skb->len;
6205
6206         ifmgd = &sdata->u.mgd;
6207
6208         lockdep_assert_wiphy(sdata->local->hw.wiphy);
6209
6210         /*
6211          * According to Draft P802.11ax D6.0 clause 26.17.2.3.2:
6212          * "If a 6 GHz AP receives a Probe Request frame  and responds with
6213          * a Probe Response frame [..], the Address 1 field of the Probe
6214          * Response frame shall be set to the broadcast address [..]"
6215          * So, on 6GHz band we should also accept broadcast responses.
6216          */
6217         channel = ieee80211_get_channel(sdata->local->hw.wiphy,
6218                                         rx_status->freq);
6219         if (!channel)
6220                 return;
6221
6222         if (!ether_addr_equal(mgmt->da, sdata->vif.addr) &&
6223             (channel->band != NL80211_BAND_6GHZ ||
6224              !is_broadcast_ether_addr(mgmt->da)))
6225                 return; /* ignore ProbeResp to foreign address */
6226
6227         baselen = (u8 *) mgmt->u.probe_resp.variable - (u8 *) mgmt;
6228         if (baselen > len)
6229                 return;
6230
6231         ieee80211_rx_bss_info(link, mgmt, len, rx_status);
6232
6233         if (ifmgd->associated &&
6234             ether_addr_equal(mgmt->bssid, link->u.mgd.bssid))
6235                 ieee80211_reset_ap_probe(sdata);
6236 }
6237
6238 /*
6239  * This is the canonical list of information elements we care about,
6240  * the filter code also gives us all changes to the Microsoft OUI
6241  * (00:50:F2) vendor IE which is used for WMM which we need to track,
6242  * as well as the DTPC IE (part of the Cisco OUI) used for signaling
6243  * changes to requested client power.
6244  *
6245  * We implement beacon filtering in software since that means we can
6246  * avoid processing the frame here and in cfg80211, and userspace
6247  * will not be able to tell whether the hardware supports it or not.
6248  *
6249  * XXX: This list needs to be dynamic -- userspace needs to be able to
6250  *      add items it requires. It also needs to be able to tell us to
6251  *      look out for other vendor IEs.
6252  */
6253 static const u64 care_about_ies =
6254         (1ULL << WLAN_EID_COUNTRY) |
6255         (1ULL << WLAN_EID_ERP_INFO) |
6256         (1ULL << WLAN_EID_CHANNEL_SWITCH) |
6257         (1ULL << WLAN_EID_PWR_CONSTRAINT) |
6258         (1ULL << WLAN_EID_HT_CAPABILITY) |
6259         (1ULL << WLAN_EID_HT_OPERATION) |
6260         (1ULL << WLAN_EID_EXT_CHANSWITCH_ANN);
6261
6262 static void ieee80211_handle_beacon_sig(struct ieee80211_link_data *link,
6263                                         struct ieee80211_if_managed *ifmgd,
6264                                         struct ieee80211_bss_conf *bss_conf,
6265                                         struct ieee80211_local *local,
6266                                         struct ieee80211_rx_status *rx_status)
6267 {
6268         struct ieee80211_sub_if_data *sdata = link->sdata;
6269
6270         /* Track average RSSI from the Beacon frames of the current AP */
6271
6272         if (!link->u.mgd.tracking_signal_avg) {
6273                 link->u.mgd.tracking_signal_avg = true;
6274                 ewma_beacon_signal_init(&link->u.mgd.ave_beacon_signal);
6275                 link->u.mgd.last_cqm_event_signal = 0;
6276                 link->u.mgd.count_beacon_signal = 1;
6277                 link->u.mgd.last_ave_beacon_signal = 0;
6278         } else {
6279                 link->u.mgd.count_beacon_signal++;
6280         }
6281
6282         ewma_beacon_signal_add(&link->u.mgd.ave_beacon_signal,
6283                                -rx_status->signal);
6284
6285         if (ifmgd->rssi_min_thold != ifmgd->rssi_max_thold &&
6286             link->u.mgd.count_beacon_signal >= IEEE80211_SIGNAL_AVE_MIN_COUNT) {
6287                 int sig = -ewma_beacon_signal_read(&link->u.mgd.ave_beacon_signal);
6288                 int last_sig = link->u.mgd.last_ave_beacon_signal;
6289                 struct ieee80211_event event = {
6290                         .type = RSSI_EVENT,
6291                 };
6292
6293                 /*
6294                  * if signal crosses either of the boundaries, invoke callback
6295                  * with appropriate parameters
6296                  */
6297                 if (sig > ifmgd->rssi_max_thold &&
6298                     (last_sig <= ifmgd->rssi_min_thold || last_sig == 0)) {
6299                         link->u.mgd.last_ave_beacon_signal = sig;
6300                         event.u.rssi.data = RSSI_EVENT_HIGH;
6301                         drv_event_callback(local, sdata, &event);
6302                 } else if (sig < ifmgd->rssi_min_thold &&
6303                            (last_sig >= ifmgd->rssi_max_thold ||
6304                            last_sig == 0)) {
6305                         link->u.mgd.last_ave_beacon_signal = sig;
6306                         event.u.rssi.data = RSSI_EVENT_LOW;
6307                         drv_event_callback(local, sdata, &event);
6308                 }
6309         }
6310
6311         if (bss_conf->cqm_rssi_thold &&
6312             link->u.mgd.count_beacon_signal >= IEEE80211_SIGNAL_AVE_MIN_COUNT &&
6313             !(sdata->vif.driver_flags & IEEE80211_VIF_SUPPORTS_CQM_RSSI)) {
6314                 int sig = -ewma_beacon_signal_read(&link->u.mgd.ave_beacon_signal);
6315                 int last_event = link->u.mgd.last_cqm_event_signal;
6316                 int thold = bss_conf->cqm_rssi_thold;
6317                 int hyst = bss_conf->cqm_rssi_hyst;
6318
6319                 if (sig < thold &&
6320                     (last_event == 0 || sig < last_event - hyst)) {
6321                         link->u.mgd.last_cqm_event_signal = sig;
6322                         ieee80211_cqm_rssi_notify(
6323                                 &sdata->vif,
6324                                 NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW,
6325                                 sig, GFP_KERNEL);
6326                 } else if (sig > thold &&
6327                            (last_event == 0 || sig > last_event + hyst)) {
6328                         link->u.mgd.last_cqm_event_signal = sig;
6329                         ieee80211_cqm_rssi_notify(
6330                                 &sdata->vif,
6331                                 NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH,
6332                                 sig, GFP_KERNEL);
6333                 }
6334         }
6335
6336         if (bss_conf->cqm_rssi_low &&
6337             link->u.mgd.count_beacon_signal >= IEEE80211_SIGNAL_AVE_MIN_COUNT) {
6338                 int sig = -ewma_beacon_signal_read(&link->u.mgd.ave_beacon_signal);
6339                 int last_event = link->u.mgd.last_cqm_event_signal;
6340                 int low = bss_conf->cqm_rssi_low;
6341                 int high = bss_conf->cqm_rssi_high;
6342
6343                 if (sig < low &&
6344                     (last_event == 0 || last_event >= low)) {
6345                         link->u.mgd.last_cqm_event_signal = sig;
6346                         ieee80211_cqm_rssi_notify(
6347                                 &sdata->vif,
6348                                 NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW,
6349                                 sig, GFP_KERNEL);
6350                 } else if (sig > high &&
6351                            (last_event == 0 || last_event <= high)) {
6352                         link->u.mgd.last_cqm_event_signal = sig;
6353                         ieee80211_cqm_rssi_notify(
6354                                 &sdata->vif,
6355                                 NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH,
6356                                 sig, GFP_KERNEL);
6357                 }
6358         }
6359 }
6360
6361 static bool ieee80211_rx_our_beacon(const u8 *tx_bssid,
6362                                     struct cfg80211_bss *bss)
6363 {
6364         if (ether_addr_equal(tx_bssid, bss->bssid))
6365                 return true;
6366         if (!bss->transmitted_bss)
6367                 return false;
6368         return ether_addr_equal(tx_bssid, bss->transmitted_bss->bssid);
6369 }
6370
6371 static void ieee80211_ml_reconf_work(struct wiphy *wiphy,
6372                                      struct wiphy_work *work)
6373 {
6374         struct ieee80211_sub_if_data *sdata =
6375                 container_of(work, struct ieee80211_sub_if_data,
6376                              u.mgd.ml_reconf_work.work);
6377         u16 new_valid_links, new_active_links, new_dormant_links;
6378         int ret;
6379
6380         if (!sdata->u.mgd.removed_links)
6381                 return;
6382
6383         sdata_info(sdata,
6384                    "MLO Reconfiguration: work: valid=0x%x, removed=0x%x\n",
6385                    sdata->vif.valid_links, sdata->u.mgd.removed_links);
6386
6387         new_valid_links = sdata->vif.valid_links & ~sdata->u.mgd.removed_links;
6388         if (new_valid_links == sdata->vif.valid_links)
6389                 return;
6390
6391         if (!new_valid_links ||
6392             !(new_valid_links & ~sdata->vif.dormant_links)) {
6393                 sdata_info(sdata, "No valid links after reconfiguration\n");
6394                 ret = -EINVAL;
6395                 goto out;
6396         }
6397
6398         new_active_links = sdata->vif.active_links & ~sdata->u.mgd.removed_links;
6399         if (new_active_links != sdata->vif.active_links) {
6400                 if (!new_active_links)
6401                         new_active_links =
6402                                 BIT(ffs(new_valid_links &
6403                                         ~sdata->vif.dormant_links) - 1);
6404
6405                 ret = ieee80211_set_active_links(&sdata->vif, new_active_links);
6406                 if (ret) {
6407                         sdata_info(sdata,
6408                                    "Failed setting active links\n");
6409                         goto out;
6410                 }
6411         }
6412
6413         new_dormant_links = sdata->vif.dormant_links & ~sdata->u.mgd.removed_links;
6414
6415         ret = ieee80211_vif_set_links(sdata, new_valid_links,
6416                                       new_dormant_links);
6417         if (ret)
6418                 sdata_info(sdata, "Failed setting valid links\n");
6419
6420         ieee80211_vif_cfg_change_notify(sdata, BSS_CHANGED_MLD_VALID_LINKS);
6421
6422 out:
6423         if (!ret)
6424                 cfg80211_links_removed(sdata->dev, sdata->u.mgd.removed_links);
6425         else
6426                 __ieee80211_disconnect(sdata);
6427
6428         sdata->u.mgd.removed_links = 0;
6429 }
6430
6431 static void ieee80211_ml_reconfiguration(struct ieee80211_sub_if_data *sdata,
6432                                          struct ieee802_11_elems *elems)
6433 {
6434         const struct element *sub;
6435         unsigned long removed_links = 0;
6436         u16 link_removal_timeout[IEEE80211_MLD_MAX_NUM_LINKS] = {};
6437         u8 link_id;
6438         u32 delay;
6439
6440         if (!ieee80211_vif_is_mld(&sdata->vif) || !elems->ml_reconf)
6441                 return;
6442
6443         /* Directly parse the sub elements as the common information doesn't
6444          * hold any useful information.
6445          */
6446         for_each_mle_subelement(sub, (const u8 *)elems->ml_reconf,
6447                                 elems->ml_reconf_len) {
6448                 struct ieee80211_mle_per_sta_profile *prof = (void *)sub->data;
6449                 u8 *pos = prof->variable;
6450                 u16 control;
6451
6452                 if (sub->id != IEEE80211_MLE_SUBELEM_PER_STA_PROFILE)
6453                         continue;
6454
6455                 if (!ieee80211_mle_reconf_sta_prof_size_ok(sub->data,
6456                                                            sub->datalen))
6457                         return;
6458
6459                 control = le16_to_cpu(prof->control);
6460                 link_id = control & IEEE80211_MLE_STA_RECONF_CONTROL_LINK_ID;
6461
6462                 removed_links |= BIT(link_id);
6463
6464                 /* the MAC address should not be included, but handle it */
6465                 if (control &
6466                     IEEE80211_MLE_STA_RECONF_CONTROL_STA_MAC_ADDR_PRESENT)
6467                         pos += 6;
6468
6469                 /* According to Draft P802.11be_D3.0, the control should
6470                  * include the AP Removal Timer present. If the AP Removal Timer
6471                  * is not present assume immediate removal.
6472                  */
6473                 if (control &
6474                     IEEE80211_MLE_STA_RECONF_CONTROL_AP_REM_TIMER_PRESENT)
6475                         link_removal_timeout[link_id] = get_unaligned_le16(pos);
6476         }
6477
6478         removed_links &= sdata->vif.valid_links;
6479         if (!removed_links) {
6480                 /* In case the removal was cancelled, abort it */
6481                 if (sdata->u.mgd.removed_links) {
6482                         sdata->u.mgd.removed_links = 0;
6483                         wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
6484                                                   &sdata->u.mgd.ml_reconf_work);
6485                 }
6486                 return;
6487         }
6488
6489         delay = 0;
6490         for_each_set_bit(link_id, &removed_links, IEEE80211_MLD_MAX_NUM_LINKS) {
6491                 struct ieee80211_bss_conf *link_conf =
6492                         sdata_dereference(sdata->vif.link_conf[link_id], sdata);
6493                 u32 link_delay;
6494
6495                 if (!link_conf) {
6496                         removed_links &= ~BIT(link_id);
6497                         continue;
6498                 }
6499
6500                 if (link_removal_timeout[link_id] < 1)
6501                         link_delay = 0;
6502                 else
6503                         link_delay = link_conf->beacon_int *
6504                                 (link_removal_timeout[link_id] - 1);
6505
6506                 if (!delay)
6507                         delay = link_delay;
6508                 else
6509                         delay = min(delay, link_delay);
6510         }
6511
6512         sdata->u.mgd.removed_links = removed_links;
6513         wiphy_delayed_work_queue(sdata->local->hw.wiphy,
6514                                  &sdata->u.mgd.ml_reconf_work,
6515                                  TU_TO_JIFFIES(delay));
6516 }
6517
6518 static int ieee80211_ttlm_set_links(struct ieee80211_sub_if_data *sdata,
6519                                     u16 active_links, u16 dormant_links,
6520                                     u16 suspended_links)
6521 {
6522         u64 changed = 0;
6523         int ret;
6524
6525         if (!active_links) {
6526                 ret = -EINVAL;
6527                 goto out;
6528         }
6529
6530         /* If there is an active negotiated TTLM, it should be discarded by
6531          * the new negotiated/advertised TTLM.
6532          */
6533         if (sdata->vif.neg_ttlm.valid) {
6534                 memset(&sdata->vif.neg_ttlm, 0, sizeof(sdata->vif.neg_ttlm));
6535                 sdata->vif.suspended_links = 0;
6536                 changed = BSS_CHANGED_MLD_TTLM;
6537         }
6538
6539         if (sdata->vif.active_links != active_links) {
6540                 /* usable links are affected when active_links are changed,
6541                  * so notify the driver about the status change
6542                  */
6543                 changed |= BSS_CHANGED_MLD_VALID_LINKS;
6544                 active_links &= sdata->vif.active_links;
6545                 if (!active_links)
6546                         active_links =
6547                                 BIT(__ffs(sdata->vif.valid_links &
6548                                     ~dormant_links));
6549                 ret = ieee80211_set_active_links(&sdata->vif, active_links);
6550                 if (ret) {
6551                         sdata_info(sdata, "Failed to set TTLM active links\n");
6552                         goto out;
6553                 }
6554         }
6555
6556         ret = ieee80211_vif_set_links(sdata, sdata->vif.valid_links,
6557                                       dormant_links);
6558         if (ret) {
6559                 sdata_info(sdata, "Failed to set TTLM dormant links\n");
6560                 goto out;
6561         }
6562
6563         sdata->vif.suspended_links = suspended_links;
6564         if (sdata->vif.suspended_links)
6565                 changed |= BSS_CHANGED_MLD_TTLM;
6566
6567         ieee80211_vif_cfg_change_notify(sdata, changed);
6568
6569 out:
6570         if (ret)
6571                 ieee80211_disconnect(&sdata->vif, false);
6572
6573         return ret;
6574 }
6575
6576 static void ieee80211_tid_to_link_map_work(struct wiphy *wiphy,
6577                                            struct wiphy_work *work)
6578 {
6579         u16 new_active_links, new_dormant_links;
6580         struct ieee80211_sub_if_data *sdata =
6581                 container_of(work, struct ieee80211_sub_if_data,
6582                              u.mgd.ttlm_work.work);
6583
6584         new_active_links = sdata->u.mgd.ttlm_info.map &
6585                            sdata->vif.valid_links;
6586         new_dormant_links = ~sdata->u.mgd.ttlm_info.map &
6587                             sdata->vif.valid_links;
6588
6589         ieee80211_vif_set_links(sdata, sdata->vif.valid_links, 0);
6590         if (ieee80211_ttlm_set_links(sdata, new_active_links, new_dormant_links,
6591                                      0))
6592                 return;
6593
6594         sdata->u.mgd.ttlm_info.active = true;
6595         sdata->u.mgd.ttlm_info.switch_time = 0;
6596 }
6597
6598 static u16 ieee80211_get_ttlm(u8 bm_size, u8 *data)
6599 {
6600         if (bm_size == 1)
6601                 return *data;
6602         else
6603                 return get_unaligned_le16(data);
6604 }
6605
6606 static int
6607 ieee80211_parse_adv_t2l(struct ieee80211_sub_if_data *sdata,
6608                         const struct ieee80211_ttlm_elem *ttlm,
6609                         struct ieee80211_adv_ttlm_info *ttlm_info)
6610 {
6611         /* The element size was already validated in
6612          * ieee80211_tid_to_link_map_size_ok()
6613          */
6614         u8 control, link_map_presence, map_size, tid;
6615         u8 *pos;
6616
6617         memset(ttlm_info, 0, sizeof(*ttlm_info));
6618         pos = (void *)ttlm->optional;
6619         control = ttlm->control;
6620
6621         if ((control & IEEE80211_TTLM_CONTROL_DEF_LINK_MAP) ||
6622             !(control & IEEE80211_TTLM_CONTROL_SWITCH_TIME_PRESENT))
6623                 return 0;
6624
6625         if ((control & IEEE80211_TTLM_CONTROL_DIRECTION) !=
6626             IEEE80211_TTLM_DIRECTION_BOTH) {
6627                 sdata_info(sdata, "Invalid advertised T2L map direction\n");
6628                 return -EINVAL;
6629         }
6630
6631         link_map_presence = *pos;
6632         pos++;
6633
6634         ttlm_info->switch_time = get_unaligned_le16(pos);
6635
6636         /* Since ttlm_info->switch_time == 0 means no switch time, bump it
6637          * by 1.
6638          */
6639         if (!ttlm_info->switch_time)
6640                 ttlm_info->switch_time = 1;
6641
6642         pos += 2;
6643
6644         if (control & IEEE80211_TTLM_CONTROL_EXPECTED_DUR_PRESENT) {
6645                 ttlm_info->duration = pos[0] | pos[1] << 8 | pos[2] << 16;
6646                 pos += 3;
6647         }
6648
6649         if (control & IEEE80211_TTLM_CONTROL_LINK_MAP_SIZE)
6650                 map_size = 1;
6651         else
6652                 map_size = 2;
6653
6654         /* According to Draft P802.11be_D3.0 clause 35.3.7.1.7, an AP MLD shall
6655          * not advertise a TID-to-link mapping that does not map all TIDs to the
6656          * same link set, reject frame if not all links have mapping
6657          */
6658         if (link_map_presence != 0xff) {
6659                 sdata_info(sdata,
6660                            "Invalid advertised T2L mapping presence indicator\n");
6661                 return -EINVAL;
6662         }
6663
6664         ttlm_info->map = ieee80211_get_ttlm(map_size, pos);
6665         if (!ttlm_info->map) {
6666                 sdata_info(sdata,
6667                            "Invalid advertised T2L map for TID 0\n");
6668                 return -EINVAL;
6669         }
6670
6671         pos += map_size;
6672
6673         for (tid = 1; tid < 8; tid++) {
6674                 u16 map = ieee80211_get_ttlm(map_size, pos);
6675
6676                 if (map != ttlm_info->map) {
6677                         sdata_info(sdata, "Invalid advertised T2L map for tid %d\n",
6678                                    tid);
6679                         return -EINVAL;
6680                 }
6681
6682                 pos += map_size;
6683         }
6684         return 0;
6685 }
6686
6687 static void ieee80211_process_adv_ttlm(struct ieee80211_sub_if_data *sdata,
6688                                           struct ieee802_11_elems *elems,
6689                                           u64 beacon_ts)
6690 {
6691         u8 i;
6692         int ret;
6693
6694         if (!ieee80211_vif_is_mld(&sdata->vif))
6695                 return;
6696
6697         if (!elems->ttlm_num) {
6698                 if (sdata->u.mgd.ttlm_info.switch_time) {
6699                         /* if a planned TID-to-link mapping was cancelled -
6700                          * abort it
6701                          */
6702                         wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
6703                                                   &sdata->u.mgd.ttlm_work);
6704                 } else if (sdata->u.mgd.ttlm_info.active) {
6705                         /* if no TID-to-link element, set to default mapping in
6706                          * which all TIDs are mapped to all setup links
6707                          */
6708                         ret = ieee80211_vif_set_links(sdata,
6709                                                       sdata->vif.valid_links,
6710                                                       0);
6711                         if (ret) {
6712                                 sdata_info(sdata, "Failed setting valid/dormant links\n");
6713                                 return;
6714                         }
6715                         ieee80211_vif_cfg_change_notify(sdata,
6716                                                         BSS_CHANGED_MLD_VALID_LINKS);
6717                 }
6718                 memset(&sdata->u.mgd.ttlm_info, 0,
6719                        sizeof(sdata->u.mgd.ttlm_info));
6720                 return;
6721         }
6722
6723         for (i = 0; i < elems->ttlm_num; i++) {
6724                 struct ieee80211_adv_ttlm_info ttlm_info;
6725                 u32 res;
6726
6727                 res = ieee80211_parse_adv_t2l(sdata, elems->ttlm[i],
6728                                               &ttlm_info);
6729
6730                 if (res) {
6731                         __ieee80211_disconnect(sdata);
6732                         return;
6733                 }
6734
6735                 if (ttlm_info.switch_time) {
6736                         u16 beacon_ts_tu, st_tu, delay;
6737                         u32 delay_jiffies;
6738                         u64 mask;
6739
6740                         /* The t2l map switch time is indicated with a partial
6741                          * TSF value (bits 10 to 25), get the partial beacon TS
6742                          * as well, and calc the delay to the start time.
6743                          */
6744                         mask = GENMASK_ULL(25, 10);
6745                         beacon_ts_tu = (beacon_ts & mask) >> 10;
6746                         st_tu = ttlm_info.switch_time;
6747                         delay = st_tu - beacon_ts_tu;
6748
6749                         /*
6750                          * If the switch time is far in the future, then it
6751                          * could also be the previous switch still being
6752                          * announced.
6753                          * We can simply ignore it for now, if it is a future
6754                          * switch the AP will continue to announce it anyway.
6755                          */
6756                         if (delay > IEEE80211_ADV_TTLM_ST_UNDERFLOW)
6757                                 return;
6758
6759                         delay_jiffies = TU_TO_JIFFIES(delay);
6760
6761                         /* Link switching can take time, so schedule it
6762                          * 100ms before to be ready on time
6763                          */
6764                         if (delay_jiffies > IEEE80211_ADV_TTLM_SAFETY_BUFFER_MS)
6765                                 delay_jiffies -=
6766                                         IEEE80211_ADV_TTLM_SAFETY_BUFFER_MS;
6767                         else
6768                                 delay_jiffies = 0;
6769
6770                         sdata->u.mgd.ttlm_info = ttlm_info;
6771                         wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
6772                                                   &sdata->u.mgd.ttlm_work);
6773                         wiphy_delayed_work_queue(sdata->local->hw.wiphy,
6774                                                  &sdata->u.mgd.ttlm_work,
6775                                                  delay_jiffies);
6776                         return;
6777                 }
6778         }
6779 }
6780
6781 static void
6782 ieee80211_mgd_check_cross_link_csa(struct ieee80211_sub_if_data *sdata,
6783                                    int reporting_link_id,
6784                                    struct ieee802_11_elems *elems)
6785 {
6786         const struct element *sta_profiles[IEEE80211_MLD_MAX_NUM_LINKS] = {};
6787         ssize_t sta_profiles_len[IEEE80211_MLD_MAX_NUM_LINKS] = {};
6788         const struct element *sub;
6789         const u8 *subelems;
6790         size_t subelems_len;
6791         u8 common_size;
6792         int link_id;
6793
6794         if (!ieee80211_mle_size_ok((u8 *)elems->ml_basic, elems->ml_basic_len))
6795                 return;
6796
6797         common_size = ieee80211_mle_common_size((u8 *)elems->ml_basic);
6798         subelems = (u8 *)elems->ml_basic + common_size;
6799         subelems_len = elems->ml_basic_len - common_size;
6800
6801         for_each_element_id(sub, IEEE80211_MLE_SUBELEM_PER_STA_PROFILE,
6802                             subelems, subelems_len) {
6803                 struct ieee80211_mle_per_sta_profile *prof = (void *)sub->data;
6804                 struct ieee80211_link_data *link;
6805                 ssize_t len;
6806
6807                 if (!ieee80211_mle_basic_sta_prof_size_ok(sub->data,
6808                                                           sub->datalen))
6809                         continue;
6810
6811                 link_id = le16_get_bits(prof->control,
6812                                         IEEE80211_MLE_STA_CONTROL_LINK_ID);
6813                 /* need a valid link ID, but also not our own, both AP bugs */
6814                 if (link_id == reporting_link_id ||
6815                     link_id >= IEEE80211_MLD_MAX_NUM_LINKS)
6816                         continue;
6817
6818                 link = sdata_dereference(sdata->link[link_id], sdata);
6819                 if (!link)
6820                         continue;
6821
6822                 len = cfg80211_defragment_element(sub, subelems, subelems_len,
6823                                                   NULL, 0,
6824                                                   IEEE80211_MLE_SUBELEM_FRAGMENT);
6825                 if (WARN_ON(len < 0))
6826                         continue;
6827
6828                 sta_profiles[link_id] = sub;
6829                 sta_profiles_len[link_id] = len;
6830         }
6831
6832         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
6833                 struct ieee80211_mle_per_sta_profile *prof;
6834                 struct ieee802_11_elems *prof_elems;
6835                 struct ieee80211_link_data *link;
6836                 ssize_t len;
6837
6838                 if (link_id == reporting_link_id)
6839                         continue;
6840
6841                 link = sdata_dereference(sdata->link[link_id], sdata);
6842                 if (!link)
6843                         continue;
6844
6845                 if (!sta_profiles[link_id]) {
6846                         prof_elems = NULL;
6847                         goto handle;
6848                 }
6849
6850                 /* we can defragment in-place, won't use the buffer again */
6851                 len = cfg80211_defragment_element(sta_profiles[link_id],
6852                                                   subelems, subelems_len,
6853                                                   (void *)sta_profiles[link_id],
6854                                                   sta_profiles_len[link_id],
6855                                                   IEEE80211_MLE_SUBELEM_FRAGMENT);
6856                 if (WARN_ON(len != sta_profiles_len[link_id]))
6857                         continue;
6858
6859                 prof = (void *)sta_profiles[link_id];
6860                 prof_elems = ieee802_11_parse_elems(prof->variable +
6861                                                     (prof->sta_info_len - 1),
6862                                                     len -
6863                                                     (prof->sta_info_len - 1),
6864                                                     false, NULL);
6865
6866                 /* memory allocation failed - let's hope that's transient */
6867                 if (!prof_elems)
6868                         continue;
6869
6870 handle:
6871                 /*
6872                  * FIXME: the timings here are obviously incorrect,
6873                  * but only older Intel drivers seem to care, and
6874                  * those don't have MLO. If you really need this,
6875                  * the problem is having to calculate it with the
6876                  * TSF offset etc. The device_timestamp is still
6877                  * correct, of course.
6878                  */
6879                 ieee80211_sta_process_chanswitch(link, 0, 0, elems, prof_elems,
6880                                                  IEEE80211_CSA_SOURCE_OTHER_LINK);
6881                 kfree(prof_elems);
6882         }
6883 }
6884
6885 static bool ieee80211_mgd_ssid_mismatch(struct ieee80211_sub_if_data *sdata,
6886                                         const struct ieee802_11_elems *elems)
6887 {
6888         struct ieee80211_vif_cfg *cfg = &sdata->vif.cfg;
6889         static u8 zero_ssid[IEEE80211_MAX_SSID_LEN];
6890
6891         if (!elems->ssid)
6892                 return false;
6893
6894         /* hidden SSID: zero length */
6895         if (elems->ssid_len == 0)
6896                 return false;
6897
6898         if (elems->ssid_len != cfg->ssid_len)
6899                 return true;
6900
6901         /* hidden SSID: zeroed out */
6902         if (!memcmp(elems->ssid, zero_ssid, elems->ssid_len))
6903                 return false;
6904
6905         return memcmp(elems->ssid, cfg->ssid, cfg->ssid_len);
6906 }
6907
6908 static void ieee80211_rx_mgmt_beacon(struct ieee80211_link_data *link,
6909                                      struct ieee80211_hdr *hdr, size_t len,
6910                                      struct ieee80211_rx_status *rx_status)
6911 {
6912         struct ieee80211_sub_if_data *sdata = link->sdata;
6913         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
6914         struct ieee80211_bss_conf *bss_conf = link->conf;
6915         struct ieee80211_vif_cfg *vif_cfg = &sdata->vif.cfg;
6916         struct ieee80211_mgmt *mgmt = (void *) hdr;
6917         size_t baselen;
6918         struct ieee802_11_elems *elems;
6919         struct ieee80211_local *local = sdata->local;
6920         struct ieee80211_chanctx_conf *chanctx_conf;
6921         struct ieee80211_supported_band *sband;
6922         struct ieee80211_channel *chan;
6923         struct link_sta_info *link_sta;
6924         struct sta_info *sta;
6925         u64 changed = 0;
6926         bool erp_valid;
6927         u8 erp_value = 0;
6928         u32 ncrc = 0;
6929         u8 *bssid, *variable = mgmt->u.beacon.variable;
6930         u8 deauth_buf[IEEE80211_DEAUTH_FRAME_LEN];
6931         struct ieee80211_elems_parse_params parse_params = {
6932                 .mode = link->u.mgd.conn.mode,
6933                 .link_id = -1,
6934                 .from_ap = true,
6935         };
6936
6937         lockdep_assert_wiphy(local->hw.wiphy);
6938
6939         /* Process beacon from the current BSS */
6940         bssid = ieee80211_get_bssid(hdr, len, sdata->vif.type);
6941         if (ieee80211_is_s1g_beacon(mgmt->frame_control)) {
6942                 struct ieee80211_ext *ext = (void *) mgmt;
6943
6944                 if (ieee80211_is_s1g_short_beacon(ext->frame_control))
6945                         variable = ext->u.s1g_short_beacon.variable;
6946                 else
6947                         variable = ext->u.s1g_beacon.variable;
6948         }
6949
6950         baselen = (u8 *) variable - (u8 *) mgmt;
6951         if (baselen > len)
6952                 return;
6953
6954         parse_params.start = variable;
6955         parse_params.len = len - baselen;
6956
6957         rcu_read_lock();
6958         chanctx_conf = rcu_dereference(bss_conf->chanctx_conf);
6959         if (!chanctx_conf) {
6960                 rcu_read_unlock();
6961                 return;
6962         }
6963
6964         if (ieee80211_rx_status_to_khz(rx_status) !=
6965             ieee80211_channel_to_khz(chanctx_conf->def.chan)) {
6966                 rcu_read_unlock();
6967                 return;
6968         }
6969         chan = chanctx_conf->def.chan;
6970         rcu_read_unlock();
6971
6972         if (ifmgd->assoc_data && ifmgd->assoc_data->need_beacon &&
6973             !WARN_ON(ieee80211_vif_is_mld(&sdata->vif)) &&
6974             ieee80211_rx_our_beacon(bssid, ifmgd->assoc_data->link[0].bss)) {
6975                 parse_params.bss = ifmgd->assoc_data->link[0].bss;
6976                 elems = ieee802_11_parse_elems_full(&parse_params);
6977                 if (!elems)
6978                         return;
6979
6980                 ieee80211_rx_bss_info(link, mgmt, len, rx_status);
6981
6982                 if (elems->dtim_period)
6983                         link->u.mgd.dtim_period = elems->dtim_period;
6984                 link->u.mgd.have_beacon = true;
6985                 ifmgd->assoc_data->need_beacon = false;
6986                 if (ieee80211_hw_check(&local->hw, TIMING_BEACON_ONLY) &&
6987                     !ieee80211_is_s1g_beacon(hdr->frame_control)) {
6988                         bss_conf->sync_tsf =
6989                                 le64_to_cpu(mgmt->u.beacon.timestamp);
6990                         bss_conf->sync_device_ts =
6991                                 rx_status->device_timestamp;
6992                         bss_conf->sync_dtim_count = elems->dtim_count;
6993                 }
6994
6995                 if (elems->mbssid_config_ie)
6996                         bss_conf->profile_periodicity =
6997                                 elems->mbssid_config_ie->profile_periodicity;
6998                 else
6999                         bss_conf->profile_periodicity = 0;
7000
7001                 if (elems->ext_capab_len >= 11 &&
7002                     (elems->ext_capab[10] & WLAN_EXT_CAPA11_EMA_SUPPORT))
7003                         bss_conf->ema_ap = true;
7004                 else
7005                         bss_conf->ema_ap = false;
7006
7007                 /* continue assoc process */
7008                 ifmgd->assoc_data->timeout = jiffies;
7009                 ifmgd->assoc_data->timeout_started = true;
7010                 run_again(sdata, ifmgd->assoc_data->timeout);
7011                 kfree(elems);
7012                 return;
7013         }
7014
7015         if (!ifmgd->associated ||
7016             !ieee80211_rx_our_beacon(bssid, bss_conf->bss))
7017                 return;
7018         bssid = link->u.mgd.bssid;
7019
7020         if (!(rx_status->flag & RX_FLAG_NO_SIGNAL_VAL))
7021                 ieee80211_handle_beacon_sig(link, ifmgd, bss_conf,
7022                                             local, rx_status);
7023
7024         if (ifmgd->flags & IEEE80211_STA_CONNECTION_POLL) {
7025                 mlme_dbg_ratelimited(sdata,
7026                                      "cancelling AP probe due to a received beacon\n");
7027                 ieee80211_reset_ap_probe(sdata);
7028         }
7029
7030         /*
7031          * Push the beacon loss detection into the future since
7032          * we are processing a beacon from the AP just now.
7033          */
7034         ieee80211_sta_reset_beacon_monitor(sdata);
7035
7036         /* TODO: CRC urrently not calculated on S1G Beacon Compatibility
7037          * element (which carries the beacon interval). Don't forget to add a
7038          * bit to care_about_ies[] above if mac80211 is interested in a
7039          * changing S1G element.
7040          */
7041         if (!ieee80211_is_s1g_beacon(hdr->frame_control))
7042                 ncrc = crc32_be(0, (void *)&mgmt->u.beacon.beacon_int, 4);
7043         parse_params.bss = bss_conf->bss;
7044         parse_params.filter = care_about_ies;
7045         parse_params.crc = ncrc;
7046         elems = ieee802_11_parse_elems_full(&parse_params);
7047         if (!elems)
7048                 return;
7049
7050         if (rx_status->flag & RX_FLAG_DECRYPTED &&
7051             ieee80211_mgd_ssid_mismatch(sdata, elems)) {
7052                 sdata_info(sdata, "SSID mismatch for AP %pM, disconnect\n",
7053                            sdata->vif.cfg.ap_addr);
7054                 __ieee80211_disconnect(sdata);
7055                 return;
7056         }
7057
7058         ncrc = elems->crc;
7059
7060         if (ieee80211_hw_check(&local->hw, PS_NULLFUNC_STACK) &&
7061             ieee80211_check_tim(elems->tim, elems->tim_len, vif_cfg->aid)) {
7062                 if (local->hw.conf.dynamic_ps_timeout > 0) {
7063                         if (local->hw.conf.flags & IEEE80211_CONF_PS) {
7064                                 local->hw.conf.flags &= ~IEEE80211_CONF_PS;
7065                                 ieee80211_hw_config(local,
7066                                                     IEEE80211_CONF_CHANGE_PS);
7067                         }
7068                         ieee80211_send_nullfunc(local, sdata, false);
7069                 } else if (!local->pspolling && sdata->u.mgd.powersave) {
7070                         local->pspolling = true;
7071
7072                         /*
7073                          * Here is assumed that the driver will be
7074                          * able to send ps-poll frame and receive a
7075                          * response even though power save mode is
7076                          * enabled, but some drivers might require
7077                          * to disable power save here. This needs
7078                          * to be investigated.
7079                          */
7080                         ieee80211_send_pspoll(local, sdata);
7081                 }
7082         }
7083
7084         if (sdata->vif.p2p ||
7085             sdata->vif.driver_flags & IEEE80211_VIF_GET_NOA_UPDATE) {
7086                 struct ieee80211_p2p_noa_attr noa = {};
7087                 int ret;
7088
7089                 ret = cfg80211_get_p2p_attr(variable,
7090                                             len - baselen,
7091                                             IEEE80211_P2P_ATTR_ABSENCE_NOTICE,
7092                                             (u8 *) &noa, sizeof(noa));
7093                 if (ret >= 2) {
7094                         if (link->u.mgd.p2p_noa_index != noa.index) {
7095                                 /* valid noa_attr and index changed */
7096                                 link->u.mgd.p2p_noa_index = noa.index;
7097                                 memcpy(&bss_conf->p2p_noa_attr, &noa, sizeof(noa));
7098                                 changed |= BSS_CHANGED_P2P_PS;
7099                                 /*
7100                                  * make sure we update all information, the CRC
7101                                  * mechanism doesn't look at P2P attributes.
7102                                  */
7103                                 link->u.mgd.beacon_crc_valid = false;
7104                         }
7105                 } else if (link->u.mgd.p2p_noa_index != -1) {
7106                         /* noa_attr not found and we had valid noa_attr before */
7107                         link->u.mgd.p2p_noa_index = -1;
7108                         memset(&bss_conf->p2p_noa_attr, 0, sizeof(bss_conf->p2p_noa_attr));
7109                         changed |= BSS_CHANGED_P2P_PS;
7110                         link->u.mgd.beacon_crc_valid = false;
7111                 }
7112         }
7113
7114         /*
7115          * Update beacon timing and dtim count on every beacon appearance. This
7116          * will allow the driver to use the most updated values. Do it before
7117          * comparing this one with last received beacon.
7118          * IMPORTANT: These parameters would possibly be out of sync by the time
7119          * the driver will use them. The synchronized view is currently
7120          * guaranteed only in certain callbacks.
7121          */
7122         if (ieee80211_hw_check(&local->hw, TIMING_BEACON_ONLY) &&
7123             !ieee80211_is_s1g_beacon(hdr->frame_control)) {
7124                 bss_conf->sync_tsf =
7125                         le64_to_cpu(mgmt->u.beacon.timestamp);
7126                 bss_conf->sync_device_ts =
7127                         rx_status->device_timestamp;
7128                 bss_conf->sync_dtim_count = elems->dtim_count;
7129         }
7130
7131         if ((ncrc == link->u.mgd.beacon_crc && link->u.mgd.beacon_crc_valid) ||
7132             ieee80211_is_s1g_short_beacon(mgmt->frame_control))
7133                 goto free;
7134         link->u.mgd.beacon_crc = ncrc;
7135         link->u.mgd.beacon_crc_valid = true;
7136
7137         ieee80211_rx_bss_info(link, mgmt, len, rx_status);
7138
7139         ieee80211_sta_process_chanswitch(link, rx_status->mactime,
7140                                          rx_status->device_timestamp,
7141                                          elems, elems,
7142                                          IEEE80211_CSA_SOURCE_BEACON);
7143
7144         /* note that after this elems->ml_basic can no longer be used fully */
7145         ieee80211_mgd_check_cross_link_csa(sdata, rx_status->link_id, elems);
7146
7147         ieee80211_mgd_update_bss_param_ch_cnt(sdata, bss_conf, elems);
7148
7149         if (!link->u.mgd.disable_wmm_tracking &&
7150             ieee80211_sta_wmm_params(local, link, elems->wmm_param,
7151                                      elems->wmm_param_len,
7152                                      elems->mu_edca_param_set))
7153                 changed |= BSS_CHANGED_QOS;
7154
7155         /*
7156          * If we haven't had a beacon before, tell the driver about the
7157          * DTIM period (and beacon timing if desired) now.
7158          */
7159         if (!link->u.mgd.have_beacon) {
7160                 /* a few bogus AP send dtim_period = 0 or no TIM IE */
7161                 bss_conf->dtim_period = elems->dtim_period ?: 1;
7162
7163                 changed |= BSS_CHANGED_BEACON_INFO;
7164                 link->u.mgd.have_beacon = true;
7165
7166                 ieee80211_recalc_ps(local);
7167
7168                 ieee80211_recalc_ps_vif(sdata);
7169         }
7170
7171         if (elems->erp_info) {
7172                 erp_valid = true;
7173                 erp_value = elems->erp_info[0];
7174         } else {
7175                 erp_valid = false;
7176         }
7177
7178         if (!ieee80211_is_s1g_beacon(hdr->frame_control))
7179                 changed |= ieee80211_handle_bss_capability(link,
7180                                 le16_to_cpu(mgmt->u.beacon.capab_info),
7181                                 erp_valid, erp_value);
7182
7183         sta = sta_info_get(sdata, sdata->vif.cfg.ap_addr);
7184         if (WARN_ON(!sta)) {
7185                 goto free;
7186         }
7187         link_sta = rcu_dereference_protected(sta->link[link->link_id],
7188                                              lockdep_is_held(&local->hw.wiphy->mtx));
7189         if (WARN_ON(!link_sta)) {
7190                 goto free;
7191         }
7192
7193         if (WARN_ON(!bss_conf->chanreq.oper.chan))
7194                 goto free;
7195
7196         sband = local->hw.wiphy->bands[bss_conf->chanreq.oper.chan->band];
7197
7198         changed |= ieee80211_recalc_twt_req(sdata, sband, link, link_sta, elems);
7199
7200         if (ieee80211_config_bw(link, elems, true, &changed, "beacon")) {
7201                 ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH,
7202                                        WLAN_REASON_DEAUTH_LEAVING,
7203                                        true, deauth_buf);
7204                 ieee80211_report_disconnect(sdata, deauth_buf,
7205                                             sizeof(deauth_buf), true,
7206                                             WLAN_REASON_DEAUTH_LEAVING,
7207                                             false);
7208                 goto free;
7209         }
7210
7211         if (elems->opmode_notif)
7212                 ieee80211_vht_handle_opmode(sdata, link_sta,
7213                                             *elems->opmode_notif,
7214                                             rx_status->band);
7215
7216         changed |= ieee80211_handle_pwr_constr(link, chan, mgmt,
7217                                                elems->country_elem,
7218                                                elems->country_elem_len,
7219                                                elems->pwr_constr_elem,
7220                                                elems->cisco_dtpc_elem);
7221
7222         ieee80211_ml_reconfiguration(sdata, elems);
7223         ieee80211_process_adv_ttlm(sdata, elems,
7224                                       le64_to_cpu(mgmt->u.beacon.timestamp));
7225
7226         ieee80211_link_info_change_notify(sdata, link, changed);
7227 free:
7228         kfree(elems);
7229 }
7230
7231 static void ieee80211_apply_neg_ttlm(struct ieee80211_sub_if_data *sdata,
7232                                      struct ieee80211_neg_ttlm neg_ttlm)
7233 {
7234         u16 new_active_links, new_dormant_links, new_suspended_links, map = 0;
7235         u8 i;
7236
7237         for (i = 0; i < IEEE80211_TTLM_NUM_TIDS; i++)
7238                 map |= neg_ttlm.downlink[i] | neg_ttlm.uplink[i];
7239
7240         /* If there is an active TTLM, unset previously suspended links */
7241         if (sdata->vif.neg_ttlm.valid)
7242                 sdata->vif.dormant_links &= ~sdata->vif.suspended_links;
7243
7244         /* exclude links that are already disabled by advertised TTLM */
7245         new_active_links =
7246                 map & sdata->vif.valid_links & ~sdata->vif.dormant_links;
7247         new_suspended_links =
7248                 (~map & sdata->vif.valid_links) & ~sdata->vif.dormant_links;
7249         new_dormant_links = sdata->vif.dormant_links | new_suspended_links;
7250         if (ieee80211_ttlm_set_links(sdata, new_active_links,
7251                                      new_dormant_links, new_suspended_links))
7252                 return;
7253
7254         sdata->vif.neg_ttlm = neg_ttlm;
7255         sdata->vif.neg_ttlm.valid = true;
7256 }
7257
7258 static void ieee80211_neg_ttlm_timeout_work(struct wiphy *wiphy,
7259                                             struct wiphy_work *work)
7260 {
7261         struct ieee80211_sub_if_data *sdata =
7262                 container_of(work, struct ieee80211_sub_if_data,
7263                              u.mgd.neg_ttlm_timeout_work.work);
7264
7265         sdata_info(sdata,
7266                    "No negotiated TTLM response from AP, disconnecting.\n");
7267
7268         __ieee80211_disconnect(sdata);
7269 }
7270
7271 static void
7272 ieee80211_neg_ttlm_add_suggested_map(struct sk_buff *skb,
7273                                      struct ieee80211_neg_ttlm *neg_ttlm)
7274 {
7275         u8 i, direction[IEEE80211_TTLM_MAX_CNT];
7276
7277         if (memcmp(neg_ttlm->downlink, neg_ttlm->uplink,
7278                    sizeof(neg_ttlm->downlink))) {
7279                 direction[0] = IEEE80211_TTLM_DIRECTION_DOWN;
7280                 direction[1] = IEEE80211_TTLM_DIRECTION_UP;
7281         } else {
7282                 direction[0] = IEEE80211_TTLM_DIRECTION_BOTH;
7283         }
7284
7285         for (i = 0; i < ARRAY_SIZE(direction); i++) {
7286                 u8 tid, len, map_ind = 0, *len_pos, *map_ind_pos, *pos;
7287                 __le16 map;
7288
7289                 len = sizeof(struct ieee80211_ttlm_elem) + 1 + 1;
7290
7291                 pos = skb_put(skb, len + 2);
7292                 *pos++ = WLAN_EID_EXTENSION;
7293                 len_pos = pos++;
7294                 *pos++ = WLAN_EID_EXT_TID_TO_LINK_MAPPING;
7295                 *pos++ = direction[i];
7296                 map_ind_pos = pos++;
7297                 for (tid = 0; tid < IEEE80211_TTLM_NUM_TIDS; tid++) {
7298                         map = direction[i] == IEEE80211_TTLM_DIRECTION_UP ?
7299                                 cpu_to_le16(neg_ttlm->uplink[tid]) :
7300                                 cpu_to_le16(neg_ttlm->downlink[tid]);
7301                         if (!map)
7302                                 continue;
7303
7304                         len += 2;
7305                         map_ind |= BIT(tid);
7306                         skb_put_data(skb, &map, sizeof(map));
7307                 }
7308
7309                 *map_ind_pos = map_ind;
7310                 *len_pos = len;
7311
7312                 if (direction[i] == IEEE80211_TTLM_DIRECTION_BOTH)
7313                         break;
7314         }
7315 }
7316
7317 static void
7318 ieee80211_send_neg_ttlm_req(struct ieee80211_sub_if_data *sdata,
7319                             struct ieee80211_neg_ttlm *neg_ttlm,
7320                             u8 dialog_token)
7321 {
7322         struct ieee80211_local *local = sdata->local;
7323         struct ieee80211_mgmt *mgmt;
7324         struct sk_buff *skb;
7325         int hdr_len = offsetofend(struct ieee80211_mgmt, u.action.u.ttlm_req);
7326         int ttlm_max_len = 2 + 1 + sizeof(struct ieee80211_ttlm_elem) + 1 +
7327                 2 * 2 * IEEE80211_TTLM_NUM_TIDS;
7328
7329         skb = dev_alloc_skb(local->tx_headroom + hdr_len + ttlm_max_len);
7330         if (!skb)
7331                 return;
7332
7333         skb_reserve(skb, local->tx_headroom);
7334         mgmt = skb_put_zero(skb, hdr_len);
7335         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
7336                                           IEEE80211_STYPE_ACTION);
7337         memcpy(mgmt->da, sdata->vif.cfg.ap_addr, ETH_ALEN);
7338         memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
7339         memcpy(mgmt->bssid, sdata->vif.cfg.ap_addr, ETH_ALEN);
7340
7341         mgmt->u.action.category = WLAN_CATEGORY_PROTECTED_EHT;
7342         mgmt->u.action.u.ttlm_req.action_code =
7343                 WLAN_PROTECTED_EHT_ACTION_TTLM_REQ;
7344         mgmt->u.action.u.ttlm_req.dialog_token = dialog_token;
7345         ieee80211_neg_ttlm_add_suggested_map(skb, neg_ttlm);
7346         ieee80211_tx_skb(sdata, skb);
7347 }
7348
7349 int ieee80211_req_neg_ttlm(struct ieee80211_sub_if_data *sdata,
7350                            struct cfg80211_ttlm_params *params)
7351 {
7352         struct ieee80211_neg_ttlm neg_ttlm = {};
7353         u8 i;
7354
7355         if (!ieee80211_vif_is_mld(&sdata->vif) ||
7356             !(sdata->vif.cfg.mld_capa_op &
7357               IEEE80211_MLD_CAP_OP_TID_TO_LINK_MAP_NEG_SUPP))
7358                 return -EINVAL;
7359
7360         for (i = 0; i < IEEE80211_TTLM_NUM_TIDS; i++) {
7361                 if ((params->dlink[i] & ~sdata->vif.valid_links) ||
7362                     (params->ulink[i] & ~sdata->vif.valid_links))
7363                         return -EINVAL;
7364
7365                 neg_ttlm.downlink[i] = params->dlink[i];
7366                 neg_ttlm.uplink[i] = params->ulink[i];
7367         }
7368
7369         if (drv_can_neg_ttlm(sdata->local, sdata, &neg_ttlm) !=
7370             NEG_TTLM_RES_ACCEPT)
7371                 return -EINVAL;
7372
7373         ieee80211_apply_neg_ttlm(sdata, neg_ttlm);
7374         sdata->u.mgd.dialog_token_alloc++;
7375         ieee80211_send_neg_ttlm_req(sdata, &sdata->vif.neg_ttlm,
7376                                     sdata->u.mgd.dialog_token_alloc);
7377         wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
7378                                   &sdata->u.mgd.neg_ttlm_timeout_work);
7379         wiphy_delayed_work_queue(sdata->local->hw.wiphy,
7380                                  &sdata->u.mgd.neg_ttlm_timeout_work,
7381                                  IEEE80211_NEG_TTLM_REQ_TIMEOUT);
7382         return 0;
7383 }
7384
7385 static void
7386 ieee80211_send_neg_ttlm_res(struct ieee80211_sub_if_data *sdata,
7387                             enum ieee80211_neg_ttlm_res ttlm_res,
7388                             u8 dialog_token,
7389                             struct ieee80211_neg_ttlm *neg_ttlm)
7390 {
7391         struct ieee80211_local *local = sdata->local;
7392         struct ieee80211_mgmt *mgmt;
7393         struct sk_buff *skb;
7394         int hdr_len = offsetofend(struct ieee80211_mgmt, u.action.u.ttlm_res);
7395         int ttlm_max_len = 2 + 1 + sizeof(struct ieee80211_ttlm_elem) + 1 +
7396                 2 * 2 * IEEE80211_TTLM_NUM_TIDS;
7397
7398         skb = dev_alloc_skb(local->tx_headroom + hdr_len + ttlm_max_len);
7399         if (!skb)
7400                 return;
7401
7402         skb_reserve(skb, local->tx_headroom);
7403         mgmt = skb_put_zero(skb, hdr_len);
7404         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
7405                                           IEEE80211_STYPE_ACTION);
7406         memcpy(mgmt->da, sdata->vif.cfg.ap_addr, ETH_ALEN);
7407         memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
7408         memcpy(mgmt->bssid, sdata->vif.cfg.ap_addr, ETH_ALEN);
7409
7410         mgmt->u.action.category = WLAN_CATEGORY_PROTECTED_EHT;
7411         mgmt->u.action.u.ttlm_res.action_code =
7412                 WLAN_PROTECTED_EHT_ACTION_TTLM_RES;
7413         mgmt->u.action.u.ttlm_res.dialog_token = dialog_token;
7414         switch (ttlm_res) {
7415         default:
7416                 WARN_ON(1);
7417                 fallthrough;
7418         case NEG_TTLM_RES_REJECT:
7419                 mgmt->u.action.u.ttlm_res.status_code =
7420                         WLAN_STATUS_DENIED_TID_TO_LINK_MAPPING;
7421                 break;
7422         case NEG_TTLM_RES_ACCEPT:
7423                 mgmt->u.action.u.ttlm_res.status_code = WLAN_STATUS_SUCCESS;
7424                 break;
7425         case NEG_TTLM_RES_SUGGEST_PREFERRED:
7426                 mgmt->u.action.u.ttlm_res.status_code =
7427                         WLAN_STATUS_PREF_TID_TO_LINK_MAPPING_SUGGESTED;
7428                 ieee80211_neg_ttlm_add_suggested_map(skb, neg_ttlm);
7429                 break;
7430         }
7431
7432         ieee80211_tx_skb(sdata, skb);
7433 }
7434
7435 static int
7436 ieee80211_parse_neg_ttlm(struct ieee80211_sub_if_data *sdata,
7437                          const struct ieee80211_ttlm_elem *ttlm,
7438                          struct ieee80211_neg_ttlm *neg_ttlm,
7439                          u8 *direction)
7440 {
7441         u8 control, link_map_presence, map_size, tid;
7442         u8 *pos;
7443
7444         /* The element size was already validated in
7445          * ieee80211_tid_to_link_map_size_ok()
7446          */
7447         pos = (void *)ttlm->optional;
7448
7449         control = ttlm->control;
7450
7451         /* mapping switch time and expected duration fields are not expected
7452          * in case of negotiated TTLM
7453          */
7454         if (control & (IEEE80211_TTLM_CONTROL_SWITCH_TIME_PRESENT |
7455                        IEEE80211_TTLM_CONTROL_EXPECTED_DUR_PRESENT)) {
7456                 mlme_dbg(sdata,
7457                          "Invalid TTLM element in negotiated TTLM request\n");
7458                 return -EINVAL;
7459         }
7460
7461         if (control & IEEE80211_TTLM_CONTROL_DEF_LINK_MAP) {
7462                 for (tid = 0; tid < IEEE80211_TTLM_NUM_TIDS; tid++) {
7463                         neg_ttlm->downlink[tid] = sdata->vif.valid_links;
7464                         neg_ttlm->uplink[tid] = sdata->vif.valid_links;
7465                 }
7466                 *direction = IEEE80211_TTLM_DIRECTION_BOTH;
7467                 return 0;
7468         }
7469
7470         *direction = u8_get_bits(control, IEEE80211_TTLM_CONTROL_DIRECTION);
7471         if (*direction != IEEE80211_TTLM_DIRECTION_DOWN &&
7472             *direction != IEEE80211_TTLM_DIRECTION_UP &&
7473             *direction != IEEE80211_TTLM_DIRECTION_BOTH)
7474                 return -EINVAL;
7475
7476         link_map_presence = *pos;
7477         pos++;
7478
7479         if (control & IEEE80211_TTLM_CONTROL_LINK_MAP_SIZE)
7480                 map_size = 1;
7481         else
7482                 map_size = 2;
7483
7484         for (tid = 0; tid < IEEE80211_TTLM_NUM_TIDS; tid++) {
7485                 u16 map;
7486
7487                 if (link_map_presence & BIT(tid)) {
7488                         map = ieee80211_get_ttlm(map_size, pos);
7489                         if (!map) {
7490                                 mlme_dbg(sdata,
7491                                          "No active links for TID %d", tid);
7492                                 return -EINVAL;
7493                         }
7494                 } else {
7495                         map = 0;
7496                 }
7497
7498                 switch (*direction) {
7499                 case IEEE80211_TTLM_DIRECTION_BOTH:
7500                         neg_ttlm->downlink[tid] = map;
7501                         neg_ttlm->uplink[tid] = map;
7502                         break;
7503                 case IEEE80211_TTLM_DIRECTION_DOWN:
7504                         neg_ttlm->downlink[tid] = map;
7505                         break;
7506                 case IEEE80211_TTLM_DIRECTION_UP:
7507                         neg_ttlm->uplink[tid] = map;
7508                         break;
7509                 default:
7510                         return -EINVAL;
7511                 }
7512                 pos += map_size;
7513         }
7514         return 0;
7515 }
7516
7517 void ieee80211_process_neg_ttlm_req(struct ieee80211_sub_if_data *sdata,
7518                                     struct ieee80211_mgmt *mgmt, size_t len)
7519 {
7520         u8 dialog_token, direction[IEEE80211_TTLM_MAX_CNT] = {}, i;
7521         size_t ies_len;
7522         enum ieee80211_neg_ttlm_res ttlm_res = NEG_TTLM_RES_ACCEPT;
7523         struct ieee802_11_elems *elems = NULL;
7524         struct ieee80211_neg_ttlm neg_ttlm = {};
7525
7526         BUILD_BUG_ON(ARRAY_SIZE(direction) != ARRAY_SIZE(elems->ttlm));
7527
7528         if (!ieee80211_vif_is_mld(&sdata->vif))
7529                 return;
7530
7531         dialog_token = mgmt->u.action.u.ttlm_req.dialog_token;
7532         ies_len  = len - offsetof(struct ieee80211_mgmt,
7533                                   u.action.u.ttlm_req.variable);
7534         elems = ieee802_11_parse_elems(mgmt->u.action.u.ttlm_req.variable,
7535                                        ies_len, true, NULL);
7536         if (!elems) {
7537                 ttlm_res = NEG_TTLM_RES_REJECT;
7538                 goto out;
7539         }
7540
7541         for (i = 0; i < elems->ttlm_num; i++) {
7542                 if (ieee80211_parse_neg_ttlm(sdata, elems->ttlm[i],
7543                                              &neg_ttlm, &direction[i]) ||
7544                     (direction[i] == IEEE80211_TTLM_DIRECTION_BOTH &&
7545                      elems->ttlm_num != 1)) {
7546                         ttlm_res = NEG_TTLM_RES_REJECT;
7547                         goto out;
7548                 }
7549         }
7550
7551         if (!elems->ttlm_num ||
7552             (elems->ttlm_num == 2 && direction[0] == direction[1])) {
7553                 ttlm_res = NEG_TTLM_RES_REJECT;
7554                 goto out;
7555         }
7556
7557         for (i = 0; i < IEEE80211_TTLM_NUM_TIDS; i++) {
7558                 if ((neg_ttlm.downlink[i] &&
7559                      (neg_ttlm.downlink[i] & ~sdata->vif.valid_links)) ||
7560                     (neg_ttlm.uplink[i] &&
7561                      (neg_ttlm.uplink[i] & ~sdata->vif.valid_links))) {
7562                         ttlm_res = NEG_TTLM_RES_REJECT;
7563                         goto out;
7564                 }
7565         }
7566
7567         ttlm_res = drv_can_neg_ttlm(sdata->local, sdata, &neg_ttlm);
7568
7569         if (ttlm_res != NEG_TTLM_RES_ACCEPT)
7570                 goto out;
7571
7572         ieee80211_apply_neg_ttlm(sdata, neg_ttlm);
7573 out:
7574         kfree(elems);
7575         ieee80211_send_neg_ttlm_res(sdata, ttlm_res, dialog_token, &neg_ttlm);
7576 }
7577
7578 void ieee80211_process_neg_ttlm_res(struct ieee80211_sub_if_data *sdata,
7579                                     struct ieee80211_mgmt *mgmt, size_t len)
7580 {
7581         if (!ieee80211_vif_is_mld(&sdata->vif) ||
7582             mgmt->u.action.u.ttlm_req.dialog_token !=
7583             sdata->u.mgd.dialog_token_alloc)
7584                 return;
7585
7586         wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
7587                                   &sdata->u.mgd.neg_ttlm_timeout_work);
7588
7589         /* MLD station sends a TID to link mapping request, mainly to handle
7590          * BTM (BSS transition management) request, in which case it needs to
7591          * restrict the active links set.
7592          * In this case it's not expected that the MLD AP will reject the
7593          * negotiated TTLM request.
7594          * This can be better implemented in the future, to handle request
7595          * rejections.
7596          */
7597         if (mgmt->u.action.u.ttlm_res.status_code != WLAN_STATUS_SUCCESS)
7598                 __ieee80211_disconnect(sdata);
7599 }
7600
7601 static void ieee80211_teardown_ttlm_work(struct wiphy *wiphy,
7602                                          struct wiphy_work *work)
7603 {
7604         u16 new_dormant_links;
7605         struct ieee80211_sub_if_data *sdata =
7606                 container_of(work, struct ieee80211_sub_if_data,
7607                              u.mgd.teardown_ttlm_work);
7608
7609         if (!sdata->vif.neg_ttlm.valid)
7610                 return;
7611
7612         memset(&sdata->vif.neg_ttlm, 0, sizeof(sdata->vif.neg_ttlm));
7613         new_dormant_links =
7614                 sdata->vif.dormant_links & ~sdata->vif.suspended_links;
7615         sdata->vif.suspended_links = 0;
7616         ieee80211_vif_set_links(sdata, sdata->vif.valid_links,
7617                                 new_dormant_links);
7618         ieee80211_vif_cfg_change_notify(sdata, BSS_CHANGED_MLD_TTLM |
7619                                                BSS_CHANGED_MLD_VALID_LINKS);
7620 }
7621
7622 void ieee80211_send_teardown_neg_ttlm(struct ieee80211_vif *vif)
7623 {
7624         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
7625         struct ieee80211_local *local = sdata->local;
7626         struct ieee80211_mgmt *mgmt;
7627         struct sk_buff *skb;
7628         int frame_len = offsetofend(struct ieee80211_mgmt,
7629                                   u.action.u.ttlm_tear_down);
7630         struct ieee80211_tx_info *info;
7631
7632         skb = dev_alloc_skb(local->hw.extra_tx_headroom + frame_len);
7633         if (!skb)
7634                 return;
7635
7636         skb_reserve(skb, local->hw.extra_tx_headroom);
7637         mgmt = skb_put_zero(skb, frame_len);
7638         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
7639                                           IEEE80211_STYPE_ACTION);
7640         memcpy(mgmt->da, sdata->vif.cfg.ap_addr, ETH_ALEN);
7641         memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
7642         memcpy(mgmt->bssid, sdata->vif.cfg.ap_addr, ETH_ALEN);
7643
7644         mgmt->u.action.category = WLAN_CATEGORY_PROTECTED_EHT;
7645         mgmt->u.action.u.ttlm_tear_down.action_code =
7646                 WLAN_PROTECTED_EHT_ACTION_TTLM_TEARDOWN;
7647
7648         info = IEEE80211_SKB_CB(skb);
7649         info->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
7650         info->status_data = IEEE80211_STATUS_TYPE_NEG_TTLM;
7651         ieee80211_tx_skb(sdata, skb);
7652 }
7653 EXPORT_SYMBOL(ieee80211_send_teardown_neg_ttlm);
7654
7655 void ieee80211_sta_rx_queued_ext(struct ieee80211_sub_if_data *sdata,
7656                                  struct sk_buff *skb)
7657 {
7658         struct ieee80211_link_data *link = &sdata->deflink;
7659         struct ieee80211_rx_status *rx_status;
7660         struct ieee80211_hdr *hdr;
7661         u16 fc;
7662
7663         lockdep_assert_wiphy(sdata->local->hw.wiphy);
7664
7665         rx_status = (struct ieee80211_rx_status *) skb->cb;
7666         hdr = (struct ieee80211_hdr *) skb->data;
7667         fc = le16_to_cpu(hdr->frame_control);
7668
7669         switch (fc & IEEE80211_FCTL_STYPE) {
7670         case IEEE80211_STYPE_S1G_BEACON:
7671                 ieee80211_rx_mgmt_beacon(link, hdr, skb->len, rx_status);
7672                 break;
7673         }
7674 }
7675
7676 void ieee80211_sta_rx_queued_mgmt(struct ieee80211_sub_if_data *sdata,
7677                                   struct sk_buff *skb)
7678 {
7679         struct ieee80211_link_data *link = &sdata->deflink;
7680         struct ieee80211_rx_status *rx_status;
7681         struct ieee802_11_elems *elems;
7682         struct ieee80211_mgmt *mgmt;
7683         u16 fc;
7684         int ies_len;
7685
7686         lockdep_assert_wiphy(sdata->local->hw.wiphy);
7687
7688         rx_status = (struct ieee80211_rx_status *) skb->cb;
7689         mgmt = (struct ieee80211_mgmt *) skb->data;
7690         fc = le16_to_cpu(mgmt->frame_control);
7691
7692         if (rx_status->link_valid) {
7693                 link = sdata_dereference(sdata->link[rx_status->link_id],
7694                                          sdata);
7695                 if (!link)
7696                         return;
7697         }
7698
7699         switch (fc & IEEE80211_FCTL_STYPE) {
7700         case IEEE80211_STYPE_BEACON:
7701                 ieee80211_rx_mgmt_beacon(link, (void *)mgmt,
7702                                          skb->len, rx_status);
7703                 break;
7704         case IEEE80211_STYPE_PROBE_RESP:
7705                 ieee80211_rx_mgmt_probe_resp(link, skb);
7706                 break;
7707         case IEEE80211_STYPE_AUTH:
7708                 ieee80211_rx_mgmt_auth(sdata, mgmt, skb->len);
7709                 break;
7710         case IEEE80211_STYPE_DEAUTH:
7711                 ieee80211_rx_mgmt_deauth(sdata, mgmt, skb->len);
7712                 break;
7713         case IEEE80211_STYPE_DISASSOC:
7714                 ieee80211_rx_mgmt_disassoc(sdata, mgmt, skb->len);
7715                 break;
7716         case IEEE80211_STYPE_ASSOC_RESP:
7717         case IEEE80211_STYPE_REASSOC_RESP:
7718                 ieee80211_rx_mgmt_assoc_resp(sdata, mgmt, skb->len);
7719                 break;
7720         case IEEE80211_STYPE_ACTION:
7721                 if (!sdata->u.mgd.associated ||
7722                     !ether_addr_equal(mgmt->bssid, sdata->vif.cfg.ap_addr))
7723                         break;
7724
7725                 switch (mgmt->u.action.category) {
7726                 case WLAN_CATEGORY_SPECTRUM_MGMT:
7727                         ies_len = skb->len -
7728                                   offsetof(struct ieee80211_mgmt,
7729                                            u.action.u.chan_switch.variable);
7730
7731                         if (ies_len < 0)
7732                                 break;
7733
7734                         /* CSA IE cannot be overridden, no need for BSSID */
7735                         elems = ieee802_11_parse_elems(
7736                                         mgmt->u.action.u.chan_switch.variable,
7737                                         ies_len, true, NULL);
7738
7739                         if (elems && !elems->parse_error) {
7740                                 enum ieee80211_csa_source src =
7741                                         IEEE80211_CSA_SOURCE_PROT_ACTION;
7742
7743                                 ieee80211_sta_process_chanswitch(link,
7744                                                                  rx_status->mactime,
7745                                                                  rx_status->device_timestamp,
7746                                                                  elems, elems,
7747                                                                  src);
7748                         }
7749                         kfree(elems);
7750                         break;
7751                 case WLAN_CATEGORY_PUBLIC:
7752                 case WLAN_CATEGORY_PROTECTED_DUAL_OF_ACTION:
7753                         ies_len = skb->len -
7754                                   offsetof(struct ieee80211_mgmt,
7755                                            u.action.u.ext_chan_switch.variable);
7756
7757                         if (ies_len < 0)
7758                                 break;
7759
7760                         /*
7761                          * extended CSA IE can't be overridden, no need for
7762                          * BSSID
7763                          */
7764                         elems = ieee802_11_parse_elems(
7765                                         mgmt->u.action.u.ext_chan_switch.variable,
7766                                         ies_len, true, NULL);
7767
7768                         if (elems && !elems->parse_error) {
7769                                 enum ieee80211_csa_source src;
7770
7771                                 if (mgmt->u.action.category ==
7772                                                 WLAN_CATEGORY_PROTECTED_DUAL_OF_ACTION)
7773                                         src = IEEE80211_CSA_SOURCE_PROT_ACTION;
7774                                 else
7775                                         src = IEEE80211_CSA_SOURCE_UNPROT_ACTION;
7776
7777                                 /* for the handling code pretend it was an IE */
7778                                 elems->ext_chansw_ie =
7779                                         &mgmt->u.action.u.ext_chan_switch.data;
7780
7781                                 ieee80211_sta_process_chanswitch(link,
7782                                                                  rx_status->mactime,
7783                                                                  rx_status->device_timestamp,
7784                                                                  elems, elems,
7785                                                                  src);
7786                         }
7787
7788                         kfree(elems);
7789                         break;
7790                 }
7791                 break;
7792         }
7793 }
7794
7795 static void ieee80211_sta_timer(struct timer_list *t)
7796 {
7797         struct ieee80211_sub_if_data *sdata =
7798                 from_timer(sdata, t, u.mgd.timer);
7799
7800         wiphy_work_queue(sdata->local->hw.wiphy, &sdata->work);
7801 }
7802
7803 void ieee80211_sta_connection_lost(struct ieee80211_sub_if_data *sdata,
7804                                    u8 reason, bool tx)
7805 {
7806         u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
7807
7808         ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH, reason,
7809                                tx, frame_buf);
7810
7811         ieee80211_report_disconnect(sdata, frame_buf, sizeof(frame_buf), true,
7812                                     reason, false);
7813 }
7814
7815 static int ieee80211_auth(struct ieee80211_sub_if_data *sdata)
7816 {
7817         struct ieee80211_local *local = sdata->local;
7818         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
7819         struct ieee80211_mgd_auth_data *auth_data = ifmgd->auth_data;
7820         u32 tx_flags = 0;
7821         u16 trans = 1;
7822         u16 status = 0;
7823         struct ieee80211_prep_tx_info info = {
7824                 .subtype = IEEE80211_STYPE_AUTH,
7825         };
7826
7827         lockdep_assert_wiphy(sdata->local->hw.wiphy);
7828
7829         if (WARN_ON_ONCE(!auth_data))
7830                 return -EINVAL;
7831
7832         auth_data->tries++;
7833
7834         if (auth_data->tries > IEEE80211_AUTH_MAX_TRIES) {
7835                 sdata_info(sdata, "authentication with %pM timed out\n",
7836                            auth_data->ap_addr);
7837
7838                 /*
7839                  * Most likely AP is not in the range so remove the
7840                  * bss struct for that AP.
7841                  */
7842                 cfg80211_unlink_bss(local->hw.wiphy, auth_data->bss);
7843
7844                 return -ETIMEDOUT;
7845         }
7846
7847         if (auth_data->algorithm == WLAN_AUTH_SAE)
7848                 info.duration = jiffies_to_msecs(IEEE80211_AUTH_TIMEOUT_SAE);
7849
7850         info.link_id = auth_data->link_id;
7851         drv_mgd_prepare_tx(local, sdata, &info);
7852
7853         sdata_info(sdata, "send auth to %pM (try %d/%d)\n",
7854                    auth_data->ap_addr, auth_data->tries,
7855                    IEEE80211_AUTH_MAX_TRIES);
7856
7857         auth_data->expected_transaction = 2;
7858
7859         if (auth_data->algorithm == WLAN_AUTH_SAE) {
7860                 trans = auth_data->sae_trans;
7861                 status = auth_data->sae_status;
7862                 auth_data->expected_transaction = trans;
7863         }
7864
7865         if (ieee80211_hw_check(&local->hw, REPORTS_TX_ACK_STATUS))
7866                 tx_flags = IEEE80211_TX_CTL_REQ_TX_STATUS |
7867                            IEEE80211_TX_INTFL_MLME_CONN_TX;
7868
7869         ieee80211_send_auth(sdata, trans, auth_data->algorithm, status,
7870                             auth_data->data, auth_data->data_len,
7871                             auth_data->ap_addr, auth_data->ap_addr,
7872                             NULL, 0, 0, tx_flags);
7873
7874         if (tx_flags == 0) {
7875                 if (auth_data->algorithm == WLAN_AUTH_SAE)
7876                         auth_data->timeout = jiffies +
7877                                 IEEE80211_AUTH_TIMEOUT_SAE;
7878                 else
7879                         auth_data->timeout = jiffies + IEEE80211_AUTH_TIMEOUT;
7880         } else {
7881                 auth_data->timeout =
7882                         round_jiffies_up(jiffies + IEEE80211_AUTH_TIMEOUT_LONG);
7883         }
7884
7885         auth_data->timeout_started = true;
7886         run_again(sdata, auth_data->timeout);
7887
7888         return 0;
7889 }
7890
7891 static int ieee80211_do_assoc(struct ieee80211_sub_if_data *sdata)
7892 {
7893         struct ieee80211_mgd_assoc_data *assoc_data = sdata->u.mgd.assoc_data;
7894         struct ieee80211_local *local = sdata->local;
7895         int ret;
7896
7897         lockdep_assert_wiphy(sdata->local->hw.wiphy);
7898
7899         assoc_data->tries++;
7900         assoc_data->comeback = false;
7901         if (assoc_data->tries > IEEE80211_ASSOC_MAX_TRIES) {
7902                 sdata_info(sdata, "association with %pM timed out\n",
7903                            assoc_data->ap_addr);
7904
7905                 /*
7906                  * Most likely AP is not in the range so remove the
7907                  * bss struct for that AP.
7908                  */
7909                 cfg80211_unlink_bss(local->hw.wiphy,
7910                                     assoc_data->link[assoc_data->assoc_link_id].bss);
7911
7912                 return -ETIMEDOUT;
7913         }
7914
7915         sdata_info(sdata, "associate with %pM (try %d/%d)\n",
7916                    assoc_data->ap_addr, assoc_data->tries,
7917                    IEEE80211_ASSOC_MAX_TRIES);
7918         ret = ieee80211_send_assoc(sdata);
7919         if (ret)
7920                 return ret;
7921
7922         if (!ieee80211_hw_check(&local->hw, REPORTS_TX_ACK_STATUS)) {
7923                 assoc_data->timeout = jiffies + IEEE80211_ASSOC_TIMEOUT;
7924                 assoc_data->timeout_started = true;
7925                 run_again(sdata, assoc_data->timeout);
7926         } else {
7927                 assoc_data->timeout =
7928                         round_jiffies_up(jiffies +
7929                                          IEEE80211_ASSOC_TIMEOUT_LONG);
7930                 assoc_data->timeout_started = true;
7931                 run_again(sdata, assoc_data->timeout);
7932         }
7933
7934         return 0;
7935 }
7936
7937 void ieee80211_mgd_conn_tx_status(struct ieee80211_sub_if_data *sdata,
7938                                   __le16 fc, bool acked)
7939 {
7940         struct ieee80211_local *local = sdata->local;
7941
7942         sdata->u.mgd.status_fc = fc;
7943         sdata->u.mgd.status_acked = acked;
7944         sdata->u.mgd.status_received = true;
7945
7946         wiphy_work_queue(local->hw.wiphy, &sdata->work);
7947 }
7948
7949 void ieee80211_sta_work(struct ieee80211_sub_if_data *sdata)
7950 {
7951         struct ieee80211_local *local = sdata->local;
7952         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
7953
7954         lockdep_assert_wiphy(sdata->local->hw.wiphy);
7955
7956         if (ifmgd->status_received) {
7957                 __le16 fc = ifmgd->status_fc;
7958                 bool status_acked = ifmgd->status_acked;
7959
7960                 ifmgd->status_received = false;
7961                 if (ifmgd->auth_data && ieee80211_is_auth(fc)) {
7962                         if (status_acked) {
7963                                 if (ifmgd->auth_data->algorithm ==
7964                                     WLAN_AUTH_SAE)
7965                                         ifmgd->auth_data->timeout =
7966                                                 jiffies +
7967                                                 IEEE80211_AUTH_TIMEOUT_SAE;
7968                                 else
7969                                         ifmgd->auth_data->timeout =
7970                                                 jiffies +
7971                                                 IEEE80211_AUTH_TIMEOUT_SHORT;
7972                                 run_again(sdata, ifmgd->auth_data->timeout);
7973                         } else {
7974                                 ifmgd->auth_data->timeout = jiffies - 1;
7975                         }
7976                         ifmgd->auth_data->timeout_started = true;
7977                 } else if (ifmgd->assoc_data &&
7978                            !ifmgd->assoc_data->comeback &&
7979                            (ieee80211_is_assoc_req(fc) ||
7980                             ieee80211_is_reassoc_req(fc))) {
7981                         /*
7982                          * Update association timeout based on the TX status
7983                          * for the (Re)Association Request frame. Skip this if
7984                          * we have already processed a (Re)Association Response
7985                          * frame that indicated need for association comeback
7986                          * at a specific time in the future. This could happen
7987                          * if the TX status information is delayed enough for
7988                          * the response to be received and processed first.
7989                          */
7990                         if (status_acked) {
7991                                 ifmgd->assoc_data->timeout =
7992                                         jiffies + IEEE80211_ASSOC_TIMEOUT_SHORT;
7993                                 run_again(sdata, ifmgd->assoc_data->timeout);
7994                         } else {
7995                                 ifmgd->assoc_data->timeout = jiffies - 1;
7996                         }
7997                         ifmgd->assoc_data->timeout_started = true;
7998                 }
7999         }
8000
8001         if (ifmgd->auth_data && ifmgd->auth_data->timeout_started &&
8002             time_after(jiffies, ifmgd->auth_data->timeout)) {
8003                 if (ifmgd->auth_data->done || ifmgd->auth_data->waiting) {
8004                         /*
8005                          * ok ... we waited for assoc or continuation but
8006                          * userspace didn't do it, so kill the auth data
8007                          */
8008                         ieee80211_destroy_auth_data(sdata, false);
8009                 } else if (ieee80211_auth(sdata)) {
8010                         u8 ap_addr[ETH_ALEN];
8011                         struct ieee80211_event event = {
8012                                 .type = MLME_EVENT,
8013                                 .u.mlme.data = AUTH_EVENT,
8014                                 .u.mlme.status = MLME_TIMEOUT,
8015                         };
8016
8017                         memcpy(ap_addr, ifmgd->auth_data->ap_addr, ETH_ALEN);
8018
8019                         ieee80211_destroy_auth_data(sdata, false);
8020
8021                         cfg80211_auth_timeout(sdata->dev, ap_addr);
8022                         drv_event_callback(sdata->local, sdata, &event);
8023                 }
8024         } else if (ifmgd->auth_data && ifmgd->auth_data->timeout_started)
8025                 run_again(sdata, ifmgd->auth_data->timeout);
8026
8027         if (ifmgd->assoc_data && ifmgd->assoc_data->timeout_started &&
8028             time_after(jiffies, ifmgd->assoc_data->timeout)) {
8029                 if ((ifmgd->assoc_data->need_beacon &&
8030                      !sdata->deflink.u.mgd.have_beacon) ||
8031                     ieee80211_do_assoc(sdata)) {
8032                         struct ieee80211_event event = {
8033                                 .type = MLME_EVENT,
8034                                 .u.mlme.data = ASSOC_EVENT,
8035                                 .u.mlme.status = MLME_TIMEOUT,
8036                         };
8037
8038                         ieee80211_destroy_assoc_data(sdata, ASSOC_TIMEOUT);
8039                         drv_event_callback(sdata->local, sdata, &event);
8040                 }
8041         } else if (ifmgd->assoc_data && ifmgd->assoc_data->timeout_started)
8042                 run_again(sdata, ifmgd->assoc_data->timeout);
8043
8044         if (ifmgd->flags & IEEE80211_STA_CONNECTION_POLL &&
8045             ifmgd->associated) {
8046                 u8 *bssid = sdata->deflink.u.mgd.bssid;
8047                 int max_tries;
8048
8049                 if (ieee80211_hw_check(&local->hw, REPORTS_TX_ACK_STATUS))
8050                         max_tries = max_nullfunc_tries;
8051                 else
8052                         max_tries = max_probe_tries;
8053
8054                 /* ACK received for nullfunc probing frame */
8055                 if (!ifmgd->probe_send_count)
8056                         ieee80211_reset_ap_probe(sdata);
8057                 else if (ifmgd->nullfunc_failed) {
8058                         if (ifmgd->probe_send_count < max_tries) {
8059                                 mlme_dbg(sdata,
8060                                          "No ack for nullfunc frame to AP %pM, try %d/%i\n",
8061                                          bssid, ifmgd->probe_send_count,
8062                                          max_tries);
8063                                 ieee80211_mgd_probe_ap_send(sdata);
8064                         } else {
8065                                 mlme_dbg(sdata,
8066                                          "No ack for nullfunc frame to AP %pM, disconnecting.\n",
8067                                          bssid);
8068                                 ieee80211_sta_connection_lost(sdata,
8069                                         WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY,
8070                                         false);
8071                         }
8072                 } else if (time_is_after_jiffies(ifmgd->probe_timeout))
8073                         run_again(sdata, ifmgd->probe_timeout);
8074                 else if (ieee80211_hw_check(&local->hw, REPORTS_TX_ACK_STATUS)) {
8075                         mlme_dbg(sdata,
8076                                  "Failed to send nullfunc to AP %pM after %dms, disconnecting\n",
8077                                  bssid, probe_wait_ms);
8078                         ieee80211_sta_connection_lost(sdata,
8079                                 WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY, false);
8080                 } else if (ifmgd->probe_send_count < max_tries) {
8081                         mlme_dbg(sdata,
8082                                  "No probe response from AP %pM after %dms, try %d/%i\n",
8083                                  bssid, probe_wait_ms,
8084                                  ifmgd->probe_send_count, max_tries);
8085                         ieee80211_mgd_probe_ap_send(sdata);
8086                 } else {
8087                         /*
8088                          * We actually lost the connection ... or did we?
8089                          * Let's make sure!
8090                          */
8091                         mlme_dbg(sdata,
8092                                  "No probe response from AP %pM after %dms, disconnecting.\n",
8093                                  bssid, probe_wait_ms);
8094
8095                         ieee80211_sta_connection_lost(sdata,
8096                                 WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY, false);
8097                 }
8098         }
8099 }
8100
8101 static void ieee80211_sta_bcn_mon_timer(struct timer_list *t)
8102 {
8103         struct ieee80211_sub_if_data *sdata =
8104                 from_timer(sdata, t, u.mgd.bcn_mon_timer);
8105
8106         if (WARN_ON(ieee80211_vif_is_mld(&sdata->vif)))
8107                 return;
8108
8109         if (sdata->vif.bss_conf.csa_active &&
8110             !sdata->deflink.u.mgd.csa.waiting_bcn)
8111                 return;
8112
8113         if (sdata->vif.driver_flags & IEEE80211_VIF_BEACON_FILTER)
8114                 return;
8115
8116         sdata->u.mgd.connection_loss = false;
8117         wiphy_work_queue(sdata->local->hw.wiphy,
8118                          &sdata->u.mgd.beacon_connection_loss_work);
8119 }
8120
8121 static void ieee80211_sta_conn_mon_timer(struct timer_list *t)
8122 {
8123         struct ieee80211_sub_if_data *sdata =
8124                 from_timer(sdata, t, u.mgd.conn_mon_timer);
8125         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
8126         struct ieee80211_local *local = sdata->local;
8127         struct sta_info *sta;
8128         unsigned long timeout;
8129
8130         if (WARN_ON(ieee80211_vif_is_mld(&sdata->vif)))
8131                 return;
8132
8133         if (sdata->vif.bss_conf.csa_active &&
8134             !sdata->deflink.u.mgd.csa.waiting_bcn)
8135                 return;
8136
8137         sta = sta_info_get(sdata, sdata->vif.cfg.ap_addr);
8138         if (!sta)
8139                 return;
8140
8141         timeout = sta->deflink.status_stats.last_ack;
8142         if (time_before(sta->deflink.status_stats.last_ack, sta->deflink.rx_stats.last_rx))
8143                 timeout = sta->deflink.rx_stats.last_rx;
8144         timeout += IEEE80211_CONNECTION_IDLE_TIME;
8145
8146         /* If timeout is after now, then update timer to fire at
8147          * the later date, but do not actually probe at this time.
8148          */
8149         if (time_is_after_jiffies(timeout)) {
8150                 mod_timer(&ifmgd->conn_mon_timer, round_jiffies_up(timeout));
8151                 return;
8152         }
8153
8154         wiphy_work_queue(local->hw.wiphy, &sdata->u.mgd.monitor_work);
8155 }
8156
8157 static void ieee80211_sta_monitor_work(struct wiphy *wiphy,
8158                                        struct wiphy_work *work)
8159 {
8160         struct ieee80211_sub_if_data *sdata =
8161                 container_of(work, struct ieee80211_sub_if_data,
8162                              u.mgd.monitor_work);
8163
8164         ieee80211_mgd_probe_ap(sdata, false);
8165 }
8166
8167 static void ieee80211_restart_sta_timer(struct ieee80211_sub_if_data *sdata)
8168 {
8169         if (sdata->vif.type == NL80211_IFTYPE_STATION) {
8170                 __ieee80211_stop_poll(sdata);
8171
8172                 /* let's probe the connection once */
8173                 if (!ieee80211_hw_check(&sdata->local->hw, CONNECTION_MONITOR))
8174                         wiphy_work_queue(sdata->local->hw.wiphy,
8175                                          &sdata->u.mgd.monitor_work);
8176         }
8177 }
8178
8179 #ifdef CONFIG_PM
8180 void ieee80211_mgd_quiesce(struct ieee80211_sub_if_data *sdata)
8181 {
8182         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
8183         u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
8184
8185         lockdep_assert_wiphy(sdata->local->hw.wiphy);
8186
8187         if (ifmgd->auth_data || ifmgd->assoc_data) {
8188                 const u8 *ap_addr = ifmgd->auth_data ?
8189                                 ifmgd->auth_data->ap_addr :
8190                                 ifmgd->assoc_data->ap_addr;
8191
8192                 /*
8193                  * If we are trying to authenticate / associate while suspending,
8194                  * cfg80211 won't know and won't actually abort those attempts,
8195                  * thus we need to do that ourselves.
8196                  */
8197                 ieee80211_send_deauth_disassoc(sdata, ap_addr, ap_addr,
8198                                                IEEE80211_STYPE_DEAUTH,
8199                                                WLAN_REASON_DEAUTH_LEAVING,
8200                                                false, frame_buf);
8201                 if (ifmgd->assoc_data)
8202                         ieee80211_destroy_assoc_data(sdata, ASSOC_ABANDON);
8203                 if (ifmgd->auth_data)
8204                         ieee80211_destroy_auth_data(sdata, false);
8205                 cfg80211_tx_mlme_mgmt(sdata->dev, frame_buf,
8206                                       IEEE80211_DEAUTH_FRAME_LEN,
8207                                       false);
8208         }
8209
8210         /* This is a bit of a hack - we should find a better and more generic
8211          * solution to this. Normally when suspending, cfg80211 will in fact
8212          * deauthenticate. However, it doesn't (and cannot) stop an ongoing
8213          * auth (not so important) or assoc (this is the problem) process.
8214          *
8215          * As a consequence, it can happen that we are in the process of both
8216          * associating and suspending, and receive an association response
8217          * after cfg80211 has checked if it needs to disconnect, but before
8218          * we actually set the flag to drop incoming frames. This will then
8219          * cause the workqueue flush to process the association response in
8220          * the suspend, resulting in a successful association just before it
8221          * tries to remove the interface from the driver, which now though
8222          * has a channel context assigned ... this results in issues.
8223          *
8224          * To work around this (for now) simply deauth here again if we're
8225          * now connected.
8226          */
8227         if (ifmgd->associated && !sdata->local->wowlan) {
8228                 u8 bssid[ETH_ALEN];
8229                 struct cfg80211_deauth_request req = {
8230                         .reason_code = WLAN_REASON_DEAUTH_LEAVING,
8231                         .bssid = bssid,
8232                 };
8233
8234                 memcpy(bssid, sdata->vif.cfg.ap_addr, ETH_ALEN);
8235                 ieee80211_mgd_deauth(sdata, &req);
8236         }
8237 }
8238 #endif
8239
8240 void ieee80211_sta_restart(struct ieee80211_sub_if_data *sdata)
8241 {
8242         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
8243
8244         lockdep_assert_wiphy(sdata->local->hw.wiphy);
8245
8246         if (!ifmgd->associated)
8247                 return;
8248
8249         if (sdata->flags & IEEE80211_SDATA_DISCONNECT_RESUME) {
8250                 sdata->flags &= ~IEEE80211_SDATA_DISCONNECT_RESUME;
8251                 mlme_dbg(sdata, "driver requested disconnect after resume\n");
8252                 ieee80211_sta_connection_lost(sdata,
8253                                               WLAN_REASON_UNSPECIFIED,
8254                                               true);
8255                 return;
8256         }
8257
8258         if (sdata->flags & IEEE80211_SDATA_DISCONNECT_HW_RESTART) {
8259                 sdata->flags &= ~IEEE80211_SDATA_DISCONNECT_HW_RESTART;
8260                 mlme_dbg(sdata, "driver requested disconnect after hardware restart\n");
8261                 ieee80211_sta_connection_lost(sdata,
8262                                               WLAN_REASON_UNSPECIFIED,
8263                                               true);
8264                 return;
8265         }
8266 }
8267
8268 static void ieee80211_request_smps_mgd_work(struct wiphy *wiphy,
8269                                             struct wiphy_work *work)
8270 {
8271         struct ieee80211_link_data *link =
8272                 container_of(work, struct ieee80211_link_data,
8273                              u.mgd.request_smps_work);
8274
8275         __ieee80211_request_smps_mgd(link->sdata, link,
8276                                      link->u.mgd.driver_smps_mode);
8277 }
8278
8279 static void ieee80211_ml_sta_reconf_timeout(struct wiphy *wiphy,
8280                                             struct wiphy_work *work)
8281 {
8282         struct ieee80211_sub_if_data *sdata =
8283                 container_of(work, struct ieee80211_sub_if_data,
8284                              u.mgd.reconf.wk.work);
8285
8286         if (!sdata->u.mgd.reconf.added_links &&
8287             !sdata->u.mgd.reconf.removed_links)
8288                 return;
8289
8290         sdata_info(sdata,
8291                    "mlo: reconf: timeout: added=0x%x, removed=0x%x\n",
8292                    sdata->u.mgd.reconf.added_links,
8293                    sdata->u.mgd.reconf.removed_links);
8294
8295         __ieee80211_disconnect(sdata);
8296 }
8297
8298 /* interface setup */
8299 void ieee80211_sta_setup_sdata(struct ieee80211_sub_if_data *sdata)
8300 {
8301         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
8302
8303         wiphy_work_init(&ifmgd->monitor_work, ieee80211_sta_monitor_work);
8304         wiphy_work_init(&ifmgd->beacon_connection_loss_work,
8305                         ieee80211_beacon_connection_loss_work);
8306         wiphy_work_init(&ifmgd->csa_connection_drop_work,
8307                         ieee80211_csa_connection_drop_work);
8308         wiphy_delayed_work_init(&ifmgd->tdls_peer_del_work,
8309                                 ieee80211_tdls_peer_del_work);
8310         wiphy_delayed_work_init(&ifmgd->ml_reconf_work,
8311                                 ieee80211_ml_reconf_work);
8312         wiphy_delayed_work_init(&ifmgd->reconf.wk,
8313                                 ieee80211_ml_sta_reconf_timeout);
8314         timer_setup(&ifmgd->timer, ieee80211_sta_timer, 0);
8315         timer_setup(&ifmgd->bcn_mon_timer, ieee80211_sta_bcn_mon_timer, 0);
8316         timer_setup(&ifmgd->conn_mon_timer, ieee80211_sta_conn_mon_timer, 0);
8317         wiphy_delayed_work_init(&ifmgd->tx_tspec_wk,
8318                                 ieee80211_sta_handle_tspec_ac_params_wk);
8319         wiphy_delayed_work_init(&ifmgd->ttlm_work,
8320                                 ieee80211_tid_to_link_map_work);
8321         wiphy_delayed_work_init(&ifmgd->neg_ttlm_timeout_work,
8322                                 ieee80211_neg_ttlm_timeout_work);
8323         wiphy_work_init(&ifmgd->teardown_ttlm_work,
8324                         ieee80211_teardown_ttlm_work);
8325
8326         ifmgd->flags = 0;
8327         ifmgd->powersave = sdata->wdev.ps;
8328         ifmgd->uapsd_queues = sdata->local->hw.uapsd_queues;
8329         ifmgd->uapsd_max_sp_len = sdata->local->hw.uapsd_max_sp_len;
8330         /* Setup TDLS data */
8331         spin_lock_init(&ifmgd->teardown_lock);
8332         ifmgd->teardown_skb = NULL;
8333         ifmgd->orig_teardown_skb = NULL;
8334         ifmgd->mcast_seq_last = IEEE80211_SN_MODULO;
8335 }
8336
8337 static void ieee80211_recalc_smps_work(struct wiphy *wiphy,
8338                                        struct wiphy_work *work)
8339 {
8340         struct ieee80211_link_data *link =
8341                 container_of(work, struct ieee80211_link_data,
8342                              u.mgd.recalc_smps);
8343
8344         ieee80211_recalc_smps(link->sdata, link);
8345 }
8346
8347 void ieee80211_mgd_setup_link(struct ieee80211_link_data *link)
8348 {
8349         struct ieee80211_sub_if_data *sdata = link->sdata;
8350         struct ieee80211_local *local = sdata->local;
8351         unsigned int link_id = link->link_id;
8352
8353         link->u.mgd.p2p_noa_index = -1;
8354         link->conf->bssid = link->u.mgd.bssid;
8355         link->smps_mode = IEEE80211_SMPS_OFF;
8356
8357         wiphy_work_init(&link->u.mgd.request_smps_work,
8358                         ieee80211_request_smps_mgd_work);
8359         wiphy_work_init(&link->u.mgd.recalc_smps,
8360                         ieee80211_recalc_smps_work);
8361         if (local->hw.wiphy->features & NL80211_FEATURE_DYNAMIC_SMPS)
8362                 link->u.mgd.req_smps = IEEE80211_SMPS_AUTOMATIC;
8363         else
8364                 link->u.mgd.req_smps = IEEE80211_SMPS_OFF;
8365
8366         wiphy_delayed_work_init(&link->u.mgd.csa.switch_work,
8367                                 ieee80211_csa_switch_work);
8368
8369         ieee80211_clear_tpe(&link->conf->tpe);
8370
8371         if (sdata->u.mgd.assoc_data)
8372                 ether_addr_copy(link->conf->addr,
8373                                 sdata->u.mgd.assoc_data->link[link_id].addr);
8374         else if (sdata->u.mgd.reconf.add_links_data)
8375                 ether_addr_copy(link->conf->addr,
8376                                 sdata->u.mgd.reconf.add_links_data->link[link_id].addr);
8377         else if (!is_valid_ether_addr(link->conf->addr))
8378                 eth_random_addr(link->conf->addr);
8379 }
8380
8381 /* scan finished notification */
8382 void ieee80211_mlme_notify_scan_completed(struct ieee80211_local *local)
8383 {
8384         struct ieee80211_sub_if_data *sdata;
8385
8386         /* Restart STA timers */
8387         rcu_read_lock();
8388         list_for_each_entry_rcu(sdata, &local->interfaces, list) {
8389                 if (ieee80211_sdata_running(sdata))
8390                         ieee80211_restart_sta_timer(sdata);
8391         }
8392         rcu_read_unlock();
8393 }
8394
8395 static int ieee80211_prep_connection(struct ieee80211_sub_if_data *sdata,
8396                                      struct cfg80211_bss *cbss, s8 link_id,
8397                                      const u8 *ap_mld_addr, bool assoc,
8398                                      struct ieee80211_conn_settings *conn,
8399                                      bool override,
8400                                      unsigned long *userspace_selectors)
8401 {
8402         struct ieee80211_local *local = sdata->local;
8403         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
8404         struct ieee80211_bss *bss = (void *)cbss->priv;
8405         struct sta_info *new_sta = NULL;
8406         struct ieee80211_link_data *link;
8407         bool have_sta = false;
8408         bool mlo;
8409         int err;
8410
8411         if (link_id >= 0) {
8412                 mlo = true;
8413                 if (WARN_ON(!ap_mld_addr))
8414                         return -EINVAL;
8415                 err = ieee80211_vif_set_links(sdata, BIT(link_id), 0);
8416         } else {
8417                 if (WARN_ON(ap_mld_addr))
8418                         return -EINVAL;
8419                 ap_mld_addr = cbss->bssid;
8420                 err = ieee80211_vif_set_links(sdata, 0, 0);
8421                 link_id = 0;
8422                 mlo = false;
8423         }
8424
8425         if (err)
8426                 return err;
8427
8428         link = sdata_dereference(sdata->link[link_id], sdata);
8429         if (WARN_ON(!link)) {
8430                 err = -ENOLINK;
8431                 goto out_err;
8432         }
8433
8434         if (WARN_ON(!ifmgd->auth_data && !ifmgd->assoc_data)) {
8435                 err = -EINVAL;
8436                 goto out_err;
8437         }
8438
8439         /* If a reconfig is happening, bail out */
8440         if (local->in_reconfig) {
8441                 err = -EBUSY;
8442                 goto out_err;
8443         }
8444
8445         if (assoc) {
8446                 rcu_read_lock();
8447                 have_sta = sta_info_get(sdata, ap_mld_addr);
8448                 rcu_read_unlock();
8449         }
8450
8451         if (!have_sta) {
8452                 if (mlo)
8453                         new_sta = sta_info_alloc_with_link(sdata, ap_mld_addr,
8454                                                            link_id, cbss->bssid,
8455                                                            GFP_KERNEL);
8456                 else
8457                         new_sta = sta_info_alloc(sdata, ap_mld_addr, GFP_KERNEL);
8458
8459                 if (!new_sta) {
8460                         err = -ENOMEM;
8461                         goto out_err;
8462                 }
8463
8464                 new_sta->sta.mlo = mlo;
8465         }
8466
8467         /*
8468          * Set up the information for the new channel before setting the
8469          * new channel. We can't - completely race-free - change the basic
8470          * rates bitmap and the channel (sband) that it refers to, but if
8471          * we set it up before we at least avoid calling into the driver's
8472          * bss_info_changed() method with invalid information (since we do
8473          * call that from changing the channel - only for IDLE and perhaps
8474          * some others, but ...).
8475          *
8476          * So to avoid that, just set up all the new information before the
8477          * channel, but tell the driver to apply it only afterwards, since
8478          * it might need the new channel for that.
8479          */
8480         if (new_sta) {
8481                 const struct cfg80211_bss_ies *ies;
8482                 struct link_sta_info *link_sta;
8483
8484                 rcu_read_lock();
8485                 link_sta = rcu_dereference(new_sta->link[link_id]);
8486                 if (WARN_ON(!link_sta)) {
8487                         rcu_read_unlock();
8488                         sta_info_free(local, new_sta);
8489                         err = -EINVAL;
8490                         goto out_err;
8491                 }
8492
8493                 err = ieee80211_mgd_setup_link_sta(link, new_sta,
8494                                                    link_sta, cbss);
8495                 if (err) {
8496                         rcu_read_unlock();
8497                         sta_info_free(local, new_sta);
8498                         goto out_err;
8499                 }
8500
8501                 memcpy(link->u.mgd.bssid, cbss->bssid, ETH_ALEN);
8502
8503                 /* set timing information */
8504                 link->conf->beacon_int = cbss->beacon_interval;
8505                 ies = rcu_dereference(cbss->beacon_ies);
8506                 if (ies) {
8507                         link->conf->sync_tsf = ies->tsf;
8508                         link->conf->sync_device_ts =
8509                                 bss->device_ts_beacon;
8510
8511                         ieee80211_get_dtim(ies,
8512                                            &link->conf->sync_dtim_count,
8513                                            NULL);
8514                 } else if (!ieee80211_hw_check(&sdata->local->hw,
8515                                                TIMING_BEACON_ONLY)) {
8516                         ies = rcu_dereference(cbss->proberesp_ies);
8517                         /* must be non-NULL since beacon IEs were NULL */
8518                         link->conf->sync_tsf = ies->tsf;
8519                         link->conf->sync_device_ts =
8520                                 bss->device_ts_presp;
8521                         link->conf->sync_dtim_count = 0;
8522                 } else {
8523                         link->conf->sync_tsf = 0;
8524                         link->conf->sync_device_ts = 0;
8525                         link->conf->sync_dtim_count = 0;
8526                 }
8527                 rcu_read_unlock();
8528         }
8529
8530         if (new_sta || override) {
8531                 /*
8532                  * Only set this if we're also going to calculate the AP
8533                  * settings etc., otherwise this was set before in a
8534                  * previous call. Note override is set to %true in assoc
8535                  * if the settings were changed.
8536                  */
8537                 link->u.mgd.conn = *conn;
8538                 err = ieee80211_prep_channel(sdata, link, link->link_id, cbss,
8539                                              mlo, &link->u.mgd.conn,
8540                                              userspace_selectors);
8541                 if (err) {
8542                         if (new_sta)
8543                                 sta_info_free(local, new_sta);
8544                         goto out_err;
8545                 }
8546                 /* pass out for use in assoc */
8547                 *conn = link->u.mgd.conn;
8548         }
8549
8550         if (new_sta) {
8551                 /*
8552                  * tell driver about BSSID, basic rates and timing
8553                  * this was set up above, before setting the channel
8554                  */
8555                 ieee80211_link_info_change_notify(sdata, link,
8556                                                   BSS_CHANGED_BSSID |
8557                                                   BSS_CHANGED_BASIC_RATES |
8558                                                   BSS_CHANGED_BEACON_INT);
8559
8560                 if (assoc)
8561                         sta_info_pre_move_state(new_sta, IEEE80211_STA_AUTH);
8562
8563                 err = sta_info_insert(new_sta);
8564                 new_sta = NULL;
8565                 if (err) {
8566                         sdata_info(sdata,
8567                                    "failed to insert STA entry for the AP (error %d)\n",
8568                                    err);
8569                         goto out_release_chan;
8570                 }
8571         } else
8572                 WARN_ON_ONCE(!ether_addr_equal(link->u.mgd.bssid, cbss->bssid));
8573
8574         /* Cancel scan to ensure that nothing interferes with connection */
8575         if (local->scanning)
8576                 ieee80211_scan_cancel(local);
8577
8578         return 0;
8579
8580 out_release_chan:
8581         ieee80211_link_release_channel(link);
8582 out_err:
8583         ieee80211_vif_set_links(sdata, 0, 0);
8584         return err;
8585 }
8586
8587 static bool ieee80211_mgd_csa_present(struct ieee80211_sub_if_data *sdata,
8588                                       const struct cfg80211_bss_ies *ies,
8589                                       u8 cur_channel, bool ignore_ecsa)
8590 {
8591         const struct element *csa_elem, *ecsa_elem;
8592         struct ieee80211_channel_sw_ie *csa = NULL;
8593         struct ieee80211_ext_chansw_ie *ecsa = NULL;
8594
8595         if (!ies)
8596                 return false;
8597
8598         csa_elem = cfg80211_find_elem(WLAN_EID_CHANNEL_SWITCH,
8599                                       ies->data, ies->len);
8600         if (csa_elem && csa_elem->datalen == sizeof(*csa))
8601                 csa = (void *)csa_elem->data;
8602
8603         ecsa_elem = cfg80211_find_elem(WLAN_EID_EXT_CHANSWITCH_ANN,
8604                                        ies->data, ies->len);
8605         if (ecsa_elem && ecsa_elem->datalen == sizeof(*ecsa))
8606                 ecsa = (void *)ecsa_elem->data;
8607
8608         if (csa && csa->count == 0)
8609                 csa = NULL;
8610         if (csa && !csa->mode && csa->new_ch_num == cur_channel)
8611                 csa = NULL;
8612
8613         if (ecsa && ecsa->count == 0)
8614                 ecsa = NULL;
8615         if (ecsa && !ecsa->mode && ecsa->new_ch_num == cur_channel)
8616                 ecsa = NULL;
8617
8618         if (ignore_ecsa && ecsa) {
8619                 sdata_info(sdata,
8620                            "Ignoring ECSA in probe response - was considered stuck!\n");
8621                 return csa;
8622         }
8623
8624         return csa || ecsa;
8625 }
8626
8627 static bool ieee80211_mgd_csa_in_process(struct ieee80211_sub_if_data *sdata,
8628                                          struct cfg80211_bss *bss)
8629 {
8630         u8 cur_channel;
8631         bool ret;
8632
8633         cur_channel = ieee80211_frequency_to_channel(bss->channel->center_freq);
8634
8635         rcu_read_lock();
8636         if (ieee80211_mgd_csa_present(sdata,
8637                                       rcu_dereference(bss->beacon_ies),
8638                                       cur_channel, false)) {
8639                 ret = true;
8640                 goto out;
8641         }
8642
8643         if (ieee80211_mgd_csa_present(sdata,
8644                                       rcu_dereference(bss->proberesp_ies),
8645                                       cur_channel, bss->proberesp_ecsa_stuck)) {
8646                 ret = true;
8647                 goto out;
8648         }
8649
8650         ret = false;
8651 out:
8652         rcu_read_unlock();
8653         return ret;
8654 }
8655
8656 static void ieee80211_parse_cfg_selectors(unsigned long *userspace_selectors,
8657                                           const u8 *supported_selectors,
8658                                           u8 supported_selectors_len)
8659 {
8660         if (supported_selectors) {
8661                 for (int i = 0; i < supported_selectors_len; i++) {
8662                         set_bit(supported_selectors[i],
8663                                 userspace_selectors);
8664                 }
8665         } else {
8666                 /* Assume SAE_H2E support for backward compatibility. */
8667                 set_bit(BSS_MEMBERSHIP_SELECTOR_SAE_H2E,
8668                         userspace_selectors);
8669         }
8670 }
8671
8672 /* config hooks */
8673 int ieee80211_mgd_auth(struct ieee80211_sub_if_data *sdata,
8674                        struct cfg80211_auth_request *req)
8675 {
8676         struct ieee80211_local *local = sdata->local;
8677         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
8678         struct ieee80211_mgd_auth_data *auth_data;
8679         struct ieee80211_conn_settings conn;
8680         struct ieee80211_link_data *link;
8681         struct ieee80211_supported_band *sband;
8682         struct ieee80211_bss *bss;
8683         u16 auth_alg;
8684         int err;
8685         bool cont_auth, wmm_used;
8686
8687         lockdep_assert_wiphy(sdata->local->hw.wiphy);
8688
8689         /* prepare auth data structure */
8690
8691         switch (req->auth_type) {
8692         case NL80211_AUTHTYPE_OPEN_SYSTEM:
8693                 auth_alg = WLAN_AUTH_OPEN;
8694                 break;
8695         case NL80211_AUTHTYPE_SHARED_KEY:
8696                 if (fips_enabled)
8697                         return -EOPNOTSUPP;
8698                 auth_alg = WLAN_AUTH_SHARED_KEY;
8699                 break;
8700         case NL80211_AUTHTYPE_FT:
8701                 auth_alg = WLAN_AUTH_FT;
8702                 break;
8703         case NL80211_AUTHTYPE_NETWORK_EAP:
8704                 auth_alg = WLAN_AUTH_LEAP;
8705                 break;
8706         case NL80211_AUTHTYPE_SAE:
8707                 auth_alg = WLAN_AUTH_SAE;
8708                 break;
8709         case NL80211_AUTHTYPE_FILS_SK:
8710                 auth_alg = WLAN_AUTH_FILS_SK;
8711                 break;
8712         case NL80211_AUTHTYPE_FILS_SK_PFS:
8713                 auth_alg = WLAN_AUTH_FILS_SK_PFS;
8714                 break;
8715         case NL80211_AUTHTYPE_FILS_PK:
8716                 auth_alg = WLAN_AUTH_FILS_PK;
8717                 break;
8718         default:
8719                 return -EOPNOTSUPP;
8720         }
8721
8722         if (ifmgd->assoc_data)
8723                 return -EBUSY;
8724
8725         if (ieee80211_mgd_csa_in_process(sdata, req->bss)) {
8726                 sdata_info(sdata, "AP is in CSA process, reject auth\n");
8727                 return -EINVAL;
8728         }
8729
8730         auth_data = kzalloc(sizeof(*auth_data) + req->auth_data_len +
8731                             req->ie_len, GFP_KERNEL);
8732         if (!auth_data)
8733                 return -ENOMEM;
8734
8735         memcpy(auth_data->ap_addr,
8736                req->ap_mld_addr ?: req->bss->bssid,
8737                ETH_ALEN);
8738         auth_data->bss = req->bss;
8739         auth_data->link_id = req->link_id;
8740
8741         if (req->auth_data_len >= 4) {
8742                 if (req->auth_type == NL80211_AUTHTYPE_SAE) {
8743                         __le16 *pos = (__le16 *) req->auth_data;
8744
8745                         auth_data->sae_trans = le16_to_cpu(pos[0]);
8746                         auth_data->sae_status = le16_to_cpu(pos[1]);
8747                 }
8748                 memcpy(auth_data->data, req->auth_data + 4,
8749                        req->auth_data_len - 4);
8750                 auth_data->data_len += req->auth_data_len - 4;
8751         }
8752
8753         /* Check if continuing authentication or trying to authenticate with the
8754          * same BSS that we were in the process of authenticating with and avoid
8755          * removal and re-addition of the STA entry in
8756          * ieee80211_prep_connection().
8757          */
8758         cont_auth = ifmgd->auth_data && req->bss == ifmgd->auth_data->bss &&
8759                     ifmgd->auth_data->link_id == req->link_id;
8760
8761         if (req->ie && req->ie_len) {
8762                 memcpy(&auth_data->data[auth_data->data_len],
8763                        req->ie, req->ie_len);
8764                 auth_data->data_len += req->ie_len;
8765         }
8766
8767         if (req->key && req->key_len) {
8768                 auth_data->key_len = req->key_len;
8769                 auth_data->key_idx = req->key_idx;
8770                 memcpy(auth_data->key, req->key, req->key_len);
8771         }
8772
8773         ieee80211_parse_cfg_selectors(auth_data->userspace_selectors,
8774                                       req->supported_selectors,
8775                                       req->supported_selectors_len);
8776
8777         auth_data->algorithm = auth_alg;
8778
8779         /* try to authenticate/probe */
8780
8781         if (ifmgd->auth_data) {
8782                 if (cont_auth && req->auth_type == NL80211_AUTHTYPE_SAE) {
8783                         auth_data->peer_confirmed =
8784                                 ifmgd->auth_data->peer_confirmed;
8785                 }
8786                 ieee80211_destroy_auth_data(sdata, cont_auth);
8787         }
8788
8789         /* prep auth_data so we don't go into idle on disassoc */
8790         ifmgd->auth_data = auth_data;
8791
8792         /* If this is continuation of an ongoing SAE authentication exchange
8793          * (i.e., request to send SAE Confirm) and the peer has already
8794          * confirmed, mark authentication completed since we are about to send
8795          * out SAE Confirm.
8796          */
8797         if (cont_auth && req->auth_type == NL80211_AUTHTYPE_SAE &&
8798             auth_data->peer_confirmed && auth_data->sae_trans == 2)
8799                 ieee80211_mark_sta_auth(sdata);
8800
8801         if (ifmgd->associated) {
8802                 u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
8803
8804                 sdata_info(sdata,
8805                            "disconnect from AP %pM for new auth to %pM\n",
8806                            sdata->vif.cfg.ap_addr, auth_data->ap_addr);
8807                 ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH,
8808                                        WLAN_REASON_UNSPECIFIED,
8809                                        false, frame_buf);
8810
8811                 ieee80211_report_disconnect(sdata, frame_buf,
8812                                             sizeof(frame_buf), true,
8813                                             WLAN_REASON_UNSPECIFIED,
8814                                             false);
8815         }
8816
8817         /* needed for transmitting the auth frame(s) properly */
8818         memcpy(sdata->vif.cfg.ap_addr, auth_data->ap_addr, ETH_ALEN);
8819
8820         bss = (void *)req->bss->priv;
8821         wmm_used = bss->wmm_used && (local->hw.queues >= IEEE80211_NUM_ACS);
8822
8823         sband = local->hw.wiphy->bands[req->bss->channel->band];
8824
8825         ieee80211_determine_our_sta_mode_auth(sdata, sband, req, wmm_used,
8826                                               &conn);
8827
8828         err = ieee80211_prep_connection(sdata, req->bss, req->link_id,
8829                                         req->ap_mld_addr, cont_auth,
8830                                         &conn, false,
8831                                         auth_data->userspace_selectors);
8832         if (err)
8833                 goto err_clear;
8834
8835         if (req->link_id >= 0)
8836                 link = sdata_dereference(sdata->link[req->link_id], sdata);
8837         else
8838                 link = &sdata->deflink;
8839
8840         if (WARN_ON(!link)) {
8841                 err = -ENOLINK;
8842                 goto err_clear;
8843         }
8844
8845         sdata_info(sdata, "authenticate with %pM (local address=%pM)\n",
8846                    auth_data->ap_addr, link->conf->addr);
8847
8848         err = ieee80211_auth(sdata);
8849         if (err) {
8850                 sta_info_destroy_addr(sdata, auth_data->ap_addr);
8851                 goto err_clear;
8852         }
8853
8854         /* hold our own reference */
8855         cfg80211_ref_bss(local->hw.wiphy, auth_data->bss);
8856         return 0;
8857
8858  err_clear:
8859         if (!ieee80211_vif_is_mld(&sdata->vif)) {
8860                 eth_zero_addr(sdata->deflink.u.mgd.bssid);
8861                 ieee80211_link_info_change_notify(sdata, &sdata->deflink,
8862                                                   BSS_CHANGED_BSSID);
8863                 ieee80211_link_release_channel(&sdata->deflink);
8864         }
8865         ifmgd->auth_data = NULL;
8866         kfree(auth_data);
8867         return err;
8868 }
8869
8870 static void
8871 ieee80211_setup_assoc_link(struct ieee80211_sub_if_data *sdata,
8872                            struct ieee80211_mgd_assoc_data *assoc_data,
8873                            struct cfg80211_assoc_request *req,
8874                            struct ieee80211_conn_settings *conn,
8875                            unsigned int link_id)
8876 {
8877         struct ieee80211_local *local = sdata->local;
8878         const struct cfg80211_bss_ies *bss_ies;
8879         struct ieee80211_supported_band *sband;
8880         struct ieee80211_link_data *link;
8881         struct cfg80211_bss *cbss;
8882         struct ieee80211_bss *bss;
8883
8884         cbss = assoc_data->link[link_id].bss;
8885         if (WARN_ON(!cbss))
8886                 return;
8887
8888         bss = (void *)cbss->priv;
8889
8890         sband = local->hw.wiphy->bands[cbss->channel->band];
8891         if (WARN_ON(!sband))
8892                 return;
8893
8894         link = sdata_dereference(sdata->link[link_id], sdata);
8895         if (WARN_ON(!link))
8896                 return;
8897
8898         /* for MLO connections assume advertising all rates is OK */
8899         if (!req->ap_mld_addr) {
8900                 assoc_data->supp_rates = bss->supp_rates;
8901                 assoc_data->supp_rates_len = bss->supp_rates_len;
8902         }
8903
8904         /* copy and link elems for the STA profile */
8905         if (req->links[link_id].elems_len) {
8906                 memcpy(assoc_data->ie_pos, req->links[link_id].elems,
8907                        req->links[link_id].elems_len);
8908                 assoc_data->link[link_id].elems = assoc_data->ie_pos;
8909                 assoc_data->link[link_id].elems_len = req->links[link_id].elems_len;
8910                 assoc_data->ie_pos += req->links[link_id].elems_len;
8911         }
8912
8913         link->u.mgd.beacon_crc_valid = false;
8914         link->u.mgd.dtim_period = 0;
8915         link->u.mgd.have_beacon = false;
8916
8917         /* override HT configuration only if the AP and we support it */
8918         if (conn->mode >= IEEE80211_CONN_MODE_HT) {
8919                 struct ieee80211_sta_ht_cap sta_ht_cap;
8920
8921                 memcpy(&sta_ht_cap, &sband->ht_cap, sizeof(sta_ht_cap));
8922                 ieee80211_apply_htcap_overrides(sdata, &sta_ht_cap);
8923         }
8924
8925         rcu_read_lock();
8926         bss_ies = rcu_dereference(cbss->beacon_ies);
8927         if (bss_ies) {
8928                 u8 dtim_count = 0;
8929
8930                 ieee80211_get_dtim(bss_ies, &dtim_count,
8931                                    &link->u.mgd.dtim_period);
8932
8933                 sdata->deflink.u.mgd.have_beacon = true;
8934
8935                 if (ieee80211_hw_check(&local->hw, TIMING_BEACON_ONLY)) {
8936                         link->conf->sync_tsf = bss_ies->tsf;
8937                         link->conf->sync_device_ts = bss->device_ts_beacon;
8938                         link->conf->sync_dtim_count = dtim_count;
8939                 }
8940         } else {
8941                 bss_ies = rcu_dereference(cbss->ies);
8942         }
8943
8944         if (bss_ies) {
8945                 const struct element *elem;
8946
8947                 elem = cfg80211_find_ext_elem(WLAN_EID_EXT_MULTIPLE_BSSID_CONFIGURATION,
8948                                               bss_ies->data, bss_ies->len);
8949                 if (elem && elem->datalen >= 3)
8950                         link->conf->profile_periodicity = elem->data[2];
8951                 else
8952                         link->conf->profile_periodicity = 0;
8953
8954                 elem = cfg80211_find_elem(WLAN_EID_EXT_CAPABILITY,
8955                                           bss_ies->data, bss_ies->len);
8956                 if (elem && elem->datalen >= 11 &&
8957                     (elem->data[10] & WLAN_EXT_CAPA11_EMA_SUPPORT))
8958                         link->conf->ema_ap = true;
8959                 else
8960                         link->conf->ema_ap = false;
8961         }
8962         rcu_read_unlock();
8963
8964         if (bss->corrupt_data) {
8965                 char *corrupt_type = "data";
8966
8967                 if (bss->corrupt_data & IEEE80211_BSS_CORRUPT_BEACON) {
8968                         if (bss->corrupt_data & IEEE80211_BSS_CORRUPT_PROBE_RESP)
8969                                 corrupt_type = "beacon and probe response";
8970                         else
8971                                 corrupt_type = "beacon";
8972                 } else if (bss->corrupt_data & IEEE80211_BSS_CORRUPT_PROBE_RESP) {
8973                         corrupt_type = "probe response";
8974                 }
8975                 sdata_info(sdata, "associating to AP %pM with corrupt %s\n",
8976                            cbss->bssid, corrupt_type);
8977         }
8978
8979         if (link->u.mgd.req_smps == IEEE80211_SMPS_AUTOMATIC) {
8980                 if (sdata->u.mgd.powersave)
8981                         link->smps_mode = IEEE80211_SMPS_DYNAMIC;
8982                 else
8983                         link->smps_mode = IEEE80211_SMPS_OFF;
8984         } else {
8985                 link->smps_mode = link->u.mgd.req_smps;
8986         }
8987 }
8988
8989 static int
8990 ieee80211_mgd_get_ap_ht_vht_capa(struct ieee80211_sub_if_data *sdata,
8991                                  struct ieee80211_mgd_assoc_data *assoc_data,
8992                                  int link_id)
8993 {
8994         struct cfg80211_bss *cbss = assoc_data->link[link_id].bss;
8995         enum nl80211_band band = cbss->channel->band;
8996         struct ieee80211_supported_band *sband;
8997         const struct element *elem;
8998         int err;
8999
9000         /* neither HT nor VHT elements used on 6 GHz */
9001         if (band == NL80211_BAND_6GHZ)
9002                 return 0;
9003
9004         if (assoc_data->link[link_id].conn.mode < IEEE80211_CONN_MODE_HT)
9005                 return 0;
9006
9007         rcu_read_lock();
9008         elem = ieee80211_bss_get_elem(cbss, WLAN_EID_HT_OPERATION);
9009         if (!elem || elem->datalen < sizeof(struct ieee80211_ht_operation)) {
9010                 mlme_link_id_dbg(sdata, link_id, "no HT operation on BSS %pM\n",
9011                                  cbss->bssid);
9012                 err = -EINVAL;
9013                 goto out_rcu;
9014         }
9015         assoc_data->link[link_id].ap_ht_param =
9016                 ((struct ieee80211_ht_operation *)(elem->data))->ht_param;
9017         rcu_read_unlock();
9018
9019         if (assoc_data->link[link_id].conn.mode < IEEE80211_CONN_MODE_VHT)
9020                 return 0;
9021
9022         /* some drivers want to support VHT on 2.4 GHz even */
9023         sband = sdata->local->hw.wiphy->bands[band];
9024         if (!sband->vht_cap.vht_supported)
9025                 return 0;
9026
9027         rcu_read_lock();
9028         elem = ieee80211_bss_get_elem(cbss, WLAN_EID_VHT_CAPABILITY);
9029         /* but even then accept it not being present on the AP */
9030         if (!elem && band == NL80211_BAND_2GHZ) {
9031                 err = 0;
9032                 goto out_rcu;
9033         }
9034         if (!elem || elem->datalen < sizeof(struct ieee80211_vht_cap)) {
9035                 mlme_link_id_dbg(sdata, link_id, "no VHT capa on BSS %pM\n",
9036                                  cbss->bssid);
9037                 err = -EINVAL;
9038                 goto out_rcu;
9039         }
9040         memcpy(&assoc_data->link[link_id].ap_vht_cap, elem->data,
9041                sizeof(struct ieee80211_vht_cap));
9042         rcu_read_unlock();
9043
9044         return 0;
9045 out_rcu:
9046         rcu_read_unlock();
9047         return err;
9048 }
9049
9050 int ieee80211_mgd_assoc(struct ieee80211_sub_if_data *sdata,
9051                         struct cfg80211_assoc_request *req)
9052 {
9053         unsigned int assoc_link_id = req->link_id < 0 ? 0 : req->link_id;
9054         struct ieee80211_local *local = sdata->local;
9055         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
9056         struct ieee80211_mgd_assoc_data *assoc_data;
9057         const struct element *ssid_elem;
9058         struct ieee80211_vif_cfg *vif_cfg = &sdata->vif.cfg;
9059         struct ieee80211_link_data *link;
9060         struct cfg80211_bss *cbss;
9061         bool override, uapsd_supported;
9062         bool match_auth;
9063         int i, err;
9064         size_t size = sizeof(*assoc_data) + req->ie_len;
9065
9066         for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++)
9067                 size += req->links[i].elems_len;
9068
9069         /* FIXME: no support for 4-addr MLO yet */
9070         if (sdata->u.mgd.use_4addr && req->link_id >= 0)
9071                 return -EOPNOTSUPP;
9072
9073         assoc_data = kzalloc(size, GFP_KERNEL);
9074         if (!assoc_data)
9075                 return -ENOMEM;
9076
9077         cbss = req->link_id < 0 ? req->bss : req->links[req->link_id].bss;
9078
9079         if (ieee80211_mgd_csa_in_process(sdata, cbss)) {
9080                 sdata_info(sdata, "AP is in CSA process, reject assoc\n");
9081                 err = -EINVAL;
9082                 goto err_free;
9083         }
9084
9085         rcu_read_lock();
9086         ssid_elem = ieee80211_bss_get_elem(cbss, WLAN_EID_SSID);
9087         if (!ssid_elem || ssid_elem->datalen > sizeof(assoc_data->ssid)) {
9088                 rcu_read_unlock();
9089                 err = -EINVAL;
9090                 goto err_free;
9091         }
9092
9093         memcpy(assoc_data->ssid, ssid_elem->data, ssid_elem->datalen);
9094         assoc_data->ssid_len = ssid_elem->datalen;
9095         rcu_read_unlock();
9096
9097         if (req->ap_mld_addr)
9098                 memcpy(assoc_data->ap_addr, req->ap_mld_addr, ETH_ALEN);
9099         else
9100                 memcpy(assoc_data->ap_addr, cbss->bssid, ETH_ALEN);
9101
9102         if (ifmgd->associated) {
9103                 u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
9104
9105                 sdata_info(sdata,
9106                            "disconnect from AP %pM for new assoc to %pM\n",
9107                            sdata->vif.cfg.ap_addr, assoc_data->ap_addr);
9108                 ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH,
9109                                        WLAN_REASON_UNSPECIFIED,
9110                                        false, frame_buf);
9111
9112                 ieee80211_report_disconnect(sdata, frame_buf,
9113                                             sizeof(frame_buf), true,
9114                                             WLAN_REASON_UNSPECIFIED,
9115                                             false);
9116         }
9117
9118         ieee80211_parse_cfg_selectors(assoc_data->userspace_selectors,
9119                                       req->supported_selectors,
9120                                       req->supported_selectors_len);
9121
9122         memcpy(&ifmgd->ht_capa, &req->ht_capa, sizeof(ifmgd->ht_capa));
9123         memcpy(&ifmgd->ht_capa_mask, &req->ht_capa_mask,
9124                sizeof(ifmgd->ht_capa_mask));
9125
9126         memcpy(&ifmgd->vht_capa, &req->vht_capa, sizeof(ifmgd->vht_capa));
9127         memcpy(&ifmgd->vht_capa_mask, &req->vht_capa_mask,
9128                sizeof(ifmgd->vht_capa_mask));
9129
9130         memcpy(&ifmgd->s1g_capa, &req->s1g_capa, sizeof(ifmgd->s1g_capa));
9131         memcpy(&ifmgd->s1g_capa_mask, &req->s1g_capa_mask,
9132                sizeof(ifmgd->s1g_capa_mask));
9133
9134         /* keep some setup (AP STA, channel, ...) if matching */
9135         match_auth = ifmgd->auth_data &&
9136                      ether_addr_equal(ifmgd->auth_data->ap_addr,
9137                                       assoc_data->ap_addr) &&
9138                      ifmgd->auth_data->link_id == req->link_id;
9139
9140         if (req->ap_mld_addr) {
9141                 uapsd_supported = true;
9142
9143                 if (req->flags & (ASSOC_REQ_DISABLE_HT |
9144                                   ASSOC_REQ_DISABLE_VHT |
9145                                   ASSOC_REQ_DISABLE_HE |
9146                                   ASSOC_REQ_DISABLE_EHT)) {
9147                         err = -EINVAL;
9148                         goto err_free;
9149                 }
9150
9151                 for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++) {
9152                         struct ieee80211_supported_band *sband;
9153                         struct cfg80211_bss *link_cbss = req->links[i].bss;
9154                         struct ieee80211_bss *bss;
9155
9156                         if (!link_cbss)
9157                                 continue;
9158
9159                         bss = (void *)link_cbss->priv;
9160
9161                         if (!bss->wmm_used) {
9162                                 err = -EINVAL;
9163                                 req->links[i].error = err;
9164                                 goto err_free;
9165                         }
9166
9167                         if (link_cbss->channel->band == NL80211_BAND_S1GHZ) {
9168                                 err = -EINVAL;
9169                                 req->links[i].error = err;
9170                                 goto err_free;
9171                         }
9172
9173                         link = sdata_dereference(sdata->link[i], sdata);
9174                         if (link)
9175                                 ether_addr_copy(assoc_data->link[i].addr,
9176                                                 link->conf->addr);
9177                         else
9178                                 eth_random_addr(assoc_data->link[i].addr);
9179                         sband = local->hw.wiphy->bands[link_cbss->channel->band];
9180
9181                         if (match_auth && i == assoc_link_id && link)
9182                                 assoc_data->link[i].conn = link->u.mgd.conn;
9183                         else
9184                                 assoc_data->link[i].conn =
9185                                         ieee80211_conn_settings_unlimited;
9186                         ieee80211_determine_our_sta_mode_assoc(sdata, sband,
9187                                                                req, true, i,
9188                                                                &assoc_data->link[i].conn);
9189                         assoc_data->link[i].bss = link_cbss;
9190                         assoc_data->link[i].disabled = req->links[i].disabled;
9191
9192                         if (!bss->uapsd_supported)
9193                                 uapsd_supported = false;
9194
9195                         if (assoc_data->link[i].conn.mode < IEEE80211_CONN_MODE_EHT) {
9196                                 err = -EINVAL;
9197                                 req->links[i].error = err;
9198                                 goto err_free;
9199                         }
9200
9201                         err = ieee80211_mgd_get_ap_ht_vht_capa(sdata,
9202                                                                assoc_data, i);
9203                         if (err) {
9204                                 err = -EINVAL;
9205                                 req->links[i].error = err;
9206                                 goto err_free;
9207                         }
9208                 }
9209
9210                 assoc_data->wmm = true;
9211         } else {
9212                 struct ieee80211_supported_band *sband;
9213                 struct ieee80211_bss *bss = (void *)cbss->priv;
9214
9215                 memcpy(assoc_data->link[0].addr, sdata->vif.addr, ETH_ALEN);
9216                 assoc_data->s1g = cbss->channel->band == NL80211_BAND_S1GHZ;
9217
9218                 assoc_data->wmm = bss->wmm_used &&
9219                                   (local->hw.queues >= IEEE80211_NUM_ACS);
9220
9221                 if (cbss->channel->band == NL80211_BAND_6GHZ &&
9222                     req->flags & (ASSOC_REQ_DISABLE_HT |
9223                                   ASSOC_REQ_DISABLE_VHT |
9224                                   ASSOC_REQ_DISABLE_HE)) {
9225                         err = -EINVAL;
9226                         goto err_free;
9227                 }
9228
9229                 sband = local->hw.wiphy->bands[cbss->channel->band];
9230
9231                 assoc_data->link[0].bss = cbss;
9232
9233                 if (match_auth)
9234                         assoc_data->link[0].conn = sdata->deflink.u.mgd.conn;
9235                 else
9236                         assoc_data->link[0].conn =
9237                                 ieee80211_conn_settings_unlimited;
9238                 ieee80211_determine_our_sta_mode_assoc(sdata, sband, req,
9239                                                        assoc_data->wmm, 0,
9240                                                        &assoc_data->link[0].conn);
9241
9242                 uapsd_supported = bss->uapsd_supported;
9243
9244                 err = ieee80211_mgd_get_ap_ht_vht_capa(sdata, assoc_data, 0);
9245                 if (err)
9246                         goto err_free;
9247         }
9248
9249         assoc_data->spp_amsdu = req->flags & ASSOC_REQ_SPP_AMSDU;
9250
9251         if (ifmgd->auth_data && !ifmgd->auth_data->done) {
9252                 err = -EBUSY;
9253                 goto err_free;
9254         }
9255
9256         if (ifmgd->assoc_data) {
9257                 err = -EBUSY;
9258                 goto err_free;
9259         }
9260
9261         /* Cleanup is delayed if auth_data matches */
9262         if (ifmgd->auth_data && !match_auth)
9263                 ieee80211_destroy_auth_data(sdata, false);
9264
9265         if (req->ie && req->ie_len) {
9266                 memcpy(assoc_data->ie, req->ie, req->ie_len);
9267                 assoc_data->ie_len = req->ie_len;
9268                 assoc_data->ie_pos = assoc_data->ie + assoc_data->ie_len;
9269         } else {
9270                 assoc_data->ie_pos = assoc_data->ie;
9271         }
9272
9273         if (req->fils_kek) {
9274                 /* should already be checked in cfg80211 - so warn */
9275                 if (WARN_ON(req->fils_kek_len > FILS_MAX_KEK_LEN)) {
9276                         err = -EINVAL;
9277                         goto err_free;
9278                 }
9279                 memcpy(assoc_data->fils_kek, req->fils_kek,
9280                        req->fils_kek_len);
9281                 assoc_data->fils_kek_len = req->fils_kek_len;
9282         }
9283
9284         if (req->fils_nonces)
9285                 memcpy(assoc_data->fils_nonces, req->fils_nonces,
9286                        2 * FILS_NONCE_LEN);
9287
9288         /* default timeout */
9289         assoc_data->timeout = jiffies;
9290         assoc_data->timeout_started = true;
9291
9292         assoc_data->assoc_link_id = assoc_link_id;
9293
9294         if (req->ap_mld_addr) {
9295                 /* if there was no authentication, set up the link */
9296                 err = ieee80211_vif_set_links(sdata, BIT(assoc_link_id), 0);
9297                 if (err)
9298                         goto err_clear;
9299         }
9300
9301         link = sdata_dereference(sdata->link[assoc_link_id], sdata);
9302         if (WARN_ON(!link)) {
9303                 err = -EINVAL;
9304                 goto err_clear;
9305         }
9306
9307         override = link->u.mgd.conn.mode !=
9308                         assoc_data->link[assoc_link_id].conn.mode ||
9309                    link->u.mgd.conn.bw_limit !=
9310                         assoc_data->link[assoc_link_id].conn.bw_limit;
9311         link->u.mgd.conn = assoc_data->link[assoc_link_id].conn;
9312
9313         ieee80211_setup_assoc_link(sdata, assoc_data, req, &link->u.mgd.conn,
9314                                    assoc_link_id);
9315
9316         if (WARN((sdata->vif.driver_flags & IEEE80211_VIF_SUPPORTS_UAPSD) &&
9317                  ieee80211_hw_check(&local->hw, PS_NULLFUNC_STACK),
9318              "U-APSD not supported with HW_PS_NULLFUNC_STACK\n"))
9319                 sdata->vif.driver_flags &= ~IEEE80211_VIF_SUPPORTS_UAPSD;
9320
9321         if (assoc_data->wmm && uapsd_supported &&
9322             (sdata->vif.driver_flags & IEEE80211_VIF_SUPPORTS_UAPSD)) {
9323                 assoc_data->uapsd = true;
9324                 ifmgd->flags |= IEEE80211_STA_UAPSD_ENABLED;
9325         } else {
9326                 assoc_data->uapsd = false;
9327                 ifmgd->flags &= ~IEEE80211_STA_UAPSD_ENABLED;
9328         }
9329
9330         if (req->prev_bssid)
9331                 memcpy(assoc_data->prev_ap_addr, req->prev_bssid, ETH_ALEN);
9332
9333         if (req->use_mfp) {
9334                 ifmgd->mfp = IEEE80211_MFP_REQUIRED;
9335                 ifmgd->flags |= IEEE80211_STA_MFP_ENABLED;
9336         } else {
9337                 ifmgd->mfp = IEEE80211_MFP_DISABLED;
9338                 ifmgd->flags &= ~IEEE80211_STA_MFP_ENABLED;
9339         }
9340
9341         if (req->flags & ASSOC_REQ_USE_RRM)
9342                 ifmgd->flags |= IEEE80211_STA_ENABLE_RRM;
9343         else
9344                 ifmgd->flags &= ~IEEE80211_STA_ENABLE_RRM;
9345
9346         if (req->crypto.control_port)
9347                 ifmgd->flags |= IEEE80211_STA_CONTROL_PORT;
9348         else
9349                 ifmgd->flags &= ~IEEE80211_STA_CONTROL_PORT;
9350
9351         sdata->control_port_protocol = req->crypto.control_port_ethertype;
9352         sdata->control_port_no_encrypt = req->crypto.control_port_no_encrypt;
9353         sdata->control_port_over_nl80211 =
9354                                         req->crypto.control_port_over_nl80211;
9355         sdata->control_port_no_preauth = req->crypto.control_port_no_preauth;
9356
9357         /* kick off associate process */
9358         ifmgd->assoc_data = assoc_data;
9359
9360         for (i = 0; i < ARRAY_SIZE(assoc_data->link); i++) {
9361                 if (!assoc_data->link[i].bss)
9362                         continue;
9363                 if (i == assoc_data->assoc_link_id)
9364                         continue;
9365                 /* only calculate the mode, hence link == NULL */
9366                 err = ieee80211_prep_channel(sdata, NULL, i,
9367                                              assoc_data->link[i].bss, true,
9368                                              &assoc_data->link[i].conn,
9369                                              assoc_data->userspace_selectors);
9370                 if (err) {
9371                         req->links[i].error = err;
9372                         goto err_clear;
9373                 }
9374         }
9375
9376         memcpy(vif_cfg->ssid, assoc_data->ssid, assoc_data->ssid_len);
9377         vif_cfg->ssid_len = assoc_data->ssid_len;
9378
9379         /* needed for transmitting the assoc frames properly */
9380         memcpy(sdata->vif.cfg.ap_addr, assoc_data->ap_addr, ETH_ALEN);
9381
9382         err = ieee80211_prep_connection(sdata, cbss, req->link_id,
9383                                         req->ap_mld_addr, true,
9384                                         &assoc_data->link[assoc_link_id].conn,
9385                                         override,
9386                                         assoc_data->userspace_selectors);
9387         if (err)
9388                 goto err_clear;
9389
9390         if (ieee80211_hw_check(&sdata->local->hw, NEED_DTIM_BEFORE_ASSOC)) {
9391                 const struct cfg80211_bss_ies *beacon_ies;
9392
9393                 rcu_read_lock();
9394                 beacon_ies = rcu_dereference(req->bss->beacon_ies);
9395                 if (!beacon_ies) {
9396                         /*
9397                          * Wait up to one beacon interval ...
9398                          * should this be more if we miss one?
9399                          */
9400                         sdata_info(sdata, "waiting for beacon from %pM\n",
9401                                    link->u.mgd.bssid);
9402                         assoc_data->timeout = TU_TO_EXP_TIME(req->bss->beacon_interval);
9403                         assoc_data->timeout_started = true;
9404                         assoc_data->need_beacon = true;
9405                 }
9406                 rcu_read_unlock();
9407         }
9408
9409         run_again(sdata, assoc_data->timeout);
9410
9411         /* We are associating, clean up auth_data */
9412         if (ifmgd->auth_data)
9413                 ieee80211_destroy_auth_data(sdata, true);
9414
9415         return 0;
9416  err_clear:
9417         if (!ifmgd->auth_data) {
9418                 eth_zero_addr(sdata->deflink.u.mgd.bssid);
9419                 ieee80211_link_info_change_notify(sdata, &sdata->deflink,
9420                                                   BSS_CHANGED_BSSID);
9421         }
9422         ifmgd->assoc_data = NULL;
9423  err_free:
9424         kfree(assoc_data);
9425         return err;
9426 }
9427
9428 int ieee80211_mgd_deauth(struct ieee80211_sub_if_data *sdata,
9429                          struct cfg80211_deauth_request *req)
9430 {
9431         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
9432         u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
9433         bool tx = !req->local_state_change;
9434         struct ieee80211_prep_tx_info info = {
9435                 .subtype = IEEE80211_STYPE_DEAUTH,
9436         };
9437
9438         if (ifmgd->auth_data &&
9439             ether_addr_equal(ifmgd->auth_data->ap_addr, req->bssid)) {
9440                 sdata_info(sdata,
9441                            "aborting authentication with %pM by local choice (Reason: %u=%s)\n",
9442                            req->bssid, req->reason_code,
9443                            ieee80211_get_reason_code_string(req->reason_code));
9444
9445                 info.link_id = ifmgd->auth_data->link_id;
9446                 drv_mgd_prepare_tx(sdata->local, sdata, &info);
9447                 ieee80211_send_deauth_disassoc(sdata, req->bssid, req->bssid,
9448                                                IEEE80211_STYPE_DEAUTH,
9449                                                req->reason_code, tx,
9450                                                frame_buf);
9451                 ieee80211_destroy_auth_data(sdata, false);
9452                 ieee80211_report_disconnect(sdata, frame_buf,
9453                                             sizeof(frame_buf), true,
9454                                             req->reason_code, false);
9455                 drv_mgd_complete_tx(sdata->local, sdata, &info);
9456                 return 0;
9457         }
9458
9459         if (ifmgd->assoc_data &&
9460             ether_addr_equal(ifmgd->assoc_data->ap_addr, req->bssid)) {
9461                 sdata_info(sdata,
9462                            "aborting association with %pM by local choice (Reason: %u=%s)\n",
9463                            req->bssid, req->reason_code,
9464                            ieee80211_get_reason_code_string(req->reason_code));
9465
9466                 info.link_id = ifmgd->assoc_data->assoc_link_id;
9467                 drv_mgd_prepare_tx(sdata->local, sdata, &info);
9468                 ieee80211_send_deauth_disassoc(sdata, req->bssid, req->bssid,
9469                                                IEEE80211_STYPE_DEAUTH,
9470                                                req->reason_code, tx,
9471                                                frame_buf);
9472                 ieee80211_destroy_assoc_data(sdata, ASSOC_ABANDON);
9473                 ieee80211_report_disconnect(sdata, frame_buf,
9474                                             sizeof(frame_buf), true,
9475                                             req->reason_code, false);
9476                 drv_mgd_complete_tx(sdata->local, sdata, &info);
9477                 return 0;
9478         }
9479
9480         if (ifmgd->associated &&
9481             ether_addr_equal(sdata->vif.cfg.ap_addr, req->bssid)) {
9482                 sdata_info(sdata,
9483                            "deauthenticating from %pM by local choice (Reason: %u=%s)\n",
9484                            req->bssid, req->reason_code,
9485                            ieee80211_get_reason_code_string(req->reason_code));
9486
9487                 ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH,
9488                                        req->reason_code, tx, frame_buf);
9489                 ieee80211_report_disconnect(sdata, frame_buf,
9490                                             sizeof(frame_buf), true,
9491                                             req->reason_code, false);
9492                 drv_mgd_complete_tx(sdata->local, sdata, &info);
9493                 return 0;
9494         }
9495
9496         return -ENOTCONN;
9497 }
9498
9499 int ieee80211_mgd_disassoc(struct ieee80211_sub_if_data *sdata,
9500                            struct cfg80211_disassoc_request *req)
9501 {
9502         u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
9503
9504         if (!sdata->u.mgd.associated ||
9505             memcmp(sdata->vif.cfg.ap_addr, req->ap_addr, ETH_ALEN))
9506                 return -ENOTCONN;
9507
9508         sdata_info(sdata,
9509                    "disassociating from %pM by local choice (Reason: %u=%s)\n",
9510                    req->ap_addr, req->reason_code,
9511                    ieee80211_get_reason_code_string(req->reason_code));
9512
9513         ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DISASSOC,
9514                                req->reason_code, !req->local_state_change,
9515                                frame_buf);
9516
9517         ieee80211_report_disconnect(sdata, frame_buf, sizeof(frame_buf), true,
9518                                     req->reason_code, false);
9519
9520         return 0;
9521 }
9522
9523 void ieee80211_mgd_stop_link(struct ieee80211_link_data *link)
9524 {
9525         wiphy_work_cancel(link->sdata->local->hw.wiphy,
9526                           &link->u.mgd.request_smps_work);
9527         wiphy_work_cancel(link->sdata->local->hw.wiphy,
9528                           &link->u.mgd.recalc_smps);
9529         wiphy_delayed_work_cancel(link->sdata->local->hw.wiphy,
9530                                   &link->u.mgd.csa.switch_work);
9531 }
9532
9533 void ieee80211_mgd_stop(struct ieee80211_sub_if_data *sdata)
9534 {
9535         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
9536
9537         /*
9538          * Make sure some work items will not run after this,
9539          * they will not do anything but might not have been
9540          * cancelled when disconnecting.
9541          */
9542         wiphy_work_cancel(sdata->local->hw.wiphy,
9543                           &ifmgd->monitor_work);
9544         wiphy_work_cancel(sdata->local->hw.wiphy,
9545                           &ifmgd->beacon_connection_loss_work);
9546         wiphy_work_cancel(sdata->local->hw.wiphy,
9547                           &ifmgd->csa_connection_drop_work);
9548         wiphy_delayed_work_cancel(sdata->local->hw.wiphy,
9549                                   &ifmgd->tdls_peer_del_work);
9550
9551         if (ifmgd->assoc_data)
9552                 ieee80211_destroy_assoc_data(sdata, ASSOC_TIMEOUT);
9553         if (ifmgd->auth_data)
9554                 ieee80211_destroy_auth_data(sdata, false);
9555         spin_lock_bh(&ifmgd->teardown_lock);
9556         if (ifmgd->teardown_skb) {
9557                 kfree_skb(ifmgd->teardown_skb);
9558                 ifmgd->teardown_skb = NULL;
9559                 ifmgd->orig_teardown_skb = NULL;
9560         }
9561         kfree(ifmgd->assoc_req_ies);
9562         ifmgd->assoc_req_ies = NULL;
9563         ifmgd->assoc_req_ies_len = 0;
9564         spin_unlock_bh(&ifmgd->teardown_lock);
9565         del_timer_sync(&ifmgd->timer);
9566 }
9567
9568 void ieee80211_cqm_rssi_notify(struct ieee80211_vif *vif,
9569                                enum nl80211_cqm_rssi_threshold_event rssi_event,
9570                                s32 rssi_level,
9571                                gfp_t gfp)
9572 {
9573         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
9574
9575         trace_api_cqm_rssi_notify(sdata, rssi_event, rssi_level);
9576
9577         cfg80211_cqm_rssi_notify(sdata->dev, rssi_event, rssi_level, gfp);
9578 }
9579 EXPORT_SYMBOL(ieee80211_cqm_rssi_notify);
9580
9581 void ieee80211_cqm_beacon_loss_notify(struct ieee80211_vif *vif, gfp_t gfp)
9582 {
9583         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
9584
9585         trace_api_cqm_beacon_loss_notify(sdata->local, sdata);
9586
9587         cfg80211_cqm_beacon_loss_notify(sdata->dev, gfp);
9588 }
9589 EXPORT_SYMBOL(ieee80211_cqm_beacon_loss_notify);
9590
9591 static void _ieee80211_enable_rssi_reports(struct ieee80211_sub_if_data *sdata,
9592                                             int rssi_min_thold,
9593                                             int rssi_max_thold)
9594 {
9595         trace_api_enable_rssi_reports(sdata, rssi_min_thold, rssi_max_thold);
9596
9597         if (WARN_ON(sdata->vif.type != NL80211_IFTYPE_STATION))
9598                 return;
9599
9600         /*
9601          * Scale up threshold values before storing it, as the RSSI averaging
9602          * algorithm uses a scaled up value as well. Change this scaling
9603          * factor if the RSSI averaging algorithm changes.
9604          */
9605         sdata->u.mgd.rssi_min_thold = rssi_min_thold*16;
9606         sdata->u.mgd.rssi_max_thold = rssi_max_thold*16;
9607 }
9608
9609 void ieee80211_enable_rssi_reports(struct ieee80211_vif *vif,
9610                                     int rssi_min_thold,
9611                                     int rssi_max_thold)
9612 {
9613         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
9614
9615         WARN_ON(rssi_min_thold == rssi_max_thold ||
9616                 rssi_min_thold > rssi_max_thold);
9617
9618         _ieee80211_enable_rssi_reports(sdata, rssi_min_thold,
9619                                        rssi_max_thold);
9620 }
9621 EXPORT_SYMBOL(ieee80211_enable_rssi_reports);
9622
9623 void ieee80211_disable_rssi_reports(struct ieee80211_vif *vif)
9624 {
9625         struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
9626
9627         _ieee80211_enable_rssi_reports(sdata, 0, 0);
9628 }
9629 EXPORT_SYMBOL(ieee80211_disable_rssi_reports);
9630
9631 static void ieee80211_ml_reconf_selectors(unsigned long *userspace_selectors)
9632 {
9633         *userspace_selectors = 0;
9634
9635         /* these selectors are mandatory for ML reconfiguration */
9636         set_bit(BSS_MEMBERSHIP_SELECTOR_SAE_H2E, userspace_selectors);
9637         set_bit(BSS_MEMBERSHIP_SELECTOR_HE_PHY, userspace_selectors);
9638         set_bit(BSS_MEMBERSHIP_SELECTOR_EHT_PHY, userspace_selectors);
9639 }
9640
9641 void ieee80211_process_ml_reconf_resp(struct ieee80211_sub_if_data *sdata,
9642                                       struct ieee80211_mgmt *mgmt, size_t len)
9643 {
9644         struct ieee80211_local *local = sdata->local;
9645         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
9646         struct ieee80211_mgd_assoc_data *add_links_data =
9647                 ifmgd->reconf.add_links_data;
9648         struct sta_info *sta;
9649         struct cfg80211_mlo_reconf_done_data done_data = {};
9650         u16 sta_changed_links = sdata->u.mgd.reconf.added_links |
9651                                 sdata->u.mgd.reconf.removed_links;
9652         u16 link_mask, valid_links;
9653         unsigned int link_id;
9654         unsigned long userspace_selectors;
9655         size_t orig_len = len;
9656         u8 i, group_key_data_len;
9657         u8 *pos;
9658
9659         if (!ieee80211_vif_is_mld(&sdata->vif) ||
9660             len < offsetofend(typeof(*mgmt), u.action.u.ml_reconf_resp) ||
9661             mgmt->u.action.u.ml_reconf_resp.dialog_token !=
9662             sdata->u.mgd.reconf.dialog_token ||
9663             !sta_changed_links)
9664                 return;
9665
9666         pos = mgmt->u.action.u.ml_reconf_resp.variable;
9667         len -= offsetofend(typeof(*mgmt), u.action.u.ml_reconf_resp);
9668
9669         /* each status duple is 3 octets */
9670         if (len < mgmt->u.action.u.ml_reconf_resp.count * 3) {
9671                 sdata_info(sdata,
9672                            "mlo: reconf: unexpected len=%zu, count=%u\n",
9673                            len, mgmt->u.action.u.ml_reconf_resp.count);
9674                 goto disconnect;
9675         }
9676
9677         link_mask = sta_changed_links;
9678         for (i = 0; i < mgmt->u.action.u.ml_reconf_resp.count; i++) {
9679                 u16 status = get_unaligned_le16(pos + 1);
9680
9681                 link_id = *pos;
9682
9683                 if (!(link_mask & BIT(link_id))) {
9684                         sdata_info(sdata,
9685                                    "mlo: reconf: unexpected link: %u, changed=0x%x\n",
9686                                    link_id, sta_changed_links);
9687                         goto disconnect;
9688                 }
9689
9690                 /* clear the corresponding link, to detect the case that
9691                  * the same link was included more than one time
9692                  */
9693                 link_mask &= ~BIT(link_id);
9694
9695                 /* Handle failure to remove links here. Failure to remove added
9696                  * links will be done later in the flow.
9697                  */
9698                 if (status != WLAN_STATUS_SUCCESS) {
9699                         sdata_info(sdata,
9700                                    "mlo: reconf: failed on link=%u, status=%u\n",
9701                                    link_id, status);
9702
9703                         /* The AP MLD failed to remove a link that was already
9704                          * removed locally. As this is not expected behavior,
9705                          * disconnect
9706                          */
9707                         if (sdata->u.mgd.reconf.removed_links & BIT(link_id))
9708                                 goto disconnect;
9709
9710                         /* The AP MLD failed to add a link. Remove it from the
9711                          * added links.
9712                          */
9713                         sdata->u.mgd.reconf.added_links &= ~BIT(link_id);
9714                 }
9715
9716                 pos += 3;
9717                 len -= 3;
9718         }
9719
9720         if (link_mask) {
9721                 sdata_info(sdata,
9722                            "mlo: reconf: no response for links=0x%x\n",
9723                            link_mask);
9724                 goto disconnect;
9725         }
9726
9727         if (!sdata->u.mgd.reconf.added_links)
9728                 goto out;
9729
9730         if (len < 1 || len < 1 + *pos) {
9731                 sdata_info(sdata,
9732                            "mlo: reconf: invalid group key data length");
9733                 goto disconnect;
9734         }
9735
9736         /* The Group Key Data field must be present when links are added. This
9737          * field should be processed by userland.
9738          */
9739         group_key_data_len = *pos++;
9740
9741         pos += group_key_data_len;
9742         len -= group_key_data_len + 1;
9743
9744         /* Process the information for the added links */
9745         sta = sta_info_get(sdata, sdata->vif.cfg.ap_addr);
9746         if (WARN_ON(!sta))
9747                 goto disconnect;
9748
9749         valid_links = sdata->vif.valid_links;
9750         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
9751                 if (!add_links_data->link[link_id].bss ||
9752                     !(sdata->u.mgd.reconf.added_links & BIT(link_id)))
9753
9754                         continue;
9755
9756                 valid_links |= BIT(link_id);
9757                 if (ieee80211_sta_allocate_link(sta, link_id))
9758                         goto disconnect;
9759         }
9760
9761         ieee80211_vif_set_links(sdata, valid_links, sdata->vif.dormant_links);
9762         ieee80211_ml_reconf_selectors(&userspace_selectors);
9763         link_mask = 0;
9764         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
9765                 struct cfg80211_bss *cbss = add_links_data->link[link_id].bss;
9766                 struct ieee80211_link_data *link;
9767                 struct link_sta_info *link_sta;
9768                 u64 changed = 0;
9769
9770                 if (!cbss)
9771                         continue;
9772
9773                 link = sdata_dereference(sdata->link[link_id], sdata);
9774                 if (WARN_ON(!link))
9775                         goto disconnect;
9776
9777                 link_info(link,
9778                           "mlo: reconf: local address %pM, AP link address %pM\n",
9779                           add_links_data->link[link_id].addr,
9780                           add_links_data->link[link_id].bss->bssid);
9781
9782                 link_sta = rcu_dereference_protected(sta->link[link_id],
9783                                                      lockdep_is_held(&local->hw.wiphy->mtx));
9784                 if (WARN_ON(!link_sta))
9785                         goto disconnect;
9786
9787                 if (!link->u.mgd.have_beacon) {
9788                         const struct cfg80211_bss_ies *ies;
9789
9790                         rcu_read_lock();
9791                         ies = rcu_dereference(cbss->beacon_ies);
9792                         if (ies)
9793                                 link->u.mgd.have_beacon = true;
9794                         else
9795                                 ies = rcu_dereference(cbss->ies);
9796                         ieee80211_get_dtim(ies,
9797                                            &link->conf->sync_dtim_count,
9798                                            &link->u.mgd.dtim_period);
9799                         link->conf->beacon_int = cbss->beacon_interval;
9800                         rcu_read_unlock();
9801                 }
9802
9803                 link->conf->dtim_period = link->u.mgd.dtim_period ?: 1;
9804
9805                 link->u.mgd.conn = add_links_data->link[link_id].conn;
9806                 if (ieee80211_prep_channel(sdata, link, link_id, cbss,
9807                                            true, &link->u.mgd.conn,
9808                                            &userspace_selectors)) {
9809                         link_info(link, "mlo: reconf: prep_channel failed\n");
9810                         goto disconnect;
9811                 }
9812
9813                 if (ieee80211_mgd_setup_link_sta(link, sta, link_sta,
9814                                                  add_links_data->link[link_id].bss))
9815                         goto disconnect;
9816
9817                 if (!ieee80211_assoc_config_link(link, link_sta,
9818                                                  add_links_data->link[link_id].bss,
9819                                                  mgmt, pos, len,
9820                                                  &changed))
9821                         goto disconnect;
9822
9823                 /* The AP MLD indicated success for this link, but the station
9824                  * profile status indicated otherwise. Since there is an
9825                  * inconsistency in the ML reconfiguration response, disconnect
9826                  */
9827                 if (add_links_data->link[link_id].status != WLAN_STATUS_SUCCESS)
9828                         goto disconnect;
9829
9830                 ieee80211_sta_init_nss(link_sta);
9831                 if (ieee80211_sta_activate_link(sta, link_id))
9832                         goto disconnect;
9833
9834                 changed |= ieee80211_link_set_associated(link, cbss);
9835                 ieee80211_link_info_change_notify(sdata, link, changed);
9836
9837                 ieee80211_recalc_smps(sdata, link);
9838                 link_mask |= BIT(link_id);
9839         }
9840
9841         sdata_info(sdata,
9842                    "mlo: reconf: current valid_links=0x%x, added=0x%x\n",
9843                    valid_links, link_mask);
9844
9845         /* links might have changed due to rejected ones, set them again */
9846         ieee80211_vif_set_links(sdata, valid_links, sdata->vif.dormant_links);
9847         ieee80211_vif_cfg_change_notify(sdata, BSS_CHANGED_MLD_VALID_LINKS);
9848
9849         ieee80211_recalc_ps(local);
9850         ieee80211_recalc_ps_vif(sdata);
9851
9852         done_data.buf = (const u8 *)mgmt;
9853         done_data.len = orig_len;
9854         done_data.added_links = link_mask;
9855
9856         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++)
9857                 done_data.links[link_id].bss = add_links_data->link[link_id].bss;
9858
9859         cfg80211_mlo_reconf_add_done(sdata->dev, &done_data);
9860         kfree(sdata->u.mgd.reconf.add_links_data);
9861         sdata->u.mgd.reconf.add_links_data = NULL;
9862 out:
9863         ieee80211_ml_reconf_reset(sdata);
9864         return;
9865
9866 disconnect:
9867         __ieee80211_disconnect(sdata);
9868 }
9869
9870 static struct sk_buff *
9871 ieee80211_build_ml_reconf_req(struct ieee80211_sub_if_data *sdata,
9872                               struct ieee80211_mgd_assoc_data *add_links_data,
9873                               u16 removed_links)
9874 {
9875         struct ieee80211_local *local = sdata->local;
9876         struct ieee80211_mgmt *mgmt;
9877         struct ieee80211_multi_link_elem *ml_elem;
9878         struct ieee80211_mle_basic_common_info *common;
9879         enum nl80211_iftype iftype = ieee80211_vif_type_p2p(&sdata->vif);
9880         struct sk_buff *skb;
9881         size_t size;
9882         unsigned int link_id;
9883         __le16 eml_capa = 0, mld_capa_ops = 0;
9884         struct ieee80211_tx_info *info;
9885         u8 common_size, var_common_size;
9886         u8 *ml_elem_len;
9887         u16 capab = 0;
9888
9889         size = local->hw.extra_tx_headroom + sizeof(*mgmt);
9890
9891         /* Consider the maximal length of the reconfiguration ML element */
9892         size += sizeof(struct ieee80211_multi_link_elem);
9893
9894         /* The Basic ML element and the Reconfiguration ML element have the same
9895          * fixed common information fields in the context of ML reconfiguration
9896          * action frame. The AP MLD MAC address must always be present
9897          */
9898         common_size = sizeof(*common);
9899
9900         /* when adding links, the MLD capabilities must be present */
9901         var_common_size = 0;
9902         if (add_links_data) {
9903                 const struct wiphy_iftype_ext_capab *ift_ext_capa =
9904                         cfg80211_get_iftype_ext_capa(local->hw.wiphy,
9905                                                      ieee80211_vif_type_p2p(&sdata->vif));
9906
9907                 if (ift_ext_capa) {
9908                         eml_capa = cpu_to_le16(ift_ext_capa->eml_capabilities);
9909                         mld_capa_ops =
9910                                 cpu_to_le16(ift_ext_capa->mld_capa_and_ops);
9911                 }
9912
9913                 /* MLD capabilities and operation */
9914                 var_common_size += 2;
9915
9916                 /* EML capabilities */
9917                 if (eml_capa & cpu_to_le16((IEEE80211_EML_CAP_EMLSR_SUPP |
9918                                             IEEE80211_EML_CAP_EMLMR_SUPPORT)))
9919                         var_common_size += 2;
9920         }
9921
9922         /* Add the common information length */
9923         size += common_size + var_common_size;
9924
9925         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
9926                 struct cfg80211_bss *cbss;
9927                 size_t elems_len;
9928
9929                 if (removed_links & BIT(link_id)) {
9930                         size += sizeof(struct ieee80211_mle_per_sta_profile) +
9931                                 ETH_ALEN;
9932                         continue;
9933                 }
9934
9935                 if (!add_links_data || !add_links_data->link[link_id].bss)
9936                         continue;
9937
9938                 elems_len = add_links_data->link[link_id].elems_len;
9939                 cbss = add_links_data->link[link_id].bss;
9940
9941                 /* should be the same across all BSSes */
9942                 if (cbss->capability & WLAN_CAPABILITY_PRIVACY)
9943                         capab |= WLAN_CAPABILITY_PRIVACY;
9944
9945                 size += 2 + sizeof(struct ieee80211_mle_per_sta_profile) +
9946                         ETH_ALEN;
9947
9948                 /* SSID element + WMM */
9949                 size += 2 + sdata->vif.cfg.ssid_len + 9;
9950                 size += ieee80211_link_common_elems_size(sdata, iftype, cbss,
9951                                                          elems_len);
9952         }
9953
9954         skb = alloc_skb(size, GFP_KERNEL);
9955         if (!skb)
9956                 return NULL;
9957
9958         skb_reserve(skb, local->hw.extra_tx_headroom);
9959         mgmt = skb_put_zero(skb, offsetofend(struct ieee80211_mgmt,
9960                                              u.action.u.ml_reconf_req));
9961
9962         /* Add the MAC header */
9963         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
9964                                           IEEE80211_STYPE_ACTION);
9965         memcpy(mgmt->da, sdata->vif.cfg.ap_addr, ETH_ALEN);
9966         memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
9967         memcpy(mgmt->bssid, sdata->vif.cfg.ap_addr, ETH_ALEN);
9968
9969         /* Add the action frame fixed fields */
9970         mgmt->u.action.category = WLAN_CATEGORY_PROTECTED_EHT;
9971         mgmt->u.action.u.ml_reconf_req.action_code =
9972                 WLAN_PROTECTED_EHT_ACTION_LINK_RECONFIG_REQ;
9973
9974         /* allocate a dialog token and store it */
9975         sdata->u.mgd.reconf.dialog_token = ++sdata->u.mgd.dialog_token_alloc;
9976         mgmt->u.action.u.ml_reconf_req.dialog_token =
9977                 sdata->u.mgd.reconf.dialog_token;
9978
9979         /* Add the ML reconfiguration element and the common information  */
9980         skb_put_u8(skb, WLAN_EID_EXTENSION);
9981         ml_elem_len = skb_put(skb, 1);
9982         skb_put_u8(skb, WLAN_EID_EXT_EHT_MULTI_LINK);
9983         ml_elem = skb_put(skb, sizeof(*ml_elem));
9984         ml_elem->control =
9985                 cpu_to_le16(IEEE80211_ML_CONTROL_TYPE_RECONF |
9986                             IEEE80211_MLC_RECONF_PRES_MLD_MAC_ADDR);
9987         common = skb_put(skb, common_size);
9988         common->len = common_size + var_common_size;
9989         memcpy(common->mld_mac_addr, sdata->vif.addr, ETH_ALEN);
9990
9991         if (add_links_data) {
9992                 if (eml_capa &
9993                     cpu_to_le16((IEEE80211_EML_CAP_EMLSR_SUPP |
9994                                  IEEE80211_EML_CAP_EMLMR_SUPPORT))) {
9995                         ml_elem->control |=
9996                                 cpu_to_le16(IEEE80211_MLC_RECONF_PRES_EML_CAPA);
9997                         skb_put_data(skb, &eml_capa, sizeof(eml_capa));
9998                 }
9999
10000                 ml_elem->control |=
10001                         cpu_to_le16(IEEE80211_MLC_RECONF_PRES_MLD_CAPA_OP);
10002
10003                 skb_put_data(skb, &mld_capa_ops, sizeof(mld_capa_ops));
10004         }
10005
10006         if (sdata->u.mgd.flags & IEEE80211_STA_ENABLE_RRM)
10007                 capab |= WLAN_CAPABILITY_RADIO_MEASURE;
10008
10009         /* Add the per station profile */
10010         for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS; link_id++) {
10011                 u8 *subelem_len = NULL;
10012                 u16 ctrl;
10013                 const u8 *addr;
10014
10015                 /* Skip links that are not changing */
10016                 if (!(removed_links & BIT(link_id)) &&
10017                     (!add_links_data || !add_links_data->link[link_id].bss))
10018                         continue;
10019
10020                 ctrl = link_id |
10021                        IEEE80211_MLE_STA_RECONF_CONTROL_STA_MAC_ADDR_PRESENT;
10022
10023                 if (removed_links & BIT(link_id)) {
10024                         struct ieee80211_bss_conf *conf =
10025                                 sdata_dereference(sdata->vif.link_conf[link_id],
10026                                                   sdata);
10027                         if (!conf)
10028                                 continue;
10029
10030                         addr = conf->addr;
10031                         ctrl |= u16_encode_bits(IEEE80211_MLE_STA_RECONF_CONTROL_OPERATION_TYPE_DEL_LINK,
10032                                                 IEEE80211_MLE_STA_RECONF_CONTROL_OPERATION_TYPE);
10033                 } else {
10034                         addr = add_links_data->link[link_id].addr;
10035                         ctrl |= IEEE80211_MLE_STA_RECONF_CONTROL_COMPLETE_PROFILE |
10036                                 u16_encode_bits(IEEE80211_MLE_STA_RECONF_CONTROL_OPERATION_TYPE_ADD_LINK,
10037                                                 IEEE80211_MLE_STA_RECONF_CONTROL_OPERATION_TYPE);
10038                 }
10039
10040                 skb_put_u8(skb, IEEE80211_MLE_SUBELEM_PER_STA_PROFILE);
10041                 subelem_len = skb_put(skb, 1);
10042
10043                 put_unaligned_le16(ctrl, skb_put(skb, sizeof(ctrl)));
10044                 skb_put_u8(skb, 1 + ETH_ALEN);
10045                 skb_put_data(skb, addr, ETH_ALEN);
10046
10047                 if (!(removed_links & BIT(link_id))) {
10048                         u16 link_present_elems[PRESENT_ELEMS_MAX] = {};
10049                         size_t extra_used;
10050                         void *capab_pos;
10051                         u8 qos_info;
10052
10053                         capab_pos = skb_put(skb, 2);
10054
10055                         skb_put_u8(skb, WLAN_EID_SSID);
10056                         skb_put_u8(skb, sdata->vif.cfg.ssid_len);
10057                         skb_put_data(skb, sdata->vif.cfg.ssid,
10058                                      sdata->vif.cfg.ssid_len);
10059
10060                         extra_used =
10061                                 ieee80211_add_link_elems(sdata, skb, &capab, NULL,
10062                                                          add_links_data->link[link_id].elems,
10063                                                          add_links_data->link[link_id].elems_len,
10064                                                          link_id, NULL,
10065                                                          link_present_elems,
10066                                                          add_links_data);
10067
10068                         if (add_links_data->link[link_id].elems)
10069                                 skb_put_data(skb,
10070                                              add_links_data->link[link_id].elems +
10071                                              extra_used,
10072                                              add_links_data->link[link_id].elems_len -
10073                                              extra_used);
10074                         if (sdata->u.mgd.flags & IEEE80211_STA_UAPSD_ENABLED) {
10075                                 qos_info = sdata->u.mgd.uapsd_queues;
10076                                 qos_info |= (sdata->u.mgd.uapsd_max_sp_len <<
10077                                              IEEE80211_WMM_IE_STA_QOSINFO_SP_SHIFT);
10078                         } else {
10079                                 qos_info = 0;
10080                         }
10081
10082                         ieee80211_add_wmm_info_ie(skb_put(skb, 9), qos_info);
10083                         put_unaligned_le16(capab, capab_pos);
10084                 }
10085
10086                 ieee80211_fragment_element(skb, subelem_len,
10087                                            IEEE80211_MLE_SUBELEM_FRAGMENT);
10088         }
10089
10090         ieee80211_fragment_element(skb, ml_elem_len, WLAN_EID_FRAGMENT);
10091
10092         info = IEEE80211_SKB_CB(skb);
10093         info->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
10094
10095         return skb;
10096 }
10097
10098 int ieee80211_mgd_assoc_ml_reconf(struct ieee80211_sub_if_data *sdata,
10099                                   struct cfg80211_assoc_link *add_links,
10100                                   u16 rem_links)
10101 {
10102         struct ieee80211_local *local = sdata->local;
10103         struct ieee80211_mgd_assoc_data *data = NULL;
10104         struct sta_info *sta;
10105         struct sk_buff *skb;
10106         u16 added_links, new_valid_links;
10107         int link_id, err;
10108
10109         if (!ieee80211_vif_is_mld(&sdata->vif) ||
10110             !(sdata->vif.cfg.mld_capa_op &
10111               IEEE80211_MLD_CAP_OP_LINK_RECONF_SUPPORT))
10112                 return -EINVAL;
10113
10114         /* No support for concurrent ML reconfiguration operation */
10115         if (sdata->u.mgd.reconf.added_links ||
10116             sdata->u.mgd.reconf.removed_links)
10117                 return -EBUSY;
10118
10119         added_links = 0;
10120         for (link_id = 0; add_links && link_id < IEEE80211_MLD_MAX_NUM_LINKS;
10121              link_id++) {
10122                 if (!add_links[link_id].bss)
10123                         continue;
10124
10125                 added_links |= BIT(link_id);
10126         }
10127
10128         sta = sta_info_get(sdata, sdata->vif.cfg.ap_addr);
10129         if (WARN_ON(!sta))
10130                 return -ENOLINK;
10131
10132         if (rem_links & BIT(sta->sta.deflink.link_id))
10133                 return -EINVAL;
10134
10135         /* Adding links to the set of valid link is done only after a successful
10136          * ML reconfiguration frame exchange. Here prepare the data for the ML
10137          * reconfiguration frame construction and allocate the required
10138          * resources
10139          */
10140         if (added_links) {
10141                 bool uapsd_supported;
10142                 unsigned long userspace_selectors;
10143
10144                 data = kzalloc(sizeof(*data), GFP_KERNEL);
10145                 if (!data)
10146                         return -ENOMEM;
10147
10148                 uapsd_supported = true;
10149                 ieee80211_ml_reconf_selectors(&userspace_selectors);
10150                 for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS;
10151                      link_id++) {
10152                         struct ieee80211_supported_band *sband;
10153                         struct cfg80211_bss *link_cbss = add_links[link_id].bss;
10154                         struct ieee80211_bss *bss;
10155
10156                         if (!link_cbss)
10157                                 continue;
10158
10159                         bss = (void *)link_cbss->priv;
10160
10161                         if (!bss->wmm_used) {
10162                                 err = -EINVAL;
10163                                 goto err_free;
10164                         }
10165
10166                         if (link_cbss->channel->band == NL80211_BAND_S1GHZ) {
10167                                 err = -EINVAL;
10168                                 goto err_free;
10169                         }
10170
10171                         eth_random_addr(data->link[link_id].addr);
10172                         data->link[link_id].conn =
10173                                 ieee80211_conn_settings_unlimited;
10174                         sband =
10175                                 local->hw.wiphy->bands[link_cbss->channel->band];
10176
10177                         ieee80211_determine_our_sta_mode(sdata, sband,
10178                                                          NULL, true, link_id,
10179                                                          &data->link[link_id].conn);
10180
10181                         data->link[link_id].bss = link_cbss;
10182                         data->link[link_id].disabled =
10183                                 add_links[link_id].disabled;
10184                         data->link[link_id].elems =
10185                                 (u8 *)add_links[link_id].elems;
10186                         data->link[link_id].elems_len =
10187                                 add_links[link_id].elems_len;
10188
10189                         if (!bss->uapsd_supported)
10190                                 uapsd_supported = false;
10191
10192                         if (data->link[link_id].conn.mode <
10193                             IEEE80211_CONN_MODE_EHT) {
10194                                 err = -EINVAL;
10195                                 goto err_free;
10196                         }
10197
10198                         err = ieee80211_mgd_get_ap_ht_vht_capa(sdata, data,
10199                                                                link_id);
10200                         if (err) {
10201                                 err = -EINVAL;
10202                                 goto err_free;
10203                         }
10204                 }
10205
10206                 /* Require U-APSD support to be similar to the current valid
10207                  * links
10208                  */
10209                 if (uapsd_supported !=
10210                     !!(sdata->u.mgd.flags & IEEE80211_STA_UAPSD_ENABLED)) {
10211                         err = -EINVAL;
10212                         goto err_free;
10213                 }
10214
10215                 for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS;
10216                      link_id++) {
10217                         if (!data->link[link_id].bss)
10218                                 continue;
10219
10220                         /* only used to verify the mode, nothing is allocated */
10221                         err = ieee80211_prep_channel(sdata, NULL, link_id,
10222                                                      data->link[link_id].bss,
10223                                                      true,
10224                                                      &data->link[link_id].conn,
10225                                                      &userspace_selectors);
10226                         if (err)
10227                                 goto err_free;
10228                 }
10229         }
10230
10231         /* link removal is done before the ML reconfiguration frame exchange so
10232          * that these links will not be used between their removal by the AP MLD
10233          * and before the station got the ML reconfiguration response. Based on
10234          * Section 35.3.6.4 in Draft P802.11be_D7.0 the AP MLD should accept the
10235          * link removal request.
10236          */
10237         if (rem_links) {
10238                 u16 new_active_links = sdata->vif.active_links & ~rem_links;
10239
10240                 new_valid_links = sdata->vif.valid_links & ~rem_links;
10241
10242                 /* Should not be left with no valid links to perform the
10243                  * ML reconfiguration
10244                  */
10245                 if (!new_valid_links ||
10246                     !(new_valid_links & ~sdata->vif.dormant_links)) {
10247                         sdata_info(sdata, "mlo: reconf: no valid links\n");
10248                         err = -EINVAL;
10249                         goto err_free;
10250                 }
10251
10252                 if (new_active_links != sdata->vif.active_links) {
10253                         if (!new_active_links)
10254                                 new_active_links =
10255                                         BIT(__ffs(new_valid_links &
10256                                                   ~sdata->vif.dormant_links));
10257
10258                         err = ieee80211_set_active_links(&sdata->vif,
10259                                                          new_active_links);
10260                         if (err) {
10261                                 sdata_info(sdata,
10262                                            "mlo: reconf: failed set active links\n");
10263                                 goto err_free;
10264                         }
10265                 }
10266         }
10267
10268         /* Build the SKB before the link removal as the construction of the
10269          * station info for removed links requires the local address.
10270          * Invalidate the removed links, so that the transmission of the ML
10271          * reconfiguration request frame would not be done using them, as the AP
10272          * is expected to send the ML reconfiguration response frame on the link
10273          * on which the request was received.
10274          */
10275         skb = ieee80211_build_ml_reconf_req(sdata, data, rem_links);
10276         if (!skb) {
10277                 err = -ENOMEM;
10278                 goto err_free;
10279         }
10280
10281         if (rem_links) {
10282                 u16 new_dormant_links = sdata->vif.dormant_links & ~rem_links;
10283
10284                 err = ieee80211_vif_set_links(sdata, new_valid_links,
10285                                               new_dormant_links);
10286                 if (err) {
10287                         sdata_info(sdata,
10288                                    "mlo: reconf: failed set valid links\n");
10289                         kfree_skb(skb);
10290                         goto err_free;
10291                 }
10292
10293                 for (link_id = 0; link_id < IEEE80211_MLD_MAX_NUM_LINKS;
10294                      link_id++) {
10295                         if (!(rem_links & BIT(link_id)))
10296                                 continue;
10297
10298                         ieee80211_sta_remove_link(sta, link_id);
10299                 }
10300
10301                 /* notify the driver and upper layers */
10302                 ieee80211_vif_cfg_change_notify(sdata,
10303                                                 BSS_CHANGED_MLD_VALID_LINKS);
10304                 cfg80211_links_removed(sdata->dev, rem_links);
10305         }
10306
10307         sdata_info(sdata, "mlo: reconf: adding=0x%x, removed=0x%x\n",
10308                    added_links, rem_links);
10309
10310         ieee80211_tx_skb(sdata, skb);
10311
10312         sdata->u.mgd.reconf.added_links = added_links;
10313         sdata->u.mgd.reconf.add_links_data = data;
10314         sdata->u.mgd.reconf.removed_links = rem_links;
10315         wiphy_delayed_work_queue(sdata->local->hw.wiphy,
10316                                  &sdata->u.mgd.reconf.wk,
10317                                  IEEE80211_ASSOC_TIMEOUT_SHORT);
10318         return 0;
10319
10320  err_free:
10321         kfree(data);
10322         return err;
10323 }