Quelle  sta.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
/*
 * Copyright (C) 2012-2015, 2018-2025 Intel Corporation
 * Copyright (C) 2013-2015 Intel Mobile Communications GmbH
 * Copyright (C) 2016-2017 Intel Deutschland GmbH
 */
#include <net/mac80211.h>

#include "mvm.h"
#include "sta.h"
#include "rs.h"

/*
 * New version of ADD_STA_sta command added new fields at the end of the
 * structure, so sending the size of the relevant API's structure is enough to
 * support both API versions.
 */
static inline int iwl_mvm_add_sta_cmd_size(struct iwl_mvm *mvm)
{
 if (iwl_mvm_has_new_rx_api(mvm) ||
     fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE))
  return sizeof(struct iwl_mvm_add_sta_cmd);
 else
  return sizeof(struct iwl_mvm_add_sta_cmd_v7);
}

int iwl_mvm_find_free_sta_id(struct iwl_mvm *mvm, enum nl80211_iftype iftype)
{
 int sta_id;
 u32 reserved_ids = 0;

 BUILD_BUG_ON(IWL_STATION_COUNT_MAX > 32);
 WARN_ON_ONCE(test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status));

 lockdep_assert_held(&mvm->mutex);

 /* d0i3/d3 assumes the AP's sta_id (of sta vif) is 0. reserve it. */
 if (iftype != NL80211_IFTYPE_STATION)
  reserved_ids = BIT(0);

 /* Don't take rcu_read_lock() since we are protected by mvm->mutex */
 for (sta_id = 0; sta_id < mvm->fw->ucode_capa.num_stations; sta_id++) {
  if (BIT(sta_id) & reserved_ids)
   continue;

  if (!rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
            lockdep_is_held(&mvm->mutex)))
   return sta_id;
 }
 return IWL_INVALID_STA;
}

/* Calculate the ampdu density and max size */
u32 iwl_mvm_get_sta_ampdu_dens(struct ieee80211_link_sta *link_sta,
          struct ieee80211_bss_conf *link_conf,
          u32 *_agg_size)
{
 u32 agg_size = 0, mpdu_dens = 0;

 if (WARN_ON(!link_sta))
  return 0;

 /* Note that we always use only legacy & highest supported PPDUs, so
 * of Draft P802.11be D.30 Table 10-12a--Fields used for calculating
 * the maximum A-MPDU size of various PPDU types in different bands,
 * we only need to worry about the highest supported PPDU type here.
 */

 if (link_sta->ht_cap.ht_supported) {
  agg_size = link_sta->ht_cap.ampdu_factor;
  mpdu_dens = link_sta->ht_cap.ampdu_density;
 }

 if (link_conf->chanreq.oper.chan->band == NL80211_BAND_6GHZ) {
  /* overwrite HT values on 6 GHz */
  mpdu_dens = le16_get_bits(link_sta->he_6ghz_capa.capa,
       IEEE80211_HE_6GHZ_CAP_MIN_MPDU_START);
  agg_size = le16_get_bits(link_sta->he_6ghz_capa.capa,
      IEEE80211_HE_6GHZ_CAP_MAX_AMPDU_LEN_EXP);
 } else if (link_sta->vht_cap.vht_supported) {
  /* if VHT supported overwrite HT value */
  agg_size = u32_get_bits(link_sta->vht_cap.cap,
     IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK);
 }

 /* D6.0 10.12.2 A-MPDU length limit rules
 * A STA indicates the maximum length of the A-MPDU preEOF padding
 * that it can receive in an HE PPDU in the Maximum A-MPDU Length
 * Exponent field in its HT Capabilities, VHT Capabilities,
 * and HE 6 GHz Band Capabilities elements (if present) and the
 * Maximum AMPDU Length Exponent Extension field in its HE
 * Capabilities element
 */
 if (link_sta->he_cap.has_he)
  agg_size +=
   u8_get_bits(link_sta->he_cap.he_cap_elem.mac_cap_info[3],
        IEEE80211_HE_MAC_CAP3_MAX_AMPDU_LEN_EXP_MASK);

 if (link_sta->eht_cap.has_eht)
  agg_size += u8_get_bits(link_sta->eht_cap.eht_cap_elem.mac_cap_info[1],
     IEEE80211_EHT_MAC_CAP1_MAX_AMPDU_LEN_MASK);

 /* Limit to max A-MPDU supported by FW */
 agg_size = min_t(u32, agg_size,
    STA_FLG_MAX_AGG_SIZE_4M >> STA_FLG_MAX_AGG_SIZE_SHIFT);

 *_agg_size = agg_size;
 return mpdu_dens;
}

u8 iwl_mvm_get_sta_uapsd_acs(struct ieee80211_sta *sta)
{
 u8 uapsd_acs = 0;

 if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_BK)
  uapsd_acs |= BIT(AC_BK);
 if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_BE)
  uapsd_acs |= BIT(AC_BE);
 if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VI)
  uapsd_acs |= BIT(AC_VI);
 if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VO)
  uapsd_acs |= BIT(AC_VO);

 return uapsd_acs | uapsd_acs << 4;
}

/* send station add/update command to firmware */
int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
      bool update, unsigned int flags)
{
 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 struct iwl_mvm_add_sta_cmd add_sta_cmd = {
  .sta_id = mvm_sta->deflink.sta_id,
  .mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color),
  .add_modify = update ? 1 : 0,
  .station_flags_msk = cpu_to_le32(STA_FLG_FAT_EN_MSK |
       STA_FLG_MIMO_EN_MSK |
       STA_FLG_RTS_MIMO_PROT),
  .tid_disable_tx = cpu_to_le16(mvm_sta->tid_disable_agg),
 };
 int ret;
 u32 status;
 u32 agg_size = 0, mpdu_dens = 0;

 if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE))
  add_sta_cmd.station_type = mvm_sta->sta_type;

 if (!update || (flags & STA_MODIFY_QUEUES)) {
  memcpy(&add_sta_cmd.addr, sta->addr, ETH_ALEN);

  if (!iwl_mvm_has_new_tx_api(mvm)) {
   add_sta_cmd.tfd_queue_msk =
    cpu_to_le32(mvm_sta->tfd_queue_msk);

   if (flags & STA_MODIFY_QUEUES)
    add_sta_cmd.modify_mask |= STA_MODIFY_QUEUES;
  } else {
   WARN_ON(flags & STA_MODIFY_QUEUES);
  }
 }

 switch (sta->deflink.bandwidth) {
 case IEEE80211_STA_RX_BW_320:
 case IEEE80211_STA_RX_BW_160:
  add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_160MHZ);
  fallthrough;
 case IEEE80211_STA_RX_BW_80:
  add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_80MHZ);
  fallthrough;
 case IEEE80211_STA_RX_BW_40:
  add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_40MHZ);
  fallthrough;
 case IEEE80211_STA_RX_BW_20:
  if (sta->deflink.ht_cap.ht_supported)
   add_sta_cmd.station_flags |=
    cpu_to_le32(STA_FLG_FAT_EN_20MHZ);
  break;
 }

 switch (sta->deflink.rx_nss) {
 case 1:
  add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_SISO);
  break;
 case 2:
  add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_MIMO2);
  break;
 case 3 ... 8:
  add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_MIMO3);
  break;
 }

 switch (sta->deflink.smps_mode) {
 case IEEE80211_SMPS_AUTOMATIC:
 case IEEE80211_SMPS_NUM_MODES:
  WARN_ON(1);
  break;
 case IEEE80211_SMPS_STATIC:
  /* override NSS */
  add_sta_cmd.station_flags &= ~cpu_to_le32(STA_FLG_MIMO_EN_MSK);
  add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_SISO);
  break;
 case IEEE80211_SMPS_DYNAMIC:
  add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_RTS_MIMO_PROT);
  break;
 case IEEE80211_SMPS_OFF:
  /* nothing */
  break;
 }

 if (sta->deflink.ht_cap.ht_supported ||
     mvm_sta->vif->bss_conf.chanreq.oper.chan->band == NL80211_BAND_6GHZ)
  add_sta_cmd.station_flags_msk |=
   cpu_to_le32(STA_FLG_MAX_AGG_SIZE_MSK |
        STA_FLG_AGG_MPDU_DENS_MSK);

 mpdu_dens = iwl_mvm_get_sta_ampdu_dens(&sta->deflink,
            &mvm_sta->vif->bss_conf,
            &agg_size);
 add_sta_cmd.station_flags |=
  cpu_to_le32(agg_size << STA_FLG_MAX_AGG_SIZE_SHIFT);
 add_sta_cmd.station_flags |=
  cpu_to_le32(mpdu_dens << STA_FLG_AGG_MPDU_DENS_SHIFT);

 if (mvm_sta->sta_state >= IEEE80211_STA_ASSOC)
  add_sta_cmd.assoc_id = cpu_to_le16(sta->aid);

 if (sta->wme) {
  add_sta_cmd.modify_mask |= STA_MODIFY_UAPSD_ACS;
  add_sta_cmd.uapsd_acs = iwl_mvm_get_sta_uapsd_acs(sta);
  add_sta_cmd.sp_length = sta->max_sp ? sta->max_sp * 2 : 128;
 }

 status = ADD_STA_SUCCESS;
 ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
       iwl_mvm_add_sta_cmd_size(mvm),
       &add_sta_cmd, &status);
 if (ret)
  return ret;

 switch (status & IWL_ADD_STA_STATUS_MASK) {
 case ADD_STA_SUCCESS:
  IWL_DEBUG_ASSOC(mvm, "ADD_STA PASSED\n");
  break;
 default:
  ret = -EIO;
  IWL_ERR(mvm, "ADD_STA failed\n");
  break;
 }

 return ret;
}

