Quellcode-Bibliothek link.c  Sprache: unbekannt

// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
/*
 * Copyright (C) 2022 - 2024 Intel Corporation
 */
#include "mvm.h"
#include "time-event.h"

#define HANDLE_ESR_REASONS(HOW)  \
 HOW(BLOCKED_PREVENTION)  \
 HOW(BLOCKED_WOWLAN)  \
 HOW(BLOCKED_TPT)  \
 HOW(BLOCKED_FW)   \
 HOW(BLOCKED_NON_BSS)  \
 HOW(BLOCKED_ROC)  \
 HOW(BLOCKED_TMP_NON_BSS) \
 HOW(EXIT_MISSED_BEACON)  \
 HOW(EXIT_LOW_RSSI)  \
 HOW(EXIT_COEX)   \
 HOW(EXIT_BANDWIDTH)  \
 HOW(EXIT_CSA)   \
 HOW(EXIT_LINK_USAGE)

static const char *const iwl_mvm_esr_states_names[] = {
#define NAME_ENTRY(x) [ilog2(IWL_MVM_ESR_##x)] = #x,
 HANDLE_ESR_REASONS(NAME_ENTRY)
};

const char *iwl_get_esr_state_string(enum iwl_mvm_esr_state state)
{
 int offs = ilog2(state);

 if (offs >= ARRAY_SIZE(iwl_mvm_esr_states_names) ||
     !iwl_mvm_esr_states_names[offs])
  return "UNKNOWN";

 return iwl_mvm_esr_states_names[offs];
}

static void iwl_mvm_print_esr_state(struct iwl_mvm *mvm, u32 mask)
{
#define NAME_FMT(x) "%s"
#define NAME_PR(x) (mask & IWL_MVM_ESR_##x) ? "[" #x "]" : "",
 IWL_DEBUG_INFO(mvm,
         "EMLSR state = " HANDLE_ESR_REASONS(NAME_FMT)
         " (0x%x)\n",
         HANDLE_ESR_REASONS(NAME_PR)
         mask);
#undef NAME_FMT
#undef NAME_PR
}

static int iwl_mvm_link_cmd_send(struct iwl_mvm *mvm,
     struct iwl_link_config_cmd *cmd,
     enum iwl_ctxt_action action)
{
 int ret;

 cmd->action = cpu_to_le32(action);
 ret = iwl_mvm_send_cmd_pdu(mvm,
       WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD), 0,
       sizeof(*cmd), cmd);
 if (ret)
  IWL_ERR(mvm, "Failed to send LINK_CONFIG_CMD (action:%d): %d\n",
   action, ret);
 return ret;
}

void iwl_mvm_set_link_fw_id(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
       struct ieee80211_bss_conf *link_conf)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 struct iwl_mvm_vif_link_info *link_info =
  mvmvif->link[link_conf->link_id];

 if (link_info->fw_link_id == IWL_MVM_FW_LINK_ID_INVALID)
  link_info->fw_link_id = mvmvif->id;
}

int iwl_mvm_add_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
       struct ieee80211_bss_conf *link_conf)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 unsigned int link_id = link_conf->link_id;
 struct iwl_mvm_vif_link_info *link_info = mvmvif->link[link_id];
 struct iwl_link_config_cmd cmd = {};
 unsigned int cmd_id = WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD);
 u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 1);

 if (WARN_ON_ONCE(!link_info))
  return -EINVAL;

 iwl_mvm_set_link_fw_id(mvm, vif, link_conf);

 /* Update SF - Disable if needed. if this fails, SF might still be on
  * while many macs are bound, which is forbidden - so fail the binding.
 */
 if (iwl_mvm_sf_update(mvm, vif, false))
  return -EINVAL;

 cmd.link_id = cpu_to_le32(link_info->fw_link_id);
 cmd.mac_id = cpu_to_le32(mvmvif->id);
 cmd.spec_link_id = link_conf->link_id;
 WARN_ON_ONCE(link_info->phy_ctxt);
 cmd.phy_id = cpu_to_le32(FW_CTXT_INVALID);

 memcpy(cmd.local_link_addr, link_conf->addr, ETH_ALEN);

 if (vif->type == NL80211_IFTYPE_ADHOC && link_conf->bssid)
  memcpy(cmd.ibss_bssid_addr, link_conf->bssid, ETH_ALEN);

 if (cmd_ver < 2)
  cmd.listen_lmac = cpu_to_le32(link_info->listen_lmac);

 return iwl_mvm_link_cmd_send(mvm, &cmd, FW_CTXT_ACTION_ADD);
}

struct iwl_mvm_esr_iter_data {
 struct ieee80211_vif *vif;
 unsigned int link_id;
 bool lift_block;
};

static void iwl_mvm_esr_vif_iterator(void *_data, u8 *mac,
         struct ieee80211_vif *vif)
{
 struct iwl_mvm_esr_iter_data *data = _data;
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 int link_id;

 if (ieee80211_vif_type_p2p(vif) == NL80211_IFTYPE_STATION)
  return;

 for_each_mvm_vif_valid_link(mvmvif, link_id) {
  struct iwl_mvm_vif_link_info *link_info =
   mvmvif->link[link_id];
  if (vif == data->vif && link_id == data->link_id)
   continue;
  if (link_info->active)
   data->lift_block = false;
 }
}

