drivers/net/wireless/intel/iwlwifi/mvm/link.c

   1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
   2 /*
   3  * Copyright (C) 2022 - 2024 Intel Corporation
   4  */
   5 #include "mvm.h"
   6 #include "time-event.h"
   7
   8 #define HANDLE_ESR_REASONS(HOW)         \
   9         HOW(BLOCKED_PREVENTION)         \
  10         HOW(BLOCKED_WOWLAN)             \
  11         HOW(BLOCKED_TPT)                \
  12         HOW(BLOCKED_FW)                 \
  13         HOW(BLOCKED_NON_BSS)            \
  14         HOW(BLOCKED_ROC)                \
  15         HOW(BLOCKED_TMP_NON_BSS)        \
  16         HOW(EXIT_MISSED_BEACON)         \
  17         HOW(EXIT_LOW_RSSI)              \
  18         HOW(EXIT_COEX)                  \
  19         HOW(EXIT_BANDWIDTH)             \
  20         HOW(EXIT_CSA)                   \
  21         HOW(EXIT_LINK_USAGE)            \
  22         HOW(EXIT_FAIL_ENTRY)
  23
  24 static const char *const iwl_mvm_esr_states_names[] = {
  25 #define NAME_ENTRY(x) [ilog2(IWL_MVM_ESR_##x)] = #x,
  26         HANDLE_ESR_REASONS(NAME_ENTRY)
  27 };
  28
  29 const char *iwl_get_esr_state_string(enum iwl_mvm_esr_state state)
  30 {
  31         int offs = ilog2(state);
  32
  33         if (offs >= ARRAY_SIZE(iwl_mvm_esr_states_names) ||
  34             !iwl_mvm_esr_states_names[offs])
  35                 return "UNKNOWN";
  36
  37         return iwl_mvm_esr_states_names[offs];
  38 }
  39
  40 static void iwl_mvm_print_esr_state(struct iwl_mvm *mvm, u32 mask)
  41 {
  42 #define NAME_FMT(x) "%s"
  43 #define NAME_PR(x) (mask & IWL_MVM_ESR_##x) ? "[" #x "]" : "",
  44         IWL_DEBUG_INFO(mvm,
  45                        "EMLSR state = " HANDLE_ESR_REASONS(NAME_FMT)
  46                        " (0x%x)\n",
  47                        HANDLE_ESR_REASONS(NAME_PR)
  48                        mask);
  49 #undef NAME_FMT
  50 #undef NAME_PR
  51 }
  52
  53 static u32 iwl_mvm_get_free_fw_link_id(struct iwl_mvm *mvm,
  54                                        struct iwl_mvm_vif *mvm_vif)
  55 {
  56         u32 i;
  57
  58         lockdep_assert_held(&mvm->mutex);
  59
  60         for (i = 0; i < ARRAY_SIZE(mvm->link_id_to_link_conf); i++)
  61                 if (!rcu_access_pointer(mvm->link_id_to_link_conf[i]))
  62                         return i;
  63
  64         return IWL_MVM_FW_LINK_ID_INVALID;
  65 }
  66
  67 static int iwl_mvm_link_cmd_send(struct iwl_mvm *mvm,
  68                                  struct iwl_link_config_cmd *cmd,
  69                                  enum iwl_ctxt_action action)
  70 {
  71         int ret;
  72
  73         cmd->action = cpu_to_le32(action);
  74         ret = iwl_mvm_send_cmd_pdu(mvm,
  75                                    WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD), 0,
  76                                    sizeof(*cmd), cmd);
  77         if (ret)
  78                 IWL_ERR(mvm, "Failed to send LINK_CONFIG_CMD (action:%d): %d\n",
  79                         action, ret);
  80         return ret;
  81 }
  82
  83 int iwl_mvm_set_link_mapping(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
  84                              struct ieee80211_bss_conf *link_conf)
  85 {
  86         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
  87         struct iwl_mvm_vif_link_info *link_info =
  88                 mvmvif->link[link_conf->link_id];
  89
  90         if (link_info->fw_link_id == IWL_MVM_FW_LINK_ID_INVALID) {
  91                 link_info->fw_link_id = iwl_mvm_get_free_fw_link_id(mvm,
  92                                                                     mvmvif);
  93                 if (link_info->fw_link_id >=
  94                     ARRAY_SIZE(mvm->link_id_to_link_conf))
  95                         return -EINVAL;
  96
  97                 rcu_assign_pointer(mvm->link_id_to_link_conf[link_info->fw_link_id],
  98                                    link_conf);
  99         }
 100
 101         return 0;
 102 }
 103
 104 int iwl_mvm_add_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 105                      struct ieee80211_bss_conf *link_conf)
 106 {
 107         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 108         unsigned int link_id = link_conf->link_id;
 109         struct iwl_mvm_vif_link_info *link_info = mvmvif->link[link_id];
 110         struct iwl_link_config_cmd cmd = {};
 111         unsigned int cmd_id = WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD);
 112         u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 1);
 113         int ret;
 114
 115         if (WARN_ON_ONCE(!link_info))
 116                 return -EINVAL;
 117
 118         ret = iwl_mvm_set_link_mapping(mvm, vif, link_conf);
 119         if (ret)
 120                 return ret;
 121
 122         /* Update SF - Disable if needed. if this fails, SF might still be on
 123          * while many macs are bound, which is forbidden - so fail the binding.
 124          */
 125         if (iwl_mvm_sf_update(mvm, vif, false))
 126                 return -EINVAL;
 127
 128         cmd.link_id = cpu_to_le32(link_info->fw_link_id);
 129         cmd.mac_id = cpu_to_le32(mvmvif->id);
 130         cmd.spec_link_id = link_conf->link_id;
 131         WARN_ON_ONCE(link_info->phy_ctxt);
 132         cmd.phy_id = cpu_to_le32(FW_CTXT_INVALID);
 133
 134         memcpy(cmd.local_link_addr, link_conf->addr, ETH_ALEN);
 135
 136         if (vif->type == NL80211_IFTYPE_ADHOC && link_conf->bssid)
 137                 memcpy(cmd.ibss_bssid_addr, link_conf->bssid, ETH_ALEN);
 138
 139         if (cmd_ver < 2)
 140                 cmd.listen_lmac = cpu_to_le32(link_info->listen_lmac);
 141
 142         return iwl_mvm_link_cmd_send(mvm, &cmd, FW_CTXT_ACTION_ADD);
 143 }
 144
 145 struct iwl_mvm_esr_iter_data {
 146         struct ieee80211_vif *vif;
 147         unsigned int link_id;
 148         bool lift_block;
 149 };
 150
 151 static void iwl_mvm_esr_vif_iterator(void *_data, u8 *mac,
 152                                      struct ieee80211_vif *vif)
 153 {
 154         struct iwl_mvm_esr_iter_data *data = _data;
 155         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 156         int link_id;
 157
 158         if (ieee80211_vif_type_p2p(vif) == NL80211_IFTYPE_STATION)
 159                 return;
 160
 161         for_each_mvm_vif_valid_link(mvmvif, link_id) {
 162                 struct iwl_mvm_vif_link_info *link_info =
 163                         mvmvif->link[link_id];
 164                 if (vif == data->vif && link_id == data->link_id)
 165                         continue;
 166                 if (link_info->active)
 167                         data->lift_block = false;
 168         }
 169 }
 170
 171 int iwl_mvm_esr_non_bss_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 172                              unsigned int link_id, bool active)
 173 {
 174         /* An active link of a non-station vif blocks EMLSR. Upon activation
 175          * block EMLSR on the bss vif. Upon deactivation, check if this link
 176          * was the last non-station link active, and if so unblock the bss vif
 177          */
 178         struct ieee80211_vif *bss_vif = iwl_mvm_get_bss_vif(mvm);
 179         struct iwl_mvm_esr_iter_data data = {
 180                 .vif = vif,
 181                 .link_id = link_id,
 182                 .lift_block = true,
 183         };
 184
 185         if (IS_ERR_OR_NULL(bss_vif))
 186                 return 0;
 187
 188         if (active)
 189                 return iwl_mvm_block_esr_sync(mvm, bss_vif,
 190                                               IWL_MVM_ESR_BLOCKED_NON_BSS);
 191
 192         ieee80211_iterate_active_interfaces(mvm->hw,
 193                                             IEEE80211_IFACE_ITER_NORMAL,
 194                                             iwl_mvm_esr_vif_iterator, &data);
 195         if (data.lift_block) {
 196                 mutex_lock(&mvm->mutex);
 197                 iwl_mvm_unblock_esr(mvm, bss_vif, IWL_MVM_ESR_BLOCKED_NON_BSS);
 198                 mutex_unlock(&mvm->mutex);
 199         }
 200
 201         return 0;
 202 }
 203
 204 int iwl_mvm_link_changed(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 205                          struct ieee80211_bss_conf *link_conf,
 206                          u32 changes, bool active)
 207 {
 208         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 209         unsigned int link_id = link_conf->link_id;
 210         struct iwl_mvm_vif_link_info *link_info = mvmvif->link[link_id];
 211         struct iwl_mvm_phy_ctxt *phyctxt;
 212         struct iwl_link_config_cmd cmd = {};
 213         u32 ht_flag, flags = 0, flags_mask = 0;
 214         int ret;
 215         unsigned int cmd_id = WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD);
 216         u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 1);
 217
 218         if (WARN_ON_ONCE(!link_info ||
 219                          link_info->fw_link_id == IWL_MVM_FW_LINK_ID_INVALID))
 220                 return -EINVAL;
 221
 222         if (changes & LINK_CONTEXT_MODIFY_ACTIVE) {
 223                 /* When activating a link, phy context should be valid;
 224                  * when deactivating a link, it also should be valid since
 225                  * the link was active before. So, do nothing in this case.
 226                  * Since a link is added first with FW_CTXT_INVALID, then we
 227                  * can get here in case it's removed before it was activated.
 228                  */
 229                 if (!link_info->phy_ctxt)
 230                         return 0;
 231
 232                 /* Catch early if driver tries to activate or deactivate a link
 233                  * twice.
 234                  */
 235                 WARN_ON_ONCE(active == link_info->active);
 236
 237                 /* When deactivating a link session protection should
 238                  * be stopped. Also let the firmware know if we can't Tx.
 239                  */
 240                 if (!active && vif->type == NL80211_IFTYPE_STATION) {
 241                         iwl_mvm_stop_session_protection(mvm, vif);
 242                         if (link_info->csa_block_tx) {
 243                                 cmd.block_tx = 1;
 244                                 link_info->csa_block_tx = false;
 245                         }
 246                 }
 247         }
 248
 249         cmd.link_id = cpu_to_le32(link_info->fw_link_id);
 250
 251         /* The phy_id, link address and listen_lmac can be modified only until
 252          * the link becomes active, otherwise they will be ignored.
 253          */
 254         phyctxt = link_info->phy_ctxt;
 255         if (phyctxt)
 256                 cmd.phy_id = cpu_to_le32(phyctxt->id);
 257         else
 258                 cmd.phy_id = cpu_to_le32(FW_CTXT_INVALID);
 259         cmd.mac_id = cpu_to_le32(mvmvif->id);
 260
 261         memcpy(cmd.local_link_addr, link_conf->addr, ETH_ALEN);
 262
 263         cmd.active = cpu_to_le32(active);
 264
 265         if (vif->type == NL80211_IFTYPE_ADHOC && link_conf->bssid)
 266                 memcpy(cmd.ibss_bssid_addr, link_conf->bssid, ETH_ALEN);
 267
 268         iwl_mvm_set_fw_basic_rates(mvm, vif, link_info,
 269                                    &cmd.cck_rates, &cmd.ofdm_rates);
 270
 271         cmd.cck_short_preamble = cpu_to_le32(link_conf->use_short_preamble);
 272         cmd.short_slot = cpu_to_le32(link_conf->use_short_slot);
 273
 274         /* The fw does not distinguish between ht and fat */
 275         ht_flag = LINK_PROT_FLG_HT_PROT | LINK_PROT_FLG_FAT_PROT;
 276         iwl_mvm_set_fw_protection_flags(mvm, vif, link_conf,
 277                                         &cmd.protection_flags,
 278                                         ht_flag, LINK_PROT_FLG_TGG_PROTECT);
 279
 280         iwl_mvm_set_fw_qos_params(mvm, vif, link_conf, cmd.ac,
 281                                   &cmd.qos_flags);
 282
 283
 284         cmd.bi = cpu_to_le32(link_conf->beacon_int);
 285         cmd.dtim_interval = cpu_to_le32(link_conf->beacon_int *
 286                                         link_conf->dtim_period);
 287
 288         if (!link_conf->he_support || iwlwifi_mod_params.disable_11ax ||
 289             (vif->type == NL80211_IFTYPE_STATION && !vif->cfg.assoc)) {
 290                 changes &= ~LINK_CONTEXT_MODIFY_HE_PARAMS;
 291                 goto send_cmd;
 292         }
 293
 294         cmd.htc_trig_based_pkt_ext = link_conf->htc_trig_based_pkt_ext;
 295
 296         if (link_conf->uora_exists) {
 297                 cmd.rand_alloc_ecwmin =
 298                         link_conf->uora_ocw_range & 0x7;
 299                 cmd.rand_alloc_ecwmax =
 300                         (link_conf->uora_ocw_range >> 3) & 0x7;
 301         }
 302
 303         /* ap_sta may be NULL if we're disconnecting */
 304         if (changes & LINK_CONTEXT_MODIFY_HE_PARAMS && mvmvif->ap_sta) {
 305                 struct ieee80211_link_sta *link_sta =
 306                         link_sta_dereference_check(mvmvif->ap_sta, link_id);
 307
 308                 if (!WARN_ON(!link_sta) && link_sta->he_cap.has_he &&
 309                     link_sta->he_cap.he_cap_elem.mac_cap_info[5] &
 310                     IEEE80211_HE_MAC_CAP5_OM_CTRL_UL_MU_DATA_DIS_RX)
 311                         cmd.ul_mu_data_disable = 1;
 312         }
 313
 314         /* TODO  how to set ndp_fdbk_buff_th_exp? */
 315
 316         if (iwl_mvm_set_fw_mu_edca_params(mvm, mvmvif->link[link_id],
 317                                           &cmd.trig_based_txf[0])) {
 318                 flags |= LINK_FLG_MU_EDCA_CW;
 319                 flags_mask |= LINK_FLG_MU_EDCA_CW;
 320         }
 321
 322         if (changes & LINK_CONTEXT_MODIFY_EHT_PARAMS) {
 323                 struct ieee80211_chanctx_conf *ctx;
 324                 struct cfg80211_chan_def *def = NULL;
 325
 326                 rcu_read_lock();
 327                 ctx = rcu_dereference(link_conf->chanctx_conf);
 328                 if (ctx)
 329                         def = iwl_mvm_chanctx_def(mvm, ctx);
 330
 331                 if (iwlwifi_mod_params.disable_11be ||
 332                     !link_conf->eht_support || !def ||
 333                     iwl_fw_lookup_cmd_ver(mvm->fw, PHY_CONTEXT_CMD, 1) >= 6)
 334                         changes &= ~LINK_CONTEXT_MODIFY_EHT_PARAMS;
 335                 else
 336                         cmd.puncture_mask = cpu_to_le16(def->punctured);
 337                 rcu_read_unlock();
 338         }
 339
 340         cmd.bss_color = link_conf->he_bss_color.color;
 341
 342         if (!link_conf->he_bss_color.enabled) {
 343                 flags |= LINK_FLG_BSS_COLOR_DIS;
 344                 flags_mask |= LINK_FLG_BSS_COLOR_DIS;
 345         }
 346
 347         cmd.frame_time_rts_th = cpu_to_le16(link_conf->frame_time_rts_th);
 348
 349         /* Block 26-tone RU OFDMA transmissions */
 350         if (link_info->he_ru_2mhz_block) {
 351                 flags |= LINK_FLG_RU_2MHZ_BLOCK;
 352                 flags_mask |= LINK_FLG_RU_2MHZ_BLOCK;
 353         }
 354
 355         if (link_conf->nontransmitted) {
 356                 ether_addr_copy(cmd.ref_bssid_addr,
 357                                 link_conf->transmitter_bssid);
 358                 cmd.bssid_index = link_conf->bssid_index;
 359         }
 360
 361 send_cmd:
 362         cmd.modify_mask = cpu_to_le32(changes);
 363         cmd.flags = cpu_to_le32(flags);
 364         if (cmd_ver < 6)
 365                 cmd.flags_mask = cpu_to_le32(flags_mask);
 366         cmd.spec_link_id = link_conf->link_id;
 367         if (cmd_ver < 2)
 368                 cmd.listen_lmac = cpu_to_le32(link_info->listen_lmac);
 369
 370         ret = iwl_mvm_link_cmd_send(mvm, &cmd, FW_CTXT_ACTION_MODIFY);
 371         if (!ret && (changes & LINK_CONTEXT_MODIFY_ACTIVE))
 372                 link_info->active = active;
 373
 374         return ret;
 375 }
 376
 377 int iwl_mvm_unset_link_mapping(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 378                                struct ieee80211_bss_conf *link_conf)
 379 {
 380         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 381         struct iwl_mvm_vif_link_info *link_info =
 382                 mvmvif->link[link_conf->link_id];
 383
 384         /* mac80211 thought we have the link, but it was never configured */
 385         if (WARN_ON(!link_info ||
 386                     link_info->fw_link_id >=
 387                     ARRAY_SIZE(mvm->link_id_to_link_conf)))
 388                 return -EINVAL;
 389
 390         RCU_INIT_POINTER(mvm->link_id_to_link_conf[link_info->fw_link_id],
 391                          NULL);
 392         return 0;
 393 }
 394
 395 int iwl_mvm_remove_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 396                         struct ieee80211_bss_conf *link_conf)
 397 {
 398         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 399         unsigned int link_id = link_conf->link_id;
 400         struct iwl_mvm_vif_link_info *link_info = mvmvif->link[link_id];
 401         struct iwl_link_config_cmd cmd = {};
 402         int ret;
 403
 404         ret = iwl_mvm_unset_link_mapping(mvm, vif, link_conf);
 405         if (ret)
 406                 return 0;
 407
 408         cmd.link_id = cpu_to_le32(link_info->fw_link_id);
 409         link_info->fw_link_id = IWL_MVM_FW_LINK_ID_INVALID;
 410         cmd.spec_link_id = link_conf->link_id;
 411         cmd.phy_id = cpu_to_le32(FW_CTXT_INVALID);
 412
 413         ret = iwl_mvm_link_cmd_send(mvm, &cmd, FW_CTXT_ACTION_REMOVE);
 414
 415         if (!ret && iwl_mvm_sf_update(mvm, vif, true))
 416                 IWL_ERR(mvm, "Failed to update SF state\n");
 417
 418         return ret;
 419 }
 420
 421 /* link should be deactivated before removal, so in most cases we need to
 422  * perform these two operations together
 423  */
 424 int iwl_mvm_disable_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 425                          struct ieee80211_bss_conf *link_conf)
 426 {
 427         int ret;
 428
 429         ret = iwl_mvm_link_changed(mvm, vif, link_conf,
 430                                    LINK_CONTEXT_MODIFY_ACTIVE, false);
 431         if (ret)
 432                 return ret;
 433
 434         ret = iwl_mvm_remove_link(mvm, vif, link_conf);
 435         if (ret)
 436                 return ret;
 437
 438         return ret;
 439 }
 440
 441 struct iwl_mvm_rssi_to_grade {
 442         s8 rssi[2];
 443         u16 grade;
 444 };
 445
 446 #define RSSI_TO_GRADE_LINE(_lb, _hb_uhb, _grade) \
 447         { \
 448                 .rssi = {_lb, _hb_uhb}, \
 449                 .grade = _grade \
 450         }
 451
 452 /*
 453  * This array must be sorted by increasing RSSI for proper functionality.
 454  * The grades are actually estimated throughput, represented as fixed-point
 455  * with a scale factor of 1/10.
 456  */
 457 static const struct iwl_mvm_rssi_to_grade rssi_to_grade_map[] = {
 458         RSSI_TO_GRADE_LINE(-85, -89, 177),
 459         RSSI_TO_GRADE_LINE(-83, -86, 344),
 460         RSSI_TO_GRADE_LINE(-82, -85, 516),
 461         RSSI_TO_GRADE_LINE(-80, -83, 688),
 462         RSSI_TO_GRADE_LINE(-77, -79, 1032),
 463         RSSI_TO_GRADE_LINE(-73, -76, 1376),
 464         RSSI_TO_GRADE_LINE(-70, -74, 1548),
 465         RSSI_TO_GRADE_LINE(-69, -72, 1750),
 466         RSSI_TO_GRADE_LINE(-65, -68, 2064),
 467         RSSI_TO_GRADE_LINE(-61, -66, 2294),
 468         RSSI_TO_GRADE_LINE(-58, -61, 2580),
 469         RSSI_TO_GRADE_LINE(-55, -58, 2868),
 470         RSSI_TO_GRADE_LINE(-46, -55, 3098),
 471         RSSI_TO_GRADE_LINE(-43, -54, 3442)
 472 };
 473
 474 #define MAX_GRADE (rssi_to_grade_map[ARRAY_SIZE(rssi_to_grade_map) - 1].grade)
 475
 476 #define DEFAULT_CHAN_LOAD_LB    30
 477 #define DEFAULT_CHAN_LOAD_HB    15
 478 #define DEFAULT_CHAN_LOAD_UHB   0
 479
 480 /* Factors calculation is done with fixed-point with a scaling factor of 1/256 */
 481 #define SCALE_FACTOR 256
 482
 483 /* Convert a percentage from [0,100] to [0,255] */
 484 #define NORMALIZE_PERCENT_TO_255(percentage) ((percentage) * SCALE_FACTOR / 100)
 485
 486 static unsigned int
 487 iwl_mvm_get_puncturing_factor(const struct ieee80211_bss_conf *link_conf)
 488 {
 489         enum nl80211_chan_width chan_width =
 490                 link_conf->chanreq.oper.width;
 491         int mhz = nl80211_chan_width_to_mhz(chan_width);
 492         unsigned int n_subchannels, n_punctured, puncturing_penalty;
 493
 494         if (WARN_ONCE(mhz < 20 || mhz > 320,
 495                       "Invalid channel width : (%d)\n", mhz))
 496                 return SCALE_FACTOR;
 497
 498         /* No puncturing, no penalty */
 499         if (mhz < 80)
 500                 return SCALE_FACTOR;
 501
 502         /* total number of subchannels */
 503         n_subchannels = mhz / 20;
 504         /* how many of these are punctured */
 505         n_punctured = hweight16(link_conf->chanreq.oper.punctured);
 506
 507         puncturing_penalty = n_punctured * SCALE_FACTOR / n_subchannels;
 508         return SCALE_FACTOR - puncturing_penalty;
 509 }
 510
 511 static unsigned int
 512 iwl_mvm_get_chan_load(struct ieee80211_bss_conf *link_conf)
 513 {
 514         struct ieee80211_vif *vif = link_conf->vif;
 515         struct iwl_mvm_vif_link_info *mvm_link =
 516                 iwl_mvm_vif_from_mac80211(link_conf->vif)->link[link_conf->link_id];
 517         const struct element *bss_load_elem;
 518         const struct ieee80211_bss_load_elem *bss_load;
 519         enum nl80211_band band = link_conf->chanreq.oper.chan->band;
 520         const struct cfg80211_bss_ies *ies;
 521         unsigned int chan_load;
 522         u32 chan_load_by_us;
 523
 524         rcu_read_lock();
 525         if (ieee80211_vif_link_active(vif, link_conf->link_id))
 526                 ies = rcu_dereference(link_conf->bss->beacon_ies);
 527         else
 528                 ies = rcu_dereference(link_conf->bss->ies);
 529
 530         if (ies)
 531                 bss_load_elem = cfg80211_find_elem(WLAN_EID_QBSS_LOAD,
 532                                                    ies->data, ies->len);
 533         else
 534                 bss_load_elem = NULL;
 535
 536         /* If there isn't BSS Load element, take the defaults */
 537         if (!bss_load_elem ||
 538             bss_load_elem->datalen != sizeof(*bss_load)) {
 539                 rcu_read_unlock();
 540                 switch (band) {
 541                 case NL80211_BAND_2GHZ:
 542                         chan_load = DEFAULT_CHAN_LOAD_LB;
 543                         break;
 544                 case NL80211_BAND_5GHZ:
 545                         chan_load = DEFAULT_CHAN_LOAD_HB;
 546                         break;
 547                 case NL80211_BAND_6GHZ:
 548                         chan_load = DEFAULT_CHAN_LOAD_UHB;
 549                         break;
 550                 default:
 551                         chan_load = 0;
 552                         break;
 553                 }
 554                 /* The defaults are given in percentage */
 555                 return NORMALIZE_PERCENT_TO_255(chan_load);
 556         }
 557
 558         bss_load = (const void *)bss_load_elem->data;
 559         /* Channel util is in range 0-255 */
 560         chan_load = bss_load->channel_util;
 561         rcu_read_unlock();
 562
 563         if (!mvm_link || !mvm_link->active)
 564                 return chan_load;
 565
 566         if (WARN_ONCE(!mvm_link->phy_ctxt,
 567                       "Active link (%u) without phy ctxt assigned!\n",
 568                       link_conf->link_id))
 569                 return chan_load;
 570
 571         /* channel load by us is given in percentage */
 572         chan_load_by_us =
 573                 NORMALIZE_PERCENT_TO_255(mvm_link->phy_ctxt->channel_load_by_us);
 574
 575         /* Use only values that firmware sends that can possibly be valid */
 576         if (chan_load_by_us <= chan_load)
 577                 chan_load -= chan_load_by_us;
 578
 579         return chan_load;
 580 }
 581
 582 static unsigned int
 583 iwl_mvm_get_chan_load_factor(struct ieee80211_bss_conf *link_conf)
 584 {
 585         return SCALE_FACTOR - iwl_mvm_get_chan_load(link_conf);
 586 }
 587
 588 /* This function calculates the grade of a link. Returns 0 in error case */
 589 VISIBLE_IF_IWLWIFI_KUNIT
 590 unsigned int iwl_mvm_get_link_grade(struct ieee80211_bss_conf *link_conf)
 591 {
 592         enum nl80211_band band;
 593         int i, rssi_idx;
 594         s32 link_rssi;
 595         unsigned int grade = MAX_GRADE;
 596
 597         if (WARN_ON_ONCE(!link_conf))
 598                 return 0;
 599
 600         band = link_conf->chanreq.oper.chan->band;
 601         if (WARN_ONCE(band != NL80211_BAND_2GHZ &&
 602                       band != NL80211_BAND_5GHZ &&
 603                       band != NL80211_BAND_6GHZ,
 604                       "Invalid band (%u)\n", band))
 605                 return 0;
 606
 607         link_rssi = MBM_TO_DBM(link_conf->bss->signal);
 608         /*
 609          * For 6 GHz the RSSI of the beacons is lower than
 610          * the RSSI of the data.
 611          */
 612         if (band == NL80211_BAND_6GHZ)
 613                 link_rssi += 4;
 614
 615         rssi_idx = band == NL80211_BAND_2GHZ ? 0 : 1;
 616
 617         /* No valid RSSI - take the lowest grade */
 618         if (!link_rssi)
 619                 link_rssi = rssi_to_grade_map[0].rssi[rssi_idx];
 620
 621         /* Get grade based on RSSI */
 622         for (i = 0; i < ARRAY_SIZE(rssi_to_grade_map); i++) {
 623                 const struct iwl_mvm_rssi_to_grade *line =
 624                         &rssi_to_grade_map[i];
 625
 626                 if (link_rssi > line->rssi[rssi_idx])
 627                         continue;
 628                 grade = line->grade;
 629                 break;
 630         }
 631
 632         /* apply the channel load and puncturing factors */
 633         grade = grade * iwl_mvm_get_chan_load_factor(link_conf) / SCALE_FACTOR;
 634         grade = grade * iwl_mvm_get_puncturing_factor(link_conf) / SCALE_FACTOR;
 635         return grade;
 636 }
 637 EXPORT_SYMBOL_IF_IWLWIFI_KUNIT(iwl_mvm_get_link_grade);
 638
 639 static
 640 u8 iwl_mvm_set_link_selection_data(struct ieee80211_vif *vif,
 641                                    struct iwl_mvm_link_sel_data *data,
 642                                    unsigned long usable_links,
 643                                    u8 *best_link_idx)
 644 {
 645         u8 n_data = 0;
 646         u16 max_grade = 0;
 647         unsigned long link_id;
 648
 649         /* TODO: don't select links that weren't discovered in the last scan */
 650         for_each_set_bit(link_id, &usable_links, IEEE80211_MLD_MAX_NUM_LINKS) {
 651                 struct ieee80211_bss_conf *link_conf =
 652                         link_conf_dereference_protected(vif, link_id);
 653
 654                 if (WARN_ON_ONCE(!link_conf))
 655                         continue;
 656
 657                 data[n_data].link_id = link_id;
 658                 data[n_data].chandef = &link_conf->chanreq.oper;
 659                 data[n_data].signal = link_conf->bss->signal / 100;
 660                 data[n_data].grade = iwl_mvm_get_link_grade(link_conf);
 661
 662                 if (data[n_data].grade > max_grade) {
 663                         max_grade = data[n_data].grade;
 664                         *best_link_idx = n_data;
 665                 }
 666                 n_data++;
 667         }
 668
 669         return n_data;
 670 }
 671
 672 struct iwl_mvm_bw_to_rssi_threshs {
 673         s8 low;
 674         s8 high;
 675 };
 676
 677 #define BW_TO_RSSI_THRESHOLDS(_bw)                              \
 678         [IWL_PHY_CHANNEL_MODE ## _bw] = {                       \
 679                 .low = IWL_MVM_LOW_RSSI_THRESH_##_bw##MHZ,      \
 680                 .high = IWL_MVM_HIGH_RSSI_THRESH_##_bw##MHZ     \
 681         }
 682
 683 s8 iwl_mvm_get_esr_rssi_thresh(struct iwl_mvm *mvm,
 684                                const struct cfg80211_chan_def *chandef,
 685                                bool low)
 686 {
 687         const struct iwl_mvm_bw_to_rssi_threshs bw_to_rssi_threshs_map[] = {
 688                 BW_TO_RSSI_THRESHOLDS(20),
 689                 BW_TO_RSSI_THRESHOLDS(40),
 690                 BW_TO_RSSI_THRESHOLDS(80),
 691                 BW_TO_RSSI_THRESHOLDS(160)
 692                 /* 320 MHz has the same thresholds as 20 MHz */
 693         };
 694         const struct iwl_mvm_bw_to_rssi_threshs *threshs;
 695         u8 chan_width = iwl_mvm_get_channel_width(chandef);
 696
 697         if (WARN_ON(chandef->chan->band != NL80211_BAND_2GHZ &&
 698                     chandef->chan->band != NL80211_BAND_5GHZ &&
 699                     chandef->chan->band != NL80211_BAND_6GHZ))
 700                 return S8_MAX;
 701
 702         /* 6 GHz will always use 20 MHz thresholds, regardless of the BW */
 703         if (chan_width == IWL_PHY_CHANNEL_MODE320)
 704                 chan_width = IWL_PHY_CHANNEL_MODE20;
 705
 706         threshs = &bw_to_rssi_threshs_map[chan_width];
 707
 708         return low ? threshs->low : threshs->high;
 709 }
 710
 711 static u32
 712 iwl_mvm_esr_disallowed_with_link(struct iwl_mvm *mvm,
 713                                  struct ieee80211_vif *vif,
 714                                  const struct iwl_mvm_link_sel_data *link,
 715                                  bool primary)
 716 {
 717         struct wiphy *wiphy = mvm->hw->wiphy;
 718         struct ieee80211_bss_conf *conf;
 719         enum iwl_mvm_esr_state ret = 0;
 720         s8 thresh;
 721
 722         conf = wiphy_dereference(wiphy, vif->link_conf[link->link_id]);
 723         if (WARN_ON_ONCE(!conf))
 724                 return false;
 725
 726         /* BT Coex effects eSR mode only if one of the links is on LB */
 727         if (link->chandef->chan->band == NL80211_BAND_2GHZ &&
 728             (!iwl_mvm_bt_coex_calculate_esr_mode(mvm, vif, link->signal,
 729                                                  primary)))
 730                 ret |= IWL_MVM_ESR_EXIT_COEX;
 731
 732         thresh = iwl_mvm_get_esr_rssi_thresh(mvm, link->chandef,
 733                                              false);
 734
 735         if (link->signal < thresh)
 736                 ret |= IWL_MVM_ESR_EXIT_LOW_RSSI;
 737
 738         if (conf->csa_active)
 739                 ret |= IWL_MVM_ESR_EXIT_CSA;
 740
 741         if (ret) {
 742                 IWL_DEBUG_INFO(mvm,
 743                                "Link %d is not allowed for esr\n",
 744                                link->link_id);
 745                 iwl_mvm_print_esr_state(mvm, ret);
 746         }
 747         return ret;
 748 }
 749
 750 VISIBLE_IF_IWLWIFI_KUNIT
 751 bool iwl_mvm_mld_valid_link_pair(struct ieee80211_vif *vif,
 752                                  const struct iwl_mvm_link_sel_data *a,
 753                                  const struct iwl_mvm_link_sel_data *b)
 754 {
 755         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 756         struct iwl_mvm *mvm = mvmvif->mvm;
 757         enum iwl_mvm_esr_state ret = 0;
 758
 759         /* Per-link considerations */
 760         if (iwl_mvm_esr_disallowed_with_link(mvm, vif, a, true) ||
 761             iwl_mvm_esr_disallowed_with_link(mvm, vif, b, false))
 762                 return false;
 763
 764         if (a->chandef->chan->band == b->chandef->chan->band ||
 765             a->chandef->width != b->chandef->width)
 766                 ret |= IWL_MVM_ESR_EXIT_BANDWIDTH;
 767
 768         if (ret) {
 769                 IWL_DEBUG_INFO(mvm,
 770                                "Links %d and %d are not a valid pair for EMLSR\n",
 771                                a->link_id, b->link_id);
 772                 iwl_mvm_print_esr_state(mvm, ret);
 773                 return false;
 774         }
 775
 776         return true;
 777
 778 }
 779 EXPORT_SYMBOL_IF_IWLWIFI_KUNIT(iwl_mvm_mld_valid_link_pair);
 780
 781 /*
 782  * Returns the combined eSR grade of two given links.
 783  * Returns 0 if eSR is not allowed with these 2 links.
 784  */
 785 static
 786 unsigned int iwl_mvm_get_esr_grade(struct ieee80211_vif *vif,
 787                                    const struct iwl_mvm_link_sel_data *a,
 788                                    const struct iwl_mvm_link_sel_data *b,
 789                                    u8 *primary_id)
 790 {
 791         struct ieee80211_bss_conf *primary_conf;
 792         struct wiphy *wiphy = ieee80211_vif_to_wdev(vif)->wiphy;
 793         unsigned int primary_load;
 794
 795         lockdep_assert_wiphy(wiphy);
 796
 797         /* a is always primary, b is always secondary */
 798         if (b->grade > a->grade)
 799                 swap(a, b);
 800
 801         *primary_id = a->link_id;
 802
 803         if (!iwl_mvm_mld_valid_link_pair(vif, a, b))
 804                 return 0;
 805
 806         primary_conf = wiphy_dereference(wiphy, vif->link_conf[*primary_id]);
 807
 808         if (WARN_ON_ONCE(!primary_conf))
 809                 return 0;
 810
 811         primary_load = iwl_mvm_get_chan_load(primary_conf);
 812
 813         return a->grade +
 814                 ((b->grade * primary_load) / SCALE_FACTOR);
 815 }
 816
 817 void iwl_mvm_select_links(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 818 {
 819         struct iwl_mvm_link_sel_data data[IEEE80211_MLD_MAX_NUM_LINKS];
 820         struct iwl_mvm_link_sel_data *best_link;
 821         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 822         u32 max_active_links = iwl_mvm_max_active_links(mvm, vif);
 823         u16 usable_links = ieee80211_vif_usable_links(vif);
 824         u8 best, primary_link, best_in_pair, n_data;
 825         u16 max_esr_grade = 0, new_active_links;
 826
 827         lockdep_assert_wiphy(mvm->hw->wiphy);
 828
 829         if (!mvmvif->authorized || !ieee80211_vif_is_mld(vif))
 830                 return;
 831
 832         if (!IWL_MVM_AUTO_EML_ENABLE)
 833                 return;
 834
 835         /* The logic below is a simple version that doesn't suit more than 2
 836          * links
 837          */
 838         WARN_ON_ONCE(max_active_links > 2);
 839
 840         n_data = iwl_mvm_set_link_selection_data(vif, data, usable_links,
 841                                                  &best);
 842
 843         if (WARN(!n_data, "Couldn't find a valid grade for any link!\n"))
 844                 return;
 845
 846         best_link = &data[best];
 847         primary_link = best_link->link_id;
 848         new_active_links = BIT(best_link->link_id);
 849
 850         /* eSR is not supported/blocked, or only one usable link */
 851         if (max_active_links == 1 || !iwl_mvm_vif_has_esr_cap(mvm, vif) ||
 852             mvmvif->esr_disable_reason || n_data == 1)
 853                 goto set_active;
 854
 855         for (u8 a = 0; a < n_data; a++)
 856                 for (u8 b = a + 1; b < n_data; b++) {
 857                         u16 esr_grade = iwl_mvm_get_esr_grade(vif, &data[a],
 858                                                               &data[b],
 859                                                               &best_in_pair);
 860
 861                         if (esr_grade <= max_esr_grade)
 862                                 continue;
 863
 864                         max_esr_grade = esr_grade;
 865                         primary_link = best_in_pair;
 866                         new_active_links = BIT(data[a].link_id) |
 867                                            BIT(data[b].link_id);
 868                 }
 869
 870         /* No valid pair was found, go with the best link */
 871         if (hweight16(new_active_links) <= 1)
 872                 goto set_active;
 873
 874         /* For equal grade - prefer EMLSR */
 875         if (best_link->grade > max_esr_grade) {
 876                 primary_link = best_link->link_id;
 877                 new_active_links = BIT(best_link->link_id);
 878         }
 879 set_active:
 880         IWL_DEBUG_INFO(mvm, "Link selection result: 0x%x. Primary = %d\n",
 881                        new_active_links, primary_link);
 882         ieee80211_set_active_links_async(vif, new_active_links);
 883         mvmvif->link_selection_res = new_active_links;
 884         mvmvif->link_selection_primary = primary_link;
 885 }
 886
 887 u8 iwl_mvm_get_primary_link(struct ieee80211_vif *vif)
 888 {
 889         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 890
 891         /* relevant data is written with both locks held, so read with either */
 892         lockdep_assert(lockdep_is_held(&mvmvif->mvm->mutex) ||
 893                        lockdep_is_held(&mvmvif->mvm->hw->wiphy->mtx));
 894
 895         if (!ieee80211_vif_is_mld(vif))
 896                 return 0;
 897
 898         /* In AP mode, there is no primary link */
 899         if (vif->type == NL80211_IFTYPE_AP)
 900                 return __ffs(vif->active_links);
 901
 902         if (mvmvif->esr_active &&
 903             !WARN_ON(!(BIT(mvmvif->primary_link) & vif->active_links)))
 904                 return mvmvif->primary_link;
 905
 906         return __ffs(vif->active_links);
 907 }
 908
 909 /*
 910  * For non-MLO/single link, this will return the deflink/single active link,
 911  * respectively
 912  */
 913 u8 iwl_mvm_get_other_link(struct ieee80211_vif *vif, u8 link_id)
 914 {
 915         switch (hweight16(vif->active_links)) {
 916         case 0:
 917                 return 0;
 918         default:
 919                 WARN_ON(1);
 920                 fallthrough;
 921         case 1:
 922                 return __ffs(vif->active_links);
 923         case 2:
 924                 return __ffs(vif->active_links & ~BIT(link_id));
 925         }
 926 }
 927
 928 /* Reasons that can cause esr prevention */
 929 #define IWL_MVM_ESR_PREVENT_REASONS     IWL_MVM_ESR_EXIT_MISSED_BEACON
 930 #define IWL_MVM_PREVENT_ESR_TIMEOUT     (HZ * 400)
 931 #define IWL_MVM_ESR_PREVENT_SHORT       (HZ * 300)
 932 #define IWL_MVM_ESR_PREVENT_LONG        (HZ * 600)
 933
 934 static bool iwl_mvm_check_esr_prevention(struct iwl_mvm *mvm,
 935                                          struct iwl_mvm_vif *mvmvif,
 936                                          enum iwl_mvm_esr_state reason)
 937 {
 938         bool timeout_expired = time_after(jiffies,
 939                                           mvmvif->last_esr_exit.ts +
 940                                           IWL_MVM_PREVENT_ESR_TIMEOUT);
 941         unsigned long delay;
 942
 943         lockdep_assert_held(&mvm->mutex);
 944
 945         /* Only handle reasons that can cause prevention */
 946         if (!(reason & IWL_MVM_ESR_PREVENT_REASONS))
 947                 return false;
 948
 949         /*
 950          * Reset the counter if more than 400 seconds have passed between one
 951          * exit and the other, or if we exited due to a different reason.
 952          * Will also reset the counter after the long prevention is done.
 953          */
 954         if (timeout_expired || mvmvif->last_esr_exit.reason != reason) {
 955                 mvmvif->exit_same_reason_count = 1;
 956                 return false;
 957         }
 958
 959         mvmvif->exit_same_reason_count++;
 960         if (WARN_ON(mvmvif->exit_same_reason_count < 2 ||
 961                     mvmvif->exit_same_reason_count > 3))
 962                 return false;
 963
 964         mvmvif->esr_disable_reason |= IWL_MVM_ESR_BLOCKED_PREVENTION;
 965
 966         /*
 967          * For the second exit, use a short prevention, and for the third one,
 968          * use a long prevention.
 969          */
 970         delay = mvmvif->exit_same_reason_count == 2 ?
 971                 IWL_MVM_ESR_PREVENT_SHORT :
 972                 IWL_MVM_ESR_PREVENT_LONG;
 973
 974         IWL_DEBUG_INFO(mvm,
 975                        "Preventing EMLSR for %ld seconds due to %u exits with the reason = %s (0x%x)\n",
 976                        delay / HZ, mvmvif->exit_same_reason_count,
 977                        iwl_get_esr_state_string(reason), reason);
 978
 979         wiphy_delayed_work_queue(mvm->hw->wiphy,
 980                                  &mvmvif->prevent_esr_done_wk, delay);
 981         return true;
 982 }
 983
 984 #define IWL_MVM_TRIGGER_LINK_SEL_TIME (IWL_MVM_TRIGGER_LINK_SEL_TIME_SEC * HZ)
 985
 986 /* API to exit eSR mode */
 987 void iwl_mvm_exit_esr(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 988                       enum iwl_mvm_esr_state reason,
 989                       u8 link_to_keep)
 990 {
 991         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 992         u16 new_active_links;
 993         bool prevented;
 994
 995         lockdep_assert_held(&mvm->mutex);
 996
 997         if (!IWL_MVM_AUTO_EML_ENABLE)
 998                 return;
 999
1000         /* Nothing to do */
1001         if (!mvmvif->esr_active)
1002                 return;
1003
1004         if (WARN_ON(!ieee80211_vif_is_mld(vif) || !mvmvif->authorized))
1005                 return;
1006
1007         if (WARN_ON(!(vif->active_links & BIT(link_to_keep))))
1008                 link_to_keep = __ffs(vif->active_links);
1009
1010         new_active_links = BIT(link_to_keep);
1011         IWL_DEBUG_INFO(mvm,
1012                        "Exiting EMLSR. reason = %s (0x%x). Current active links=0x%x, new active links = 0x%x\n",
1013                        iwl_get_esr_state_string(reason), reason,
1014                        vif->active_links, new_active_links);
1015
1016         ieee80211_set_active_links_async(vif, new_active_links);
1017
1018         /* Prevent EMLSR if needed */
1019         prevented = iwl_mvm_check_esr_prevention(mvm, mvmvif, reason);
1020
1021         /* Remember why and when we exited EMLSR */
1022         mvmvif->last_esr_exit.ts = jiffies;
1023         mvmvif->last_esr_exit.reason = reason;
1024
1025         /*
1026          * If EMLSR is prevented now - don't try to get back to EMLSR.
1027          * If we exited due to a blocking event, we will try to get back to
1028          * EMLSR when the corresponding unblocking event will happen.
1029          */
1030         if (prevented || reason & IWL_MVM_BLOCK_ESR_REASONS)
1031                 return;
1032
1033         /* If EMLSR is not blocked - try enabling it again in 30 seconds */
1034         wiphy_delayed_work_queue(mvm->hw->wiphy,
1035                                  &mvmvif->mlo_int_scan_wk,
1036                                  round_jiffies_relative(IWL_MVM_TRIGGER_LINK_SEL_TIME));
1037 }
1038
1039 void iwl_mvm_block_esr(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
1040                        enum iwl_mvm_esr_state reason,
1041                        u8 link_to_keep)
1042 {
1043         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1044
1045         lockdep_assert_held(&mvm->mutex);
1046
1047         if (!IWL_MVM_AUTO_EML_ENABLE)
1048                 return;
1049
1050         /* This should be called only with disable reasons */
1051         if (WARN_ON(!(reason & IWL_MVM_BLOCK_ESR_REASONS)))
1052                 return;
1053
1054         if (mvmvif->esr_disable_reason & reason)
1055                 return;
1056
1057         IWL_DEBUG_INFO(mvm,
1058                        "Blocking EMLSR mode. reason = %s (0x%x)\n",
1059                        iwl_get_esr_state_string(reason), reason);
1060
1061         mvmvif->esr_disable_reason |= reason;
1062
1063         iwl_mvm_print_esr_state(mvm, mvmvif->esr_disable_reason);
1064
1065         iwl_mvm_exit_esr(mvm, vif, reason, link_to_keep);
1066 }
1067
1068 int iwl_mvm_block_esr_sync(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
1069                            enum iwl_mvm_esr_state reason)
1070 {
1071         int primary_link = iwl_mvm_get_primary_link(vif);
1072         int ret;
1073
1074         if (!IWL_MVM_AUTO_EML_ENABLE || !ieee80211_vif_is_mld(vif))
1075                 return 0;
1076
1077         /* This should be called only with blocking reasons */
1078         if (WARN_ON(!(reason & IWL_MVM_BLOCK_ESR_REASONS)))
1079                 return 0;
1080
1081         /* leave ESR immediately, not only async with iwl_mvm_block_esr() */
1082         ret = ieee80211_set_active_links(vif, BIT(primary_link));
1083         if (ret)
1084                 return ret;
1085
1086         mutex_lock(&mvm->mutex);
1087         /* only additionally block for consistency and to avoid concurrency */
1088         iwl_mvm_block_esr(mvm, vif, reason, primary_link);
1089         mutex_unlock(&mvm->mutex);
1090
1091         return 0;
1092 }
1093
1094 static void iwl_mvm_esr_unblocked(struct iwl_mvm *mvm,
1095                                   struct ieee80211_vif *vif)
1096 {
1097         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1098         bool need_new_sel = time_after(jiffies, mvmvif->last_esr_exit.ts +
1099                                                 IWL_MVM_TRIGGER_LINK_SEL_TIME);
1100
1101         lockdep_assert_held(&mvm->mutex);
1102
1103         if (!ieee80211_vif_is_mld(vif) || !mvmvif->authorized ||
1104             mvmvif->esr_active)
1105                 return;
1106
1107         IWL_DEBUG_INFO(mvm, "EMLSR is unblocked\n");
1108
1109         /* If we exited due to an EXIT reason, and the exit was in less than
1110          * 30 seconds, then a MLO scan was scheduled already.
1111          */
1112         if (!need_new_sel &&
1113             !(mvmvif->last_esr_exit.reason & IWL_MVM_BLOCK_ESR_REASONS)) {
1114                 IWL_DEBUG_INFO(mvm, "Wait for MLO scan\n");
1115                 return;
1116         }
1117
1118         /*
1119          * If EMLSR was blocked for more than 30 seconds, or the last link
1120          * selection decided to not enter EMLSR, trigger a new scan.
1121          */
1122         if (need_new_sel || hweight16(mvmvif->link_selection_res) < 2) {
1123                 IWL_DEBUG_INFO(mvm, "Trigger MLO scan\n");
1124                 wiphy_delayed_work_queue(mvm->hw->wiphy,
1125                                          &mvmvif->mlo_int_scan_wk, 0);
1126         /*
1127          * If EMLSR was blocked for less than 30 seconds, and the last link
1128          * selection decided to use EMLSR, activate EMLSR using the previous
1129          * link selection result.
1130          */
1131         } else {
1132                 IWL_DEBUG_INFO(mvm,
1133                                "Use the latest link selection result: 0x%x\n",
1134                                mvmvif->link_selection_res);
1135                 ieee80211_set_active_links_async(vif,
1136                                                  mvmvif->link_selection_res);
1137         }
1138 }
1139
1140 void iwl_mvm_unblock_esr(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
1141                          enum iwl_mvm_esr_state reason)
1142 {
1143         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1144
1145         lockdep_assert_held(&mvm->mutex);
1146
1147         if (!IWL_MVM_AUTO_EML_ENABLE)
1148                 return;
1149
1150         /* This should be called only with disable reasons */
1151         if (WARN_ON(!(reason & IWL_MVM_BLOCK_ESR_REASONS)))
1152                 return;
1153
1154         /* No Change */
1155         if (!(mvmvif->esr_disable_reason & reason))
1156                 return;
1157
1158         mvmvif->esr_disable_reason &= ~reason;
1159
1160         IWL_DEBUG_INFO(mvm,
1161                        "Unblocking EMLSR mode. reason = %s (0x%x)\n",
1162                        iwl_get_esr_state_string(reason), reason);
1163         iwl_mvm_print_esr_state(mvm, mvmvif->esr_disable_reason);
1164
1165         if (!mvmvif->esr_disable_reason)
1166                 iwl_mvm_esr_unblocked(mvm, vif);
1167 }
1168
1169 void iwl_mvm_init_link(struct iwl_mvm_vif_link_info *link)
1170 {
1171         link->bcast_sta.sta_id = IWL_INVALID_STA;
1172         link->mcast_sta.sta_id = IWL_INVALID_STA;
1173         link->ap_sta_id = IWL_INVALID_STA;
1174
1175         for (int r = 0; r < NUM_IWL_MVM_SMPS_REQ; r++)
1176                 link->smps_requests[r] =
1177                         IEEE80211_SMPS_AUTOMATIC;
1178 }