static void iwl_mvm_rx_agg_session_expired(struct timer_list *t)
{
 struct iwl_mvm_baid_data *data =
  timer_container_of(data, t, session_timer);
 struct iwl_mvm_baid_data __rcu **rcu_ptr = data->rcu_ptr;
 struct iwl_mvm_baid_data *ba_data;
 struct ieee80211_sta *sta;
 struct iwl_mvm_sta *mvm_sta;
 unsigned long timeout;
 unsigned int sta_id;

 rcu_read_lock();

 ba_data = rcu_dereference(*rcu_ptr);

 if (WARN_ON(!ba_data))
  goto unlock;

 if (!ba_data->timeout)
  goto unlock;

 timeout = ba_data->last_rx + TU_TO_JIFFIES(ba_data->timeout * 2);
 if (time_is_after_jiffies(timeout)) {
  mod_timer(&ba_data->session_timer, timeout);
  goto unlock;
 }

 /* Timer expired */
 sta_id = ffs(ba_data->sta_mask) - 1; /* don't care which one */
 sta = rcu_dereference(ba_data->mvm->fw_id_to_mac_id[sta_id]);

 /*
 * sta should be valid unless the following happens:
 * The firmware asserts which triggers a reconfig flow, but
 * the reconfig fails before we set the pointer to sta into
 * the fw_id_to_mac_id pointer table. Mac80211 can't stop
 * A-MDPU and hence the timer continues to run. Then, the
 * timer expires and sta is NULL.
 */
 if (IS_ERR_OR_NULL(sta))
  goto unlock;

 mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 ieee80211_rx_ba_timer_expired(mvm_sta->vif,
          sta->addr, ba_data->tid);
unlock:
 rcu_read_unlock();
}

/* Disable aggregations for a bitmap of TIDs for a given station */
static int iwl_mvm_invalidate_sta_queue(struct iwl_mvm *mvm, int queue,
     unsigned long disable_agg_tids,
     bool remove_queue)
{
 struct iwl_mvm_add_sta_cmd cmd = {};
 struct ieee80211_sta *sta;
 struct iwl_mvm_sta *mvmsta;
 u32 status;
 u8 sta_id;

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return -EINVAL;

 sta_id = mvm->queue_info[queue].ra_sta_id;

 rcu_read_lock();

 sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);

 if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta))) {
  rcu_read_unlock();
  return -EINVAL;
 }

 mvmsta = iwl_mvm_sta_from_mac80211(sta);

 mvmsta->tid_disable_agg |= disable_agg_tids;

 cmd.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color);
 cmd.sta_id = mvmsta->deflink.sta_id;
 cmd.add_modify = STA_MODE_MODIFY;
 cmd.modify_mask = STA_MODIFY_QUEUES;
 if (disable_agg_tids)
  cmd.modify_mask |= STA_MODIFY_TID_DISABLE_TX;
 if (remove_queue)
  cmd.modify_mask |= STA_MODIFY_QUEUE_REMOVAL;
 cmd.tfd_queue_msk = cpu_to_le32(mvmsta->tfd_queue_msk);
 cmd.tid_disable_tx = cpu_to_le16(mvmsta->tid_disable_agg);

 rcu_read_unlock();

 /* Notify FW of queue removal from the STA queues */
 status = ADD_STA_SUCCESS;
 return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
        iwl_mvm_add_sta_cmd_size(mvm),
        &cmd, &status);
}

static int iwl_mvm_disable_txq(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
          int sta_id, u16 *queueptr, u8 tid)
{
 int queue = *queueptr;
 struct iwl_scd_txq_cfg_cmd cmd = {
  .scd_queue = queue,
  .action = SCD_CFG_DISABLE_QUEUE,
 };
 int ret;

 lockdep_assert_held(&mvm->mutex);

 if (iwl_mvm_has_new_tx_api(mvm)) {
  if (mvm->sta_remove_requires_queue_remove) {
   u32 cmd_id = WIDE_ID(DATA_PATH_GROUP,
          SCD_QUEUE_CONFIG_CMD);
   struct iwl_scd_queue_cfg_cmd remove_cmd = {
    .operation = cpu_to_le32(IWL_SCD_QUEUE_REMOVE),
    .u.remove.sta_mask = cpu_to_le32(BIT(sta_id)),
   };

   if (tid == IWL_MAX_TID_COUNT)
    tid = IWL_MGMT_TID;

   remove_cmd.u.remove.tid = cpu_to_le32(tid);

   ret = iwl_mvm_send_cmd_pdu(mvm, cmd_id, 0,
         sizeof(remove_cmd),
         &remove_cmd);
  } else {
   ret = 0;
  }

  iwl_trans_txq_free(mvm->trans, queue);
  *queueptr = IWL_MVM_INVALID_QUEUE;

  return ret;
 }

 if (WARN_ON(mvm->queue_info[queue].tid_bitmap == 0))
  return 0;

 mvm->queue_info[queue].tid_bitmap &= ~BIT(tid);

 cmd.action = mvm->queue_info[queue].tid_bitmap ?
  SCD_CFG_ENABLE_QUEUE : SCD_CFG_DISABLE_QUEUE;
 if (cmd.action == SCD_CFG_DISABLE_QUEUE)
  mvm->queue_info[queue].status = IWL_MVM_QUEUE_FREE;

 IWL_DEBUG_TX_QUEUES(mvm,
       "Disabling TXQ #%d tids=0x%x\n",
       queue,
       mvm->queue_info[queue].tid_bitmap);

 /* If the queue is still enabled - nothing left to do in this func */
 if (cmd.action == SCD_CFG_ENABLE_QUEUE)
  return 0;

 cmd.sta_id = mvm->queue_info[queue].ra_sta_id;
 cmd.tid = mvm->queue_info[queue].txq_tid;

 /* Make sure queue info is correct even though we overwrite it */
 WARN(mvm->queue_info[queue].tid_bitmap,
      "TXQ #%d info out-of-sync - tids=0x%x\n",
      queue, mvm->queue_info[queue].tid_bitmap);

 /* If we are here - the queue is freed and we can zero out these vals */
 mvm->queue_info[queue].tid_bitmap = 0;

 if (sta) {
  struct iwl_mvm_txq *mvmtxq =
   iwl_mvm_txq_from_tid(sta, tid);

  spin_lock_bh(&mvm->add_stream_lock);
  list_del_init(&mvmtxq->list);
  clear_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state);
  mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
  spin_unlock_bh(&mvm->add_stream_lock);
 }

 /* Regardless if this is a reserved TXQ for a STA - mark it as false */
 mvm->queue_info[queue].reserved = false;

 iwl_trans_txq_disable(mvm->trans, queue, false);
 ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0,
       sizeof(struct iwl_scd_txq_cfg_cmd), &cmd);

 if (ret)
  IWL_ERR(mvm, "Failed to disable queue %d (ret=%d)\n",
   queue, ret);
 return ret;
}

static int iwl_mvm_get_queue_agg_tids(struct iwl_mvm *mvm, int queue)
{
 struct ieee80211_sta *sta;
 struct iwl_mvm_sta *mvmsta;
 unsigned long tid_bitmap;
 unsigned long agg_tids = 0;
 u8 sta_id;
 int tid;

 lockdep_assert_held(&mvm->mutex);

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return -EINVAL;

 sta_id = mvm->queue_info[queue].ra_sta_id;
 tid_bitmap = mvm->queue_info[queue].tid_bitmap;

 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
     lockdep_is_held(&mvm->mutex));

 if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta)))
  return -EINVAL;

 mvmsta = iwl_mvm_sta_from_mac80211(sta);

 spin_lock_bh(&mvmsta->lock);
 for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
  if (mvmsta->tid_data[tid].state == IWL_AGG_ON)
   agg_tids |= BIT(tid);
 }
 spin_unlock_bh(&mvmsta->lock);

 return agg_tids;
}

/*
 * Remove a queue from a station's resources.
 * Note that this only marks as free. It DOESN'T delete a BA agreement, and
 * doesn't disable the queue
 */
static int iwl_mvm_remove_sta_queue_marking(struct iwl_mvm *mvm, int queue)
{
 struct ieee80211_sta *sta;
 struct iwl_mvm_sta *mvmsta;
 unsigned long tid_bitmap;
 unsigned long disable_agg_tids = 0;
 u8 sta_id;
 int tid;

 lockdep_assert_held(&mvm->mutex);

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return -EINVAL;

 sta_id = mvm->queue_info[queue].ra_sta_id;
 tid_bitmap = mvm->queue_info[queue].tid_bitmap;

 rcu_read_lock();

 sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);

 if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta))) {
  rcu_read_unlock();
  return 0;
 }

 mvmsta = iwl_mvm_sta_from_mac80211(sta);

 spin_lock_bh(&mvmsta->lock);
 /* Unmap MAC queues and TIDs from this queue */
 for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
  struct iwl_mvm_txq *mvmtxq =
   iwl_mvm_txq_from_tid(sta, tid);

  if (mvmsta->tid_data[tid].state == IWL_AGG_ON)
   disable_agg_tids |= BIT(tid);
  mvmsta->tid_data[tid].txq_id = IWL_MVM_INVALID_QUEUE;

  spin_lock_bh(&mvm->add_stream_lock);
  list_del_init(&mvmtxq->list);
  clear_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state);
  mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
  spin_unlock_bh(&mvm->add_stream_lock);
 }

 mvmsta->tfd_queue_msk &= ~BIT(queue); /* Don't use this queue anymore */
 spin_unlock_bh(&mvmsta->lock);

 rcu_read_unlock();

 /*
 * The TX path may have been using this TXQ_ID from the tid_data,
 * so make sure it's no longer running so that we can safely reuse
 * this TXQ later. We've set all the TIDs to IWL_MVM_INVALID_QUEUE
 * above, but nothing guarantees we've stopped using them. Thus,
 * without this, we could get to iwl_mvm_disable_txq() and remove
 * the queue while still sending frames to it.
 */
 synchronize_net();

 return disable_agg_tids;
}

static int iwl_mvm_free_inactive_queue(struct iwl_mvm *mvm, int queue,
           struct ieee80211_sta *old_sta,
           u8 new_sta_id)
{
 struct iwl_mvm_sta *mvmsta;
 u8 sta_id, tid;
 unsigned long disable_agg_tids = 0;
 bool same_sta;
 u16 queue_tmp = queue;
 int ret;

 lockdep_assert_held(&mvm->mutex);

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return -EINVAL;

 sta_id = mvm->queue_info[queue].ra_sta_id;
 tid = mvm->queue_info[queue].txq_tid;

 same_sta = sta_id == new_sta_id;

 mvmsta = iwl_mvm_sta_from_staid_protected(mvm, sta_id);
 if (WARN_ON(!mvmsta))
  return -EINVAL;

 disable_agg_tids = iwl_mvm_remove_sta_queue_marking(mvm, queue);
 /* Disable the queue */
 if (disable_agg_tids)
  iwl_mvm_invalidate_sta_queue(mvm, queue,
          disable_agg_tids, false);

 ret = iwl_mvm_disable_txq(mvm, old_sta, sta_id, &queue_tmp, tid);
 if (ret) {
  IWL_ERR(mvm,
   "Failed to free inactive queue %d (ret=%d)\n",
   queue, ret);

  return ret;
 }

 /* If TXQ is allocated to another STA, update removal in FW */
 if (!same_sta)
  iwl_mvm_invalidate_sta_queue(mvm, queue, 0, true);

 return 0;
}