int iwl_mvm_esr_non_bss_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
        unsigned int link_id, bool active)
{
 /* An active link of a non-station vif blocks EMLSR. Upon activation
  * block EMLSR on the bss vif. Upon deactivation, check if this link
  * was the last non-station link active, and if so unblock the bss vif
 */
 struct ieee80211_vif *bss_vif = iwl_mvm_get_bss_vif(mvm);
 struct iwl_mvm_esr_iter_data data = {
  .vif = vif,
  .link_id = link_id,
  .lift_block = true,
 };

 if (IS_ERR_OR_NULL(bss_vif))
  return 0;

 if (active)
  return iwl_mvm_block_esr_sync(mvm, bss_vif,
           IWL_MVM_ESR_BLOCKED_NON_BSS);

 ieee80211_iterate_active_interfaces(mvm->hw,
         IEEE80211_IFACE_ITER_NORMAL,
         iwl_mvm_esr_vif_iterator, &data);
 if (data.lift_block) {
  mutex_lock(&mvm->mutex);
  iwl_mvm_unblock_esr(mvm, bss_vif, IWL_MVM_ESR_BLOCKED_NON_BSS);
  mutex_unlock(&mvm->mutex);
 }

 return 0;
}

int iwl_mvm_link_changed(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
    struct ieee80211_bss_conf *link_conf,
    u32 changes, bool active)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 unsigned int link_id = link_conf->link_id;
 struct iwl_mvm_vif_link_info *link_info = mvmvif->link[link_id];
 struct iwl_mvm_phy_ctxt *phyctxt;
 struct iwl_link_config_cmd cmd = {};
 u32 ht_flag, flags = 0, flags_mask = 0;
 int ret;
 unsigned int cmd_id = WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD);
 u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 1);

 if (WARN_ON_ONCE(!link_info ||
    link_info->fw_link_id == IWL_MVM_FW_LINK_ID_INVALID))
  return -EINVAL;

 if (changes & LINK_CONTEXT_MODIFY_ACTIVE) {
  /* When activating a link, phy context should be valid;
   * when deactivating a link, it also should be valid since
   * the link was active before. So, do nothing in this case.
   * Since a link is added first with FW_CTXT_INVALID, then we
   * can get here in case it's removed before it was activated.
 */
  if (!link_info->phy_ctxt)
   return 0;

  /* Catch early if driver tries to activate or deactivate a link
   * twice.
 */
  WARN_ON_ONCE(active == link_info->active);

  /* When deactivating a link session protection should
   * be stopped. Also let the firmware know if we can't Tx.
 */
  if (!active && vif->type == NL80211_IFTYPE_STATION) {
   iwl_mvm_stop_session_protection(mvm, vif);
   if (link_info->csa_block_tx) {
    cmd.block_tx = 1;
    link_info->csa_block_tx = false;
   }
  }
 }

 cmd.link_id = cpu_to_le32(link_info->fw_link_id);

 /* The phy_id, link address and listen_lmac can be modified only until
  * the link becomes active, otherwise they will be ignored.
 */
 phyctxt = link_info->phy_ctxt;
 if (phyctxt)
  cmd.phy_id = cpu_to_le32(phyctxt->id);
 else
  cmd.phy_id = cpu_to_le32(FW_CTXT_INVALID);
 cmd.mac_id = cpu_to_le32(mvmvif->id);

 memcpy(cmd.local_link_addr, link_conf->addr, ETH_ALEN);

 cmd.active = cpu_to_le32(active);

 if (vif->type == NL80211_IFTYPE_ADHOC && link_conf->bssid)
  memcpy(cmd.ibss_bssid_addr, link_conf->bssid, ETH_ALEN);

 iwl_mvm_set_fw_basic_rates(mvm, vif, link_info,
       &cmd.cck_rates, &cmd.ofdm_rates);

 cmd.cck_short_preamble = cpu_to_le32(link_conf->use_short_preamble);
 cmd.short_slot = cpu_to_le32(link_conf->use_short_slot);

 /* The fw does not distinguish between ht and fat */
 ht_flag = LINK_PROT_FLG_HT_PROT | LINK_PROT_FLG_FAT_PROT;
 iwl_mvm_set_fw_protection_flags(mvm, vif, link_conf,
     &cmd.protection_flags,
     ht_flag, LINK_PROT_FLG_TGG_PROTECT);

 iwl_mvm_set_fw_qos_params(mvm, vif, link_conf, cmd.ac,
      &cmd.qos_flags);


 cmd.bi = cpu_to_le32(link_conf->beacon_int);
 cmd.dtim_interval = cpu_to_le32(link_conf->beacon_int *
     link_conf->dtim_period);

 if (!link_conf->he_support || iwlwifi_mod_params.disable_11ax ||
     (vif->type == NL80211_IFTYPE_STATION && !vif->cfg.assoc)) {
  changes &= ~LINK_CONTEXT_MODIFY_HE_PARAMS;
  goto send_cmd;
 }

 cmd.htc_trig_based_pkt_ext = link_conf->htc_trig_based_pkt_ext;

 if (link_conf->uora_exists) {
  cmd.rand_alloc_ecwmin =
   link_conf->uora_ocw_range & 0x7;
  cmd.rand_alloc_ecwmax =
   (link_conf->uora_ocw_range >> 3) & 0x7;
 }

 /* ap_sta may be NULL if we're disconnecting */
 if (changes & LINK_CONTEXT_MODIFY_HE_PARAMS && mvmvif->ap_sta) {
  struct ieee80211_link_sta *link_sta =
   link_sta_dereference_check(mvmvif->ap_sta, link_id);

  if (!WARN_ON(!link_sta) && link_sta->he_cap.has_he &&
      link_sta->he_cap.he_cap_elem.mac_cap_info[5] &
      IEEE80211_HE_MAC_CAP5_OM_CTRL_UL_MU_DATA_DIS_RX)
   cmd.ul_mu_data_disable = 1;
 }

 /* TODO  how to set ndp_fdbk_buff_th_exp? */

 if (iwl_mvm_set_fw_mu_edca_params(mvm, mvmvif->link[link_id],
       &cmd.trig_based_txf[0])) {
  flags |= LINK_FLG_MU_EDCA_CW;
  flags_mask |= LINK_FLG_MU_EDCA_CW;
 }

 if (changes & LINK_CONTEXT_MODIFY_EHT_PARAMS) {
  struct ieee80211_chanctx_conf *ctx;
  struct cfg80211_chan_def *def = NULL;

  rcu_read_lock();
  ctx = rcu_dereference(link_conf->chanctx_conf);
  if (ctx)
   def = iwl_mvm_chanctx_def(mvm, ctx);

  if (iwlwifi_mod_params.disable_11be ||
      !link_conf->eht_support || !def ||
      iwl_fw_lookup_cmd_ver(mvm->fw, PHY_CONTEXT_CMD, 1) >= 6)
   changes &= ~LINK_CONTEXT_MODIFY_EHT_PARAMS;
  else
   cmd.puncture_mask = cpu_to_le16(def->punctured);
  rcu_read_unlock();
 }

 cmd.bss_color = link_conf->he_bss_color.color;

 if (!link_conf->he_bss_color.enabled) {
  flags |= LINK_FLG_BSS_COLOR_DIS;
  flags_mask |= LINK_FLG_BSS_COLOR_DIS;
 }

 cmd.frame_time_rts_th = cpu_to_le16(link_conf->frame_time_rts_th);

 /* Block 26-tone RU OFDMA transmissions */
 if (link_info->he_ru_2mhz_block) {
  flags |= LINK_FLG_RU_2MHZ_BLOCK;
  flags_mask |= LINK_FLG_RU_2MHZ_BLOCK;
 }

 if (link_conf->nontransmitted) {
  ether_addr_copy(cmd.ref_bssid_addr,
    link_conf->transmitter_bssid);
  cmd.bssid_index = link_conf->bssid_index;
 }