static int iwl_mvm_get_shared_queue(struct iwl_mvm *mvm,
        unsigned long tfd_queue_mask, u8 ac)
{
 int queue = 0;
 u8 ac_to_queue[IEEE80211_NUM_ACS];
 int i;

 /*
 * This protects us against grabbing a queue that's being reconfigured
 * by the inactivity checker.
 */
 lockdep_assert_held(&mvm->mutex);

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return -EINVAL;

 memset(&ac_to_queue, IEEE80211_INVAL_HW_QUEUE, sizeof(ac_to_queue));

 /* See what ACs the existing queues for this STA have */
 for_each_set_bit(i, &tfd_queue_mask, IWL_MVM_DQA_MAX_DATA_QUEUE) {
  /* Only DATA queues can be shared */
  if (i < IWL_MVM_DQA_MIN_DATA_QUEUE &&
      i != IWL_MVM_DQA_BSS_CLIENT_QUEUE)
   continue;

  ac_to_queue[mvm->queue_info[i].mac80211_ac] = i;
 }

 /*
 * The queue to share is chosen only from DATA queues as follows (in
 * descending priority):
 * 1. An AC_BE queue
 * 2. Same AC queue
 * 3. Highest AC queue that is lower than new AC
 * 4. Any existing AC (there always is at least 1 DATA queue)
 */

 /* Priority 1: An AC_BE queue */
 if (ac_to_queue[IEEE80211_AC_BE] != IEEE80211_INVAL_HW_QUEUE)
  queue = ac_to_queue[IEEE80211_AC_BE];
 /* Priority 2: Same AC queue */
 else if (ac_to_queue[ac] != IEEE80211_INVAL_HW_QUEUE)
  queue = ac_to_queue[ac];
 /* Priority 3a: If new AC is VO and VI exists - use VI */
 else if (ac == IEEE80211_AC_VO &&
   ac_to_queue[IEEE80211_AC_VI] != IEEE80211_INVAL_HW_QUEUE)
  queue = ac_to_queue[IEEE80211_AC_VI];
 /* Priority 3b: No BE so only AC less than the new one is BK */
 else if (ac_to_queue[IEEE80211_AC_BK] != IEEE80211_INVAL_HW_QUEUE)
  queue = ac_to_queue[IEEE80211_AC_BK];
 /* Priority 4a: No BE nor BK - use VI if exists */
 else if (ac_to_queue[IEEE80211_AC_VI] != IEEE80211_INVAL_HW_QUEUE)
  queue = ac_to_queue[IEEE80211_AC_VI];
 /* Priority 4b: No BE, BK nor VI - use VO if exists */
 else if (ac_to_queue[IEEE80211_AC_VO] != IEEE80211_INVAL_HW_QUEUE)
  queue = ac_to_queue[IEEE80211_AC_VO];

 /* Make sure queue found (or not) is legal */
 if (!iwl_mvm_is_dqa_data_queue(mvm, queue) &&
     !iwl_mvm_is_dqa_mgmt_queue(mvm, queue) &&
     (queue != IWL_MVM_DQA_BSS_CLIENT_QUEUE)) {
  IWL_ERR(mvm, "No DATA queues available to share\n");
  return -ENOSPC;
 }

 return queue;
}

/* Re-configure the SCD for a queue that has already been configured */
static int iwl_mvm_reconfig_scd(struct iwl_mvm *mvm, int queue, int fifo,
    int sta_id, int tid, int frame_limit, u16 ssn)
{
 struct iwl_scd_txq_cfg_cmd cmd = {
  .scd_queue = queue,
  .action = SCD_CFG_ENABLE_QUEUE,
  .window = frame_limit,
  .sta_id = sta_id,
  .ssn = cpu_to_le16(ssn),
  .tx_fifo = fifo,
  .aggregate = (queue >= IWL_MVM_DQA_MIN_DATA_QUEUE ||
         queue == IWL_MVM_DQA_BSS_CLIENT_QUEUE),
  .tid = tid,
 };
 int ret;

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return -EINVAL;

 if (WARN(mvm->queue_info[queue].tid_bitmap == 0,
   "Trying to reconfig unallocated queue %d\n", queue))
  return -ENXIO;

 IWL_DEBUG_TX_QUEUES(mvm, "Reconfig SCD for TXQ #%d\n", queue);

 ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd);
 WARN_ONCE(ret, "Failed to re-configure queue %d on FIFO %d, ret=%d\n",
    queue, fifo, ret);

 return ret;
}

/*
 * If a given queue has a higher AC than the TID stream that is being compared
 * to, the queue needs to be redirected to the lower AC. This function does that
 * in such a case, otherwise - if no redirection required - it does nothing,
 * unless the %force param is true.
 */
static int iwl_mvm_redirect_queue(struct iwl_mvm *mvm, int queue, int tid,
      int ac, int ssn, unsigned int wdg_timeout,
      bool force, struct iwl_mvm_txq *txq)
{
 struct iwl_scd_txq_cfg_cmd cmd = {
  .scd_queue = queue,
  .action = SCD_CFG_DISABLE_QUEUE,
 };
 bool shared_queue;
 int ret;

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return -EINVAL;

 /*
 * If the AC is lower than current one - FIFO needs to be redirected to
 * the lowest one of the streams in the queue. Check if this is needed
 * here.
 * Notice that the enum ieee80211_ac_numbers is "flipped", so BK is with
 * value 3 and VO with value 0, so to check if ac X is lower than ac Y
 * we need to check if the numerical value of X is LARGER than of Y.
 */
 if (ac <= mvm->queue_info[queue].mac80211_ac && !force) {
  IWL_DEBUG_TX_QUEUES(mvm,
        "No redirection needed on TXQ #%d\n",
        queue);
  return 0;
 }

 cmd.sta_id = mvm->queue_info[queue].ra_sta_id;
 cmd.tx_fifo = iwl_mvm_ac_to_tx_fifo[mvm->queue_info[queue].mac80211_ac];
 cmd.tid = mvm->queue_info[queue].txq_tid;
 shared_queue = hweight16(mvm->queue_info[queue].tid_bitmap) > 1;

 IWL_DEBUG_TX_QUEUES(mvm, "Redirecting TXQ #%d to FIFO #%d\n",
       queue, iwl_mvm_ac_to_tx_fifo[ac]);

 /* Stop the queue and wait for it to empty */
 set_bit(IWL_MVM_TXQ_STATE_STOP_REDIRECT, &txq->state);

 ret = iwl_trans_wait_tx_queues_empty(mvm->trans, BIT(queue));
 if (ret) {
  IWL_ERR(mvm, "Error draining queue %d before reconfig\n",
   queue);
  ret = -EIO;
  goto out;
 }

 /* Before redirecting the queue we need to de-activate it */
 iwl_trans_txq_disable(mvm->trans, queue, false);
 ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd);
 if (ret)
  IWL_ERR(mvm, "Failed SCD disable TXQ %d (ret=%d)\n", queue,
   ret);

 /* Make sure the SCD wrptr is correctly set before reconfiguring */
 iwl_trans_txq_enable_cfg(mvm->trans, queue, ssn, NULL, wdg_timeout);

 /* Update the TID "owner" of the queue */
 mvm->queue_info[queue].txq_tid = tid;

 /* TODO: Work-around SCD bug when moving back by multiples of 0x40 */

 /* Redirect to lower AC */
 iwl_mvm_reconfig_scd(mvm, queue, iwl_mvm_ac_to_tx_fifo[ac],
        cmd.sta_id, tid, IWL_FRAME_LIMIT, ssn);

 /* Update AC marking of the queue */
 mvm->queue_info[queue].mac80211_ac = ac;

 /*
 * Mark queue as shared in transport if shared
 * Note this has to be done after queue enablement because enablement
 * can also set this value, and there is no indication there to shared
 * queues
 */
 if (shared_queue)
  iwl_trans_txq_set_shared_mode(mvm->trans, queue, true);

out:
 /* Continue using the queue */
 clear_bit(IWL_MVM_TXQ_STATE_STOP_REDIRECT, &txq->state);

 return ret;
}

static int iwl_mvm_find_free_queue(struct iwl_mvm *mvm, u8 sta_id,
       u8 minq, u8 maxq)
{
 int i;

 lockdep_assert_held(&mvm->mutex);

 if (WARN(maxq >= mvm->trans->mac_cfg->base->num_of_queues,
   "max queue %d >= num_of_queues (%d)", maxq,
   mvm->trans->mac_cfg->base->num_of_queues))
  maxq = mvm->trans->mac_cfg->base->num_of_queues - 1;

 /* This should not be hit with new TX path */
 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return -ENOSPC;

 /* Start by looking for a free queue */
 for (i = minq; i <= maxq; i++)
  if (mvm->queue_info[i].tid_bitmap == 0 &&
      mvm->queue_info[i].status == IWL_MVM_QUEUE_FREE)
   return i;

 return -ENOSPC;
}

static int iwl_mvm_get_queue_size(struct ieee80211_sta *sta)
{
 int max_size = IWL_DEFAULT_QUEUE_SIZE;
 unsigned int link_id;

 /* this queue isn't used for traffic (cab_queue) */
 if (!sta)
  return IWL_MGMT_QUEUE_SIZE;

 rcu_read_lock();

 for (link_id = 0; link_id < ARRAY_SIZE(sta->link); link_id++) {
  struct ieee80211_link_sta *link =
   rcu_dereference(sta->link[link_id]);

  if (!link)
   continue;

  /* support for 512 ba size */
  if (link->eht_cap.has_eht &&
      max_size < IWL_DEFAULT_QUEUE_SIZE_EHT)
   max_size = IWL_DEFAULT_QUEUE_SIZE_EHT;

  /* support for 256 ba size */
  if (link->he_cap.has_he &&
      max_size < IWL_DEFAULT_QUEUE_SIZE_HE)
   max_size = IWL_DEFAULT_QUEUE_SIZE_HE;
 }

 rcu_read_unlock();
 return max_size;
}