send_cmd:
 cmd.modify_mask = cpu_to_le32(changes);
 cmd.flags = cpu_to_le32(flags);
 if (cmd_ver < 6)
  cmd.flags_mask = cpu_to_le32(flags_mask);
 cmd.spec_link_id = link_conf->link_id;
 if (cmd_ver < 2)
  cmd.listen_lmac = cpu_to_le32(link_info->listen_lmac);

 ret = iwl_mvm_link_cmd_send(mvm, &cmd, FW_CTXT_ACTION_MODIFY);
 if (!ret && (changes & LINK_CONTEXT_MODIFY_ACTIVE))
  link_info->active = active;

 return ret;
}

int iwl_mvm_remove_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
   struct ieee80211_bss_conf *link_conf)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 unsigned int link_id = link_conf->link_id;
 struct iwl_mvm_vif_link_info *link_info = mvmvif->link[link_id];
 struct iwl_link_config_cmd cmd = {};
 int ret;

 cmd.link_id = cpu_to_le32(link_info->fw_link_id);
 link_info->fw_link_id = IWL_MVM_FW_LINK_ID_INVALID;
 cmd.spec_link_id = link_conf->link_id;
 cmd.phy_id = cpu_to_le32(FW_CTXT_INVALID);

 ret = iwl_mvm_link_cmd_send(mvm, &cmd, FW_CTXT_ACTION_REMOVE);

 if (!ret && iwl_mvm_sf_update(mvm, vif, true))
  IWL_ERR(mvm, "Failed to update SF state\n");

 return ret;
}

/* link should be deactivated before removal, so in most cases we need to
 * perform these two operations together
 */
int iwl_mvm_disable_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
    struct ieee80211_bss_conf *link_conf)
{
 int ret;

 ret = iwl_mvm_link_changed(mvm, vif, link_conf,
       LINK_CONTEXT_MODIFY_ACTIVE, false);
 if (ret)
  return ret;

 ret = iwl_mvm_remove_link(mvm, vif, link_conf);
 if (ret)
  return ret;

 return ret;
}

struct iwl_mvm_rssi_to_grade {
 s8 rssi[2];
 u16 grade;
};

#define RSSI_TO_GRADE_LINE(_lb, _hb_uhb, _grade) \
 { \
  .rssi = {_lb, _hb_uhb}, \
  .grade = _grade \
 }

/*
 * This array must be sorted by increasing RSSI for proper functionality.
 * The grades are actually estimated throughput, represented as fixed-point
 * with a scale factor of 1/10.
 */
static const struct iwl_mvm_rssi_to_grade rssi_to_grade_map[] = {
 RSSI_TO_GRADE_LINE(-85, -89, 177),
 RSSI_TO_GRADE_LINE(-83, -86, 344),
 RSSI_TO_GRADE_LINE(-82, -85, 516),
 RSSI_TO_GRADE_LINE(-80, -83, 688),
 RSSI_TO_GRADE_LINE(-77, -79, 1032),
 RSSI_TO_GRADE_LINE(-73, -76, 1376),
 RSSI_TO_GRADE_LINE(-70, -74, 1548),
 RSSI_TO_GRADE_LINE(-69, -72, 1750),
 RSSI_TO_GRADE_LINE(-65, -68, 2064),
 RSSI_TO_GRADE_LINE(-61, -66, 2294),
 RSSI_TO_GRADE_LINE(-58, -61, 2580),
 RSSI_TO_GRADE_LINE(-55, -58, 2868),
 RSSI_TO_GRADE_LINE(-46, -55, 3098),
 RSSI_TO_GRADE_LINE(-43, -54, 3442)
};

#define MAX_GRADE (rssi_to_grade_map[ARRAY_SIZE(rssi_to_grade_map) - 1].grade)

#define DEFAULT_CHAN_LOAD_LB 30
#define DEFAULT_CHAN_LOAD_HB 15
#define DEFAULT_CHAN_LOAD_UHB 0

/* Factors calculation is done with fixed-point with a scaling factor of 1/256 */
#define SCALE_FACTOR 256

/* Convert a percentage from [0,100] to [0,255] */
#define NORMALIZE_PERCENT_TO_255(percentage) ((percentage) * SCALE_FACTOR / 100)

static unsigned int
iwl_mvm_get_puncturing_factor(const struct ieee80211_bss_conf *link_conf)
{
 enum nl80211_chan_width chan_width =
  link_conf->chanreq.oper.width;
 int mhz = nl80211_chan_width_to_mhz(chan_width);
 unsigned int n_subchannels, n_punctured, puncturing_penalty;

 if (WARN_ONCE(mhz < 20 || mhz > 320,
        "Invalid channel width : (%d)\n", mhz))
  return SCALE_FACTOR;

 /* No puncturing, no penalty */
 if (mhz < 80)
  return SCALE_FACTOR;

 /* total number of subchannels */
 n_subchannels = mhz / 20;
 /* how many of these are punctured */
 n_punctured = hweight16(link_conf->chanreq.oper.punctured);

 puncturing_penalty = n_punctured * SCALE_FACTOR / n_subchannels;
 return SCALE_FACTOR - puncturing_penalty;
}

static unsigned int
iwl_mvm_get_chan_load(struct ieee80211_bss_conf *link_conf)
{
 struct ieee80211_vif *vif = link_conf->vif;
 struct iwl_mvm_vif_link_info *mvm_link =
  iwl_mvm_vif_from_mac80211(link_conf->vif)->link[link_conf->link_id];
 const struct element *bss_load_elem;
 const struct ieee80211_bss_load_elem *bss_load;
 enum nl80211_band band = link_conf->chanreq.oper.chan->band;
 const struct cfg80211_bss_ies *ies;
 unsigned int chan_load;
 u32 chan_load_by_us;

 rcu_read_lock();
 if (ieee80211_vif_link_active(vif, link_conf->link_id))
  ies = rcu_dereference(link_conf->bss->beacon_ies);
 else
  ies = rcu_dereference(link_conf->bss->ies);

 if (ies)
  bss_load_elem = cfg80211_find_elem(WLAN_EID_QBSS_LOAD,
         ies->data, ies->len);
 else
  bss_load_elem = NULL;

 /* If there isn't BSS Load element, take the defaults */
 if (!bss_load_elem ||
     bss_load_elem->datalen != sizeof(*bss_load)) {
  rcu_read_unlock();
  switch (band) {
  case NL80211_BAND_2GHZ:
   chan_load = DEFAULT_CHAN_LOAD_LB;
   break;
  case NL80211_BAND_5GHZ:
   chan_load = DEFAULT_CHAN_LOAD_HB;
   break;
  case NL80211_BAND_6GHZ:
   chan_load = DEFAULT_CHAN_LOAD_UHB;
   break;
  default:
   chan_load = 0;
   break;
  }
  /* The defaults are given in percentage */
  return NORMALIZE_PERCENT_TO_255(chan_load);
 }

 bss_load = (const void *)bss_load_elem->data;
 /* Channel util is in range 0-255 */
 chan_load = bss_load->channel_util;
 rcu_read_unlock();

 if (!mvm_link || !mvm_link->active)
  return chan_load;

 if (WARN_ONCE(!mvm_link->phy_ctxt,
        "Active link (%u) without phy ctxt assigned!\n",
        link_conf->link_id))
  return chan_load;

 /* channel load by us is given in percentage */
 chan_load_by_us =
  NORMALIZE_PERCENT_TO_255(mvm_link->phy_ctxt->channel_load_by_us);

 /* Use only values that firmware sends that can possibly be valid */
 if (chan_load_by_us <= chan_load)
  chan_load -= chan_load_by_us;

 return chan_load;
}

static unsigned int
iwl_mvm_get_chan_load_factor(struct ieee80211_bss_conf *link_conf)
{
 return SCALE_FACTOR - iwl_mvm_get_chan_load(link_conf);
}