int iwl_mvm_tvqm_enable_txq(struct iwl_mvm *mvm,
       struct ieee80211_sta *sta,
       u8 sta_id, u8 tid, unsigned int timeout)
{
 int queue, size;
 u32 sta_mask = 0;

 if (tid == IWL_MAX_TID_COUNT) {
  tid = IWL_MGMT_TID;
  size = max_t(u32, IWL_MGMT_QUEUE_SIZE,
        mvm->trans->mac_cfg->base->min_txq_size);
 } else {
  size = iwl_mvm_get_queue_size(sta);
 }

 if (sta) {
  struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
  struct ieee80211_link_sta *link_sta;
  unsigned int link_id;

  rcu_read_lock();
  for_each_sta_active_link(mvmsta->vif, sta, link_sta, link_id) {
   struct iwl_mvm_link_sta *link =
    rcu_dereference_protected(mvmsta->link[link_id],
         lockdep_is_held(&mvm->mutex));

   if (!link)
    continue;

   sta_mask |= BIT(link->sta_id);
  }
  rcu_read_unlock();
 } else {
  sta_mask |= BIT(sta_id);
 }

 if (!sta_mask)
  return -EINVAL;

 queue = iwl_trans_txq_alloc(mvm->trans, 0, sta_mask,
        tid, size, timeout);

 if (queue >= 0)
  IWL_DEBUG_TX_QUEUES(mvm,
        "Enabling TXQ #%d for sta mask 0x%x tid %d\n",
        queue, sta_mask, tid);

 return queue;
}

static int iwl_mvm_sta_alloc_queue_tvqm(struct iwl_mvm *mvm,
     struct ieee80211_sta *sta, u8 ac,
     int tid)
{
 struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 struct iwl_mvm_txq *mvmtxq =
  iwl_mvm_txq_from_tid(sta, tid);
 unsigned int wdg_timeout =
  iwl_mvm_get_wd_timeout(mvm, mvmsta->vif);
 int queue = -1;

 lockdep_assert_held(&mvm->mutex);

 IWL_DEBUG_TX_QUEUES(mvm,
       "Allocating queue for sta %d on tid %d\n",
       mvmsta->deflink.sta_id, tid);
 queue = iwl_mvm_tvqm_enable_txq(mvm, sta, mvmsta->deflink.sta_id,
     tid, wdg_timeout);
 if (queue < 0)
  return queue;

 mvmtxq->txq_id = queue;
 mvm->tvqm_info[queue].txq_tid = tid;
 mvm->tvqm_info[queue].sta_id = mvmsta->deflink.sta_id;

 IWL_DEBUG_TX_QUEUES(mvm, "Allocated queue is %d\n", queue);

 spin_lock_bh(&mvmsta->lock);
 mvmsta->tid_data[tid].txq_id = queue;
 spin_unlock_bh(&mvmsta->lock);

 return 0;
}

static bool iwl_mvm_update_txq_mapping(struct iwl_mvm *mvm,
           struct ieee80211_sta *sta,
           int queue, u8 sta_id, u8 tid)
{
 bool enable_queue = true;

 /* Make sure this TID isn't already enabled */
 if (mvm->queue_info[queue].tid_bitmap & BIT(tid)) {
  IWL_ERR(mvm, "Trying to enable TXQ %d with existing TID %d\n",
   queue, tid);
  return false;
 }

 /* Update mappings and refcounts */
 if (mvm->queue_info[queue].tid_bitmap)
  enable_queue = false;

 mvm->queue_info[queue].tid_bitmap |= BIT(tid);
 mvm->queue_info[queue].ra_sta_id = sta_id;

 if (enable_queue) {
  if (tid != IWL_MAX_TID_COUNT)
   mvm->queue_info[queue].mac80211_ac =
    tid_to_mac80211_ac[tid];
  else
   mvm->queue_info[queue].mac80211_ac = IEEE80211_AC_VO;

  mvm->queue_info[queue].txq_tid = tid;
 }

 if (sta) {
  struct iwl_mvm_txq *mvmtxq =
   iwl_mvm_txq_from_tid(sta, tid);

  mvmtxq->txq_id = queue;
 }

 IWL_DEBUG_TX_QUEUES(mvm,
       "Enabling TXQ #%d tids=0x%x\n",
       queue, mvm->queue_info[queue].tid_bitmap);

 return enable_queue;
}

static bool iwl_mvm_enable_txq(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
          int queue, u16 ssn,
          const struct iwl_trans_txq_scd_cfg *cfg,
          unsigned int wdg_timeout)
{
 struct iwl_scd_txq_cfg_cmd cmd = {
  .scd_queue = queue,
  .action = SCD_CFG_ENABLE_QUEUE,
  .window = cfg->frame_limit,
  .sta_id = cfg->sta_id,
  .ssn = cpu_to_le16(ssn),
  .tx_fifo = cfg->fifo,
  .aggregate = cfg->aggregate,
  .tid = cfg->tid,
 };
 bool inc_ssn;

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return false;

 /* Send the enabling command if we need to */
 if (!iwl_mvm_update_txq_mapping(mvm, sta, queue, cfg->sta_id, cfg->tid))
  return false;

 inc_ssn = iwl_trans_txq_enable_cfg(mvm->trans, queue, ssn,
        NULL, wdg_timeout);
 if (inc_ssn)
  le16_add_cpu(&cmd.ssn, 1);

 WARN(iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd),
      "Failed to configure queue %d on FIFO %d\n", queue, cfg->fifo);

 return inc_ssn;
}

static void iwl_mvm_change_queue_tid(struct iwl_mvm *mvm, int queue)
{
 struct iwl_scd_txq_cfg_cmd cmd = {
  .scd_queue = queue,
  .action = SCD_CFG_UPDATE_QUEUE_TID,
 };
 int tid;
 unsigned long tid_bitmap;
 int ret;

 lockdep_assert_held(&mvm->mutex);

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return;

 tid_bitmap = mvm->queue_info[queue].tid_bitmap;

 if (WARN(!tid_bitmap, "TXQ %d has no tids assigned to it\n", queue))
  return;

 /* Find any TID for queue */
 tid = find_first_bit(&tid_bitmap, IWL_MAX_TID_COUNT + 1);
 cmd.tid = tid;
 cmd.tx_fifo = iwl_mvm_ac_to_tx_fifo[tid_to_mac80211_ac[tid]];

 ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd);
 if (ret) {
  IWL_ERR(mvm, "Failed to update owner of TXQ %d (ret=%d)\n",
   queue, ret);
  return;
 }

 mvm->queue_info[queue].txq_tid = tid;
 IWL_DEBUG_TX_QUEUES(mvm, "Changed TXQ %d ownership to tid %d\n",
       queue, tid);
}

static void iwl_mvm_unshare_queue(struct iwl_mvm *mvm, int queue)
{
 struct ieee80211_sta *sta;
 struct iwl_mvm_sta *mvmsta;
 u8 sta_id;
 int tid = -1;
 unsigned long tid_bitmap;
 unsigned int wdg_timeout;
 int ssn;
 int ret = true;

 /* queue sharing is disabled on new TX path */
 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return;

 lockdep_assert_held(&mvm->mutex);

 sta_id = mvm->queue_info[queue].ra_sta_id;
 tid_bitmap = mvm->queue_info[queue].tid_bitmap;

 /* Find TID for queue, and make sure it is the only one on the queue */
 tid = find_first_bit(&tid_bitmap, IWL_MAX_TID_COUNT + 1);
 if (tid_bitmap != BIT(tid)) {
  IWL_ERR(mvm, "Failed to unshare q %d, active tids=0x%lx\n",
   queue, tid_bitmap);
  return;
 }

 IWL_DEBUG_TX_QUEUES(mvm, "Unsharing TXQ %d, keeping tid %d\n", queue,
       tid);

 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
     lockdep_is_held(&mvm->mutex));

 if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta)))
  return;

 mvmsta = iwl_mvm_sta_from_mac80211(sta);
 wdg_timeout = iwl_mvm_get_wd_timeout(mvm, mvmsta->vif);

 ssn = IEEE80211_SEQ_TO_SN(mvmsta->tid_data[tid].seq_number);

 ret = iwl_mvm_redirect_queue(mvm, queue, tid,
         tid_to_mac80211_ac[tid], ssn,
         wdg_timeout, true,
         iwl_mvm_txq_from_tid(sta, tid));
 if (ret) {
  IWL_ERR(mvm, "Failed to redirect TXQ %d\n", queue);
  return;
 }

 /* If aggs should be turned back on - do it */
 if (mvmsta->tid_data[tid].state == IWL_AGG_ON) {
  struct iwl_mvm_add_sta_cmd cmd = {0};

  mvmsta->tid_disable_agg &= ~BIT(tid);

  cmd.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color);
  cmd.sta_id = mvmsta->deflink.sta_id;
  cmd.add_modify = STA_MODE_MODIFY;
  cmd.modify_mask = STA_MODIFY_TID_DISABLE_TX;
  cmd.tfd_queue_msk = cpu_to_le32(mvmsta->tfd_queue_msk);
  cmd.tid_disable_tx = cpu_to_le16(mvmsta->tid_disable_agg);

  ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
        iwl_mvm_add_sta_cmd_size(mvm), &cmd);
  if (!ret) {
   IWL_DEBUG_TX_QUEUES(mvm,
         "TXQ #%d is now aggregated again\n",
         queue);

   /* Mark queue intenally as aggregating again */
   iwl_trans_txq_set_shared_mode(mvm->trans, queue, false);
  }
 }

 mvm->queue_info[queue].status = IWL_MVM_QUEUE_READY;
}

/*
 * Remove inactive TIDs of a given queue.
 * If all queue TIDs are inactive - mark the queue as inactive
 * If only some the queue TIDs are inactive - unmap them from the queue
 *
 * Returns %true if all TIDs were removed and the queue could be reused.
 */