/* This function calculates the grade of a link. Returns 0 in error case */
VISIBLE_IF_IWLWIFI_KUNIT
unsigned int iwl_mvm_get_link_grade(struct ieee80211_bss_conf *link_conf)
{
 enum nl80211_band band;
 int i, rssi_idx;
 s32 link_rssi;
 unsigned int grade = MAX_GRADE;

 if (WARN_ON_ONCE(!link_conf))
  return 0;

 band = link_conf->chanreq.oper.chan->band;
 if (WARN_ONCE(band != NL80211_BAND_2GHZ &&
        band != NL80211_BAND_5GHZ &&
        band != NL80211_BAND_6GHZ,
        "Invalid band (%u)\n", band))
  return 0;

 link_rssi = MBM_TO_DBM(link_conf->bss->signal);
 /*
  * For 6 GHz the RSSI of the beacons is lower than
  * the RSSI of the data.
 */
 if (band == NL80211_BAND_6GHZ)
  link_rssi += 4;

 rssi_idx = band == NL80211_BAND_2GHZ ? 0 : 1;

 /* No valid RSSI - take the lowest grade */
 if (!link_rssi)
  link_rssi = rssi_to_grade_map[0].rssi[rssi_idx];

 /* Get grade based on RSSI */
 for (i = 0; i < ARRAY_SIZE(rssi_to_grade_map); i++) {
  const struct iwl_mvm_rssi_to_grade *line =
   &rssi_to_grade_map[i];

  if (link_rssi > line->rssi[rssi_idx])
   continue;
  grade = line->grade;
  break;
 }

 /* apply the channel load and puncturing factors */
 grade = grade * iwl_mvm_get_chan_load_factor(link_conf) / SCALE_FACTOR;
 grade = grade * iwl_mvm_get_puncturing_factor(link_conf) / SCALE_FACTOR;
 return grade;
}
EXPORT_SYMBOL_IF_IWLWIFI_KUNIT(iwl_mvm_get_link_grade);

static
u8 iwl_mvm_set_link_selection_data(struct ieee80211_vif *vif,
       struct iwl_mvm_link_sel_data *data,
       unsigned long usable_links,
       u8 *best_link_idx)
{
 u8 n_data = 0;
 u16 max_grade = 0;
 unsigned long link_id;

 /* TODO: don't select links that weren't discovered in the last scan */
 for_each_set_bit(link_id, &usable_links, IEEE80211_MLD_MAX_NUM_LINKS) {
  struct ieee80211_bss_conf *link_conf =
   link_conf_dereference_protected(vif, link_id);

  if (WARN_ON_ONCE(!link_conf))
   continue;

  data[n_data].link_id = link_id;
  data[n_data].chandef = &link_conf->chanreq.oper;
  data[n_data].signal = link_conf->bss->signal / 100;
  data[n_data].grade = iwl_mvm_get_link_grade(link_conf);

  if (data[n_data].grade > max_grade) {
   max_grade = data[n_data].grade;
   *best_link_idx = n_data;
  }
  n_data++;
 }

 return n_data;
}

struct iwl_mvm_bw_to_rssi_threshs {
 s8 low;
 s8 high;
};

#define BW_TO_RSSI_THRESHOLDS(_bw)    \
 [IWL_PHY_CHANNEL_MODE ## _bw] = {   \
  .low = IWL_MVM_LOW_RSSI_THRESH_##_bw##MHZ, \
  .high = IWL_MVM_HIGH_RSSI_THRESH_##_bw##MHZ \
 }

s8 iwl_mvm_get_esr_rssi_thresh(struct iwl_mvm *mvm,
          const struct cfg80211_chan_def *chandef,
          bool low)
{
 const struct iwl_mvm_bw_to_rssi_threshs bw_to_rssi_threshs_map[] = {
  BW_TO_RSSI_THRESHOLDS(20),
  BW_TO_RSSI_THRESHOLDS(40),
  BW_TO_RSSI_THRESHOLDS(80),
  BW_TO_RSSI_THRESHOLDS(160)
  /* 320 MHz has the same thresholds as 20 MHz */
 };
 const struct iwl_mvm_bw_to_rssi_threshs *threshs;
 u8 chan_width = iwl_mvm_get_channel_width(chandef);

 if (WARN_ON(chandef->chan->band != NL80211_BAND_2GHZ &&
      chandef->chan->band != NL80211_BAND_5GHZ &&
      chandef->chan->band != NL80211_BAND_6GHZ))
  return S8_MAX;

 /* 6 GHz will always use 20 MHz thresholds, regardless of the BW */
 if (chan_width == IWL_PHY_CHANNEL_MODE320)
  chan_width = IWL_PHY_CHANNEL_MODE20;

 threshs = &bw_to_rssi_threshs_map[chan_width];

 return low ? threshs->low : threshs->high;
}

static u32
iwl_mvm_esr_disallowed_with_link(struct iwl_mvm *mvm,
     struct ieee80211_vif *vif,
     const struct iwl_mvm_link_sel_data *link,
     bool primary)
{
 struct wiphy *wiphy = mvm->hw->wiphy;
 struct ieee80211_bss_conf *conf;
 enum iwl_mvm_esr_state ret = 0;
 s8 thresh;

 conf = wiphy_dereference(wiphy, vif->link_conf[link->link_id]);
 if (WARN_ON_ONCE(!conf))
  return false;

 /* BT Coex effects eSR mode only if one of the links is on LB */
 if (link->chandef->chan->band == NL80211_BAND_2GHZ &&
     (!iwl_mvm_bt_coex_calculate_esr_mode(mvm, vif, link->signal,
       primary)))
  ret |= IWL_MVM_ESR_EXIT_COEX;

 thresh = iwl_mvm_get_esr_rssi_thresh(mvm, link->chandef,
          false);

 if (link->signal < thresh)
  ret |= IWL_MVM_ESR_EXIT_LOW_RSSI;

 if (conf->csa_active)
  ret |= IWL_MVM_ESR_EXIT_CSA;

 if (ret) {
  IWL_DEBUG_INFO(mvm,
          "Link %d is not allowed for esr\n",
          link->link_id);
  iwl_mvm_print_esr_state(mvm, ret);
 }
 return ret;
}