static bool iwl_mvm_remove_inactive_tids(struct iwl_mvm *mvm,
      struct iwl_mvm_sta *mvmsta, int queue,
      unsigned long tid_bitmap,
      unsigned long *unshare_queues,
      unsigned long *changetid_queues)
{
 unsigned int tid;

 lockdep_assert_held(&mvmsta->lock);
 lockdep_assert_held(&mvm->mutex);

 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return false;

 /* Go over all non-active TIDs, incl. IWL_MAX_TID_COUNT (for mgmt) */
 for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
  /* If some TFDs are still queued - don't mark TID as inactive */
  if (iwl_mvm_tid_queued(mvm, &mvmsta->tid_data[tid]))
   tid_bitmap &= ~BIT(tid);

  /* Don't mark as inactive any TID that has an active BA */
  if (mvmsta->tid_data[tid].state != IWL_AGG_OFF)
   tid_bitmap &= ~BIT(tid);
 }

 /* If all TIDs in the queue are inactive - return it can be reused */
 if (tid_bitmap == mvm->queue_info[queue].tid_bitmap) {
  IWL_DEBUG_TX_QUEUES(mvm, "Queue %d is inactive\n", queue);
  return true;
 }

 /*
 * If we are here, this is a shared queue and not all TIDs timed-out.
 * Remove the ones that did.
 */
 for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
  u16 q_tid_bitmap;

  mvmsta->tid_data[tid].txq_id = IWL_MVM_INVALID_QUEUE;
  mvm->queue_info[queue].tid_bitmap &= ~BIT(tid);

  q_tid_bitmap = mvm->queue_info[queue].tid_bitmap;

  /*
 * We need to take into account a situation in which a TXQ was
 * allocated to TID x, and then turned shared by adding TIDs y
 * and z. If TID x becomes inactive and is removed from the TXQ,
 * ownership must be given to one of the remaining TIDs.
 * This is mainly because if TID x continues - a new queue can't
 * be allocated for it as long as it is an owner of another TXQ.
 *
 * Mark this queue in the right bitmap, we'll send the command
 * to the firmware later.
 */
  if (!(q_tid_bitmap & BIT(mvm->queue_info[queue].txq_tid)))
   set_bit(queue, changetid_queues);

  IWL_DEBUG_TX_QUEUES(mvm,
        "Removing inactive TID %d from shared Q:%d\n",
        tid, queue);
 }

 IWL_DEBUG_TX_QUEUES(mvm,
       "TXQ #%d left with tid bitmap 0x%x\n", queue,
       mvm->queue_info[queue].tid_bitmap);

 /*
 * There may be different TIDs with the same mac queues, so make
 * sure all TIDs have existing corresponding mac queues enabled
 */
 tid_bitmap = mvm->queue_info[queue].tid_bitmap;

 /* If the queue is marked as shared - "unshare" it */
 if (hweight16(mvm->queue_info[queue].tid_bitmap) == 1 &&
     mvm->queue_info[queue].status == IWL_MVM_QUEUE_SHARED) {
  IWL_DEBUG_TX_QUEUES(mvm, "Marking Q:%d for reconfig\n",
        queue);
  set_bit(queue, unshare_queues);
 }

 return false;
}

/*
 * Check for inactivity - this includes checking if any queue
 * can be unshared and finding one (and only one) that can be
 * reused.
 * This function is also invoked as a sort of clean-up task,
 * in which case @alloc_for_sta is IWL_INVALID_STA.
 *
 * Returns the queue number, or -ENOSPC.
 */
static int iwl_mvm_inactivity_check(struct iwl_mvm *mvm, u8 alloc_for_sta)
{
 unsigned long now = jiffies;
 unsigned long unshare_queues = 0;
 unsigned long changetid_queues = 0;
 int i, ret, free_queue = -ENOSPC;
 struct ieee80211_sta *queue_owner  = NULL;

 lockdep_assert_held(&mvm->mutex);

 if (iwl_mvm_has_new_tx_api(mvm))
  return -ENOSPC;

 rcu_read_lock();

 /* we skip the CMD queue below by starting at 1 */
 BUILD_BUG_ON(IWL_MVM_DQA_CMD_QUEUE != 0);

 for (i = 1; i < IWL_MAX_HW_QUEUES; i++) {
  struct ieee80211_sta *sta;
  struct iwl_mvm_sta *mvmsta;
  u8 sta_id;
  int tid;
  unsigned long inactive_tid_bitmap = 0;
  unsigned long queue_tid_bitmap;

  queue_tid_bitmap = mvm->queue_info[i].tid_bitmap;
  if (!queue_tid_bitmap)
   continue;

  /* If TXQ isn't in active use anyway - nothing to do here... */
  if (mvm->queue_info[i].status != IWL_MVM_QUEUE_READY &&
      mvm->queue_info[i].status != IWL_MVM_QUEUE_SHARED)
   continue;

  /* Check to see if there are inactive TIDs on this queue */
  for_each_set_bit(tid, &queue_tid_bitmap,
     IWL_MAX_TID_COUNT + 1) {
   if (time_after(mvm->queue_info[i].last_frame_time[tid] +
           IWL_MVM_DQA_QUEUE_TIMEOUT, now))
    continue;

   inactive_tid_bitmap |= BIT(tid);
  }

  /* If all TIDs are active - finish check on this queue */
  if (!inactive_tid_bitmap)
   continue;

  /*
 * If we are here - the queue hadn't been served recently and is
 * in use
 */

  sta_id = mvm->queue_info[i].ra_sta_id;
  sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);

  /*
 * If the STA doesn't exist anymore, it isn't an error. It could
 * be that it was removed since getting the queues, and in this
 * case it should've inactivated its queues anyway.
 */
  if (IS_ERR_OR_NULL(sta))
   continue;

  mvmsta = iwl_mvm_sta_from_mac80211(sta);

  spin_lock_bh(&mvmsta->lock);
  ret = iwl_mvm_remove_inactive_tids(mvm, mvmsta, i,
         inactive_tid_bitmap,
         &unshare_queues,
         &changetid_queues);
  if (ret && free_queue < 0) {
   queue_owner = sta;
   free_queue = i;
  }
  /* only unlock sta lock - we still need the queue info lock */
  spin_unlock_bh(&mvmsta->lock);
 }


 /* Reconfigure queues requiring reconfiguation */
 for_each_set_bit(i, &unshare_queues, IWL_MAX_HW_QUEUES)
  iwl_mvm_unshare_queue(mvm, i);
 for_each_set_bit(i, &changetid_queues, IWL_MAX_HW_QUEUES)
  iwl_mvm_change_queue_tid(mvm, i);

 rcu_read_unlock();

 if (free_queue >= 0 && alloc_for_sta != IWL_INVALID_STA) {
  ret = iwl_mvm_free_inactive_queue(mvm, free_queue, queue_owner,
        alloc_for_sta);
  if (ret)
   return ret;
 }

 return free_queue;
}

static int iwl_mvm_sta_alloc_queue(struct iwl_mvm *mvm,
       struct ieee80211_sta *sta, u8 ac, int tid)
{
 struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 struct iwl_trans_txq_scd_cfg cfg = {
  .fifo = iwl_mvm_mac_ac_to_tx_fifo(mvm, ac),
  .sta_id = mvmsta->deflink.sta_id,
  .tid = tid,
  .frame_limit = IWL_FRAME_LIMIT,
 };
 unsigned int wdg_timeout =
  iwl_mvm_get_wd_timeout(mvm, mvmsta->vif);
 int queue = -1;
 u16 queue_tmp;
 unsigned long disable_agg_tids = 0;
 enum iwl_mvm_agg_state queue_state;
 bool shared_queue = false, inc_ssn;
 int ssn;
 unsigned long tfd_queue_mask;
 int ret;

 lockdep_assert_held(&mvm->mutex);

 if (iwl_mvm_has_new_tx_api(mvm))
  return iwl_mvm_sta_alloc_queue_tvqm(mvm, sta, ac, tid);

 spin_lock_bh(&mvmsta->lock);
 tfd_queue_mask = mvmsta->tfd_queue_msk;
 ssn = IEEE80211_SEQ_TO_SN(mvmsta->tid_data[tid].seq_number);
 spin_unlock_bh(&mvmsta->lock);

 if (tid == IWL_MAX_TID_COUNT) {
  queue = iwl_mvm_find_free_queue(mvm, mvmsta->deflink.sta_id,
      IWL_MVM_DQA_MIN_MGMT_QUEUE,
      IWL_MVM_DQA_MAX_MGMT_QUEUE);
  if (queue >= IWL_MVM_DQA_MIN_MGMT_QUEUE)
   IWL_DEBUG_TX_QUEUES(mvm, "Found free MGMT queue #%d\n",
         queue);

  /* If no such queue is found, we'll use a DATA queue instead */
 }

 if ((queue < 0 && mvmsta->reserved_queue != IEEE80211_INVAL_HW_QUEUE) &&
     (mvm->queue_info[mvmsta->reserved_queue].status ==
   IWL_MVM_QUEUE_RESERVED)) {
  queue = mvmsta->reserved_queue;
  mvm->queue_info[queue].reserved = true;
  IWL_DEBUG_TX_QUEUES(mvm, "Using reserved queue #%d\n", queue);
 }

 if (queue < 0)
  queue = iwl_mvm_find_free_queue(mvm, mvmsta->deflink.sta_id,
      IWL_MVM_DQA_MIN_DATA_QUEUE,
      IWL_MVM_DQA_MAX_DATA_QUEUE);
 if (queue < 0) {
  /* try harder - perhaps kill an inactive queue */
  queue = iwl_mvm_inactivity_check(mvm, mvmsta->deflink.sta_id);
 }

 /* No free queue - we'll have to share */
 if (queue <= 0) {
  queue = iwl_mvm_get_shared_queue(mvm, tfd_queue_mask, ac);
  if (queue > 0) {
   shared_queue = true;
   mvm->queue_info[queue].status = IWL_MVM_QUEUE_SHARED;
  }
 }

 /*
 * Mark TXQ as ready, even though it hasn't been fully configured yet,
 * to make sure no one else takes it.
 * This will allow avoiding re-acquiring the lock at the end of the
 * configuration. On error we'll mark it back as free.
 */
 if (queue > 0 && !shared_queue)
  mvm->queue_info[queue].status = IWL_MVM_QUEUE_READY;

 /* This shouldn't happen - out of queues */
 if (WARN_ON(queue <= 0)) {
  IWL_ERR(mvm, "No available queues for tid %d on sta_id %d\n",
   tid, cfg.sta_id);
  return queue;
 }

 /*
 * Actual en/disablement of aggregations is through the ADD_STA HCMD,
 * but for configuring the SCD to send A-MPDUs we need to mark the queue
 * as aggregatable.
 * Mark all DATA queues as allowing to be aggregated at some point
 */
 cfg.aggregate = (queue >= IWL_MVM_DQA_MIN_DATA_QUEUE ||
    queue == IWL_MVM_DQA_BSS_CLIENT_QUEUE);

 IWL_DEBUG_TX_QUEUES(mvm,
       "Allocating %squeue #%d to sta %d on tid %d\n",
       shared_queue ? "shared " : "", queue,
       mvmsta->deflink.sta_id, tid);

 if (shared_queue) {
  /* Disable any open aggs on this queue */
  disable_agg_tids = iwl_mvm_get_queue_agg_tids(mvm, queue);

  if (disable_agg_tids) {
   IWL_DEBUG_TX_QUEUES(mvm, "Disabling aggs on queue %d\n",
         queue);
   iwl_mvm_invalidate_sta_queue(mvm, queue,
           disable_agg_tids, false);
  }
 }