VISIBLE_IF_IWLWIFI_KUNIT
bool iwl_mvm_mld_valid_link_pair(struct ieee80211_vif *vif,
     const struct iwl_mvm_link_sel_data *a,
     const struct iwl_mvm_link_sel_data *b)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 struct iwl_mvm *mvm = mvmvif->mvm;
 enum iwl_mvm_esr_state ret = 0;

 /* Per-link considerations */
 if (iwl_mvm_esr_disallowed_with_link(mvm, vif, a, true) ||
     iwl_mvm_esr_disallowed_with_link(mvm, vif, b, false))
  return false;

 if (a->chandef->chan->band == b->chandef->chan->band ||
     a->chandef->width != b->chandef->width)
  ret |= IWL_MVM_ESR_EXIT_BANDWIDTH;

 if (ret) {
  IWL_DEBUG_INFO(mvm,
          "Links %d and %d are not a valid pair for EMLSR\n",
          a->link_id, b->link_id);
  iwl_mvm_print_esr_state(mvm, ret);
  return false;
 }

 return true;

}
EXPORT_SYMBOL_IF_IWLWIFI_KUNIT(iwl_mvm_mld_valid_link_pair);

/*
 * Returns the combined eSR grade of two given links.
 * Returns 0 if eSR is not allowed with these 2 links.
 */
static
unsigned int iwl_mvm_get_esr_grade(struct ieee80211_vif *vif,
       const struct iwl_mvm_link_sel_data *a,
       const struct iwl_mvm_link_sel_data *b,
       u8 *primary_id)
{
 struct ieee80211_bss_conf *primary_conf;
 struct wiphy *wiphy = ieee80211_vif_to_wdev(vif)->wiphy;
 unsigned int primary_load;

 lockdep_assert_wiphy(wiphy);

 /* a is always primary, b is always secondary */
 if (b->grade > a->grade)
  swap(a, b);

 *primary_id = a->link_id;

 if (!iwl_mvm_mld_valid_link_pair(vif, a, b))
  return 0;

 primary_conf = wiphy_dereference(wiphy, vif->link_conf[*primary_id]);

 if (WARN_ON_ONCE(!primary_conf))
  return 0;

 primary_load = iwl_mvm_get_chan_load(primary_conf);

 return a->grade +
  ((b->grade * primary_load) / SCALE_FACTOR);
}

void iwl_mvm_select_links(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
{
 struct iwl_mvm_link_sel_data data[IEEE80211_MLD_MAX_NUM_LINKS];
 struct iwl_mvm_link_sel_data *best_link;
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 u32 max_active_links = iwl_mvm_max_active_links(mvm, vif);
 u16 usable_links = ieee80211_vif_usable_links(vif);
 u8 best, primary_link, best_in_pair, n_data;
 u16 max_esr_grade = 0, new_active_links;

 lockdep_assert_wiphy(mvm->hw->wiphy);

 if (!mvmvif->authorized || !ieee80211_vif_is_mld(vif))
  return;

 if (!IWL_MVM_AUTO_EML_ENABLE)
  return;

 /* The logic below is a simple version that doesn't suit more than 2
  * links
 */
 WARN_ON_ONCE(max_active_links > 2);

 n_data = iwl_mvm_set_link_selection_data(vif, data, usable_links,
       &best);

 if (WARN(!n_data, "Couldn't find a valid grade for any link!\n"))
  return;

 best_link = &data[best];
 primary_link = best_link->link_id;
 new_active_links = BIT(best_link->link_id);

 /* eSR is not supported/blocked, or only one usable link */
 if (max_active_links == 1 || !iwl_mvm_vif_has_esr_cap(mvm, vif) ||
     mvmvif->esr_disable_reason || n_data == 1)
  goto set_active;

 for (u8 a = 0; a < n_data; a++)
  for (u8 b = a + 1; b < n_data; b++) {
   u16 esr_grade = iwl_mvm_get_esr_grade(vif, &data[a],
             &data[b],
             &best_in_pair);

   if (esr_grade <= max_esr_grade)
    continue;

   max_esr_grade = esr_grade;
   primary_link = best_in_pair;
   new_active_links = BIT(data[a].link_id) |
        BIT(data[b].link_id);
  }

 /* No valid pair was found, go with the best link */
 if (hweight16(new_active_links) <= 1)
  goto set_active;

 /* For equal grade - prefer EMLSR */
 if (best_link->grade > max_esr_grade) {
  primary_link = best_link->link_id;
  new_active_links = BIT(best_link->link_id);
 }
set_active:
 IWL_DEBUG_INFO(mvm, "Link selection result: 0x%x. Primary = %d\n",
         new_active_links, primary_link);
 ieee80211_set_active_links_async(vif, new_active_links);
 mvmvif->link_selection_res = new_active_links;
 mvmvif->link_selection_primary = primary_link;
}

u8 iwl_mvm_get_primary_link(struct ieee80211_vif *vif)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);

 /* relevant data is written with both locks held, so read with either */
 lockdep_assert(lockdep_is_held(&mvmvif->mvm->mutex) ||
         lockdep_is_held(&mvmvif->mvm->hw->wiphy->mtx));

 if (!ieee80211_vif_is_mld(vif))
  return 0;

 /* In AP mode, there is no primary link */
 if (vif->type == NL80211_IFTYPE_AP)
  return __ffs(vif->active_links);

 if (mvmvif->esr_active &&
     !WARN_ON(!(BIT(mvmvif->primary_link) & vif->active_links)))
  return mvmvif->primary_link;

 return __ffs(vif->active_links);
}