 inc_ssn = iwl_mvm_enable_txq(mvm, sta, queue, ssn, &cfg, wdg_timeout);

 /*
 * Mark queue as shared in transport if shared
 * Note this has to be done after queue enablement because enablement
 * can also set this value, and there is no indication there to shared
 * queues
 */
 if (shared_queue)
  iwl_trans_txq_set_shared_mode(mvm->trans, queue, true);

 spin_lock_bh(&mvmsta->lock);
 /*
 * This looks racy, but it is not. We have only one packet for
 * this ra/tid in our Tx path since we stop the Qdisc when we
 * need to allocate a new TFD queue.
 */
 if (inc_ssn) {
  mvmsta->tid_data[tid].seq_number += 0x10;
  ssn = (ssn + 1) & IEEE80211_SCTL_SEQ;
 }
 mvmsta->tid_data[tid].txq_id = queue;
 mvmsta->tfd_queue_msk |= BIT(queue);
 queue_state = mvmsta->tid_data[tid].state;

 if (mvmsta->reserved_queue == queue)
  mvmsta->reserved_queue = IEEE80211_INVAL_HW_QUEUE;
 spin_unlock_bh(&mvmsta->lock);

 if (!shared_queue) {
  ret = iwl_mvm_sta_send_to_fw(mvm, sta, true, STA_MODIFY_QUEUES);
  if (ret)
   goto out_err;

  /* If we need to re-enable aggregations... */
  if (queue_state == IWL_AGG_ON) {
   ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
   if (ret)
    goto out_err;
  }
 } else {
  /* Redirect queue, if needed */
  ret = iwl_mvm_redirect_queue(mvm, queue, tid, ac, ssn,
          wdg_timeout, false,
          iwl_mvm_txq_from_tid(sta, tid));
  if (ret)
   goto out_err;
 }

 return 0;

out_err:
 queue_tmp = queue;
 iwl_mvm_disable_txq(mvm, sta, mvmsta->deflink.sta_id, &queue_tmp, tid);

 return ret;
}

int iwl_mvm_sta_ensure_queue(struct iwl_mvm *mvm,
        struct ieee80211_txq *txq)
{
 struct iwl_mvm_txq *mvmtxq = iwl_mvm_txq_from_mac80211(txq);
 int ret = -EINVAL;

 lockdep_assert_held(&mvm->mutex);

 if (likely(test_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state)) ||
     !txq->sta) {
  return 0;
 }

 if (!iwl_mvm_sta_alloc_queue(mvm, txq->sta, txq->ac, txq->tid)) {
  set_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state);
  ret = 0;
 }

 local_bh_disable();
 spin_lock(&mvm->add_stream_lock);
 if (!list_empty(&mvmtxq->list))
  list_del_init(&mvmtxq->list);
 spin_unlock(&mvm->add_stream_lock);
 local_bh_enable();

 return ret;
}

void iwl_mvm_add_new_dqa_stream_wk(struct work_struct *wk)
{
 struct iwl_mvm *mvm = container_of(wk, struct iwl_mvm,
        add_stream_wk);

 guard(mvm)(mvm);

 /* will reschedule to run after restart */
 if (test_bit(IWL_MVM_STATUS_HW_RESTART_REQUESTED, &mvm->status) ||
     test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
  return;

 iwl_mvm_inactivity_check(mvm, IWL_INVALID_STA);

 while (!list_empty(&mvm->add_stream_txqs)) {
  struct iwl_mvm_txq *mvmtxq;
  struct ieee80211_txq *txq;
  u8 tid;

  mvmtxq = list_first_entry(&mvm->add_stream_txqs,
       struct iwl_mvm_txq, list);

  txq = container_of((void *)mvmtxq, struct ieee80211_txq,
       drv_priv);
  tid = txq->tid;
  if (tid == IEEE80211_NUM_TIDS)
   tid = IWL_MAX_TID_COUNT;

  /*
 * We can't really do much here, but if this fails we can't
 * transmit anyway - so just don't transmit the frame etc.
 * and let them back up ... we've tried our best to allocate
 * a queue in the function itself.
 */
  if (iwl_mvm_sta_alloc_queue(mvm, txq->sta, txq->ac, tid)) {
   spin_lock_bh(&mvm->add_stream_lock);
   list_del_init(&mvmtxq->list);
   spin_unlock_bh(&mvm->add_stream_lock);
   continue;
  }

  /* now we're ready, any remaining races/concurrency will be
 * handled in iwl_mvm_mac_itxq_xmit()
 */
  set_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state);

  local_bh_disable();
  spin_lock(&mvm->add_stream_lock);
  list_del_init(&mvmtxq->list);
  spin_unlock(&mvm->add_stream_lock);

  iwl_mvm_mac_itxq_xmit(mvm->hw, txq);
  local_bh_enable();
 }
}

static int iwl_mvm_reserve_sta_stream(struct iwl_mvm *mvm,
          struct ieee80211_sta *sta,
          enum nl80211_iftype vif_type)
{
 struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 int queue;

 /* queue reserving is disabled on new TX path */
 if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
  return 0;

 /* run the general cleanup/unsharing of queues */
 iwl_mvm_inactivity_check(mvm, IWL_INVALID_STA);

 /* Make sure we have free resources for this STA */
 if (vif_type == NL80211_IFTYPE_STATION && !sta->tdls &&
     !mvm->queue_info[IWL_MVM_DQA_BSS_CLIENT_QUEUE].tid_bitmap &&
     (mvm->queue_info[IWL_MVM_DQA_BSS_CLIENT_QUEUE].status ==
      IWL_MVM_QUEUE_FREE))
  queue = IWL_MVM_DQA_BSS_CLIENT_QUEUE;
 else
  queue = iwl_mvm_find_free_queue(mvm, mvmsta->deflink.sta_id,
      IWL_MVM_DQA_MIN_DATA_QUEUE,
      IWL_MVM_DQA_MAX_DATA_QUEUE);
 if (queue < 0) {
  /* try again - this time kick out a queue if needed */
  queue = iwl_mvm_inactivity_check(mvm, mvmsta->deflink.sta_id);
  if (queue < 0) {
   IWL_ERR(mvm, "No available queues for new station\n");
   return -ENOSPC;
  }
 }
 mvm->queue_info[queue].status = IWL_MVM_QUEUE_RESERVED;

 mvmsta->reserved_queue = queue;

 IWL_DEBUG_TX_QUEUES(mvm, "Reserving data queue #%d for sta_id %d\n",
       queue, mvmsta->deflink.sta_id);

 return 0;
}

/*
 * In DQA mode, after a HW restart the queues should be allocated as before, in
 * order to avoid race conditions when there are shared queues. This function
 * does the re-mapping and queue allocation.
 *
 * Note that re-enabling aggregations isn't done in this function.
 */
void iwl_mvm_realloc_queues_after_restart(struct iwl_mvm *mvm,
       struct ieee80211_sta *sta)
{
 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 unsigned int wdg =
  iwl_mvm_get_wd_timeout(mvm, mvm_sta->vif);
 int i;
 struct iwl_trans_txq_scd_cfg cfg = {
  .sta_id = mvm_sta->deflink.sta_id,
  .frame_limit = IWL_FRAME_LIMIT,
 };

 /* Make sure reserved queue is still marked as such (if allocated) */
 if (mvm_sta->reserved_queue != IEEE80211_INVAL_HW_QUEUE)
  mvm->queue_info[mvm_sta->reserved_queue].status =
   IWL_MVM_QUEUE_RESERVED;

 for (i = 0; i <= IWL_MAX_TID_COUNT; i++) {
  struct iwl_mvm_tid_data *tid_data = &mvm_sta->tid_data[i];
  int txq_id = tid_data->txq_id;
  int ac;

  if (txq_id == IWL_MVM_INVALID_QUEUE)
   continue;

  ac = tid_to_mac80211_ac[i];

  if (iwl_mvm_has_new_tx_api(mvm)) {
   IWL_DEBUG_TX_QUEUES(mvm,
         "Re-mapping sta %d tid %d\n",
         mvm_sta->deflink.sta_id, i);
   txq_id = iwl_mvm_tvqm_enable_txq(mvm, sta,
        mvm_sta->deflink.sta_id,
        i, wdg);
   /*
 * on failures, just set it to IWL_MVM_INVALID_QUEUE
 * to try again later, we have no other good way of
 * failing here
 */
   if (txq_id < 0)
    txq_id = IWL_MVM_INVALID_QUEUE;
   tid_data->txq_id = txq_id;

   /*
 * Since we don't set the seq number after reset, and HW
 * sets it now, FW reset will cause the seq num to start
 * at 0 again, so driver will need to update it
 * internally as well, so it keeps in sync with real val
 */
   tid_data->seq_number = 0;
  } else {
   u16 seq = IEEE80211_SEQ_TO_SN(tid_data->seq_number);

   cfg.tid = i;
   cfg.fifo = iwl_mvm_mac_ac_to_tx_fifo(mvm, ac);
   cfg.aggregate = (txq_id >= IWL_MVM_DQA_MIN_DATA_QUEUE ||
      txq_id ==
      IWL_MVM_DQA_BSS_CLIENT_QUEUE);

   IWL_DEBUG_TX_QUEUES(mvm,
         "Re-mapping sta %d tid %d to queue %d\n",
         mvm_sta->deflink.sta_id, i,
         txq_id);

   iwl_mvm_enable_txq(mvm, sta, txq_id, seq, &cfg, wdg);
   mvm->queue_info[txq_id].status = IWL_MVM_QUEUE_READY;
  }
 }
}

static int iwl_mvm_add_int_sta_common(struct iwl_mvm *mvm,
          struct iwl_mvm_int_sta *sta,
          const u8 *addr,
          u16 mac_id, u16 color)
{
 struct iwl_mvm_add_sta_cmd cmd;
 int ret;
 u32 status = ADD_STA_SUCCESS;

 lockdep_assert_held(&mvm->mutex);

 memset(&cmd, 0, sizeof(cmd));
 cmd.sta_id = sta->sta_id;

 if (iwl_mvm_has_new_station_api(mvm->fw) &&
     sta->type == IWL_STA_AUX_ACTIVITY)
  cmd.mac_id_n_color = cpu_to_le32(mac_id);
 else
  cmd.mac_id_n_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mac_id,
             color));

 if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE))
  cmd.station_type = sta->type;

 if (!iwl_mvm_has_new_tx_api(mvm))
  cmd.tfd_queue_msk = cpu_to_le32(sta->tfd_queue_msk);
 cmd.tid_disable_tx = cpu_to_le16(0xffff);

 if (addr)
  memcpy(cmd.addr, addr, ETH_ALEN);

 ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
       iwl_mvm_add_sta_cmd_size(mvm),
       &cmd, &status);
 if (ret)
  return ret;

 switch (status & IWL_ADD_STA_STATUS_MASK) {
 case ADD_STA_SUCCESS:
  IWL_DEBUG_INFO(mvm, "Internal station added.\n");
  return 0;
 default:
  ret = -EIO;
  IWL_ERR(mvm, "Add internal station failed, status=0x%x\n",
   status);
  break;
 }
 return ret;
}

/* Initialize driver data of a new sta */
int iwl_mvm_sta_init(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
       struct ieee80211_sta *sta, int sta_id, u8 sta_type)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 struct iwl_mvm_rxq_dup_data *dup_data;
 int i, ret = 0;

 lockdep_assert_held(&mvm->mutex);

 mvm_sta->mac_id_n_color = FW_CMD_ID_AND_COLOR(mvmvif->id,
            mvmvif->color);
 mvm_sta->vif = vif;

 /* for MLD sta_id(s) should be allocated for each link before calling
 * this function
 */
 if (!mvm->mld_api_is_used) {
  if (WARN_ON(sta_id == IWL_INVALID_STA))
   return -EINVAL;

  mvm_sta->deflink.sta_id = sta_id;
  rcu_assign_pointer(mvm_sta->link[0], &mvm_sta->deflink);

  if (!mvm->trans->mac_cfg->gen2)
   mvm_sta->deflink.lq_sta.rs_drv.pers.max_agg_bufsize =
    LINK_QUAL_AGG_FRAME_LIMIT_DEF;
  else
   mvm_sta->deflink.lq_sta.rs_drv.pers.max_agg_bufsize =
    LINK_QUAL_AGG_FRAME_LIMIT_GEN2_DEF;
 }

 mvm_sta->tt_tx_protection = false;
 mvm_sta->sta_type = sta_type;

 mvm_sta->tid_disable_agg = 0xffff; /* No aggs at first */

 for (i = 0; i <= IWL_MAX_TID_COUNT; i++) {
  /*
 * Mark all queues for this STA as unallocated and defer TX
 * frames until the queue is allocated
 */
  mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE;
 }

 for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
  struct iwl_mvm_txq *mvmtxq =
   iwl_mvm_txq_from_mac80211(sta->txq[i]);

  mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
  INIT_LIST_HEAD(&mvmtxq->list);
  atomic_set(&mvmtxq->tx_request, 0);
 }

 if (iwl_mvm_has_new_rx_api(mvm)) {
  int q;

  dup_data = kcalloc(mvm->trans->info.num_rxqs,
       sizeof(*dup_data), GFP_KERNEL);
  if (!dup_data)
   return -ENOMEM;
  /*
 * Initialize all the last_seq values to 0xffff which can never
 * compare equal to the frame's seq_ctrl in the check in
 * iwl_mvm_is_dup() since the lower 4 bits are the fragment
 * number and fragmented packets don't reach that function.
 *
 * This thus allows receiving a packet with seqno 0 and the
 * retry bit set as the very first packet on a new TID.
 */
  for (q = 0; q < mvm->trans->info.num_rxqs; q++)
   memset(dup_data[q].last_seq, 0xff,
          sizeof(dup_data[q].last_seq));
  mvm_sta->dup_data = dup_data;
 }

 if (!iwl_mvm_has_new_tx_api(mvm)) {
  ret = iwl_mvm_reserve_sta_stream(mvm, sta,
       ieee80211_vif_type_p2p(vif));
  if (ret)
   return ret;
 }

 /*
 * if rs is registered with mac80211, then "add station" will be handled
 * via the corresponding ops, otherwise need to notify rate scaling here
 */
 if (iwl_mvm_has_tlc_offload(mvm))
  iwl_mvm_rs_add_sta(mvm, mvm_sta);
 else
  spin_lock_init(&mvm_sta->deflink.lq_sta.rs_drv.pers.lock);

 iwl_mvm_toggle_tx_ant(mvm, &mvm_sta->tx_ant);

 /* MPDUs are counted only when EMLSR is possible */
 if (vif->type == NL80211_IFTYPE_STATION && !vif->p2p &&
     !sta->tdls && ieee80211_vif_is_mld(vif)) {
  mvm_sta->mpdu_counters =
   kcalloc(mvm->trans->info.num_rxqs,
    sizeof(*mvm_sta->mpdu_counters),
    GFP_KERNEL);
  if (mvm_sta->mpdu_counters)
   for (int q = 0; q < mvm->trans->info.num_rxqs; q++)
    spin_lock_init(&mvm_sta->mpdu_counters[q].lock);
 }

 return 0;
}

int iwl_mvm_add_sta(struct iwl_mvm *mvm,
      struct ieee80211_vif *vif,
      struct ieee80211_sta *sta)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 int ret, sta_id;
 bool sta_update = false;
 unsigned int sta_flags = 0;

 lockdep_assert_held(&mvm->mutex);

 if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
  sta_id = iwl_mvm_find_free_sta_id(mvm,
        ieee80211_vif_type_p2p(vif));
 else
  sta_id = mvm_sta->deflink.sta_id;

 if (sta_id == IWL_INVALID_STA)
  return -ENOSPC;

 spin_lock_init(&mvm_sta->lock);

 /* if this is a HW restart re-alloc existing queues */
 if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
  struct iwl_mvm_int_sta tmp_sta = {
   .sta_id = sta_id,
   .type = mvm_sta->sta_type,
  };

  /* First add an empty station since allocating
 * a queue requires a valid station
 */
  ret = iwl_mvm_add_int_sta_common(mvm, &tmp_sta, sta->addr,
       mvmvif->id, mvmvif->color);
  if (ret)
   goto err;

  iwl_mvm_realloc_queues_after_restart(mvm, sta);
  sta_update = true;
  sta_flags = iwl_mvm_has_new_tx_api(mvm) ? 0 : STA_MODIFY_QUEUES;
  goto update_fw;
 }

 ret = iwl_mvm_sta_init(mvm, vif, sta, sta_id,
          sta->tdls ? IWL_STA_TDLS_LINK : IWL_STA_LINK);
 if (ret)
  goto err;

update_fw:
 ret = iwl_mvm_sta_send_to_fw(mvm, sta, sta_update, sta_flags);
 if (ret)
  goto err;

 if (vif->type == NL80211_IFTYPE_STATION) {
  if (!sta->tdls) {
   WARN_ON(mvmvif->deflink.ap_sta_id != IWL_INVALID_STA);
   mvmvif->deflink.ap_sta_id = sta_id;
  } else {
   WARN_ON(mvmvif->deflink.ap_sta_id == IWL_INVALID_STA);
  }
 }

 rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);

 return 0;

err:
 return ret;
}

int iwl_mvm_drain_sta(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvmsta,
        bool drain)
{
 struct iwl_mvm_add_sta_cmd cmd = {};
 int ret;
 u32 status;

 lockdep_assert_held(&mvm->mutex);

 cmd.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color);
 cmd.sta_id = mvmsta->deflink.sta_id;
 cmd.add_modify = STA_MODE_MODIFY;
 cmd.station_flags = drain ? cpu_to_le32(STA_FLG_DRAIN_FLOW) : 0;
 cmd.station_flags_msk = cpu_to_le32(STA_FLG_DRAIN_FLOW);

 status = ADD_STA_SUCCESS;
 ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
       iwl_mvm_add_sta_cmd_size(mvm),
       &cmd, &status);
 if (ret)
  return ret;

 switch (status & IWL_ADD_STA_STATUS_MASK) {
 case ADD_STA_SUCCESS:
  IWL_DEBUG_INFO(mvm, "Frames for staid %d will drained in fw\n",
          mvmsta->deflink.sta_id);
  break;
 default:
  ret = -EIO;
  IWL_ERR(mvm, "Couldn't drain frames for staid %d\n",
   mvmsta->deflink.sta_id);
  break;
 }

 return ret;
}

/*
 * Remove a station from the FW table. Before sending the command to remove
 * the station validate that the station is indeed known to the driver (sanity
 * only).
 */
static int iwl_mvm_rm_sta_common(struct iwl_mvm *mvm, u8 sta_id)
{
 struct ieee80211_sta *sta;
 struct iwl_mvm_rm_sta_cmd rm_sta_cmd = {
  .sta_id = sta_id,
 };
 int ret;

 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
     lockdep_is_held(&mvm->mutex));

 /* Note: internal stations are marked as error values */
 if (!sta) {
  IWL_ERR(mvm, "Invalid station id\n");
  return -EINVAL;
 }

 ret = iwl_mvm_send_cmd_pdu(mvm, REMOVE_STA, 0,
       sizeof(rm_sta_cmd), &rm_sta_cmd);
 if (ret) {
  IWL_ERR(mvm, "Failed to remove station. Id=%d\n", sta_id);
  return ret;
 }

 return 0;
}

static void iwl_mvm_disable_sta_queues(struct iwl_mvm *mvm,
           struct ieee80211_vif *vif,
           struct ieee80211_sta *sta)
{
 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 int i;

 lockdep_assert_held(&mvm->mutex);

 for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
  if (mvm_sta->tid_data[i].txq_id == IWL_MVM_INVALID_QUEUE)
   continue;

  iwl_mvm_disable_txq(mvm, sta, mvm_sta->deflink.sta_id,
        &mvm_sta->tid_data[i].txq_id, i);
  mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE;
 }

 for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
  struct iwl_mvm_txq *mvmtxq =
   iwl_mvm_txq_from_mac80211(sta->txq[i]);

  spin_lock_bh(&mvm->add_stream_lock);
  mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
  list_del_init(&mvmtxq->list);
  clear_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state);
  spin_unlock_bh(&mvm->add_stream_lock);
 }
}

int iwl_mvm_wait_sta_queues_empty(struct iwl_mvm *mvm,
      struct iwl_mvm_sta *mvm_sta)
{
 int i;

 for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
  u16 txq_id;
  int ret;

  spin_lock_bh(&mvm_sta->lock);
  txq_id = mvm_sta->tid_data[i].txq_id;
  spin_unlock_bh(&mvm_sta->lock);