/*
 * For non-MLO/single link, this will return the deflink/single active link,
 * respectively
 */
u8 iwl_mvm_get_other_link(struct ieee80211_vif *vif, u8 link_id)
{
 switch (hweight16(vif->active_links)) {
 case 0:
  return 0;
 default:
  WARN_ON(1);
  fallthrough;
 case 1:
  return __ffs(vif->active_links);
 case 2:
  return __ffs(vif->active_links & ~BIT(link_id));
 }
}

/* Reasons that can cause esr prevention */
#define IWL_MVM_ESR_PREVENT_REASONS IWL_MVM_ESR_EXIT_MISSED_BEACON
#define IWL_MVM_PREVENT_ESR_TIMEOUT (HZ * 400)
#define IWL_MVM_ESR_PREVENT_SHORT (HZ * 300)
#define IWL_MVM_ESR_PREVENT_LONG (HZ * 600)

static bool iwl_mvm_check_esr_prevention(struct iwl_mvm *mvm,
      struct iwl_mvm_vif *mvmvif,
      enum iwl_mvm_esr_state reason)
{
 bool timeout_expired = time_after(jiffies,
       mvmvif->last_esr_exit.ts +
       IWL_MVM_PREVENT_ESR_TIMEOUT);
 unsigned long delay;

 lockdep_assert_held(&mvm->mutex);

 /* Only handle reasons that can cause prevention */
 if (!(reason & IWL_MVM_ESR_PREVENT_REASONS))
  return false;

 /*
  * Reset the counter if more than 400 seconds have passed between one
  * exit and the other, or if we exited due to a different reason.
  * Will also reset the counter after the long prevention is done.
 */
 if (timeout_expired || mvmvif->last_esr_exit.reason != reason) {
  mvmvif->exit_same_reason_count = 1;
  return false;
 }

 mvmvif->exit_same_reason_count++;
 if (WARN_ON(mvmvif->exit_same_reason_count < 2 ||
      mvmvif->exit_same_reason_count > 3))
  return false;

 mvmvif->esr_disable_reason |= IWL_MVM_ESR_BLOCKED_PREVENTION;

 /*
  * For the second exit, use a short prevention, and for the third one,
  * use a long prevention.
 */
 delay = mvmvif->exit_same_reason_count == 2 ?
  IWL_MVM_ESR_PREVENT_SHORT :
  IWL_MVM_ESR_PREVENT_LONG;

 IWL_DEBUG_INFO(mvm,
         "Preventing EMLSR for %ld seconds due to %u exits with the reason = %s (0x%x)\n",
         delay / HZ, mvmvif->exit_same_reason_count,
         iwl_get_esr_state_string(reason), reason);

 wiphy_delayed_work_queue(mvm->hw->wiphy,
     &mvmvif->prevent_esr_done_wk, delay);
 return true;
}

#define IWL_MVM_TRIGGER_LINK_SEL_TIME (IWL_MVM_TRIGGER_LINK_SEL_TIME_SEC * HZ)

/* API to exit eSR mode */
void iwl_mvm_exit_esr(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
        enum iwl_mvm_esr_state reason,
        u8 link_to_keep)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 u16 new_active_links;
 bool prevented;

 lockdep_assert_held(&mvm->mutex);

 if (!IWL_MVM_AUTO_EML_ENABLE)
  return;

 /* Nothing to do */
 if (!mvmvif->esr_active)
  return;

 if (WARN_ON(!ieee80211_vif_is_mld(vif) || !mvmvif->authorized))
  return;

 if (WARN_ON(!(vif->active_links & BIT(link_to_keep))))
  link_to_keep = __ffs(vif->active_links);

 new_active_links = BIT(link_to_keep);
 IWL_DEBUG_INFO(mvm,
         "Exiting EMLSR. reason = %s (0x%x). Current active links=0x%x, new active links = 0x%x\n",
         iwl_get_esr_state_string(reason), reason,
         vif->active_links, new_active_links);

 ieee80211_set_active_links_async(vif, new_active_links);

 /* Prevent EMLSR if needed */
 prevented = iwl_mvm_check_esr_prevention(mvm, mvmvif, reason);

 /* Remember why and when we exited EMLSR */
 mvmvif->last_esr_exit.ts = jiffies;
 mvmvif->last_esr_exit.reason = reason;

 /*
  * If EMLSR is prevented now - don't try to get back to EMLSR.
  * If we exited due to a blocking event, we will try to get back to
  * EMLSR when the corresponding unblocking event will happen.
 */
 if (prevented || reason & IWL_MVM_BLOCK_ESR_REASONS)
  return;

 /* If EMLSR is not blocked - try enabling it again in 30 seconds */
 wiphy_delayed_work_queue(mvm->hw->wiphy,
     &mvmvif->mlo_int_scan_wk,
     round_jiffies_relative(IWL_MVM_TRIGGER_LINK_SEL_TIME));
}