  if (txq_id == IWL_MVM_INVALID_QUEUE)
   continue;

  ret = iwl_trans_wait_txq_empty(mvm->trans, txq_id);
  if (ret)
   return ret;
 }

 return 0;
}

/* Execute the common part for both MLD and non-MLD modes.
 * Returns if we're done with removing the station, either
 * with error or success
 */
void iwl_mvm_sta_del(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
       struct ieee80211_sta *sta,
       struct ieee80211_link_sta *link_sta)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 struct iwl_mvm_vif_link_info *mvm_link =
  mvmvif->link[link_sta->link_id];
 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 struct iwl_mvm_link_sta *mvm_link_sta;
 u8 sta_id;

 lockdep_assert_held(&mvm->mutex);

 mvm_link_sta =
  rcu_dereference_protected(mvm_sta->link[link_sta->link_id],
       lockdep_is_held(&mvm->mutex));
 sta_id = mvm_link_sta->sta_id;

 if (vif->type == NL80211_IFTYPE_STATION &&
     mvm_link->ap_sta_id == sta_id) {
  /* first remove remaining keys */
  iwl_mvm_sec_key_remove_ap(mvm, vif, mvm_link,
       link_sta->link_id);

  mvm_link->ap_sta_id = IWL_INVALID_STA;
 }

 /*
 * This shouldn't happen - the TDLS channel switch should be canceled
 * before the STA is removed.
 */
 if (WARN_ON_ONCE(mvm->tdls_cs.peer.sta_id == sta_id)) {
  mvm->tdls_cs.peer.sta_id = IWL_INVALID_STA;
  cancel_delayed_work(&mvm->tdls_cs.dwork);
 }
}

int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
     struct ieee80211_vif *vif,
     struct ieee80211_sta *sta)
{
 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 int ret;

 lockdep_assert_held(&mvm->mutex);

 ret = iwl_mvm_drain_sta(mvm, mvm_sta, true);
 if (ret)
  return ret;

 /* flush its queues here since we are freeing mvm_sta */
 ret = iwl_mvm_flush_sta(mvm, mvm_sta->deflink.sta_id,
    mvm_sta->tfd_queue_msk);
 if (ret)
  return ret;
 if (iwl_mvm_has_new_tx_api(mvm)) {
  ret = iwl_mvm_wait_sta_queues_empty(mvm, mvm_sta);
 } else {
  u32 q_mask = mvm_sta->tfd_queue_msk;

  ret = iwl_trans_wait_tx_queues_empty(mvm->trans,
           q_mask);
 }
 if (ret)
  return ret;

 ret = iwl_mvm_drain_sta(mvm, mvm_sta, false);

 iwl_mvm_disable_sta_queues(mvm, vif, sta);

 /* If there is a TXQ still marked as reserved - free it */
 if (mvm_sta->reserved_queue != IEEE80211_INVAL_HW_QUEUE) {
  u8 reserved_txq = mvm_sta->reserved_queue;
  enum iwl_mvm_queue_status *status;

  /*
 * If no traffic has gone through the reserved TXQ - it
 * is still marked as IWL_MVM_QUEUE_RESERVED, and
 * should be manually marked as free again
 */
  status = &mvm->queue_info[reserved_txq].status;
  if (WARN((*status != IWL_MVM_QUEUE_RESERVED) &&
    (*status != IWL_MVM_QUEUE_FREE),
    "sta_id %d reserved txq %d status %d",
    mvm_sta->deflink.sta_id, reserved_txq, *status))
   return -EINVAL;

  *status = IWL_MVM_QUEUE_FREE;
 }

 iwl_mvm_sta_del(mvm, vif, sta, &sta->deflink);

 ret = iwl_mvm_rm_sta_common(mvm, mvm_sta->deflink.sta_id);
 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta->deflink.sta_id], NULL);

 return ret;
}

int iwl_mvm_rm_sta_id(struct iwl_mvm *mvm,
        struct ieee80211_vif *vif,
        u8 sta_id)
{
 int ret = iwl_mvm_rm_sta_common(mvm, sta_id);

 lockdep_assert_held(&mvm->mutex);

 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL);
 return ret;
}

int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm,
        struct iwl_mvm_int_sta *sta,
        u32 qmask, enum nl80211_iftype iftype,
        u8 type)
{
 if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status) ||
     sta->sta_id == IWL_INVALID_STA) {
  sta->sta_id = iwl_mvm_find_free_sta_id(mvm, iftype);
  if (WARN_ON_ONCE(sta->sta_id == IWL_INVALID_STA))
   return -ENOSPC;
 }

 sta->tfd_queue_msk = qmask;
 sta->type = type;

 /* put a non-NULL value so iterating over the stations won't stop */
 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta->sta_id], ERR_PTR(-EINVAL));
 return 0;
}

void iwl_mvm_dealloc_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta)
{
 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta->sta_id], NULL);
 memset(sta, 0, sizeof(struct iwl_mvm_int_sta));
 sta->sta_id = IWL_INVALID_STA;
}

static void iwl_mvm_enable_aux_snif_queue(struct iwl_mvm *mvm, u16 queue,
       u8 sta_id, u8 fifo)
{
 unsigned int wdg_timeout =
  mvm->trans->mac_cfg->base->wd_timeout;
 struct iwl_trans_txq_scd_cfg cfg = {
  .fifo = fifo,
  .sta_id = sta_id,
  .tid = IWL_MAX_TID_COUNT,
  .aggregate = false,
  .frame_limit = IWL_FRAME_LIMIT,
 };

 WARN_ON(iwl_mvm_has_new_tx_api(mvm));

 iwl_mvm_enable_txq(mvm, NULL, queue, 0, &cfg, wdg_timeout);
}

static int iwl_mvm_enable_aux_snif_queue_tvqm(struct iwl_mvm *mvm, u8 sta_id)
{
 unsigned int wdg_timeout =
  mvm->trans->mac_cfg->base->wd_timeout;

 WARN_ON(!iwl_mvm_has_new_tx_api(mvm));

 return iwl_mvm_tvqm_enable_txq(mvm, NULL, sta_id, IWL_MAX_TID_COUNT,
           wdg_timeout);
}

static int iwl_mvm_add_int_sta_with_queue(struct iwl_mvm *mvm, int macidx,
       int maccolor, u8 *addr,
       struct iwl_mvm_int_sta *sta,
       u16 *queue, int fifo)
{
 int ret;

 /* Map queue to fifo - needs to happen before adding station */
 if (!iwl_mvm_has_new_tx_api(mvm))
  iwl_mvm_enable_aux_snif_queue(mvm, *queue, sta->sta_id, fifo);

 ret = iwl_mvm_add_int_sta_common(mvm, sta, addr, macidx, maccolor);
 if (ret) {
  if (!iwl_mvm_has_new_tx_api(mvm))
   iwl_mvm_disable_txq(mvm, NULL, sta->sta_id, queue,
         IWL_MAX_TID_COUNT);
  return ret;
 }

 /*
 * For 22000 firmware and on we cannot add queue to a station unknown
 * to firmware so enable queue here - after the station was added
 */
 if (iwl_mvm_has_new_tx_api(mvm)) {
  int txq;

  txq = iwl_mvm_enable_aux_snif_queue_tvqm(mvm, sta->sta_id);
  if (txq < 0) {
   iwl_mvm_rm_sta_common(mvm, sta->sta_id);
   return txq;
  }

  *queue = txq;
 }

 return 0;
}

int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm, u32 lmac_id)
{
 int ret;
 u32 qmask = mvm->aux_queue == IWL_MVM_INVALID_QUEUE ? 0 :
  BIT(mvm->aux_queue);

 lockdep_assert_held(&mvm->mutex);

 /* Allocate aux station and assign to it the aux queue */
 ret = iwl_mvm_allocate_int_sta(mvm, &mvm->aux_sta, qmask,
           NL80211_IFTYPE_UNSPECIFIED,
           IWL_STA_AUX_ACTIVITY);
 if (ret)
  return ret;

 /*
 * In CDB NICs we need to specify which lmac to use for aux activity
 * using the mac_id argument place to send lmac_id to the function
 */
 ret = iwl_mvm_add_int_sta_with_queue(mvm, lmac_id, 0, NULL,
          &mvm->aux_sta, &mvm->aux_queue,
          IWL_MVM_TX_FIFO_MCAST);
 if (ret) {
  iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);
  return ret;
 }

 return 0;
}

int iwl_mvm_add_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);

 lockdep_assert_held(&mvm->mutex);

 return iwl_mvm_add_int_sta_with_queue(mvm, mvmvif->id, mvmvif->color,
           NULL, &mvm->snif_sta,
           &mvm->snif_queue,
           IWL_MVM_TX_FIFO_BE);
}

int iwl_mvm_rm_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
{
 int ret;

 lockdep_assert_held(&mvm->mutex);

 if (WARN_ON_ONCE(mvm->snif_sta.sta_id == IWL_INVALID_STA))
  return -EINVAL;

 iwl_mvm_disable_txq(mvm, NULL, mvm->snif_sta.sta_id,
       &mvm->snif_queue, IWL_MAX_TID_COUNT);
 ret = iwl_mvm_rm_sta_common(mvm, mvm->snif_sta.sta_id);
 if (ret)
  IWL_WARN(mvm, "Failed sending remove station\n");

 return ret;
}

int iwl_mvm_rm_aux_sta(struct iwl_mvm *mvm)
{
 int ret;

 lockdep_assert_held(&mvm->mutex);

 if (WARN_ON_ONCE(mvm->aux_sta.sta_id == IWL_INVALID_STA))
  return -EINVAL;

 iwl_mvm_disable_txq(mvm, NULL, mvm->aux_sta.sta_id,
       &mvm->aux_queue, IWL_MAX_TID_COUNT);
 ret = iwl_mvm_rm_sta_common(mvm, mvm->aux_sta.sta_id);
 if (ret)
  IWL_WARN(mvm, "Failed sending remove station\n");
 iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);

 return ret;
}

void iwl_mvm_dealloc_snif_sta(struct iwl_mvm *mvm)
{
 iwl_mvm_dealloc_int_sta(mvm, &mvm->snif_sta);
}

/*
 * Send the add station command for the vif's broadcast station.
 * Assumes that the station was already allocated.
 *
 * @mvm: the mvm component
 * @vif: the interface to which the broadcast station is added
 * @bsta: the broadcast station to add.
 */
int iwl_mvm_send_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
{
 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
--> --------------------

--> maximum size reached

--> --------------------