void iwl_mvm_block_esr(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
         enum iwl_mvm_esr_state reason,
         u8 link_to_keep)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);

 lockdep_assert_held(&mvm->mutex);

 if (!IWL_MVM_AUTO_EML_ENABLE)
  return;

 /* This should be called only with disable reasons */
 if (WARN_ON(!(reason & IWL_MVM_BLOCK_ESR_REASONS)))
  return;

 if (mvmvif->esr_disable_reason & reason)
  return;

 IWL_DEBUG_INFO(mvm,
         "Blocking EMLSR mode. reason = %s (0x%x)\n",
         iwl_get_esr_state_string(reason), reason);

 mvmvif->esr_disable_reason |= reason;

 iwl_mvm_print_esr_state(mvm, mvmvif->esr_disable_reason);

 iwl_mvm_exit_esr(mvm, vif, reason, link_to_keep);
}

int iwl_mvm_block_esr_sync(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
      enum iwl_mvm_esr_state reason)
{
 int primary_link = iwl_mvm_get_primary_link(vif);
 int ret;

 if (!IWL_MVM_AUTO_EML_ENABLE || !ieee80211_vif_is_mld(vif))
  return 0;

 /* This should be called only with blocking reasons */
 if (WARN_ON(!(reason & IWL_MVM_BLOCK_ESR_REASONS)))
  return 0;

 /* leave ESR immediately, not only async with iwl_mvm_block_esr() */
 ret = ieee80211_set_active_links(vif, BIT(primary_link));
 if (ret)
  return ret;

 mutex_lock(&mvm->mutex);
 /* only additionally block for consistency and to avoid concurrency */
 iwl_mvm_block_esr(mvm, vif, reason, primary_link);
 mutex_unlock(&mvm->mutex);

 return 0;
}

static void iwl_mvm_esr_unblocked(struct iwl_mvm *mvm,
      struct ieee80211_vif *vif)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 bool need_new_sel = time_after(jiffies, mvmvif->last_esr_exit.ts +
      IWL_MVM_TRIGGER_LINK_SEL_TIME);

 lockdep_assert_held(&mvm->mutex);

 if (!ieee80211_vif_is_mld(vif) || !mvmvif->authorized ||
     mvmvif->esr_active)
  return;

 IWL_DEBUG_INFO(mvm, "EMLSR is unblocked\n");

 /* If we exited due to an EXIT reason, and the exit was in less than
  * 30 seconds, then a MLO scan was scheduled already.
 */
 if (!need_new_sel &&
     !(mvmvif->last_esr_exit.reason & IWL_MVM_BLOCK_ESR_REASONS)) {
  IWL_DEBUG_INFO(mvm, "Wait for MLO scan\n");
  return;
 }

 /*
  * If EMLSR was blocked for more than 30 seconds, or the last link
  * selection decided to not enter EMLSR, trigger a new scan.
 */
 if (need_new_sel || hweight16(mvmvif->link_selection_res) < 2) {
  IWL_DEBUG_INFO(mvm, "Trigger MLO scan\n");
  wiphy_delayed_work_queue(mvm->hw->wiphy,
      &mvmvif->mlo_int_scan_wk, 0);
 /*
  * If EMLSR was blocked for less than 30 seconds, and the last link
  * selection decided to use EMLSR, activate EMLSR using the previous
  * link selection result.
 */
 } else {
  IWL_DEBUG_INFO(mvm,
          "Use the latest link selection result: 0x%x\n",
          mvmvif->link_selection_res);
  ieee80211_set_active_links_async(vif,
       mvmvif->link_selection_res);
 }
}

void iwl_mvm_unblock_esr(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
    enum iwl_mvm_esr_state reason)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);

 lockdep_assert_held(&mvm->mutex);

 if (!IWL_MVM_AUTO_EML_ENABLE)
  return;

 /* This should be called only with disable reasons */
 if (WARN_ON(!(reason & IWL_MVM_BLOCK_ESR_REASONS)))
  return;

 /* No Change */
 if (!(mvmvif->esr_disable_reason & reason))
  return;

 mvmvif->esr_disable_reason &= ~reason;

 IWL_DEBUG_INFO(mvm,
         "Unblocking EMLSR mode. reason = %s (0x%x)\n",
         iwl_get_esr_state_string(reason), reason);
 iwl_mvm_print_esr_state(mvm, mvmvif->esr_disable_reason);

 if (!mvmvif->esr_disable_reason)
  iwl_mvm_esr_unblocked(mvm, vif);
}

void iwl_mvm_init_link(struct iwl_mvm_vif_link_info *link)
{
 link->bcast_sta.sta_id = IWL_INVALID_STA;
 link->mcast_sta.sta_id = IWL_INVALID_STA;
 link->ap_sta_id = IWL_INVALID_STA;

 for (int r = 0; r < NUM_IWL_MVM_SMPS_REQ; r++)
  link->smps_requests[r] =
   IEEE80211_SMPS_AUTOMATIC;
}