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

   1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
   2 /*
   3  * Copyright (C) 2012-2015, 2018-2022 Intel Corporation
   4  * Copyright (C) 2013-2015 Intel Mobile Communications GmbH
   5  * Copyright (C) 2016-2017 Intel Deutschland GmbH
   6  */
   7 #include <net/mac80211.h>
   8
   9 #include "mvm.h"
  10 #include "sta.h"
  11 #include "rs.h"
  12
  13 /*
  14  * New version of ADD_STA_sta command added new fields at the end of the
  15  * structure, so sending the size of the relevant API's structure is enough to
  16  * support both API versions.
  17  */
  18 static inline int iwl_mvm_add_sta_cmd_size(struct iwl_mvm *mvm)
  19 {
  20         if (iwl_mvm_has_new_rx_api(mvm) ||
  21             fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE))
  22                 return sizeof(struct iwl_mvm_add_sta_cmd);
  23         else
  24                 return sizeof(struct iwl_mvm_add_sta_cmd_v7);
  25 }
  26
  27 static int iwl_mvm_find_free_sta_id(struct iwl_mvm *mvm,
  28                                     enum nl80211_iftype iftype)
  29 {
  30         int sta_id;
  31         u32 reserved_ids = 0;
  32
  33         BUILD_BUG_ON(IWL_MVM_STATION_COUNT_MAX > 32);
  34         WARN_ON_ONCE(test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status));
  35
  36         lockdep_assert_held(&mvm->mutex);
  37
  38         /* d0i3/d3 assumes the AP's sta_id (of sta vif) is 0. reserve it. */
  39         if (iftype != NL80211_IFTYPE_STATION)
  40                 reserved_ids = BIT(0);
  41
  42         /* Don't take rcu_read_lock() since we are protected by mvm->mutex */
  43         for (sta_id = 0; sta_id < mvm->fw->ucode_capa.num_stations; sta_id++) {
  44                 if (BIT(sta_id) & reserved_ids)
  45                         continue;
  46
  47                 if (!rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
  48                                                lockdep_is_held(&mvm->mutex)))
  49                         return sta_id;
  50         }
  51         return IWL_MVM_INVALID_STA;
  52 }
  53
  54 /* send station add/update command to firmware */
  55 int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
  56                            bool update, unsigned int flags)
  57 {
  58         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
  59         struct iwl_mvm_add_sta_cmd add_sta_cmd = {
  60                 .sta_id = mvm_sta->sta_id,
  61                 .mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color),
  62                 .add_modify = update ? 1 : 0,
  63                 .station_flags_msk = cpu_to_le32(STA_FLG_FAT_EN_MSK |
  64                                                  STA_FLG_MIMO_EN_MSK |
  65                                                  STA_FLG_RTS_MIMO_PROT),
  66                 .tid_disable_tx = cpu_to_le16(mvm_sta->tid_disable_agg),
  67         };
  68         int ret;
  69         u32 status;
  70         u32 agg_size = 0, mpdu_dens = 0;
  71
  72         if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE))
  73                 add_sta_cmd.station_type = mvm_sta->sta_type;
  74
  75         if (!update || (flags & STA_MODIFY_QUEUES)) {
  76                 memcpy(&add_sta_cmd.addr, sta->addr, ETH_ALEN);
  77
  78                 if (!iwl_mvm_has_new_tx_api(mvm)) {
  79                         add_sta_cmd.tfd_queue_msk =
  80                                 cpu_to_le32(mvm_sta->tfd_queue_msk);
  81
  82                         if (flags & STA_MODIFY_QUEUES)
  83                                 add_sta_cmd.modify_mask |= STA_MODIFY_QUEUES;
  84                 } else {
  85                         WARN_ON(flags & STA_MODIFY_QUEUES);
  86                 }
  87         }
  88
  89         switch (sta->deflink.bandwidth) {
  90         case IEEE80211_STA_RX_BW_320:
  91         case IEEE80211_STA_RX_BW_160:
  92                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_160MHZ);
  93                 fallthrough;
  94         case IEEE80211_STA_RX_BW_80:
  95                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_80MHZ);
  96                 fallthrough;
  97         case IEEE80211_STA_RX_BW_40:
  98                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_40MHZ);
  99                 fallthrough;
 100         case IEEE80211_STA_RX_BW_20:
 101                 if (sta->deflink.ht_cap.ht_supported)
 102                         add_sta_cmd.station_flags |=
 103                                 cpu_to_le32(STA_FLG_FAT_EN_20MHZ);
 104                 break;
 105         }
 106
 107         switch (sta->deflink.rx_nss) {
 108         case 1:
 109                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_SISO);
 110                 break;
 111         case 2:
 112                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_MIMO2);
 113                 break;
 114         case 3 ... 8:
 115                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_MIMO3);
 116                 break;
 117         }
 118
 119         switch (sta->deflink.smps_mode) {
 120         case IEEE80211_SMPS_AUTOMATIC:
 121         case IEEE80211_SMPS_NUM_MODES:
 122                 WARN_ON(1);
 123                 break;
 124         case IEEE80211_SMPS_STATIC:
 125                 /* override NSS */
 126                 add_sta_cmd.station_flags &= ~cpu_to_le32(STA_FLG_MIMO_EN_MSK);
 127                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_SISO);
 128                 break;
 129         case IEEE80211_SMPS_DYNAMIC:
 130                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_RTS_MIMO_PROT);
 131                 break;
 132         case IEEE80211_SMPS_OFF:
 133                 /* nothing */
 134                 break;
 135         }
 136
 137         if (sta->deflink.ht_cap.ht_supported) {
 138                 add_sta_cmd.station_flags_msk |=
 139                         cpu_to_le32(STA_FLG_MAX_AGG_SIZE_MSK |
 140                                     STA_FLG_AGG_MPDU_DENS_MSK);
 141
 142                 mpdu_dens = sta->deflink.ht_cap.ampdu_density;
 143         }
 144
 145         if (mvm_sta->vif->bss_conf.chandef.chan->band == NL80211_BAND_6GHZ) {
 146                 add_sta_cmd.station_flags_msk |=
 147                         cpu_to_le32(STA_FLG_MAX_AGG_SIZE_MSK |
 148                                     STA_FLG_AGG_MPDU_DENS_MSK);
 149
 150                 mpdu_dens = le16_get_bits(sta->deflink.he_6ghz_capa.capa,
 151                                           IEEE80211_HE_6GHZ_CAP_MIN_MPDU_START);
 152                 agg_size = le16_get_bits(sta->deflink.he_6ghz_capa.capa,
 153                                          IEEE80211_HE_6GHZ_CAP_MAX_AMPDU_LEN_EXP);
 154         } else if (sta->deflink.vht_cap.vht_supported) {
 155                 agg_size = sta->deflink.vht_cap.cap &
 156                         IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK;
 157                 agg_size >>=
 158                         IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_SHIFT;
 159         } else if (sta->deflink.ht_cap.ht_supported) {
 160                 agg_size = sta->deflink.ht_cap.ampdu_factor;
 161         }
 162
 163         /* D6.0 10.12.2 A-MPDU length limit rules
 164          * A STA indicates the maximum length of the A-MPDU preEOF padding
 165          * that it can receive in an HE PPDU in the Maximum A-MPDU Length
 166          * Exponent field in its HT Capabilities, VHT Capabilities,
 167          * and HE 6 GHz Band Capabilities elements (if present) and the
 168          * Maximum AMPDU Length Exponent Extension field in its HE
 169          * Capabilities element
 170          */
 171         if (sta->deflink.he_cap.has_he)
 172                 agg_size += u8_get_bits(sta->deflink.he_cap.he_cap_elem.mac_cap_info[3],
 173                                         IEEE80211_HE_MAC_CAP3_MAX_AMPDU_LEN_EXP_MASK);
 174
 175         /* Limit to max A-MPDU supported by FW */
 176         if (agg_size > (STA_FLG_MAX_AGG_SIZE_4M >> STA_FLG_MAX_AGG_SIZE_SHIFT))
 177                 agg_size = (STA_FLG_MAX_AGG_SIZE_4M >>
 178                             STA_FLG_MAX_AGG_SIZE_SHIFT);
 179
 180         add_sta_cmd.station_flags |=
 181                 cpu_to_le32(agg_size << STA_FLG_MAX_AGG_SIZE_SHIFT);
 182         add_sta_cmd.station_flags |=
 183                 cpu_to_le32(mpdu_dens << STA_FLG_AGG_MPDU_DENS_SHIFT);
 184         if (mvm_sta->sta_state >= IEEE80211_STA_ASSOC)
 185                 add_sta_cmd.assoc_id = cpu_to_le16(sta->aid);
 186
 187         if (sta->wme) {
 188                 add_sta_cmd.modify_mask |= STA_MODIFY_UAPSD_ACS;
 189
 190                 if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_BK)
 191                         add_sta_cmd.uapsd_acs |= BIT(AC_BK);
 192                 if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_BE)
 193                         add_sta_cmd.uapsd_acs |= BIT(AC_BE);
 194                 if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VI)
 195                         add_sta_cmd.uapsd_acs |= BIT(AC_VI);
 196                 if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VO)
 197                         add_sta_cmd.uapsd_acs |= BIT(AC_VO);
 198                 add_sta_cmd.uapsd_acs |= add_sta_cmd.uapsd_acs << 4;
 199                 add_sta_cmd.sp_length = sta->max_sp ? sta->max_sp * 2 : 128;
 200         }
 201
 202         status = ADD_STA_SUCCESS;
 203         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
 204                                           iwl_mvm_add_sta_cmd_size(mvm),
 205                                           &add_sta_cmd, &status);
 206         if (ret)
 207                 return ret;
 208
 209         switch (status & IWL_ADD_STA_STATUS_MASK) {
 210         case ADD_STA_SUCCESS:
 211                 IWL_DEBUG_ASSOC(mvm, "ADD_STA PASSED\n");
 212                 break;
 213         default:
 214                 ret = -EIO;
 215                 IWL_ERR(mvm, "ADD_STA failed\n");
 216                 break;
 217         }
 218
 219         return ret;
 220 }
 221
 222 static void iwl_mvm_rx_agg_session_expired(struct timer_list *t)
 223 {
 224         struct iwl_mvm_baid_data *data =
 225                 from_timer(data, t, session_timer);
 226         struct iwl_mvm_baid_data __rcu **rcu_ptr = data->rcu_ptr;
 227         struct iwl_mvm_baid_data *ba_data;
 228         struct ieee80211_sta *sta;
 229         struct iwl_mvm_sta *mvm_sta;
 230         unsigned long timeout;
 231
 232         rcu_read_lock();
 233
 234         ba_data = rcu_dereference(*rcu_ptr);
 235
 236         if (WARN_ON(!ba_data))
 237                 goto unlock;
 238
 239         if (!ba_data->timeout)
 240                 goto unlock;
 241
 242         timeout = ba_data->last_rx + TU_TO_JIFFIES(ba_data->timeout * 2);
 243         if (time_is_after_jiffies(timeout)) {
 244                 mod_timer(&ba_data->session_timer, timeout);
 245                 goto unlock;
 246         }
 247
 248         /* Timer expired */
 249         sta = rcu_dereference(ba_data->mvm->fw_id_to_mac_id[ba_data->sta_id]);
 250
 251         /*
 252          * sta should be valid unless the following happens:
 253          * The firmware asserts which triggers a reconfig flow, but
 254          * the reconfig fails before we set the pointer to sta into
 255          * the fw_id_to_mac_id pointer table. Mac80211 can't stop
 256          * A-MDPU and hence the timer continues to run. Then, the
 257          * timer expires and sta is NULL.
 258          */
 259         if (!sta)
 260                 goto unlock;
 261
 262         mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 263         ieee80211_rx_ba_timer_expired(mvm_sta->vif,
 264                                       sta->addr, ba_data->tid);
 265 unlock:
 266         rcu_read_unlock();
 267 }
 268
 269 /* Disable aggregations for a bitmap of TIDs for a given station */
 270 static int iwl_mvm_invalidate_sta_queue(struct iwl_mvm *mvm, int queue,
 271                                         unsigned long disable_agg_tids,
 272                                         bool remove_queue)
 273 {
 274         struct iwl_mvm_add_sta_cmd cmd = {};
 275         struct ieee80211_sta *sta;
 276         struct iwl_mvm_sta *mvmsta;
 277         u32 status;
 278         u8 sta_id;
 279
 280         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 281                 return -EINVAL;
 282
 283         sta_id = mvm->queue_info[queue].ra_sta_id;
 284
 285         rcu_read_lock();
 286
 287         sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
 288
 289         if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta))) {
 290                 rcu_read_unlock();
 291                 return -EINVAL;
 292         }
 293
 294         mvmsta = iwl_mvm_sta_from_mac80211(sta);
 295
 296         mvmsta->tid_disable_agg |= disable_agg_tids;
 297
 298         cmd.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color);
 299         cmd.sta_id = mvmsta->sta_id;
 300         cmd.add_modify = STA_MODE_MODIFY;
 301         cmd.modify_mask = STA_MODIFY_QUEUES;
 302         if (disable_agg_tids)
 303                 cmd.modify_mask |= STA_MODIFY_TID_DISABLE_TX;
 304         if (remove_queue)
 305                 cmd.modify_mask |= STA_MODIFY_QUEUE_REMOVAL;
 306         cmd.tfd_queue_msk = cpu_to_le32(mvmsta->tfd_queue_msk);
 307         cmd.tid_disable_tx = cpu_to_le16(mvmsta->tid_disable_agg);
 308
 309         rcu_read_unlock();
 310
 311         /* Notify FW of queue removal from the STA queues */
 312         status = ADD_STA_SUCCESS;
 313         return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
 314                                            iwl_mvm_add_sta_cmd_size(mvm),
 315                                            &cmd, &status);
 316 }
 317
 318 static int iwl_mvm_disable_txq(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 319                                u16 *queueptr, u8 tid)
 320 {
 321         int queue = *queueptr;
 322         struct iwl_scd_txq_cfg_cmd cmd = {
 323                 .scd_queue = queue,
 324                 .action = SCD_CFG_DISABLE_QUEUE,
 325         };
 326         int ret;
 327
 328         lockdep_assert_held(&mvm->mutex);
 329
 330         if (iwl_mvm_has_new_tx_api(mvm)) {
 331                 if (mvm->sta_remove_requires_queue_remove) {
 332                         u32 cmd_id = WIDE_ID(DATA_PATH_GROUP,
 333                                              SCD_QUEUE_CONFIG_CMD);
 334                         struct iwl_scd_queue_cfg_cmd remove_cmd = {
 335                                 .operation = cpu_to_le32(IWL_SCD_QUEUE_REMOVE),
 336                                 .u.remove.queue = cpu_to_le32(queue),
 337                         };
 338
 339                         ret = iwl_mvm_send_cmd_pdu(mvm, cmd_id, 0,
 340                                                    sizeof(remove_cmd),
 341                                                    &remove_cmd);
 342                 } else {
 343                         ret = 0;
 344                 }
 345
 346                 iwl_trans_txq_free(mvm->trans, queue);
 347                 *queueptr = IWL_MVM_INVALID_QUEUE;
 348
 349                 return ret;
 350         }
 351
 352         if (WARN_ON(mvm->queue_info[queue].tid_bitmap == 0))
 353                 return 0;
 354
 355         mvm->queue_info[queue].tid_bitmap &= ~BIT(tid);
 356
 357         cmd.action = mvm->queue_info[queue].tid_bitmap ?
 358                 SCD_CFG_ENABLE_QUEUE : SCD_CFG_DISABLE_QUEUE;
 359         if (cmd.action == SCD_CFG_DISABLE_QUEUE)
 360                 mvm->queue_info[queue].status = IWL_MVM_QUEUE_FREE;
 361
 362         IWL_DEBUG_TX_QUEUES(mvm,
 363                             "Disabling TXQ #%d tids=0x%x\n",
 364                             queue,
 365                             mvm->queue_info[queue].tid_bitmap);
 366
 367         /* If the queue is still enabled - nothing left to do in this func */
 368         if (cmd.action == SCD_CFG_ENABLE_QUEUE)
 369                 return 0;
 370
 371         cmd.sta_id = mvm->queue_info[queue].ra_sta_id;
 372         cmd.tid = mvm->queue_info[queue].txq_tid;
 373
 374         /* Make sure queue info is correct even though we overwrite it */
 375         WARN(mvm->queue_info[queue].tid_bitmap,
 376              "TXQ #%d info out-of-sync - tids=0x%x\n",
 377              queue, mvm->queue_info[queue].tid_bitmap);
 378
 379         /* If we are here - the queue is freed and we can zero out these vals */
 380         mvm->queue_info[queue].tid_bitmap = 0;
 381
 382         if (sta) {
 383                 struct iwl_mvm_txq *mvmtxq =
 384                         iwl_mvm_txq_from_tid(sta, tid);
 385
 386                 mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
 387                 list_del_init(&mvmtxq->list);
 388         }
 389
 390         /* Regardless if this is a reserved TXQ for a STA - mark it as false */
 391         mvm->queue_info[queue].reserved = false;
 392
 393         iwl_trans_txq_disable(mvm->trans, queue, false);
 394         ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0,
 395                                    sizeof(struct iwl_scd_txq_cfg_cmd), &cmd);
 396
 397         if (ret)
 398                 IWL_ERR(mvm, "Failed to disable queue %d (ret=%d)\n",
 399                         queue, ret);
 400         return ret;
 401 }
 402
 403 static int iwl_mvm_get_queue_agg_tids(struct iwl_mvm *mvm, int queue)
 404 {
 405         struct ieee80211_sta *sta;
 406         struct iwl_mvm_sta *mvmsta;
 407         unsigned long tid_bitmap;
 408         unsigned long agg_tids = 0;
 409         u8 sta_id;
 410         int tid;
 411
 412         lockdep_assert_held(&mvm->mutex);
 413
 414         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 415                 return -EINVAL;
 416
 417         sta_id = mvm->queue_info[queue].ra_sta_id;
 418         tid_bitmap = mvm->queue_info[queue].tid_bitmap;
 419
 420         sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
 421                                         lockdep_is_held(&mvm->mutex));
 422
 423         if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta)))
 424                 return -EINVAL;
 425
 426         mvmsta = iwl_mvm_sta_from_mac80211(sta);
 427
 428         spin_lock_bh(&mvmsta->lock);
 429         for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
 430                 if (mvmsta->tid_data[tid].state == IWL_AGG_ON)
 431                         agg_tids |= BIT(tid);
 432         }
 433         spin_unlock_bh(&mvmsta->lock);
 434
 435         return agg_tids;
 436 }
 437
 438 /*
 439  * Remove a queue from a station's resources.
 440  * Note that this only marks as free. It DOESN'T delete a BA agreement, and
 441  * doesn't disable the queue
 442  */
 443 static int iwl_mvm_remove_sta_queue_marking(struct iwl_mvm *mvm, int queue)
 444 {
 445         struct ieee80211_sta *sta;
 446         struct iwl_mvm_sta *mvmsta;
 447         unsigned long tid_bitmap;
 448         unsigned long disable_agg_tids = 0;
 449         u8 sta_id;
 450         int tid;
 451
 452         lockdep_assert_held(&mvm->mutex);
 453
 454         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 455                 return -EINVAL;
 456
 457         sta_id = mvm->queue_info[queue].ra_sta_id;
 458         tid_bitmap = mvm->queue_info[queue].tid_bitmap;
 459
 460         rcu_read_lock();
 461
 462         sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
 463
 464         if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta))) {
 465                 rcu_read_unlock();
 466                 return 0;
 467         }
 468
 469         mvmsta = iwl_mvm_sta_from_mac80211(sta);
 470
 471         spin_lock_bh(&mvmsta->lock);
 472         /* Unmap MAC queues and TIDs from this queue */
 473         for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
 474                 struct iwl_mvm_txq *mvmtxq =
 475                         iwl_mvm_txq_from_tid(sta, tid);
 476
 477                 if (mvmsta->tid_data[tid].state == IWL_AGG_ON)
 478                         disable_agg_tids |= BIT(tid);
 479                 mvmsta->tid_data[tid].txq_id = IWL_MVM_INVALID_QUEUE;
 480
 481                 mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
 482                 list_del_init(&mvmtxq->list);
 483         }
 484
 485         mvmsta->tfd_queue_msk &= ~BIT(queue); /* Don't use this queue anymore */
 486         spin_unlock_bh(&mvmsta->lock);
 487
 488         rcu_read_unlock();
 489
 490         /*
 491          * The TX path may have been using this TXQ_ID from the tid_data,
 492          * so make sure it's no longer running so that we can safely reuse
 493          * this TXQ later. We've set all the TIDs to IWL_MVM_INVALID_QUEUE
 494          * above, but nothing guarantees we've stopped using them. Thus,
 495          * without this, we could get to iwl_mvm_disable_txq() and remove
 496          * the queue while still sending frames to it.
 497          */
 498         synchronize_net();
 499
 500         return disable_agg_tids;
 501 }
 502
 503 static int iwl_mvm_free_inactive_queue(struct iwl_mvm *mvm, int queue,
 504                                        struct ieee80211_sta *old_sta,
 505                                        u8 new_sta_id)
 506 {
 507         struct iwl_mvm_sta *mvmsta;
 508         u8 sta_id, tid;
 509         unsigned long disable_agg_tids = 0;
 510         bool same_sta;
 511         u16 queue_tmp = queue;
 512         int ret;
 513
 514         lockdep_assert_held(&mvm->mutex);
 515
 516         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 517                 return -EINVAL;
 518
 519         sta_id = mvm->queue_info[queue].ra_sta_id;
 520         tid = mvm->queue_info[queue].txq_tid;
 521
 522         same_sta = sta_id == new_sta_id;
 523
 524         mvmsta = iwl_mvm_sta_from_staid_protected(mvm, sta_id);
 525         if (WARN_ON(!mvmsta))
 526                 return -EINVAL;
 527
 528         disable_agg_tids = iwl_mvm_remove_sta_queue_marking(mvm, queue);
 529         /* Disable the queue */
 530         if (disable_agg_tids)
 531                 iwl_mvm_invalidate_sta_queue(mvm, queue,
 532                                              disable_agg_tids, false);
 533
 534         ret = iwl_mvm_disable_txq(mvm, old_sta, &queue_tmp, tid);
 535         if (ret) {
 536                 IWL_ERR(mvm,
 537                         "Failed to free inactive queue %d (ret=%d)\n",
 538                         queue, ret);
 539
 540                 return ret;
 541         }
 542
 543         /* If TXQ is allocated to another STA, update removal in FW */
 544         if (!same_sta)
 545                 iwl_mvm_invalidate_sta_queue(mvm, queue, 0, true);
 546
 547         return 0;
 548 }
 549
 550 static int iwl_mvm_get_shared_queue(struct iwl_mvm *mvm,
 551                                     unsigned long tfd_queue_mask, u8 ac)
 552 {
 553         int queue = 0;
 554         u8 ac_to_queue[IEEE80211_NUM_ACS];
 555         int i;
 556
 557         /*
 558          * This protects us against grabbing a queue that's being reconfigured
 559          * by the inactivity checker.
 560          */
 561         lockdep_assert_held(&mvm->mutex);
 562
 563         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 564                 return -EINVAL;
 565
 566         memset(&ac_to_queue, IEEE80211_INVAL_HW_QUEUE, sizeof(ac_to_queue));
 567
 568         /* See what ACs the existing queues for this STA have */
 569         for_each_set_bit(i, &tfd_queue_mask, IWL_MVM_DQA_MAX_DATA_QUEUE) {
 570                 /* Only DATA queues can be shared */
 571                 if (i < IWL_MVM_DQA_MIN_DATA_QUEUE &&
 572                     i != IWL_MVM_DQA_BSS_CLIENT_QUEUE)
 573                         continue;
 574
 575                 ac_to_queue[mvm->queue_info[i].mac80211_ac] = i;
 576         }
 577
 578         /*
 579          * The queue to share is chosen only from DATA queues as follows (in
 580          * descending priority):
 581          * 1. An AC_BE queue
 582          * 2. Same AC queue
 583          * 3. Highest AC queue that is lower than new AC
 584          * 4. Any existing AC (there always is at least 1 DATA queue)
 585          */
 586
 587         /* Priority 1: An AC_BE queue */
 588         if (ac_to_queue[IEEE80211_AC_BE] != IEEE80211_INVAL_HW_QUEUE)
 589                 queue = ac_to_queue[IEEE80211_AC_BE];
 590         /* Priority 2: Same AC queue */
 591         else if (ac_to_queue[ac] != IEEE80211_INVAL_HW_QUEUE)
 592                 queue = ac_to_queue[ac];
 593         /* Priority 3a: If new AC is VO and VI exists - use VI */
 594         else if (ac == IEEE80211_AC_VO &&
 595                  ac_to_queue[IEEE80211_AC_VI] != IEEE80211_INVAL_HW_QUEUE)
 596                 queue = ac_to_queue[IEEE80211_AC_VI];
 597         /* Priority 3b: No BE so only AC less than the new one is BK */
 598         else if (ac_to_queue[IEEE80211_AC_BK] != IEEE80211_INVAL_HW_QUEUE)
 599                 queue = ac_to_queue[IEEE80211_AC_BK];
 600         /* Priority 4a: No BE nor BK - use VI if exists */
 601         else if (ac_to_queue[IEEE80211_AC_VI] != IEEE80211_INVAL_HW_QUEUE)
 602                 queue = ac_to_queue[IEEE80211_AC_VI];
 603         /* Priority 4b: No BE, BK nor VI - use VO if exists */
 604         else if (ac_to_queue[IEEE80211_AC_VO] != IEEE80211_INVAL_HW_QUEUE)
 605                 queue = ac_to_queue[IEEE80211_AC_VO];
 606
 607         /* Make sure queue found (or not) is legal */
 608         if (!iwl_mvm_is_dqa_data_queue(mvm, queue) &&
 609             !iwl_mvm_is_dqa_mgmt_queue(mvm, queue) &&
 610             (queue != IWL_MVM_DQA_BSS_CLIENT_QUEUE)) {
 611                 IWL_ERR(mvm, "No DATA queues available to share\n");
 612                 return -ENOSPC;
 613         }
 614
 615         return queue;
 616 }
 617
 618 /* Re-configure the SCD for a queue that has already been configured */
 619 static int iwl_mvm_reconfig_scd(struct iwl_mvm *mvm, int queue, int fifo,
 620                                 int sta_id, int tid, int frame_limit, u16 ssn)
 621 {
 622         struct iwl_scd_txq_cfg_cmd cmd = {
 623                 .scd_queue = queue,
 624                 .action = SCD_CFG_ENABLE_QUEUE,
 625                 .window = frame_limit,
 626                 .sta_id = sta_id,
 627                 .ssn = cpu_to_le16(ssn),
 628                 .tx_fifo = fifo,
 629                 .aggregate = (queue >= IWL_MVM_DQA_MIN_DATA_QUEUE ||
 630                               queue == IWL_MVM_DQA_BSS_CLIENT_QUEUE),
 631                 .tid = tid,
 632         };
 633         int ret;
 634
 635         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 636                 return -EINVAL;
 637
 638         if (WARN(mvm->queue_info[queue].tid_bitmap == 0,
 639                  "Trying to reconfig unallocated queue %d\n", queue))
 640                 return -ENXIO;
 641
 642         IWL_DEBUG_TX_QUEUES(mvm, "Reconfig SCD for TXQ #%d\n", queue);
 643
 644         ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd);
 645         WARN_ONCE(ret, "Failed to re-configure queue %d on FIFO %d, ret=%d\n",
 646                   queue, fifo, ret);
 647
 648         return ret;
 649 }
 650
 651 /*
 652  * If a given queue has a higher AC than the TID stream that is being compared
 653  * to, the queue needs to be redirected to the lower AC. This function does that
 654  * in such a case, otherwise - if no redirection required - it does nothing,
 655  * unless the %force param is true.
 656  */
 657 static int iwl_mvm_redirect_queue(struct iwl_mvm *mvm, int queue, int tid,
 658                                   int ac, int ssn, unsigned int wdg_timeout,
 659                                   bool force, struct iwl_mvm_txq *txq)
 660 {
 661         struct iwl_scd_txq_cfg_cmd cmd = {
 662                 .scd_queue = queue,
 663                 .action = SCD_CFG_DISABLE_QUEUE,
 664         };
 665         bool shared_queue;
 666         int ret;
 667
 668         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 669                 return -EINVAL;
 670
 671         /*
 672          * If the AC is lower than current one - FIFO needs to be redirected to
 673          * the lowest one of the streams in the queue. Check if this is needed
 674          * here.
 675          * Notice that the enum ieee80211_ac_numbers is "flipped", so BK is with
 676          * value 3 and VO with value 0, so to check if ac X is lower than ac Y
 677          * we need to check if the numerical value of X is LARGER than of Y.
 678          */
 679         if (ac <= mvm->queue_info[queue].mac80211_ac && !force) {
 680                 IWL_DEBUG_TX_QUEUES(mvm,
 681                                     "No redirection needed on TXQ #%d\n",
 682                                     queue);
 683                 return 0;
 684         }
 685
 686         cmd.sta_id = mvm->queue_info[queue].ra_sta_id;
 687         cmd.tx_fifo = iwl_mvm_ac_to_tx_fifo[mvm->queue_info[queue].mac80211_ac];
 688         cmd.tid = mvm->queue_info[queue].txq_tid;
 689         shared_queue = hweight16(mvm->queue_info[queue].tid_bitmap) > 1;
 690
 691         IWL_DEBUG_TX_QUEUES(mvm, "Redirecting TXQ #%d to FIFO #%d\n",
 692                             queue, iwl_mvm_ac_to_tx_fifo[ac]);
 693
 694         /* Stop the queue and wait for it to empty */
 695         txq->stopped = true;
 696
 697         ret = iwl_trans_wait_tx_queues_empty(mvm->trans, BIT(queue));
 698         if (ret) {
 699                 IWL_ERR(mvm, "Error draining queue %d before reconfig\n",
 700                         queue);
 701                 ret = -EIO;
 702                 goto out;
 703         }
 704
 705         /* Before redirecting the queue we need to de-activate it */
 706         iwl_trans_txq_disable(mvm->trans, queue, false);
 707         ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd);
 708         if (ret)
 709                 IWL_ERR(mvm, "Failed SCD disable TXQ %d (ret=%d)\n", queue,
 710                         ret);
 711
 712         /* Make sure the SCD wrptr is correctly set before reconfiguring */
 713         iwl_trans_txq_enable_cfg(mvm->trans, queue, ssn, NULL, wdg_timeout);
 714
 715         /* Update the TID "owner" of the queue */
 716         mvm->queue_info[queue].txq_tid = tid;
 717
 718         /* TODO: Work-around SCD bug when moving back by multiples of 0x40 */
 719
 720         /* Redirect to lower AC */
 721         iwl_mvm_reconfig_scd(mvm, queue, iwl_mvm_ac_to_tx_fifo[ac],
 722                              cmd.sta_id, tid, IWL_FRAME_LIMIT, ssn);
 723
 724         /* Update AC marking of the queue */
 725         mvm->queue_info[queue].mac80211_ac = ac;
 726
 727         /*
 728          * Mark queue as shared in transport if shared
 729          * Note this has to be done after queue enablement because enablement
 730          * can also set this value, and there is no indication there to shared
 731          * queues
 732          */
 733         if (shared_queue)
 734                 iwl_trans_txq_set_shared_mode(mvm->trans, queue, true);
 735
 736 out:
 737         /* Continue using the queue */
 738         txq->stopped = false;
 739
 740         return ret;
 741 }
 742
 743 static int iwl_mvm_find_free_queue(struct iwl_mvm *mvm, u8 sta_id,
 744                                    u8 minq, u8 maxq)
 745 {
 746         int i;
 747
 748         lockdep_assert_held(&mvm->mutex);
 749
 750         if (WARN(maxq >= mvm->trans->trans_cfg->base_params->num_of_queues,
 751                  "max queue %d >= num_of_queues (%d)", maxq,
 752                  mvm->trans->trans_cfg->base_params->num_of_queues))
 753                 maxq = mvm->trans->trans_cfg->base_params->num_of_queues - 1;
 754
 755         /* This should not be hit with new TX path */
 756         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 757                 return -ENOSPC;
 758
 759         /* Start by looking for a free queue */
 760         for (i = minq; i <= maxq; i++)
 761                 if (mvm->queue_info[i].tid_bitmap == 0 &&
 762                     mvm->queue_info[i].status == IWL_MVM_QUEUE_FREE)
 763                         return i;
 764
 765         return -ENOSPC;
 766 }
 767
 768 static int iwl_mvm_tvqm_enable_txq(struct iwl_mvm *mvm,
 769                                    u8 sta_id, u8 tid, unsigned int timeout)
 770 {
 771         int queue, size;
 772
 773         if (tid == IWL_MAX_TID_COUNT) {
 774                 tid = IWL_MGMT_TID;
 775                 size = max_t(u32, IWL_MGMT_QUEUE_SIZE,
 776                              mvm->trans->cfg->min_txq_size);
 777         } else {
 778                 struct ieee80211_sta *sta;
 779
 780                 rcu_read_lock();
 781                 sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
 782
 783                 /* this queue isn't used for traffic (cab_queue) */
 784                 if (IS_ERR_OR_NULL(sta)) {
 785                         size = IWL_MGMT_QUEUE_SIZE;
 786                 } else if (sta->deflink.he_cap.has_he) {
 787                         /* support for 256 ba size */
 788                         size = IWL_DEFAULT_QUEUE_SIZE_HE;
 789                 } else {
 790                         size = IWL_DEFAULT_QUEUE_SIZE;
 791                 }
 792
 793                 rcu_read_unlock();
 794         }
 795
 796         /* take the min with bc tbl entries allowed */
 797         size = min_t(u32, size, mvm->trans->txqs.bc_tbl_size / sizeof(u16));
 798
 799         /* size needs to be power of 2 values for calculating read/write pointers */
 800         size = rounddown_pow_of_two(size);
 801
 802         do {
 803                 queue = iwl_trans_txq_alloc(mvm->trans, 0, BIT(sta_id),
 804                                             tid, size, timeout);
 805
 806                 if (queue < 0)
 807                         IWL_DEBUG_TX_QUEUES(mvm,
 808                                             "Failed allocating TXQ of size %d for sta %d tid %d, ret: %d\n",
 809                                             size, sta_id, tid, queue);
 810                 size /= 2;
 811         } while (queue < 0 && size >= 16);
 812
 813         if (queue < 0)
 814                 return queue;
 815
 816         IWL_DEBUG_TX_QUEUES(mvm, "Enabling TXQ #%d for sta %d tid %d\n",
 817                             queue, sta_id, tid);
 818
 819         return queue;
 820 }
 821
 822 static int iwl_mvm_sta_alloc_queue_tvqm(struct iwl_mvm *mvm,
 823                                         struct ieee80211_sta *sta, u8 ac,
 824                                         int tid)
 825 {
 826         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 827         struct iwl_mvm_txq *mvmtxq =
 828                 iwl_mvm_txq_from_tid(sta, tid);
 829         unsigned int wdg_timeout =
 830                 iwl_mvm_get_wd_timeout(mvm, mvmsta->vif, false, false);
 831         int queue = -1;
 832
 833         lockdep_assert_held(&mvm->mutex);
 834
 835         IWL_DEBUG_TX_QUEUES(mvm,
 836                             "Allocating queue for sta %d on tid %d\n",
 837                             mvmsta->sta_id, tid);
 838         queue = iwl_mvm_tvqm_enable_txq(mvm, mvmsta->sta_id, tid, wdg_timeout);
 839         if (queue < 0)
 840                 return queue;
 841
 842         mvmtxq->txq_id = queue;
 843         mvm->tvqm_info[queue].txq_tid = tid;
 844         mvm->tvqm_info[queue].sta_id = mvmsta->sta_id;
 845
 846         IWL_DEBUG_TX_QUEUES(mvm, "Allocated queue is %d\n", queue);
 847
 848         spin_lock_bh(&mvmsta->lock);
 849         mvmsta->tid_data[tid].txq_id = queue;
 850         spin_unlock_bh(&mvmsta->lock);
 851
 852         return 0;
 853 }
 854
 855 static bool iwl_mvm_update_txq_mapping(struct iwl_mvm *mvm,
 856                                        struct ieee80211_sta *sta,
 857                                        int queue, u8 sta_id, u8 tid)
 858 {
 859         bool enable_queue = true;
 860
 861         /* Make sure this TID isn't already enabled */
 862         if (mvm->queue_info[queue].tid_bitmap & BIT(tid)) {
 863                 IWL_ERR(mvm, "Trying to enable TXQ %d with existing TID %d\n",
 864                         queue, tid);
 865                 return false;
 866         }
 867
 868         /* Update mappings and refcounts */
 869         if (mvm->queue_info[queue].tid_bitmap)
 870                 enable_queue = false;
 871
 872         mvm->queue_info[queue].tid_bitmap |= BIT(tid);
 873         mvm->queue_info[queue].ra_sta_id = sta_id;
 874
 875         if (enable_queue) {
 876                 if (tid != IWL_MAX_TID_COUNT)
 877                         mvm->queue_info[queue].mac80211_ac =
 878                                 tid_to_mac80211_ac[tid];
 879                 else
 880                         mvm->queue_info[queue].mac80211_ac = IEEE80211_AC_VO;
 881
 882                 mvm->queue_info[queue].txq_tid = tid;
 883         }
 884
 885         if (sta) {
 886                 struct iwl_mvm_txq *mvmtxq =
 887                         iwl_mvm_txq_from_tid(sta, tid);
 888
 889                 mvmtxq->txq_id = queue;
 890         }
 891
 892         IWL_DEBUG_TX_QUEUES(mvm,
 893                             "Enabling TXQ #%d tids=0x%x\n",
 894                             queue, mvm->queue_info[queue].tid_bitmap);
 895
 896         return enable_queue;
 897 }
 898
 899 static bool iwl_mvm_enable_txq(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 900                                int queue, u16 ssn,
 901                                const struct iwl_trans_txq_scd_cfg *cfg,
 902                                unsigned int wdg_timeout)
 903 {
 904         struct iwl_scd_txq_cfg_cmd cmd = {
 905                 .scd_queue = queue,
 906                 .action = SCD_CFG_ENABLE_QUEUE,
 907                 .window = cfg->frame_limit,
 908                 .sta_id = cfg->sta_id,
 909                 .ssn = cpu_to_le16(ssn),
 910                 .tx_fifo = cfg->fifo,
 911                 .aggregate = cfg->aggregate,
 912                 .tid = cfg->tid,
 913         };
 914         bool inc_ssn;
 915
 916         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 917                 return false;
 918
 919         /* Send the enabling command if we need to */
 920         if (!iwl_mvm_update_txq_mapping(mvm, sta, queue, cfg->sta_id, cfg->tid))
 921                 return false;
 922
 923         inc_ssn = iwl_trans_txq_enable_cfg(mvm->trans, queue, ssn,
 924                                            NULL, wdg_timeout);
 925         if (inc_ssn)
 926                 le16_add_cpu(&cmd.ssn, 1);
 927
 928         WARN(iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd),
 929              "Failed to configure queue %d on FIFO %d\n", queue, cfg->fifo);
 930
 931         return inc_ssn;
 932 }
 933
 934 static void iwl_mvm_change_queue_tid(struct iwl_mvm *mvm, int queue)
 935 {
 936         struct iwl_scd_txq_cfg_cmd cmd = {
 937                 .scd_queue = queue,
 938                 .action = SCD_CFG_UPDATE_QUEUE_TID,
 939         };
 940         int tid;
 941         unsigned long tid_bitmap;
 942         int ret;
 943
 944         lockdep_assert_held(&mvm->mutex);
 945
 946         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 947                 return;
 948
 949         tid_bitmap = mvm->queue_info[queue].tid_bitmap;
 950
 951         if (WARN(!tid_bitmap, "TXQ %d has no tids assigned to it\n", queue))
 952                 return;
 953
 954         /* Find any TID for queue */
 955         tid = find_first_bit(&tid_bitmap, IWL_MAX_TID_COUNT + 1);
 956         cmd.tid = tid;
 957         cmd.tx_fifo = iwl_mvm_ac_to_tx_fifo[tid_to_mac80211_ac[tid]];
 958
 959         ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd);
 960         if (ret) {
 961                 IWL_ERR(mvm, "Failed to update owner of TXQ %d (ret=%d)\n",
 962                         queue, ret);
 963                 return;
 964         }
 965
 966         mvm->queue_info[queue].txq_tid = tid;
 967         IWL_DEBUG_TX_QUEUES(mvm, "Changed TXQ %d ownership to tid %d\n",
 968                             queue, tid);
 969 }
 970
 971 static void iwl_mvm_unshare_queue(struct iwl_mvm *mvm, int queue)
 972 {
 973         struct ieee80211_sta *sta;
 974         struct iwl_mvm_sta *mvmsta;
 975         u8 sta_id;
 976         int tid = -1;
 977         unsigned long tid_bitmap;
 978         unsigned int wdg_timeout;
 979         int ssn;
 980         int ret = true;
 981
 982         /* queue sharing is disabled on new TX path */
 983         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
 984                 return;
 985
 986         lockdep_assert_held(&mvm->mutex);
 987
 988         sta_id = mvm->queue_info[queue].ra_sta_id;
 989         tid_bitmap = mvm->queue_info[queue].tid_bitmap;
 990
 991         /* Find TID for queue, and make sure it is the only one on the queue */
 992         tid = find_first_bit(&tid_bitmap, IWL_MAX_TID_COUNT + 1);
 993         if (tid_bitmap != BIT(tid)) {
 994                 IWL_ERR(mvm, "Failed to unshare q %d, active tids=0x%lx\n",
 995                         queue, tid_bitmap);
 996                 return;
 997         }
 998
 999         IWL_DEBUG_TX_QUEUES(mvm, "Unsharing TXQ %d, keeping tid %d\n", queue,
1000                             tid);
1001
1002         sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1003                                         lockdep_is_held(&mvm->mutex));
1004
1005         if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta)))
1006                 return;
1007
1008         mvmsta = iwl_mvm_sta_from_mac80211(sta);
1009         wdg_timeout = iwl_mvm_get_wd_timeout(mvm, mvmsta->vif, false, false);
1010
1011         ssn = IEEE80211_SEQ_TO_SN(mvmsta->tid_data[tid].seq_number);
1012
1013         ret = iwl_mvm_redirect_queue(mvm, queue, tid,
1014                                      tid_to_mac80211_ac[tid], ssn,
1015                                      wdg_timeout, true,
1016                                      iwl_mvm_txq_from_tid(sta, tid));
1017         if (ret) {
1018                 IWL_ERR(mvm, "Failed to redirect TXQ %d\n", queue);
1019                 return;
1020         }
1021
1022         /* If aggs should be turned back on - do it */
1023         if (mvmsta->tid_data[tid].state == IWL_AGG_ON) {
1024                 struct iwl_mvm_add_sta_cmd cmd = {0};
1025
1026                 mvmsta->tid_disable_agg &= ~BIT(tid);
1027
1028                 cmd.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color);
1029                 cmd.sta_id = mvmsta->sta_id;
1030                 cmd.add_modify = STA_MODE_MODIFY;
1031                 cmd.modify_mask = STA_MODIFY_TID_DISABLE_TX;
1032                 cmd.tfd_queue_msk = cpu_to_le32(mvmsta->tfd_queue_msk);
1033                 cmd.tid_disable_tx = cpu_to_le16(mvmsta->tid_disable_agg);
1034
1035                 ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
1036                                            iwl_mvm_add_sta_cmd_size(mvm), &cmd);
1037                 if (!ret) {
1038                         IWL_DEBUG_TX_QUEUES(mvm,
1039                                             "TXQ #%d is now aggregated again\n",
1040                                             queue);
1041
1042                         /* Mark queue intenally as aggregating again */
1043                         iwl_trans_txq_set_shared_mode(mvm->trans, queue, false);
1044                 }
1045         }
1046
1047         mvm->queue_info[queue].status = IWL_MVM_QUEUE_READY;
1048 }
1049
1050 /*
1051  * Remove inactive TIDs of a given queue.
1052  * If all queue TIDs are inactive - mark the queue as inactive
1053  * If only some the queue TIDs are inactive - unmap them from the queue
1054  *
1055  * Returns %true if all TIDs were removed and the queue could be reused.
1056  */
1057 static bool iwl_mvm_remove_inactive_tids(struct iwl_mvm *mvm,
1058                                          struct iwl_mvm_sta *mvmsta, int queue,
1059                                          unsigned long tid_bitmap,
1060                                          unsigned long *unshare_queues,
1061                                          unsigned long *changetid_queues)
1062 {
1063         unsigned int tid;
1064
1065         lockdep_assert_held(&mvmsta->lock);
1066         lockdep_assert_held(&mvm->mutex);
1067
1068         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
1069                 return false;
1070
1071         /* Go over all non-active TIDs, incl. IWL_MAX_TID_COUNT (for mgmt) */
1072         for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
1073                 /* If some TFDs are still queued - don't mark TID as inactive */
1074                 if (iwl_mvm_tid_queued(mvm, &mvmsta->tid_data[tid]))
1075                         tid_bitmap &= ~BIT(tid);
1076
1077                 /* Don't mark as inactive any TID that has an active BA */
1078                 if (mvmsta->tid_data[tid].state != IWL_AGG_OFF)
1079                         tid_bitmap &= ~BIT(tid);
1080         }
1081
1082         /* If all TIDs in the queue are inactive - return it can be reused */
1083         if (tid_bitmap == mvm->queue_info[queue].tid_bitmap) {
1084                 IWL_DEBUG_TX_QUEUES(mvm, "Queue %d is inactive\n", queue);
1085                 return true;
1086         }
1087
1088         /*
1089          * If we are here, this is a shared queue and not all TIDs timed-out.
1090          * Remove the ones that did.
1091          */
1092         for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
1093                 u16 q_tid_bitmap;
1094
1095                 mvmsta->tid_data[tid].txq_id = IWL_MVM_INVALID_QUEUE;
1096                 mvm->queue_info[queue].tid_bitmap &= ~BIT(tid);
1097
1098                 q_tid_bitmap = mvm->queue_info[queue].tid_bitmap;
1099
1100                 /*
1101                  * We need to take into account a situation in which a TXQ was
1102                  * allocated to TID x, and then turned shared by adding TIDs y
1103                  * and z. If TID x becomes inactive and is removed from the TXQ,
1104                  * ownership must be given to one of the remaining TIDs.
1105                  * This is mainly because if TID x continues - a new queue can't
1106                  * be allocated for it as long as it is an owner of another TXQ.
1107                  *
1108                  * Mark this queue in the right bitmap, we'll send the command
1109                  * to the firmware later.
1110                  */
1111                 if (!(q_tid_bitmap & BIT(mvm->queue_info[queue].txq_tid)))
1112                         set_bit(queue, changetid_queues);
1113
1114                 IWL_DEBUG_TX_QUEUES(mvm,
1115                                     "Removing inactive TID %d from shared Q:%d\n",
1116                                     tid, queue);
1117         }
1118
1119         IWL_DEBUG_TX_QUEUES(mvm,
1120                             "TXQ #%d left with tid bitmap 0x%x\n", queue,
1121                             mvm->queue_info[queue].tid_bitmap);
1122
1123         /*
1124          * There may be different TIDs with the same mac queues, so make
1125          * sure all TIDs have existing corresponding mac queues enabled
1126          */
1127         tid_bitmap = mvm->queue_info[queue].tid_bitmap;
1128
1129         /* If the queue is marked as shared - "unshare" it */
1130         if (hweight16(mvm->queue_info[queue].tid_bitmap) == 1 &&
1131             mvm->queue_info[queue].status == IWL_MVM_QUEUE_SHARED) {
1132                 IWL_DEBUG_TX_QUEUES(mvm, "Marking Q:%d for reconfig\n",
1133                                     queue);
1134                 set_bit(queue, unshare_queues);
1135         }
1136
1137         return false;
1138 }
1139
1140 /*
1141  * Check for inactivity - this includes checking if any queue
1142  * can be unshared and finding one (and only one) that can be
1143  * reused.
1144  * This function is also invoked as a sort of clean-up task,
1145  * in which case @alloc_for_sta is IWL_MVM_INVALID_STA.
1146  *
1147  * Returns the queue number, or -ENOSPC.
1148  */
1149 static int iwl_mvm_inactivity_check(struct iwl_mvm *mvm, u8 alloc_for_sta)
1150 {
1151         unsigned long now = jiffies;
1152         unsigned long unshare_queues = 0;
1153         unsigned long changetid_queues = 0;
1154         int i, ret, free_queue = -ENOSPC;
1155         struct ieee80211_sta *queue_owner  = NULL;
1156
1157         lockdep_assert_held(&mvm->mutex);
1158
1159         if (iwl_mvm_has_new_tx_api(mvm))
1160                 return -ENOSPC;
1161
1162         rcu_read_lock();
1163
1164         /* we skip the CMD queue below by starting at 1 */
1165         BUILD_BUG_ON(IWL_MVM_DQA_CMD_QUEUE != 0);
1166
1167         for (i = 1; i < IWL_MAX_HW_QUEUES; i++) {
1168                 struct ieee80211_sta *sta;
1169                 struct iwl_mvm_sta *mvmsta;
1170                 u8 sta_id;
1171                 int tid;
1172                 unsigned long inactive_tid_bitmap = 0;
1173                 unsigned long queue_tid_bitmap;
1174
1175                 queue_tid_bitmap = mvm->queue_info[i].tid_bitmap;
1176                 if (!queue_tid_bitmap)
1177                         continue;
1178
1179                 /* If TXQ isn't in active use anyway - nothing to do here... */
1180                 if (mvm->queue_info[i].status != IWL_MVM_QUEUE_READY &&
1181                     mvm->queue_info[i].status != IWL_MVM_QUEUE_SHARED)
1182                         continue;
1183
1184                 /* Check to see if there are inactive TIDs on this queue */
1185                 for_each_set_bit(tid, &queue_tid_bitmap,
1186                                  IWL_MAX_TID_COUNT + 1) {
1187                         if (time_after(mvm->queue_info[i].last_frame_time[tid] +
1188                                        IWL_MVM_DQA_QUEUE_TIMEOUT, now))
1189                                 continue;
1190
1191                         inactive_tid_bitmap |= BIT(tid);
1192                 }
1193
1194                 /* If all TIDs are active - finish check on this queue */
1195                 if (!inactive_tid_bitmap)
1196                         continue;
1197
1198                 /*
1199                  * If we are here - the queue hadn't been served recently and is
1200                  * in use
1201                  */
1202
1203                 sta_id = mvm->queue_info[i].ra_sta_id;
1204                 sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
1205
1206                 /*
1207                  * If the STA doesn't exist anymore, it isn't an error. It could
1208                  * be that it was removed since getting the queues, and in this
1209                  * case it should've inactivated its queues anyway.
1210                  */
1211                 if (IS_ERR_OR_NULL(sta))
1212                         continue;
1213
1214                 mvmsta = iwl_mvm_sta_from_mac80211(sta);
1215
1216                 spin_lock_bh(&mvmsta->lock);
1217                 ret = iwl_mvm_remove_inactive_tids(mvm, mvmsta, i,
1218                                                    inactive_tid_bitmap,
1219                                                    &unshare_queues,
1220                                                    &changetid_queues);
1221                 if (ret && free_queue < 0) {
1222                         queue_owner = sta;
1223                         free_queue = i;
1224                 }
1225                 /* only unlock sta lock - we still need the queue info lock */
1226                 spin_unlock_bh(&mvmsta->lock);
1227         }
1228
1229
1230         /* Reconfigure queues requiring reconfiguation */
1231         for_each_set_bit(i, &unshare_queues, IWL_MAX_HW_QUEUES)
1232                 iwl_mvm_unshare_queue(mvm, i);
1233         for_each_set_bit(i, &changetid_queues, IWL_MAX_HW_QUEUES)
1234                 iwl_mvm_change_queue_tid(mvm, i);
1235
1236         rcu_read_unlock();
1237
1238         if (free_queue >= 0 && alloc_for_sta != IWL_MVM_INVALID_STA) {
1239                 ret = iwl_mvm_free_inactive_queue(mvm, free_queue, queue_owner,
1240                                                   alloc_for_sta);
1241                 if (ret)
1242                         return ret;
1243         }
1244
1245         return free_queue;
1246 }
1247
1248 static int iwl_mvm_sta_alloc_queue(struct iwl_mvm *mvm,
1249                                    struct ieee80211_sta *sta, u8 ac, int tid)
1250 {
1251         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1252         struct iwl_trans_txq_scd_cfg cfg = {
1253                 .fifo = iwl_mvm_mac_ac_to_tx_fifo(mvm, ac),
1254                 .sta_id = mvmsta->sta_id,
1255                 .tid = tid,
1256                 .frame_limit = IWL_FRAME_LIMIT,
1257         };
1258         unsigned int wdg_timeout =
1259                 iwl_mvm_get_wd_timeout(mvm, mvmsta->vif, false, false);
1260         int queue = -1;
1261         u16 queue_tmp;
1262         unsigned long disable_agg_tids = 0;
1263         enum iwl_mvm_agg_state queue_state;
1264         bool shared_queue = false, inc_ssn;
1265         int ssn;
1266         unsigned long tfd_queue_mask;
1267         int ret;
1268
1269         lockdep_assert_held(&mvm->mutex);
1270
1271         if (iwl_mvm_has_new_tx_api(mvm))
1272                 return iwl_mvm_sta_alloc_queue_tvqm(mvm, sta, ac, tid);
1273
1274         spin_lock_bh(&mvmsta->lock);
1275         tfd_queue_mask = mvmsta->tfd_queue_msk;
1276         ssn = IEEE80211_SEQ_TO_SN(mvmsta->tid_data[tid].seq_number);
1277         spin_unlock_bh(&mvmsta->lock);
1278
1279         if (tid == IWL_MAX_TID_COUNT) {
1280                 queue = iwl_mvm_find_free_queue(mvm, mvmsta->sta_id,
1281                                                 IWL_MVM_DQA_MIN_MGMT_QUEUE,
1282                                                 IWL_MVM_DQA_MAX_MGMT_QUEUE);
1283                 if (queue >= IWL_MVM_DQA_MIN_MGMT_QUEUE)
1284                         IWL_DEBUG_TX_QUEUES(mvm, "Found free MGMT queue #%d\n",
1285                                             queue);
1286
1287                 /* If no such queue is found, we'll use a DATA queue instead */
1288         }
1289
1290         if ((queue < 0 && mvmsta->reserved_queue != IEEE80211_INVAL_HW_QUEUE) &&
1291             (mvm->queue_info[mvmsta->reserved_queue].status ==
1292                         IWL_MVM_QUEUE_RESERVED)) {
1293                 queue = mvmsta->reserved_queue;
1294                 mvm->queue_info[queue].reserved = true;
1295                 IWL_DEBUG_TX_QUEUES(mvm, "Using reserved queue #%d\n", queue);
1296         }
1297
1298         if (queue < 0)
1299                 queue = iwl_mvm_find_free_queue(mvm, mvmsta->sta_id,
1300                                                 IWL_MVM_DQA_MIN_DATA_QUEUE,
1301                                                 IWL_MVM_DQA_MAX_DATA_QUEUE);
1302         if (queue < 0) {
1303                 /* try harder - perhaps kill an inactive queue */
1304                 queue = iwl_mvm_inactivity_check(mvm, mvmsta->sta_id);
1305         }
1306
1307         /* No free queue - we'll have to share */
1308         if (queue <= 0) {
1309                 queue = iwl_mvm_get_shared_queue(mvm, tfd_queue_mask, ac);
1310                 if (queue > 0) {
1311                         shared_queue = true;
1312                         mvm->queue_info[queue].status = IWL_MVM_QUEUE_SHARED;
1313                 }
1314         }
1315
1316         /*
1317          * Mark TXQ as ready, even though it hasn't been fully configured yet,
1318          * to make sure no one else takes it.
1319          * This will allow avoiding re-acquiring the lock at the end of the
1320          * configuration. On error we'll mark it back as free.
1321          */
1322         if (queue > 0 && !shared_queue)
1323                 mvm->queue_info[queue].status = IWL_MVM_QUEUE_READY;
1324
1325         /* This shouldn't happen - out of queues */
1326         if (WARN_ON(queue <= 0)) {
1327                 IWL_ERR(mvm, "No available queues for tid %d on sta_id %d\n",
1328                         tid, cfg.sta_id);
1329                 return queue;
1330         }
1331
1332         /*
1333          * Actual en/disablement of aggregations is through the ADD_STA HCMD,
1334          * but for configuring the SCD to send A-MPDUs we need to mark the queue
1335          * as aggregatable.
1336          * Mark all DATA queues as allowing to be aggregated at some point
1337          */
1338         cfg.aggregate = (queue >= IWL_MVM_DQA_MIN_DATA_QUEUE ||
1339                          queue == IWL_MVM_DQA_BSS_CLIENT_QUEUE);
1340
1341         IWL_DEBUG_TX_QUEUES(mvm,
1342                             "Allocating %squeue #%d to sta %d on tid %d\n",
1343                             shared_queue ? "shared " : "", queue,
1344                             mvmsta->sta_id, tid);
1345
1346         if (shared_queue) {
1347                 /* Disable any open aggs on this queue */
1348                 disable_agg_tids = iwl_mvm_get_queue_agg_tids(mvm, queue);
1349
1350                 if (disable_agg_tids) {
1351                         IWL_DEBUG_TX_QUEUES(mvm, "Disabling aggs on queue %d\n",
1352                                             queue);
1353                         iwl_mvm_invalidate_sta_queue(mvm, queue,
1354                                                      disable_agg_tids, false);
1355                 }
1356         }
1357
1358         inc_ssn = iwl_mvm_enable_txq(mvm, sta, queue, ssn, &cfg, wdg_timeout);
1359
1360         /*
1361          * Mark queue as shared in transport if shared
1362          * Note this has to be done after queue enablement because enablement
1363          * can also set this value, and there is no indication there to shared
1364          * queues
1365          */
1366         if (shared_queue)
1367                 iwl_trans_txq_set_shared_mode(mvm->trans, queue, true);
1368
1369         spin_lock_bh(&mvmsta->lock);
1370         /*
1371          * This looks racy, but it is not. We have only one packet for
1372          * this ra/tid in our Tx path since we stop the Qdisc when we
1373          * need to allocate a new TFD queue.
1374          */
1375         if (inc_ssn) {
1376                 mvmsta->tid_data[tid].seq_number += 0x10;
1377                 ssn = (ssn + 1) & IEEE80211_SCTL_SEQ;
1378         }
1379         mvmsta->tid_data[tid].txq_id = queue;
1380         mvmsta->tfd_queue_msk |= BIT(queue);
1381         queue_state = mvmsta->tid_data[tid].state;
1382
1383         if (mvmsta->reserved_queue == queue)
1384                 mvmsta->reserved_queue = IEEE80211_INVAL_HW_QUEUE;
1385         spin_unlock_bh(&mvmsta->lock);
1386
1387         if (!shared_queue) {
1388                 ret = iwl_mvm_sta_send_to_fw(mvm, sta, true, STA_MODIFY_QUEUES);
1389                 if (ret)
1390                         goto out_err;
1391
1392                 /* If we need to re-enable aggregations... */
1393                 if (queue_state == IWL_AGG_ON) {
1394                         ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
1395                         if (ret)
1396                                 goto out_err;
1397                 }
1398         } else {
1399                 /* Redirect queue, if needed */
1400                 ret = iwl_mvm_redirect_queue(mvm, queue, tid, ac, ssn,
1401                                              wdg_timeout, false,
1402                                              iwl_mvm_txq_from_tid(sta, tid));
1403                 if (ret)
1404                         goto out_err;
1405         }
1406
1407         return 0;
1408
1409 out_err:
1410         queue_tmp = queue;
1411         iwl_mvm_disable_txq(mvm, sta, &queue_tmp, tid);
1412
1413         return ret;
1414 }
1415
1416 void iwl_mvm_add_new_dqa_stream_wk(struct work_struct *wk)
1417 {
1418         struct iwl_mvm *mvm = container_of(wk, struct iwl_mvm,
1419                                            add_stream_wk);
1420
1421         mutex_lock(&mvm->mutex);
1422
1423         iwl_mvm_inactivity_check(mvm, IWL_MVM_INVALID_STA);
1424
1425         while (!list_empty(&mvm->add_stream_txqs)) {
1426                 struct iwl_mvm_txq *mvmtxq;
1427                 struct ieee80211_txq *txq;
1428                 u8 tid;
1429
1430                 mvmtxq = list_first_entry(&mvm->add_stream_txqs,
1431                                           struct iwl_mvm_txq, list);
1432
1433                 txq = container_of((void *)mvmtxq, struct ieee80211_txq,
1434                                    drv_priv);
1435                 tid = txq->tid;
1436                 if (tid == IEEE80211_NUM_TIDS)
1437                         tid = IWL_MAX_TID_COUNT;
1438
1439                 /*
1440                  * We can't really do much here, but if this fails we can't
1441                  * transmit anyway - so just don't transmit the frame etc.
1442                  * and let them back up ... we've tried our best to allocate
1443                  * a queue in the function itself.
1444                  */
1445                 if (iwl_mvm_sta_alloc_queue(mvm, txq->sta, txq->ac, tid)) {
1446                         list_del_init(&mvmtxq->list);
1447                         continue;
1448                 }
1449
1450                 list_del_init(&mvmtxq->list);
1451                 local_bh_disable();
1452                 iwl_mvm_mac_itxq_xmit(mvm->hw, txq);
1453                 local_bh_enable();
1454         }
1455
1456         mutex_unlock(&mvm->mutex);
1457 }
1458
1459 static int iwl_mvm_reserve_sta_stream(struct iwl_mvm *mvm,
1460                                       struct ieee80211_sta *sta,
1461                                       enum nl80211_iftype vif_type)
1462 {
1463         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1464         int queue;
1465
1466         /* queue reserving is disabled on new TX path */
1467         if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
1468                 return 0;
1469
1470         /* run the general cleanup/unsharing of queues */
1471         iwl_mvm_inactivity_check(mvm, IWL_MVM_INVALID_STA);
1472
1473         /* Make sure we have free resources for this STA */
1474         if (vif_type == NL80211_IFTYPE_STATION && !sta->tdls &&
1475             !mvm->queue_info[IWL_MVM_DQA_BSS_CLIENT_QUEUE].tid_bitmap &&
1476             (mvm->queue_info[IWL_MVM_DQA_BSS_CLIENT_QUEUE].status ==
1477              IWL_MVM_QUEUE_FREE))
1478                 queue = IWL_MVM_DQA_BSS_CLIENT_QUEUE;
1479         else
1480                 queue = iwl_mvm_find_free_queue(mvm, mvmsta->sta_id,
1481                                                 IWL_MVM_DQA_MIN_DATA_QUEUE,
1482                                                 IWL_MVM_DQA_MAX_DATA_QUEUE);
1483         if (queue < 0) {
1484                 /* try again - this time kick out a queue if needed */
1485                 queue = iwl_mvm_inactivity_check(mvm, mvmsta->sta_id);
1486                 if (queue < 0) {
1487                         IWL_ERR(mvm, "No available queues for new station\n");
1488                         return -ENOSPC;
1489                 }
1490         }
1491         mvm->queue_info[queue].status = IWL_MVM_QUEUE_RESERVED;
1492
1493         mvmsta->reserved_queue = queue;
1494
1495         IWL_DEBUG_TX_QUEUES(mvm, "Reserving data queue #%d for sta_id %d\n",
1496                             queue, mvmsta->sta_id);
1497
1498         return 0;
1499 }
1500
1501 /*
1502  * In DQA mode, after a HW restart the queues should be allocated as before, in
1503  * order to avoid race conditions when there are shared queues. This function
1504  * does the re-mapping and queue allocation.
1505  *
1506  * Note that re-enabling aggregations isn't done in this function.
1507  */
1508 static void iwl_mvm_realloc_queues_after_restart(struct iwl_mvm *mvm,
1509                                                  struct ieee80211_sta *sta)
1510 {
1511         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1512         unsigned int wdg =
1513                 iwl_mvm_get_wd_timeout(mvm, mvm_sta->vif, false, false);
1514         int i;
1515         struct iwl_trans_txq_scd_cfg cfg = {
1516                 .sta_id = mvm_sta->sta_id,
1517                 .frame_limit = IWL_FRAME_LIMIT,
1518         };
1519
1520         /* Make sure reserved queue is still marked as such (if allocated) */
1521         if (mvm_sta->reserved_queue != IEEE80211_INVAL_HW_QUEUE)
1522                 mvm->queue_info[mvm_sta->reserved_queue].status =
1523                         IWL_MVM_QUEUE_RESERVED;
1524
1525         for (i = 0; i <= IWL_MAX_TID_COUNT; i++) {
1526                 struct iwl_mvm_tid_data *tid_data = &mvm_sta->tid_data[i];
1527                 int txq_id = tid_data->txq_id;
1528                 int ac;
1529
1530                 if (txq_id == IWL_MVM_INVALID_QUEUE)
1531                         continue;
1532
1533                 ac = tid_to_mac80211_ac[i];
1534
1535                 if (iwl_mvm_has_new_tx_api(mvm)) {
1536                         IWL_DEBUG_TX_QUEUES(mvm,
1537                                             "Re-mapping sta %d tid %d\n",
1538                                             mvm_sta->sta_id, i);
1539                         txq_id = iwl_mvm_tvqm_enable_txq(mvm, mvm_sta->sta_id,
1540                                                          i, wdg);
1541                         /*
1542                          * on failures, just set it to IWL_MVM_INVALID_QUEUE
1543                          * to try again later, we have no other good way of
1544                          * failing here
1545                          */
1546                         if (txq_id < 0)
1547                                 txq_id = IWL_MVM_INVALID_QUEUE;
1548                         tid_data->txq_id = txq_id;
1549
1550                         /*
1551                          * Since we don't set the seq number after reset, and HW
1552                          * sets it now, FW reset will cause the seq num to start
1553                          * at 0 again, so driver will need to update it
1554                          * internally as well, so it keeps in sync with real val
1555                          */
1556                         tid_data->seq_number = 0;
1557                 } else {
1558                         u16 seq = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
1559
1560                         cfg.tid = i;
1561                         cfg.fifo = iwl_mvm_mac_ac_to_tx_fifo(mvm, ac);
1562                         cfg.aggregate = (txq_id >= IWL_MVM_DQA_MIN_DATA_QUEUE ||
1563                                          txq_id ==
1564                                          IWL_MVM_DQA_BSS_CLIENT_QUEUE);
1565
1566                         IWL_DEBUG_TX_QUEUES(mvm,
1567                                             "Re-mapping sta %d tid %d to queue %d\n",
1568                                             mvm_sta->sta_id, i, txq_id);
1569
1570                         iwl_mvm_enable_txq(mvm, sta, txq_id, seq, &cfg, wdg);
1571                         mvm->queue_info[txq_id].status = IWL_MVM_QUEUE_READY;
1572                 }
1573         }
1574 }
1575
1576 static int iwl_mvm_add_int_sta_common(struct iwl_mvm *mvm,
1577                                       struct iwl_mvm_int_sta *sta,
1578                                       const u8 *addr,
1579                                       u16 mac_id, u16 color)
1580 {
1581         struct iwl_mvm_add_sta_cmd cmd;
1582         int ret;
1583         u32 status = ADD_STA_SUCCESS;
1584
1585         lockdep_assert_held(&mvm->mutex);
1586
1587         memset(&cmd, 0, sizeof(cmd));
1588         cmd.sta_id = sta->sta_id;
1589
1590         if (iwl_fw_lookup_cmd_ver(mvm->fw, ADD_STA, 0) >= 12 &&
1591             sta->type == IWL_STA_AUX_ACTIVITY)
1592                 cmd.mac_id_n_color = cpu_to_le32(mac_id);
1593         else
1594                 cmd.mac_id_n_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mac_id,
1595                                                                      color));
1596
1597         if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE))
1598                 cmd.station_type = sta->type;
1599
1600         if (!iwl_mvm_has_new_tx_api(mvm))
1601                 cmd.tfd_queue_msk = cpu_to_le32(sta->tfd_queue_msk);
1602         cmd.tid_disable_tx = cpu_to_le16(0xffff);
1603
1604         if (addr)
1605                 memcpy(cmd.addr, addr, ETH_ALEN);
1606
1607         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
1608                                           iwl_mvm_add_sta_cmd_size(mvm),
1609                                           &cmd, &status);
1610         if (ret)
1611                 return ret;
1612
1613         switch (status & IWL_ADD_STA_STATUS_MASK) {
1614         case ADD_STA_SUCCESS:
1615                 IWL_DEBUG_INFO(mvm, "Internal station added.\n");
1616                 return 0;
1617         default:
1618                 ret = -EIO;
1619                 IWL_ERR(mvm, "Add internal station failed, status=0x%x\n",
1620                         status);
1621                 break;
1622         }
1623         return ret;
1624 }
1625
1626 int iwl_mvm_add_sta(struct iwl_mvm *mvm,
1627                     struct ieee80211_vif *vif,
1628                     struct ieee80211_sta *sta)
1629 {
1630         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1631         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1632         struct iwl_mvm_rxq_dup_data *dup_data;
1633         int i, ret, sta_id;
1634         bool sta_update = false;
1635         unsigned int sta_flags = 0;
1636
1637         lockdep_assert_held(&mvm->mutex);
1638
1639         if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
1640                 sta_id = iwl_mvm_find_free_sta_id(mvm,
1641                                                   ieee80211_vif_type_p2p(vif));
1642         else
1643                 sta_id = mvm_sta->sta_id;
1644
1645         if (sta_id == IWL_MVM_INVALID_STA)
1646                 return -ENOSPC;
1647
1648         spin_lock_init(&mvm_sta->lock);
1649
1650         /* if this is a HW restart re-alloc existing queues */
1651         if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
1652                 struct iwl_mvm_int_sta tmp_sta = {
1653                         .sta_id = sta_id,
1654                         .type = mvm_sta->sta_type,
1655                 };
1656
1657                 /*
1658                  * First add an empty station since allocating
1659                  * a queue requires a valid station
1660                  */
1661                 ret = iwl_mvm_add_int_sta_common(mvm, &tmp_sta, sta->addr,
1662                                                  mvmvif->id, mvmvif->color);
1663                 if (ret)
1664                         goto err;
1665
1666                 iwl_mvm_realloc_queues_after_restart(mvm, sta);
1667                 sta_update = true;
1668                 sta_flags = iwl_mvm_has_new_tx_api(mvm) ? 0 : STA_MODIFY_QUEUES;
1669                 goto update_fw;
1670         }
1671
1672         mvm_sta->sta_id = sta_id;
1673         mvm_sta->mac_id_n_color = FW_CMD_ID_AND_COLOR(mvmvif->id,
1674                                                       mvmvif->color);
1675         mvm_sta->vif = vif;
1676         if (!mvm->trans->trans_cfg->gen2)
1677                 mvm_sta->max_agg_bufsize = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
1678         else
1679                 mvm_sta->max_agg_bufsize = LINK_QUAL_AGG_FRAME_LIMIT_GEN2_DEF;
1680         mvm_sta->tx_protection = 0;
1681         mvm_sta->tt_tx_protection = false;
1682         mvm_sta->sta_type = sta->tdls ? IWL_STA_TDLS_LINK : IWL_STA_LINK;
1683
1684         /* HW restart, don't assume the memory has been zeroed */
1685         mvm_sta->tid_disable_agg = 0xffff; /* No aggs at first */
1686         mvm_sta->tfd_queue_msk = 0;
1687
1688         /* for HW restart - reset everything but the sequence number */
1689         for (i = 0; i <= IWL_MAX_TID_COUNT; i++) {
1690                 u16 seq = mvm_sta->tid_data[i].seq_number;
1691                 memset(&mvm_sta->tid_data[i], 0, sizeof(mvm_sta->tid_data[i]));
1692                 mvm_sta->tid_data[i].seq_number = seq;
1693
1694                 /*
1695                  * Mark all queues for this STA as unallocated and defer TX
1696                  * frames until the queue is allocated
1697                  */
1698                 mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE;
1699         }
1700
1701         for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
1702                 struct iwl_mvm_txq *mvmtxq =
1703                         iwl_mvm_txq_from_mac80211(sta->txq[i]);
1704
1705                 mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
1706                 INIT_LIST_HEAD(&mvmtxq->list);
1707                 atomic_set(&mvmtxq->tx_request, 0);
1708         }
1709
1710         mvm_sta->agg_tids = 0;
1711
1712         if (iwl_mvm_has_new_rx_api(mvm) &&
1713             !test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
1714                 int q;
1715
1716                 dup_data = kcalloc(mvm->trans->num_rx_queues,
1717                                    sizeof(*dup_data), GFP_KERNEL);
1718                 if (!dup_data)
1719                         return -ENOMEM;
1720                 /*
1721                  * Initialize all the last_seq values to 0xffff which can never
1722                  * compare equal to the frame's seq_ctrl in the check in
1723                  * iwl_mvm_is_dup() since the lower 4 bits are the fragment
1724                  * number and fragmented packets don't reach that function.
1725                  *
1726                  * This thus allows receiving a packet with seqno 0 and the
1727                  * retry bit set as the very first packet on a new TID.
1728                  */
1729                 for (q = 0; q < mvm->trans->num_rx_queues; q++)
1730                         memset(dup_data[q].last_seq, 0xff,
1731                                sizeof(dup_data[q].last_seq));
1732                 mvm_sta->dup_data = dup_data;
1733         }
1734
1735         if (!iwl_mvm_has_new_tx_api(mvm)) {
1736                 ret = iwl_mvm_reserve_sta_stream(mvm, sta,
1737                                                  ieee80211_vif_type_p2p(vif));
1738                 if (ret)
1739                         goto err;
1740         }
1741
1742         /*
1743          * if rs is registered with mac80211, then "add station" will be handled
1744          * via the corresponding ops, otherwise need to notify rate scaling here
1745          */
1746         if (iwl_mvm_has_tlc_offload(mvm))
1747                 iwl_mvm_rs_add_sta(mvm, mvm_sta);
1748         else
1749                 spin_lock_init(&mvm_sta->lq_sta.rs_drv.pers.lock);
1750
1751         iwl_mvm_toggle_tx_ant(mvm, &mvm_sta->tx_ant);
1752
1753 update_fw:
1754         ret = iwl_mvm_sta_send_to_fw(mvm, sta, sta_update, sta_flags);
1755         if (ret)
1756                 goto err;
1757
1758         if (vif->type == NL80211_IFTYPE_STATION) {
1759                 if (!sta->tdls) {
1760                         WARN_ON(mvmvif->ap_sta_id != IWL_MVM_INVALID_STA);
1761                         mvmvif->ap_sta_id = sta_id;
1762                 } else {
1763                         WARN_ON(mvmvif->ap_sta_id == IWL_MVM_INVALID_STA);
1764                 }
1765         }
1766
1767         rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);
1768
1769         return 0;
1770
1771 err:
1772         return ret;
1773 }
1774
1775 int iwl_mvm_drain_sta(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvmsta,
1776                       bool drain)
1777 {
1778         struct iwl_mvm_add_sta_cmd cmd = {};
1779         int ret;
1780         u32 status;
1781
1782         lockdep_assert_held(&mvm->mutex);
1783
1784         cmd.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color);
1785         cmd.sta_id = mvmsta->sta_id;
1786         cmd.add_modify = STA_MODE_MODIFY;
1787         cmd.station_flags = drain ? cpu_to_le32(STA_FLG_DRAIN_FLOW) : 0;
1788         cmd.station_flags_msk = cpu_to_le32(STA_FLG_DRAIN_FLOW);
1789
1790         status = ADD_STA_SUCCESS;
1791         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
1792                                           iwl_mvm_add_sta_cmd_size(mvm),
1793                                           &cmd, &status);
1794         if (ret)
1795                 return ret;
1796
1797         switch (status & IWL_ADD_STA_STATUS_MASK) {
1798         case ADD_STA_SUCCESS:
1799                 IWL_DEBUG_INFO(mvm, "Frames for staid %d will drained in fw\n",
1800                                mvmsta->sta_id);
1801                 break;
1802         default:
1803                 ret = -EIO;
1804                 IWL_ERR(mvm, "Couldn't drain frames for staid %d\n",
1805                         mvmsta->sta_id);
1806                 break;
1807         }
1808
1809         return ret;
1810 }
1811
1812 /*
1813  * Remove a station from the FW table. Before sending the command to remove
1814  * the station validate that the station is indeed known to the driver (sanity
1815  * only).
1816  */
1817 static int iwl_mvm_rm_sta_common(struct iwl_mvm *mvm, u8 sta_id)
1818 {
1819         struct ieee80211_sta *sta;
1820         struct iwl_mvm_rm_sta_cmd rm_sta_cmd = {
1821                 .sta_id = sta_id,
1822         };
1823         int ret;
1824
1825         sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1826                                         lockdep_is_held(&mvm->mutex));
1827
1828         /* Note: internal stations are marked as error values */
1829         if (!sta) {
1830                 IWL_ERR(mvm, "Invalid station id\n");
1831                 return -EINVAL;
1832         }
1833
1834         ret = iwl_mvm_send_cmd_pdu(mvm, REMOVE_STA, 0,
1835                                    sizeof(rm_sta_cmd), &rm_sta_cmd);
1836         if (ret) {
1837                 IWL_ERR(mvm, "Failed to remove station. Id=%d\n", sta_id);
1838                 return ret;
1839         }
1840
1841         return 0;
1842 }
1843
1844 static void iwl_mvm_disable_sta_queues(struct iwl_mvm *mvm,
1845                                        struct ieee80211_vif *vif,
1846                                        struct ieee80211_sta *sta)
1847 {
1848         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1849         int i;
1850
1851         lockdep_assert_held(&mvm->mutex);
1852
1853         for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
1854                 if (mvm_sta->tid_data[i].txq_id == IWL_MVM_INVALID_QUEUE)
1855                         continue;
1856
1857                 iwl_mvm_disable_txq(mvm, sta, &mvm_sta->tid_data[i].txq_id, i);
1858                 mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE;
1859         }
1860
1861         for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
1862                 struct iwl_mvm_txq *mvmtxq =
1863                         iwl_mvm_txq_from_mac80211(sta->txq[i]);
1864
1865                 mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
1866                 list_del_init(&mvmtxq->list);
1867         }
1868 }
1869
1870 int iwl_mvm_wait_sta_queues_empty(struct iwl_mvm *mvm,
1871                                   struct iwl_mvm_sta *mvm_sta)
1872 {
1873         int i;
1874
1875         for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
1876                 u16 txq_id;
1877                 int ret;
1878
1879                 spin_lock_bh(&mvm_sta->lock);
1880                 txq_id = mvm_sta->tid_data[i].txq_id;
1881                 spin_unlock_bh(&mvm_sta->lock);
1882
1883                 if (txq_id == IWL_MVM_INVALID_QUEUE)
1884                         continue;
1885
1886                 ret = iwl_trans_wait_txq_empty(mvm->trans, txq_id);
1887                 if (ret)
1888                         return ret;
1889         }
1890
1891         return 0;
1892 }
1893
1894 int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
1895                    struct ieee80211_vif *vif,
1896                    struct ieee80211_sta *sta)
1897 {
1898         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1899         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1900         u8 sta_id = mvm_sta->sta_id;
1901         int ret;
1902
1903         lockdep_assert_held(&mvm->mutex);
1904
1905         if (iwl_mvm_has_new_rx_api(mvm))
1906                 kfree(mvm_sta->dup_data);
1907
1908         ret = iwl_mvm_drain_sta(mvm, mvm_sta, true);
1909         if (ret)
1910                 return ret;
1911
1912         /* flush its queues here since we are freeing mvm_sta */
1913         ret = iwl_mvm_flush_sta(mvm, mvm_sta, false);
1914         if (ret)
1915                 return ret;
1916         if (iwl_mvm_has_new_tx_api(mvm)) {
1917                 ret = iwl_mvm_wait_sta_queues_empty(mvm, mvm_sta);
1918         } else {
1919                 u32 q_mask = mvm_sta->tfd_queue_msk;
1920
1921                 ret = iwl_trans_wait_tx_queues_empty(mvm->trans,
1922                                                      q_mask);
1923         }
1924         if (ret)
1925                 return ret;
1926
1927         ret = iwl_mvm_drain_sta(mvm, mvm_sta, false);
1928
1929         iwl_mvm_disable_sta_queues(mvm, vif, sta);
1930
1931         /* If there is a TXQ still marked as reserved - free it */
1932         if (mvm_sta->reserved_queue != IEEE80211_INVAL_HW_QUEUE) {
1933                 u8 reserved_txq = mvm_sta->reserved_queue;
1934                 enum iwl_mvm_queue_status *status;
1935
1936                 /*
1937                  * If no traffic has gone through the reserved TXQ - it
1938                  * is still marked as IWL_MVM_QUEUE_RESERVED, and
1939                  * should be manually marked as free again
1940                  */
1941                 status = &mvm->queue_info[reserved_txq].status;
1942                 if (WARN((*status != IWL_MVM_QUEUE_RESERVED) &&
1943                          (*status != IWL_MVM_QUEUE_FREE),
1944                          "sta_id %d reserved txq %d status %d",
1945                          sta_id, reserved_txq, *status))
1946                         return -EINVAL;
1947
1948                 *status = IWL_MVM_QUEUE_FREE;
1949         }
1950
1951         if (vif->type == NL80211_IFTYPE_STATION &&
1952             mvmvif->ap_sta_id == sta_id) {
1953                 /* if associated - we can't remove the AP STA now */
1954                 if (vif->cfg.assoc)
1955                         return ret;
1956
1957                 /* unassoc - go ahead - remove the AP STA now */
1958                 mvmvif->ap_sta_id = IWL_MVM_INVALID_STA;
1959         }
1960
1961         /*
1962          * This shouldn't happen - the TDLS channel switch should be canceled
1963          * before the STA is removed.
1964          */
1965         if (WARN_ON_ONCE(mvm->tdls_cs.peer.sta_id == sta_id)) {
1966                 mvm->tdls_cs.peer.sta_id = IWL_MVM_INVALID_STA;
1967                 cancel_delayed_work(&mvm->tdls_cs.dwork);
1968         }
1969
1970         /*
1971          * Make sure that the tx response code sees the station as -EBUSY and
1972          * calls the drain worker.
1973          */
1974         spin_lock_bh(&mvm_sta->lock);
1975         spin_unlock_bh(&mvm_sta->lock);
1976
1977         ret = iwl_mvm_rm_sta_common(mvm, mvm_sta->sta_id);
1978         RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta->sta_id], NULL);
1979
1980         return ret;
1981 }
1982
1983 int iwl_mvm_rm_sta_id(struct iwl_mvm *mvm,
1984                       struct ieee80211_vif *vif,
1985                       u8 sta_id)
1986 {
1987         int ret = iwl_mvm_rm_sta_common(mvm, sta_id);
1988
1989         lockdep_assert_held(&mvm->mutex);
1990
1991         RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL);
1992         return ret;
1993 }
1994
1995 int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm,
1996                              struct iwl_mvm_int_sta *sta,
1997                              u32 qmask, enum nl80211_iftype iftype,
1998                              enum iwl_sta_type type)
1999 {
2000         if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status) ||
2001             sta->sta_id == IWL_MVM_INVALID_STA) {
2002                 sta->sta_id = iwl_mvm_find_free_sta_id(mvm, iftype);
2003                 if (WARN_ON_ONCE(sta->sta_id == IWL_MVM_INVALID_STA))
2004                         return -ENOSPC;
2005         }
2006
2007         sta->tfd_queue_msk = qmask;
2008         sta->type = type;
2009
2010         /* put a non-NULL value so iterating over the stations won't stop */
2011         rcu_assign_pointer(mvm->fw_id_to_mac_id[sta->sta_id], ERR_PTR(-EINVAL));
2012         return 0;
2013 }
2014
2015 void iwl_mvm_dealloc_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta)
2016 {
2017         RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta->sta_id], NULL);
2018         memset(sta, 0, sizeof(struct iwl_mvm_int_sta));
2019         sta->sta_id = IWL_MVM_INVALID_STA;
2020 }
2021
2022 static void iwl_mvm_enable_aux_snif_queue(struct iwl_mvm *mvm, u16 queue,
2023                                           u8 sta_id, u8 fifo)
2024 {
2025         unsigned int wdg_timeout =
2026                 mvm->trans->trans_cfg->base_params->wd_timeout;
2027         struct iwl_trans_txq_scd_cfg cfg = {
2028                 .fifo = fifo,
2029                 .sta_id = sta_id,
2030                 .tid = IWL_MAX_TID_COUNT,
2031                 .aggregate = false,
2032                 .frame_limit = IWL_FRAME_LIMIT,
2033         };
2034
2035         WARN_ON(iwl_mvm_has_new_tx_api(mvm));
2036
2037         iwl_mvm_enable_txq(mvm, NULL, queue, 0, &cfg, wdg_timeout);
2038 }
2039
2040 static int iwl_mvm_enable_aux_snif_queue_tvqm(struct iwl_mvm *mvm, u8 sta_id)
2041 {
2042         unsigned int wdg_timeout =
2043                 mvm->trans->trans_cfg->base_params->wd_timeout;
2044
2045         WARN_ON(!iwl_mvm_has_new_tx_api(mvm));
2046
2047         return iwl_mvm_tvqm_enable_txq(mvm, sta_id, IWL_MAX_TID_COUNT,
2048                                        wdg_timeout);
2049 }
2050
2051 static int iwl_mvm_add_int_sta_with_queue(struct iwl_mvm *mvm, int macidx,
2052                                           int maccolor, u8 *addr,
2053                                           struct iwl_mvm_int_sta *sta,
2054                                           u16 *queue, int fifo)
2055 {
2056         int ret;
2057
2058         /* Map queue to fifo - needs to happen before adding station */
2059         if (!iwl_mvm_has_new_tx_api(mvm))
2060                 iwl_mvm_enable_aux_snif_queue(mvm, *queue, sta->sta_id, fifo);
2061
2062         ret = iwl_mvm_add_int_sta_common(mvm, sta, addr, macidx, maccolor);
2063         if (ret) {
2064                 if (!iwl_mvm_has_new_tx_api(mvm))
2065                         iwl_mvm_disable_txq(mvm, NULL, queue,
2066                                             IWL_MAX_TID_COUNT);
2067                 return ret;
2068         }
2069
2070         /*
2071          * For 22000 firmware and on we cannot add queue to a station unknown
2072          * to firmware so enable queue here - after the station was added
2073          */
2074         if (iwl_mvm_has_new_tx_api(mvm)) {
2075                 int txq;
2076
2077                 txq = iwl_mvm_enable_aux_snif_queue_tvqm(mvm, sta->sta_id);
2078                 if (txq < 0) {
2079                         iwl_mvm_rm_sta_common(mvm, sta->sta_id);
2080                         return txq;
2081                 }
2082
2083                 *queue = txq;
2084         }
2085
2086         return 0;
2087 }
2088
2089 int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm, u32 lmac_id)
2090 {
2091         int ret;
2092
2093         lockdep_assert_held(&mvm->mutex);
2094
2095         /* Allocate aux station and assign to it the aux queue */
2096         ret = iwl_mvm_allocate_int_sta(mvm, &mvm->aux_sta, BIT(mvm->aux_queue),
2097                                        NL80211_IFTYPE_UNSPECIFIED,
2098                                        IWL_STA_AUX_ACTIVITY);
2099         if (ret)
2100                 return ret;
2101
2102         /*
2103          * In CDB NICs we need to specify which lmac to use for aux activity
2104          * using the mac_id argument place to send lmac_id to the function
2105          */
2106         ret = iwl_mvm_add_int_sta_with_queue(mvm, lmac_id, 0, NULL,
2107                                              &mvm->aux_sta, &mvm->aux_queue,
2108                                              IWL_MVM_TX_FIFO_MCAST);
2109         if (ret) {
2110                 iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);
2111                 return ret;
2112         }
2113
2114         return 0;
2115 }
2116
2117 int iwl_mvm_add_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2118 {
2119         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2120
2121         lockdep_assert_held(&mvm->mutex);
2122
2123         return iwl_mvm_add_int_sta_with_queue(mvm, mvmvif->id, mvmvif->color,
2124                                               NULL, &mvm->snif_sta,
2125                                               &mvm->snif_queue,
2126                                               IWL_MVM_TX_FIFO_BE);
2127 }
2128
2129 int iwl_mvm_rm_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2130 {
2131         int ret;
2132
2133         lockdep_assert_held(&mvm->mutex);
2134
2135         if (WARN_ON_ONCE(mvm->snif_sta.sta_id == IWL_MVM_INVALID_STA))
2136                 return -EINVAL;
2137
2138         iwl_mvm_disable_txq(mvm, NULL, &mvm->snif_queue, IWL_MAX_TID_COUNT);
2139         ret = iwl_mvm_rm_sta_common(mvm, mvm->snif_sta.sta_id);
2140         if (ret)
2141                 IWL_WARN(mvm, "Failed sending remove station\n");
2142
2143         return ret;
2144 }
2145
2146 int iwl_mvm_rm_aux_sta(struct iwl_mvm *mvm)
2147 {
2148         int ret;
2149
2150         lockdep_assert_held(&mvm->mutex);
2151
2152         if (WARN_ON_ONCE(mvm->aux_sta.sta_id == IWL_MVM_INVALID_STA))
2153                 return -EINVAL;
2154
2155         iwl_mvm_disable_txq(mvm, NULL, &mvm->aux_queue, IWL_MAX_TID_COUNT);
2156         ret = iwl_mvm_rm_sta_common(mvm, mvm->aux_sta.sta_id);
2157         if (ret)
2158                 IWL_WARN(mvm, "Failed sending remove station\n");
2159         iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);
2160
2161         return ret;
2162 }
2163
2164 void iwl_mvm_dealloc_snif_sta(struct iwl_mvm *mvm)
2165 {
2166         iwl_mvm_dealloc_int_sta(mvm, &mvm->snif_sta);
2167 }
2168
2169 /*
2170  * Send the add station command for the vif's broadcast station.
2171  * Assumes that the station was already allocated.
2172  *
2173  * @mvm: the mvm component
2174  * @vif: the interface to which the broadcast station is added
2175  * @bsta: the broadcast station to add.
2176  */
2177 int iwl_mvm_send_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2178 {
2179         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2180         struct iwl_mvm_int_sta *bsta = &mvmvif->bcast_sta;
2181         static const u8 _baddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
2182         const u8 *baddr = _baddr;
2183         int queue;
2184         int ret;
2185         unsigned int wdg_timeout =
2186                 iwl_mvm_get_wd_timeout(mvm, vif, false, false);
2187         struct iwl_trans_txq_scd_cfg cfg = {
2188                 .fifo = IWL_MVM_TX_FIFO_VO,
2189                 .sta_id = mvmvif->bcast_sta.sta_id,
2190                 .tid = IWL_MAX_TID_COUNT,
2191                 .aggregate = false,
2192                 .frame_limit = IWL_FRAME_LIMIT,
2193         };
2194
2195         lockdep_assert_held(&mvm->mutex);
2196
2197         if (!iwl_mvm_has_new_tx_api(mvm)) {
2198                 if (vif->type == NL80211_IFTYPE_AP ||
2199                     vif->type == NL80211_IFTYPE_ADHOC) {
2200                         queue = mvm->probe_queue;
2201                 } else if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
2202                         queue = mvm->p2p_dev_queue;
2203                 } else {
2204                         WARN(1, "Missing required TXQ for adding bcast STA\n");
2205                         return -EINVAL;
2206                 }
2207
2208                 bsta->tfd_queue_msk |= BIT(queue);
2209
2210                 iwl_mvm_enable_txq(mvm, NULL, queue, 0, &cfg, wdg_timeout);
2211         }
2212
2213         if (vif->type == NL80211_IFTYPE_ADHOC)
2214                 baddr = vif->bss_conf.bssid;
2215
2216         if (WARN_ON_ONCE(bsta->sta_id == IWL_MVM_INVALID_STA))
2217                 return -ENOSPC;
2218
2219         ret = iwl_mvm_add_int_sta_common(mvm, bsta, baddr,
2220                                          mvmvif->id, mvmvif->color);
2221         if (ret)
2222                 return ret;
2223
2224         /*
2225          * For 22000 firmware and on we cannot add queue to a station unknown
2226          * to firmware so enable queue here - after the station was added
2227          */
2228         if (iwl_mvm_has_new_tx_api(mvm)) {
2229                 queue = iwl_mvm_tvqm_enable_txq(mvm, bsta->sta_id,
2230                                                 IWL_MAX_TID_COUNT,
2231                                                 wdg_timeout);
2232                 if (queue < 0) {
2233                         iwl_mvm_rm_sta_common(mvm, bsta->sta_id);
2234                         return queue;
2235                 }
2236
2237                 if (vif->type == NL80211_IFTYPE_AP ||
2238                     vif->type == NL80211_IFTYPE_ADHOC)
2239                         mvm->probe_queue = queue;
2240                 else if (vif->type == NL80211_IFTYPE_P2P_DEVICE)
2241                         mvm->p2p_dev_queue = queue;
2242         }
2243
2244         return 0;
2245 }
2246
2247 static void iwl_mvm_free_bcast_sta_queues(struct iwl_mvm *mvm,
2248                                           struct ieee80211_vif *vif)
2249 {
2250         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2251         u16 *queueptr, queue;
2252
2253         lockdep_assert_held(&mvm->mutex);
2254
2255         iwl_mvm_flush_sta(mvm, &mvmvif->bcast_sta, true);
2256
2257         switch (vif->type) {
2258         case NL80211_IFTYPE_AP:
2259         case NL80211_IFTYPE_ADHOC:
2260                 queueptr = &mvm->probe_queue;
2261                 break;
2262         case NL80211_IFTYPE_P2P_DEVICE:
2263                 queueptr = &mvm->p2p_dev_queue;
2264                 break;
2265         default:
2266                 WARN(1, "Can't free bcast queue on vif type %d\n",
2267                      vif->type);
2268                 return;
2269         }
2270
2271         queue = *queueptr;
2272         iwl_mvm_disable_txq(mvm, NULL, queueptr, IWL_MAX_TID_COUNT);
2273         if (iwl_mvm_has_new_tx_api(mvm))
2274                 return;
2275
2276         WARN_ON(!(mvmvif->bcast_sta.tfd_queue_msk & BIT(queue)));
2277         mvmvif->bcast_sta.tfd_queue_msk &= ~BIT(queue);
2278 }
2279
2280 /* Send the FW a request to remove the station from it's internal data
2281  * structures, but DO NOT remove the entry from the local data structures. */
2282 int iwl_mvm_send_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2283 {
2284         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2285         int ret;
2286
2287         lockdep_assert_held(&mvm->mutex);
2288
2289         iwl_mvm_free_bcast_sta_queues(mvm, vif);
2290
2291         ret = iwl_mvm_rm_sta_common(mvm, mvmvif->bcast_sta.sta_id);
2292         if (ret)
2293                 IWL_WARN(mvm, "Failed sending remove station\n");
2294         return ret;
2295 }
2296
2297 int iwl_mvm_alloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2298 {
2299         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2300
2301         lockdep_assert_held(&mvm->mutex);
2302
2303         return iwl_mvm_allocate_int_sta(mvm, &mvmvif->bcast_sta, 0,
2304                                         ieee80211_vif_type_p2p(vif),
2305                                         IWL_STA_GENERAL_PURPOSE);
2306 }
2307
2308 /* Allocate a new station entry for the broadcast station to the given vif,
2309  * and send it to the FW.
2310  * Note that each P2P mac should have its own broadcast station.
2311  *
2312  * @mvm: the mvm component
2313  * @vif: the interface to which the broadcast station is added
2314  * @bsta: the broadcast station to add. */
2315 int iwl_mvm_add_p2p_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2316 {
2317         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2318         struct iwl_mvm_int_sta *bsta = &mvmvif->bcast_sta;
2319         int ret;
2320
2321         lockdep_assert_held(&mvm->mutex);
2322
2323         ret = iwl_mvm_alloc_bcast_sta(mvm, vif);
2324         if (ret)
2325                 return ret;
2326
2327         ret = iwl_mvm_send_add_bcast_sta(mvm, vif);
2328
2329         if (ret)
2330                 iwl_mvm_dealloc_int_sta(mvm, bsta);
2331
2332         return ret;
2333 }
2334
2335 void iwl_mvm_dealloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2336 {
2337         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2338
2339         iwl_mvm_dealloc_int_sta(mvm, &mvmvif->bcast_sta);
2340 }
2341
2342 /*
2343  * Send the FW a request to remove the station from it's internal data
2344  * structures, and in addition remove it from the local data structure.
2345  */
2346 int iwl_mvm_rm_p2p_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2347 {
2348         int ret;
2349
2350         lockdep_assert_held(&mvm->mutex);
2351
2352         ret = iwl_mvm_send_rm_bcast_sta(mvm, vif);
2353
2354         iwl_mvm_dealloc_bcast_sta(mvm, vif);
2355
2356         return ret;
2357 }
2358
2359 /*
2360  * Allocate a new station entry for the multicast station to the given vif,
2361  * and send it to the FW.
2362  * Note that each AP/GO mac should have its own multicast station.
2363  *
2364  * @mvm: the mvm component
2365  * @vif: the interface to which the multicast station is added
2366  */
2367 int iwl_mvm_add_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2368 {
2369         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2370         struct iwl_mvm_int_sta *msta = &mvmvif->mcast_sta;
2371         static const u8 _maddr[] = {0x03, 0x00, 0x00, 0x00, 0x00, 0x00};
2372         const u8 *maddr = _maddr;
2373         struct iwl_trans_txq_scd_cfg cfg = {
2374                 .fifo = vif->type == NL80211_IFTYPE_AP ?
2375                         IWL_MVM_TX_FIFO_MCAST : IWL_MVM_TX_FIFO_BE,
2376                 .sta_id = msta->sta_id,
2377                 .tid = 0,
2378                 .aggregate = false,
2379                 .frame_limit = IWL_FRAME_LIMIT,
2380         };
2381         unsigned int timeout = iwl_mvm_get_wd_timeout(mvm, vif, false, false);
2382         int ret;
2383
2384         lockdep_assert_held(&mvm->mutex);
2385
2386         if (WARN_ON(vif->type != NL80211_IFTYPE_AP &&
2387                     vif->type != NL80211_IFTYPE_ADHOC))
2388                 return -ENOTSUPP;
2389
2390         /*
2391          * In IBSS, ieee80211_check_queues() sets the cab_queue to be
2392          * invalid, so make sure we use the queue we want.
2393          * Note that this is done here as we want to avoid making DQA
2394          * changes in mac80211 layer.
2395          */
2396         if (vif->type == NL80211_IFTYPE_ADHOC)
2397                 mvmvif->cab_queue = IWL_MVM_DQA_GCAST_QUEUE;
2398
2399         /*
2400          * While in previous FWs we had to exclude cab queue from TFD queue
2401          * mask, now it is needed as any other queue.
2402          */
2403         if (!iwl_mvm_has_new_tx_api(mvm) &&
2404             fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE)) {
2405                 iwl_mvm_enable_txq(mvm, NULL, mvmvif->cab_queue, 0, &cfg,
2406                                    timeout);
2407                 msta->tfd_queue_msk |= BIT(mvmvif->cab_queue);
2408         }
2409         ret = iwl_mvm_add_int_sta_common(mvm, msta, maddr,
2410                                          mvmvif->id, mvmvif->color);
2411         if (ret)
2412                 goto err;
2413
2414         /*
2415          * Enable cab queue after the ADD_STA command is sent.
2416          * This is needed for 22000 firmware which won't accept SCD_QUEUE_CFG
2417          * command with unknown station id, and for FW that doesn't support
2418          * station API since the cab queue is not included in the
2419          * tfd_queue_mask.
2420          */
2421         if (iwl_mvm_has_new_tx_api(mvm)) {
2422                 int queue = iwl_mvm_tvqm_enable_txq(mvm, msta->sta_id,
2423                                                     0,
2424                                                     timeout);
2425                 if (queue < 0) {
2426                         ret = queue;
2427                         goto err;
2428                 }
2429                 mvmvif->cab_queue = queue;
2430         } else if (!fw_has_api(&mvm->fw->ucode_capa,
2431                                IWL_UCODE_TLV_API_STA_TYPE))
2432                 iwl_mvm_enable_txq(mvm, NULL, mvmvif->cab_queue, 0, &cfg,
2433                                    timeout);
2434
2435         return 0;
2436 err:
2437         iwl_mvm_dealloc_int_sta(mvm, msta);
2438         return ret;
2439 }
2440
2441 static int __iwl_mvm_remove_sta_key(struct iwl_mvm *mvm, u8 sta_id,
2442                                     struct ieee80211_key_conf *keyconf,
2443                                     bool mcast)
2444 {
2445         union {
2446                 struct iwl_mvm_add_sta_key_cmd_v1 cmd_v1;
2447                 struct iwl_mvm_add_sta_key_cmd cmd;
2448         } u = {};
2449         bool new_api = fw_has_api(&mvm->fw->ucode_capa,
2450                                   IWL_UCODE_TLV_API_TKIP_MIC_KEYS);
2451         __le16 key_flags;
2452         int ret, size;
2453         u32 status;
2454
2455         /* This is a valid situation for GTK removal */
2456         if (sta_id == IWL_MVM_INVALID_STA)
2457                 return 0;
2458
2459         key_flags = cpu_to_le16((keyconf->keyidx << STA_KEY_FLG_KEYID_POS) &
2460                                  STA_KEY_FLG_KEYID_MSK);
2461         key_flags |= cpu_to_le16(STA_KEY_FLG_NO_ENC | STA_KEY_FLG_WEP_KEY_MAP);
2462         key_flags |= cpu_to_le16(STA_KEY_NOT_VALID);
2463
2464         if (mcast)
2465                 key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
2466
2467         /*
2468          * The fields assigned here are in the same location at the start
2469          * of the command, so we can do this union trick.
2470          */
2471         u.cmd.common.key_flags = key_flags;
2472         u.cmd.common.key_offset = keyconf->hw_key_idx;
2473         u.cmd.common.sta_id = sta_id;
2474
2475         size = new_api ? sizeof(u.cmd) : sizeof(u.cmd_v1);
2476
2477         status = ADD_STA_SUCCESS;
2478         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA_KEY, size, &u.cmd,
2479                                           &status);
2480
2481         switch (status) {
2482         case ADD_STA_SUCCESS:
2483                 IWL_DEBUG_WEP(mvm, "MODIFY_STA: remove sta key passed\n");
2484                 break;
2485         default:
2486                 ret = -EIO;
2487                 IWL_ERR(mvm, "MODIFY_STA: remove sta key failed\n");
2488                 break;
2489         }
2490
2491         return ret;
2492 }
2493
2494 /*
2495  * Send the FW a request to remove the station from it's internal data
2496  * structures, and in addition remove it from the local data structure.
2497  */
2498 int iwl_mvm_rm_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2499 {
2500         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2501         int ret;
2502
2503         lockdep_assert_held(&mvm->mutex);
2504
2505         iwl_mvm_flush_sta(mvm, &mvmvif->mcast_sta, true);
2506
2507         iwl_mvm_disable_txq(mvm, NULL, &mvmvif->cab_queue, 0);
2508
2509         ret = iwl_mvm_rm_sta_common(mvm, mvmvif->mcast_sta.sta_id);
2510         if (ret)
2511                 IWL_WARN(mvm, "Failed sending remove station\n");
2512
2513         return ret;
2514 }
2515
2516 static void iwl_mvm_sync_rxq_del_ba(struct iwl_mvm *mvm, u8 baid)
2517 {
2518         struct iwl_mvm_delba_data notif = {
2519                 .baid = baid,
2520         };
2521
2522         iwl_mvm_sync_rx_queues_internal(mvm, IWL_MVM_RXQ_NOTIF_DEL_BA, true,
2523                                         &notif, sizeof(notif));
2524 };
2525
2526 static void iwl_mvm_free_reorder(struct iwl_mvm *mvm,
2527                                  struct iwl_mvm_baid_data *data)
2528 {
2529         int i;
2530
2531         iwl_mvm_sync_rxq_del_ba(mvm, data->baid);
2532
2533         for (i = 0; i < mvm->trans->num_rx_queues; i++) {
2534                 int j;
2535                 struct iwl_mvm_reorder_buffer *reorder_buf =
2536                         &data->reorder_buf[i];
2537                 struct iwl_mvm_reorder_buf_entry *entries =
2538                         &data->entries[i * data->entries_per_queue];
2539
2540                 spin_lock_bh(&reorder_buf->lock);
2541                 if (likely(!reorder_buf->num_stored)) {
2542                         spin_unlock_bh(&reorder_buf->lock);
2543                         continue;
2544                 }
2545
2546                 /*
2547                  * This shouldn't happen in regular DELBA since the internal
2548                  * delBA notification should trigger a release of all frames in
2549                  * the reorder buffer.
2550                  */
2551                 WARN_ON(1);
2552
2553                 for (j = 0; j < reorder_buf->buf_size; j++)
2554                         __skb_queue_purge(&entries[j].e.frames);
2555                 /*
2556                  * Prevent timer re-arm. This prevents a very far fetched case
2557                  * where we timed out on the notification. There may be prior
2558                  * RX frames pending in the RX queue before the notification
2559                  * that might get processed between now and the actual deletion
2560                  * and we would re-arm the timer although we are deleting the
2561                  * reorder buffer.
2562                  */
2563                 reorder_buf->removed = true;
2564                 spin_unlock_bh(&reorder_buf->lock);
2565                 del_timer_sync(&reorder_buf->reorder_timer);
2566         }
2567 }
2568
2569 static void iwl_mvm_init_reorder_buffer(struct iwl_mvm *mvm,
2570                                         struct iwl_mvm_baid_data *data,
2571                                         u16 ssn, u16 buf_size)
2572 {
2573         int i;
2574
2575         for (i = 0; i < mvm->trans->num_rx_queues; i++) {
2576                 struct iwl_mvm_reorder_buffer *reorder_buf =
2577                         &data->reorder_buf[i];
2578                 struct iwl_mvm_reorder_buf_entry *entries =
2579                         &data->entries[i * data->entries_per_queue];
2580                 int j;
2581
2582                 reorder_buf->num_stored = 0;
2583                 reorder_buf->head_sn = ssn;
2584                 reorder_buf->buf_size = buf_size;
2585                 /* rx reorder timer */
2586                 timer_setup(&reorder_buf->reorder_timer,
2587                             iwl_mvm_reorder_timer_expired, 0);
2588                 spin_lock_init(&reorder_buf->lock);
2589                 reorder_buf->mvm = mvm;
2590                 reorder_buf->queue = i;
2591                 reorder_buf->valid = false;
2592                 for (j = 0; j < reorder_buf->buf_size; j++)
2593                         __skb_queue_head_init(&entries[j].e.frames);
2594         }
2595 }
2596
2597 static int iwl_mvm_fw_baid_op_sta(struct iwl_mvm *mvm,
2598                                   struct iwl_mvm_sta *mvm_sta,
2599                                   bool start, int tid, u16 ssn,
2600                                   u16 buf_size)
2601 {
2602         struct iwl_mvm_add_sta_cmd cmd = {
2603                 .mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color),
2604                 .sta_id = mvm_sta->sta_id,
2605                 .add_modify = STA_MODE_MODIFY,
2606         };
2607         u32 status;
2608         int ret;
2609
2610         if (start) {
2611                 cmd.add_immediate_ba_tid = tid;
2612                 cmd.add_immediate_ba_ssn = cpu_to_le16(ssn);
2613                 cmd.rx_ba_window = cpu_to_le16(buf_size);
2614                 cmd.modify_mask = STA_MODIFY_ADD_BA_TID;
2615         } else {
2616                 cmd.remove_immediate_ba_tid = tid;
2617                 cmd.modify_mask = STA_MODIFY_REMOVE_BA_TID;
2618         }
2619
2620         status = ADD_STA_SUCCESS;
2621         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
2622                                           iwl_mvm_add_sta_cmd_size(mvm),
2623                                           &cmd, &status);
2624         if (ret)
2625                 return ret;
2626
2627         switch (status & IWL_ADD_STA_STATUS_MASK) {
2628         case ADD_STA_SUCCESS:
2629                 IWL_DEBUG_HT(mvm, "RX BA Session %sed in fw\n",
2630                              start ? "start" : "stopp");
2631                 if (WARN_ON(start && iwl_mvm_has_new_rx_api(mvm) &&
2632                             !(status & IWL_ADD_STA_BAID_VALID_MASK)))
2633                         return -EINVAL;
2634                 return u32_get_bits(status, IWL_ADD_STA_BAID_MASK);
2635         case ADD_STA_IMMEDIATE_BA_FAILURE:
2636                 IWL_WARN(mvm, "RX BA Session refused by fw\n");
2637                 return -ENOSPC;
2638         default:
2639                 IWL_ERR(mvm, "RX BA Session failed %sing, status 0x%x\n",
2640                         start ? "start" : "stopp", status);
2641                 return -EIO;
2642         }
2643 }
2644
2645 static int iwl_mvm_fw_baid_op_cmd(struct iwl_mvm *mvm,
2646                                   struct iwl_mvm_sta *mvm_sta,
2647                                   bool start, int tid, u16 ssn,
2648                                   u16 buf_size, int baid)
2649 {
2650         struct iwl_rx_baid_cfg_cmd cmd = {
2651                 .action = start ? cpu_to_le32(IWL_RX_BAID_ACTION_ADD) :
2652                                   cpu_to_le32(IWL_RX_BAID_ACTION_REMOVE),
2653         };
2654         u32 cmd_id = WIDE_ID(DATA_PATH_GROUP, RX_BAID_ALLOCATION_CONFIG_CMD);
2655         int ret;
2656
2657         BUILD_BUG_ON(sizeof(struct iwl_rx_baid_cfg_resp) != sizeof(baid));
2658
2659         if (start) {
2660                 cmd.alloc.sta_id_mask = cpu_to_le32(BIT(mvm_sta->sta_id));
2661                 cmd.alloc.tid = tid;
2662                 cmd.alloc.ssn = cpu_to_le16(ssn);
2663                 cmd.alloc.win_size = cpu_to_le16(buf_size);
2664                 baid = -EIO;
2665         } else if (iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 1) == 1) {
2666                 cmd.remove_v1.baid = cpu_to_le32(baid);
2667                 BUILD_BUG_ON(sizeof(cmd.remove_v1) > sizeof(cmd.remove));
2668         } else {
2669                 cmd.remove.sta_id_mask = cpu_to_le32(BIT(mvm_sta->sta_id));
2670                 cmd.remove.tid = cpu_to_le32(tid);
2671         }
2672
2673         ret = iwl_mvm_send_cmd_pdu_status(mvm, cmd_id, sizeof(cmd),
2674                                           &cmd, &baid);
2675         if (ret)
2676                 return ret;
2677
2678         if (!start) {
2679                 /* ignore firmware baid on remove */
2680                 baid = 0;
2681         }
2682
2683         IWL_DEBUG_HT(mvm, "RX BA Session %sed in fw\n",
2684                      start ? "start" : "stopp");
2685
2686         if (baid < 0 || baid >= ARRAY_SIZE(mvm->baid_map))
2687                 return -EINVAL;
2688
2689         return baid;
2690 }
2691
2692 static int iwl_mvm_fw_baid_op(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvm_sta,
2693                               bool start, int tid, u16 ssn, u16 buf_size,
2694                               int baid)
2695 {
2696         if (fw_has_capa(&mvm->fw->ucode_capa,
2697                         IWL_UCODE_TLV_CAPA_BAID_ML_SUPPORT))
2698                 return iwl_mvm_fw_baid_op_cmd(mvm, mvm_sta, start,
2699                                               tid, ssn, buf_size, baid);
2700
2701         return iwl_mvm_fw_baid_op_sta(mvm, mvm_sta, start,
2702                                       tid, ssn, buf_size);
2703 }
2704
2705 int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
2706                        int tid, u16 ssn, bool start, u16 buf_size, u16 timeout)
2707 {
2708         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
2709         struct iwl_mvm_baid_data *baid_data = NULL;
2710         int ret, baid;
2711         u32 max_ba_id_sessions = iwl_mvm_has_new_tx_api(mvm) ? IWL_MAX_BAID :
2712                                                                IWL_MAX_BAID_OLD;
2713
2714         lockdep_assert_held(&mvm->mutex);
2715
2716         if (start && mvm->rx_ba_sessions >= max_ba_id_sessions) {
2717                 IWL_WARN(mvm, "Not enough RX BA SESSIONS\n");
2718                 return -ENOSPC;
2719         }
2720
2721         if (iwl_mvm_has_new_rx_api(mvm) && start) {
2722                 u16 reorder_buf_size = buf_size * sizeof(baid_data->entries[0]);
2723
2724                 /* sparse doesn't like the __align() so don't check */
2725 #ifndef __CHECKER__
2726                 /*
2727                  * The division below will be OK if either the cache line size
2728                  * can be divided by the entry size (ALIGN will round up) or if
2729                  * if the entry size can be divided by the cache line size, in
2730                  * which case the ALIGN() will do nothing.
2731                  */
2732                 BUILD_BUG_ON(SMP_CACHE_BYTES % sizeof(baid_data->entries[0]) &&
2733                              sizeof(baid_data->entries[0]) % SMP_CACHE_BYTES);
2734 #endif
2735
2736                 /*
2737                  * Upward align the reorder buffer size to fill an entire cache
2738                  * line for each queue, to avoid sharing cache lines between
2739                  * different queues.
2740                  */
2741                 reorder_buf_size = ALIGN(reorder_buf_size, SMP_CACHE_BYTES);
2742
2743                 /*
2744                  * Allocate here so if allocation fails we can bail out early
2745                  * before starting the BA session in the firmware
2746                  */
2747                 baid_data = kzalloc(sizeof(*baid_data) +
2748                                     mvm->trans->num_rx_queues *
2749                                     reorder_buf_size,
2750                                     GFP_KERNEL);
2751                 if (!baid_data)
2752                         return -ENOMEM;
2753
2754                 /*
2755                  * This division is why we need the above BUILD_BUG_ON(),
2756                  * if that doesn't hold then this will not be right.
2757                  */
2758                 baid_data->entries_per_queue =
2759                         reorder_buf_size / sizeof(baid_data->entries[0]);
2760         }
2761
2762         if (iwl_mvm_has_new_rx_api(mvm) && !start) {
2763                 baid = mvm_sta->tid_to_baid[tid];
2764         } else {
2765                 /* we don't really need it in this case */
2766                 baid = -1;
2767         }
2768
2769         /* Don't send command to remove (start=0) BAID during restart */
2770         if (start || !test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
2771                 baid = iwl_mvm_fw_baid_op(mvm, mvm_sta, start, tid, ssn, buf_size,
2772                                           baid);
2773
2774         if (baid < 0) {
2775                 ret = baid;
2776                 goto out_free;
2777         }
2778
2779         if (start) {
2780                 mvm->rx_ba_sessions++;
2781
2782                 if (!iwl_mvm_has_new_rx_api(mvm))
2783                         return 0;
2784
2785                 baid_data->baid = baid;
2786                 baid_data->timeout = timeout;
2787                 baid_data->last_rx = jiffies;
2788                 baid_data->rcu_ptr = &mvm->baid_map[baid];
2789                 timer_setup(&baid_data->session_timer,
2790                             iwl_mvm_rx_agg_session_expired, 0);
2791                 baid_data->mvm = mvm;
2792                 baid_data->tid = tid;
2793                 baid_data->sta_id = mvm_sta->sta_id;
2794
2795                 mvm_sta->tid_to_baid[tid] = baid;
2796                 if (timeout)
2797                         mod_timer(&baid_data->session_timer,
2798                                   TU_TO_EXP_TIME(timeout * 2));
2799
2800                 iwl_mvm_init_reorder_buffer(mvm, baid_data, ssn, buf_size);
2801                 /*
2802                  * protect the BA data with RCU to cover a case where our
2803                  * internal RX sync mechanism will timeout (not that it's
2804                  * supposed to happen) and we will free the session data while
2805                  * RX is being processed in parallel
2806                  */
2807                 IWL_DEBUG_HT(mvm, "Sta %d(%d) is assigned to BAID %d\n",
2808                              mvm_sta->sta_id, tid, baid);
2809                 WARN_ON(rcu_access_pointer(mvm->baid_map[baid]));
2810                 rcu_assign_pointer(mvm->baid_map[baid], baid_data);
2811         } else  {
2812                 baid = mvm_sta->tid_to_baid[tid];
2813
2814                 if (mvm->rx_ba_sessions > 0)
2815                         /* check that restart flow didn't zero the counter */
2816                         mvm->rx_ba_sessions--;
2817                 if (!iwl_mvm_has_new_rx_api(mvm))
2818                         return 0;
2819
2820                 if (WARN_ON(baid == IWL_RX_REORDER_DATA_INVALID_BAID))
2821                         return -EINVAL;
2822
2823                 baid_data = rcu_access_pointer(mvm->baid_map[baid]);
2824                 if (WARN_ON(!baid_data))
2825                         return -EINVAL;
2826
2827                 /* synchronize all rx queues so we can safely delete */
2828                 iwl_mvm_free_reorder(mvm, baid_data);
2829                 del_timer_sync(&baid_data->session_timer);
2830                 RCU_INIT_POINTER(mvm->baid_map[baid], NULL);
2831                 kfree_rcu(baid_data, rcu_head);
2832                 IWL_DEBUG_HT(mvm, "BAID %d is free\n", baid);
2833
2834                 /*
2835                  * After we've deleted it, do another queue sync
2836                  * so if an IWL_MVM_RXQ_NSSN_SYNC was concurrently
2837                  * running it won't find a new session in the old
2838                  * BAID. It can find the NULL pointer for the BAID,
2839                  * but we must not have it find a different session.
2840                  */
2841                 iwl_mvm_sync_rx_queues_internal(mvm, IWL_MVM_RXQ_EMPTY,
2842                                                 true, NULL, 0);
2843         }
2844         return 0;
2845
2846 out_free:
2847         kfree(baid_data);
2848         return ret;
2849 }
2850
2851 int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
2852                        int tid, u8 queue, bool start)
2853 {
2854         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
2855         struct iwl_mvm_add_sta_cmd cmd = {};
2856         int ret;
2857         u32 status;
2858
2859         lockdep_assert_held(&mvm->mutex);
2860
2861         if (start) {
2862                 mvm_sta->tfd_queue_msk |= BIT(queue);
2863                 mvm_sta->tid_disable_agg &= ~BIT(tid);
2864         } else {
2865                 /* In DQA-mode the queue isn't removed on agg termination */
2866                 mvm_sta->tid_disable_agg |= BIT(tid);
2867         }
2868
2869         cmd.mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color);
2870         cmd.sta_id = mvm_sta->sta_id;
2871         cmd.add_modify = STA_MODE_MODIFY;
2872         if (!iwl_mvm_has_new_tx_api(mvm))
2873                 cmd.modify_mask = STA_MODIFY_QUEUES;
2874         cmd.modify_mask |= STA_MODIFY_TID_DISABLE_TX;
2875         cmd.tfd_queue_msk = cpu_to_le32(mvm_sta->tfd_queue_msk);
2876         cmd.tid_disable_tx = cpu_to_le16(mvm_sta->tid_disable_agg);
2877
2878         status = ADD_STA_SUCCESS;
2879         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
2880                                           iwl_mvm_add_sta_cmd_size(mvm),
2881                                           &cmd, &status);
2882         if (ret)
2883                 return ret;
2884
2885         switch (status & IWL_ADD_STA_STATUS_MASK) {
2886         case ADD_STA_SUCCESS:
2887                 break;
2888         default:
2889                 ret = -EIO;
2890                 IWL_ERR(mvm, "TX BA Session failed %sing, status 0x%x\n",
2891                         start ? "start" : "stopp", status);
2892                 break;
2893         }
2894
2895         return ret;
2896 }
2897
2898 const u8 tid_to_mac80211_ac[] = {
2899         IEEE80211_AC_BE,
2900         IEEE80211_AC_BK,
2901         IEEE80211_AC_BK,
2902         IEEE80211_AC_BE,
2903         IEEE80211_AC_VI,
2904         IEEE80211_AC_VI,
2905         IEEE80211_AC_VO,
2906         IEEE80211_AC_VO,
2907         IEEE80211_AC_VO, /* We treat MGMT as TID 8, which is set as AC_VO */
2908 };
2909
2910 static const u8 tid_to_ucode_ac[] = {
2911         AC_BE,
2912         AC_BK,
2913         AC_BK,
2914         AC_BE,
2915         AC_VI,
2916         AC_VI,
2917         AC_VO,
2918         AC_VO,
2919 };
2920
2921 int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
2922                              struct ieee80211_sta *sta, u16 tid, u16 *ssn)
2923 {
2924         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
2925         struct iwl_mvm_tid_data *tid_data;
2926         u16 normalized_ssn;
2927         u16 txq_id;
2928         int ret;
2929
2930         if (WARN_ON_ONCE(tid >= IWL_MAX_TID_COUNT))
2931                 return -EINVAL;
2932
2933         if (mvmsta->tid_data[tid].state != IWL_AGG_QUEUED &&
2934             mvmsta->tid_data[tid].state != IWL_AGG_OFF) {
2935                 IWL_ERR(mvm,
2936                         "Start AGG when state is not IWL_AGG_QUEUED or IWL_AGG_OFF %d!\n",
2937                         mvmsta->tid_data[tid].state);
2938                 return -ENXIO;
2939         }
2940
2941         lockdep_assert_held(&mvm->mutex);
2942
2943         if (mvmsta->tid_data[tid].txq_id == IWL_MVM_INVALID_QUEUE &&
2944             iwl_mvm_has_new_tx_api(mvm)) {
2945                 u8 ac = tid_to_mac80211_ac[tid];
2946
2947                 ret = iwl_mvm_sta_alloc_queue_tvqm(mvm, sta, ac, tid);
2948                 if (ret)
2949                         return ret;
2950         }
2951
2952         spin_lock_bh(&mvmsta->lock);
2953
2954         /*
2955          * Note the possible cases:
2956          *  1. An enabled TXQ - TXQ needs to become agg'ed
2957          *  2. The TXQ hasn't yet been enabled, so find a free one and mark
2958          *      it as reserved
2959          */
2960         txq_id = mvmsta->tid_data[tid].txq_id;
2961         if (txq_id == IWL_MVM_INVALID_QUEUE) {
2962                 ret = iwl_mvm_find_free_queue(mvm, mvmsta->sta_id,
2963                                               IWL_MVM_DQA_MIN_DATA_QUEUE,
2964                                               IWL_MVM_DQA_MAX_DATA_QUEUE);
2965                 if (ret < 0) {
2966                         IWL_ERR(mvm, "Failed to allocate agg queue\n");
2967                         goto out;
2968                 }
2969
2970                 txq_id = ret;
2971
2972                 /* TXQ hasn't yet been enabled, so mark it only as reserved */
2973                 mvm->queue_info[txq_id].status = IWL_MVM_QUEUE_RESERVED;
2974         } else if (WARN_ON(txq_id >= IWL_MAX_HW_QUEUES)) {
2975                 ret = -ENXIO;
2976                 IWL_ERR(mvm, "tid_id %d out of range (0, %d)!\n",
2977                         tid, IWL_MAX_HW_QUEUES - 1);
2978                 goto out;
2979
2980         } else if (unlikely(mvm->queue_info[txq_id].status ==
2981                             IWL_MVM_QUEUE_SHARED)) {
2982                 ret = -ENXIO;
2983                 IWL_DEBUG_TX_QUEUES(mvm,
2984                                     "Can't start tid %d agg on shared queue!\n",
2985                                     tid);
2986                 goto out;
2987         }
2988
2989         IWL_DEBUG_TX_QUEUES(mvm,
2990                             "AGG for tid %d will be on queue #%d\n",
2991                             tid, txq_id);
2992
2993         tid_data = &mvmsta->tid_data[tid];
2994         tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
2995         tid_data->txq_id = txq_id;
2996         *ssn = tid_data->ssn;
2997
2998         IWL_DEBUG_TX_QUEUES(mvm,
2999                             "Start AGG: sta %d tid %d queue %d - ssn = %d, next_recl = %d\n",
3000                             mvmsta->sta_id, tid, txq_id, tid_data->ssn,
3001                             tid_data->next_reclaimed);
3002
3003         /*
3004          * In 22000 HW, the next_reclaimed index is only 8 bit, so we'll need
3005          * to align the wrap around of ssn so we compare relevant values.
3006          */
3007         normalized_ssn = tid_data->ssn;
3008         if (mvm->trans->trans_cfg->gen2)
3009                 normalized_ssn &= 0xff;
3010
3011         if (normalized_ssn == tid_data->next_reclaimed) {
3012                 tid_data->state = IWL_AGG_STARTING;
3013                 ret = IEEE80211_AMPDU_TX_START_IMMEDIATE;
3014         } else {
3015                 tid_data->state = IWL_EMPTYING_HW_QUEUE_ADDBA;
3016                 ret = IEEE80211_AMPDU_TX_START_DELAY_ADDBA;
3017         }
3018
3019 out:
3020         spin_unlock_bh(&mvmsta->lock);
3021
3022         return ret;
3023 }
3024
3025 int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
3026                             struct ieee80211_sta *sta, u16 tid, u16 buf_size,
3027                             bool amsdu)
3028 {
3029         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
3030         struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
3031         unsigned int wdg_timeout =
3032                 iwl_mvm_get_wd_timeout(mvm, vif, sta->tdls, false);
3033         int queue, ret;
3034         bool alloc_queue = true;
3035         enum iwl_mvm_queue_status queue_status;
3036         u16 ssn;
3037
3038         struct iwl_trans_txq_scd_cfg cfg = {
3039                 .sta_id = mvmsta->sta_id,
3040                 .tid = tid,
3041                 .frame_limit = buf_size,
3042                 .aggregate = true,
3043         };
3044
3045         /*
3046          * When FW supports TLC_OFFLOAD, it also implements Tx aggregation
3047          * manager, so this function should never be called in this case.
3048          */
3049         if (WARN_ON_ONCE(iwl_mvm_has_tlc_offload(mvm)))
3050                 return -EINVAL;
3051
3052         BUILD_BUG_ON((sizeof(mvmsta->agg_tids) * BITS_PER_BYTE)
3053                      != IWL_MAX_TID_COUNT);
3054
3055         spin_lock_bh(&mvmsta->lock);
3056         ssn = tid_data->ssn;
3057         queue = tid_data->txq_id;
3058         tid_data->state = IWL_AGG_ON;
3059         mvmsta->agg_tids |= BIT(tid);
3060         tid_data->ssn = 0xffff;
3061         tid_data->amsdu_in_ampdu_allowed = amsdu;
3062         spin_unlock_bh(&mvmsta->lock);
3063
3064         if (iwl_mvm_has_new_tx_api(mvm)) {
3065                 /*
3066                  * If there is no queue for this tid, iwl_mvm_sta_tx_agg_start()
3067                  * would have failed, so if we are here there is no need to
3068                  * allocate a queue.
3069                  * However, if aggregation size is different than the default
3070                  * size, the scheduler should be reconfigured.
3071                  * We cannot do this with the new TX API, so return unsupported
3072                  * for now, until it will be offloaded to firmware..
3073                  * Note that if SCD default value changes - this condition
3074                  * should be updated as well.
3075                  */
3076                 if (buf_size < IWL_FRAME_LIMIT)
3077                         return -ENOTSUPP;
3078
3079                 ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
3080                 if (ret)
3081                         return -EIO;
3082                 goto out;
3083         }
3084
3085         cfg.fifo = iwl_mvm_ac_to_tx_fifo[tid_to_mac80211_ac[tid]];
3086
3087         queue_status = mvm->queue_info[queue].status;
3088
3089         /* Maybe there is no need to even alloc a queue... */
3090         if (mvm->queue_info[queue].status == IWL_MVM_QUEUE_READY)
3091                 alloc_queue = false;
3092
3093         /*
3094          * Only reconfig the SCD for the queue if the window size has
3095          * changed from current (become smaller)
3096          */
3097         if (!alloc_queue && buf_size < IWL_FRAME_LIMIT) {
3098                 /*
3099                  * If reconfiguring an existing queue, it first must be
3100                  * drained
3101                  */
3102                 ret = iwl_trans_wait_tx_queues_empty(mvm->trans,
3103                                                      BIT(queue));
3104                 if (ret) {
3105                         IWL_ERR(mvm,
3106                                 "Error draining queue before reconfig\n");
3107                         return ret;
3108                 }
3109
3110                 ret = iwl_mvm_reconfig_scd(mvm, queue, cfg.fifo,
3111                                            mvmsta->sta_id, tid,
3112                                            buf_size, ssn);
3113                 if (ret) {
3114                         IWL_ERR(mvm,
3115                                 "Error reconfiguring TXQ #%d\n", queue);
3116                         return ret;
3117                 }
3118         }
3119
3120         if (alloc_queue)
3121                 iwl_mvm_enable_txq(mvm, sta, queue, ssn,
3122                                    &cfg, wdg_timeout);
3123
3124         /* Send ADD_STA command to enable aggs only if the queue isn't shared */
3125         if (queue_status != IWL_MVM_QUEUE_SHARED) {
3126                 ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
3127                 if (ret)
3128                         return -EIO;
3129         }
3130
3131         /* No need to mark as reserved */
3132         mvm->queue_info[queue].status = IWL_MVM_QUEUE_READY;
3133
3134 out:
3135         /*
3136          * Even though in theory the peer could have different
3137          * aggregation reorder buffer sizes for different sessions,
3138          * our ucode doesn't allow for that and has a global limit
3139          * for each station. Therefore, use the minimum of all the
3140          * aggregation sessions and our default value.
3141          */
3142         mvmsta->max_agg_bufsize =
3143                 min(mvmsta->max_agg_bufsize, buf_size);
3144         mvmsta->lq_sta.rs_drv.lq.agg_frame_cnt_limit = mvmsta->max_agg_bufsize;
3145
3146         IWL_DEBUG_HT(mvm, "Tx aggregation enabled on ra = %pM tid = %d\n",
3147                      sta->addr, tid);
3148
3149         return iwl_mvm_send_lq_cmd(mvm, &mvmsta->lq_sta.rs_drv.lq);
3150 }
3151
3152 static void iwl_mvm_unreserve_agg_queue(struct iwl_mvm *mvm,
3153                                         struct iwl_mvm_sta *mvmsta,
3154                                         struct iwl_mvm_tid_data *tid_data)
3155 {
3156         u16 txq_id = tid_data->txq_id;
3157
3158         lockdep_assert_held(&mvm->mutex);
3159
3160         if (iwl_mvm_has_new_tx_api(mvm))
3161                 return;
3162
3163         /*
3164          * The TXQ is marked as reserved only if no traffic came through yet
3165          * This means no traffic has been sent on this TID (agg'd or not), so
3166          * we no longer have use for the queue. Since it hasn't even been
3167          * allocated through iwl_mvm_enable_txq, so we can just mark it back as
3168          * free.
3169          */
3170         if (mvm->queue_info[txq_id].status == IWL_MVM_QUEUE_RESERVED) {
3171                 mvm->queue_info[txq_id].status = IWL_MVM_QUEUE_FREE;
3172                 tid_data->txq_id = IWL_MVM_INVALID_QUEUE;
3173         }
3174 }
3175
3176 int iwl_mvm_sta_tx_agg_stop(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
3177                             struct ieee80211_sta *sta, u16 tid)
3178 {
3179         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
3180         struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
3181         u16 txq_id;
3182         int err;
3183
3184         /*
3185          * If mac80211 is cleaning its state, then say that we finished since
3186          * our state has been cleared anyway.
3187          */
3188         if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
3189                 ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
3190                 return 0;
3191         }
3192
3193         spin_lock_bh(&mvmsta->lock);
3194
3195         txq_id = tid_data->txq_id;
3196
3197         IWL_DEBUG_TX_QUEUES(mvm, "Stop AGG: sta %d tid %d q %d state %d\n",
3198                             mvmsta->sta_id, tid, txq_id, tid_data->state);
3199
3200         mvmsta->agg_tids &= ~BIT(tid);
3201
3202         iwl_mvm_unreserve_agg_queue(mvm, mvmsta, tid_data);
3203
3204         switch (tid_data->state) {
3205         case IWL_AGG_ON:
3206                 tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
3207
3208                 IWL_DEBUG_TX_QUEUES(mvm,
3209                                     "ssn = %d, next_recl = %d\n",
3210                                     tid_data->ssn, tid_data->next_reclaimed);
3211
3212                 tid_data->ssn = 0xffff;
3213                 tid_data->state = IWL_AGG_OFF;
3214                 spin_unlock_bh(&mvmsta->lock);
3215
3216                 ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
3217
3218                 iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
3219                 return 0;
3220         case IWL_AGG_STARTING:
3221         case IWL_EMPTYING_HW_QUEUE_ADDBA:
3222                 /*
3223                  * The agg session has been stopped before it was set up. This
3224                  * can happen when the AddBA timer times out for example.
3225                  */
3226
3227                 /* No barriers since we are under mutex */
3228                 lockdep_assert_held(&mvm->mutex);
3229
3230                 ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
3231                 tid_data->state = IWL_AGG_OFF;
3232                 err = 0;
3233                 break;
3234         default:
3235                 IWL_ERR(mvm,
3236                         "Stopping AGG while state not ON or starting for %d on %d (%d)\n",
3237                         mvmsta->sta_id, tid, tid_data->state);
3238                 IWL_ERR(mvm,
3239                         "\ttid_data->txq_id = %d\n", tid_data->txq_id);
3240                 err = -EINVAL;
3241         }
3242
3243         spin_unlock_bh(&mvmsta->lock);
3244
3245         return err;
3246 }
3247
3248 int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
3249                             struct ieee80211_sta *sta, u16 tid)
3250 {
3251         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
3252         struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
3253         u16 txq_id;
3254         enum iwl_mvm_agg_state old_state;
3255
3256         /*
3257          * First set the agg state to OFF to avoid calling
3258          * ieee80211_stop_tx_ba_cb in iwl_mvm_check_ratid_empty.
3259          */
3260         spin_lock_bh(&mvmsta->lock);
3261         txq_id = tid_data->txq_id;
3262         IWL_DEBUG_TX_QUEUES(mvm, "Flush AGG: sta %d tid %d q %d state %d\n",
3263                             mvmsta->sta_id, tid, txq_id, tid_data->state);
3264         old_state = tid_data->state;
3265         tid_data->state = IWL_AGG_OFF;
3266         mvmsta->agg_tids &= ~BIT(tid);
3267         spin_unlock_bh(&mvmsta->lock);
3268
3269         iwl_mvm_unreserve_agg_queue(mvm, mvmsta, tid_data);
3270
3271         if (old_state >= IWL_AGG_ON) {
3272                 iwl_mvm_drain_sta(mvm, mvmsta, true);
3273
3274                 if (iwl_mvm_has_new_tx_api(mvm)) {
3275                         if (iwl_mvm_flush_sta_tids(mvm, mvmsta->sta_id,
3276                                                    BIT(tid)))
3277                                 IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
3278                         iwl_trans_wait_txq_empty(mvm->trans, txq_id);
3279                 } else {
3280                         if (iwl_mvm_flush_tx_path(mvm, BIT(txq_id)))
3281                                 IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
3282                         iwl_trans_wait_tx_queues_empty(mvm->trans, BIT(txq_id));
3283                 }
3284
3285                 iwl_mvm_drain_sta(mvm, mvmsta, false);
3286
3287                 iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
3288         }
3289
3290         return 0;
3291 }
3292
3293 static int iwl_mvm_set_fw_key_idx(struct iwl_mvm *mvm)
3294 {
3295         int i, max = -1, max_offs = -1;
3296
3297         lockdep_assert_held(&mvm->mutex);
3298
3299         /* Pick the unused key offset with the highest 'deleted'
3300          * counter. Every time a key is deleted, all the counters
3301          * are incremented and the one that was just deleted is
3302          * reset to zero. Thus, the highest counter is the one
3303          * that was deleted longest ago. Pick that one.
3304          */
3305         for (i = 0; i < STA_KEY_MAX_NUM; i++) {
3306                 if (test_bit(i, mvm->fw_key_table))
3307                         continue;
3308                 if (mvm->fw_key_deleted[i] > max) {
3309                         max = mvm->fw_key_deleted[i];
3310                         max_offs = i;
3311                 }
3312         }
3313
3314         if (max_offs < 0)
3315                 return STA_KEY_IDX_INVALID;
3316
3317         return max_offs;
3318 }
3319
3320 static struct iwl_mvm_sta *iwl_mvm_get_key_sta(struct iwl_mvm *mvm,
3321                                                struct ieee80211_vif *vif,
3322                                                struct ieee80211_sta *sta)
3323 {
3324         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
3325
3326         if (sta)
3327                 return iwl_mvm_sta_from_mac80211(sta);
3328
3329         /*
3330          * The device expects GTKs for station interfaces to be
3331          * installed as GTKs for the AP station. If we have no
3332          * station ID, then use AP's station ID.
3333          */
3334         if (vif->type == NL80211_IFTYPE_STATION &&
3335             mvmvif->ap_sta_id != IWL_MVM_INVALID_STA) {
3336                 u8 sta_id = mvmvif->ap_sta_id;
3337
3338                 sta = rcu_dereference_check(mvm->fw_id_to_mac_id[sta_id],
3339                                             lockdep_is_held(&mvm->mutex));
3340
3341                 /*
3342                  * It is possible that the 'sta' parameter is NULL,
3343                  * for example when a GTK is removed - the sta_id will then
3344                  * be the AP ID, and no station was passed by mac80211.
3345                  */
3346                 if (IS_ERR_OR_NULL(sta))
3347                         return NULL;
3348
3349                 return iwl_mvm_sta_from_mac80211(sta);
3350         }
3351
3352         return NULL;
3353 }
3354
3355 static int iwl_mvm_pn_cmp(const u8 *pn1, const u8 *pn2, int len)
3356 {
3357         int i;
3358
3359         for (i = len - 1; i >= 0; i--) {
3360                 if (pn1[i] > pn2[i])
3361                         return 1;
3362                 if (pn1[i] < pn2[i])
3363                         return -1;
3364         }
3365
3366         return 0;
3367 }
3368
3369 static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
3370                                 u32 sta_id,
3371                                 struct ieee80211_key_conf *key, bool mcast,
3372                                 u32 tkip_iv32, u16 *tkip_p1k, u32 cmd_flags,
3373                                 u8 key_offset, bool mfp)
3374 {
3375         union {
3376                 struct iwl_mvm_add_sta_key_cmd_v1 cmd_v1;
3377                 struct iwl_mvm_add_sta_key_cmd cmd;
3378         } u = {};
3379         __le16 key_flags;
3380         int ret;
3381         u32 status;
3382         u16 keyidx;
3383         u64 pn = 0;
3384         int i, size;
3385         bool new_api = fw_has_api(&mvm->fw->ucode_capa,
3386                                   IWL_UCODE_TLV_API_TKIP_MIC_KEYS);
3387         int api_ver = iwl_fw_lookup_cmd_ver(mvm->fw, ADD_STA_KEY,
3388                                             new_api ? 2 : 1);
3389
3390         if (sta_id == IWL_MVM_INVALID_STA)
3391                 return -EINVAL;
3392
3393         keyidx = (key->keyidx << STA_KEY_FLG_KEYID_POS) &
3394                  STA_KEY_FLG_KEYID_MSK;
3395         key_flags = cpu_to_le16(keyidx);
3396         key_flags |= cpu_to_le16(STA_KEY_FLG_WEP_KEY_MAP);
3397
3398         switch (key->cipher) {
3399         case WLAN_CIPHER_SUITE_TKIP:
3400                 key_flags |= cpu_to_le16(STA_KEY_FLG_TKIP);
3401                 if (api_ver >= 2) {
3402                         memcpy((void *)&u.cmd.tx_mic_key,
3403                                &key->key[NL80211_TKIP_DATA_OFFSET_TX_MIC_KEY],
3404                                IWL_MIC_KEY_SIZE);
3405
3406                         memcpy((void *)&u.cmd.rx_mic_key,
3407                                &key->key[NL80211_TKIP_DATA_OFFSET_RX_MIC_KEY],
3408                                IWL_MIC_KEY_SIZE);
3409                         pn = atomic64_read(&key->tx_pn);
3410
3411                 } else {
3412                         u.cmd_v1.tkip_rx_tsc_byte2 = tkip_iv32;
3413                         for (i = 0; i < 5; i++)
3414                                 u.cmd_v1.tkip_rx_ttak[i] =
3415                                         cpu_to_le16(tkip_p1k[i]);
3416                 }
3417                 memcpy(u.cmd.common.key, key->key, key->keylen);
3418                 break;
3419         case WLAN_CIPHER_SUITE_CCMP:
3420                 key_flags |= cpu_to_le16(STA_KEY_FLG_CCM);
3421                 memcpy(u.cmd.common.key, key->key, key->keylen);
3422                 if (api_ver >= 2)
3423                         pn = atomic64_read(&key->tx_pn);
3424                 break;
3425         case WLAN_CIPHER_SUITE_WEP104:
3426                 key_flags |= cpu_to_le16(STA_KEY_FLG_WEP_13BYTES);
3427                 fallthrough;
3428         case WLAN_CIPHER_SUITE_WEP40:
3429                 key_flags |= cpu_to_le16(STA_KEY_FLG_WEP);
3430                 memcpy(u.cmd.common.key + 3, key->key, key->keylen);
3431                 break;
3432         case WLAN_CIPHER_SUITE_GCMP_256:
3433                 key_flags |= cpu_to_le16(STA_KEY_FLG_KEY_32BYTES);
3434                 fallthrough;
3435         case WLAN_CIPHER_SUITE_GCMP:
3436                 key_flags |= cpu_to_le16(STA_KEY_FLG_GCMP);
3437                 memcpy(u.cmd.common.key, key->key, key->keylen);
3438                 if (api_ver >= 2)
3439                         pn = atomic64_read(&key->tx_pn);
3440                 break;
3441         default:
3442                 key_flags |= cpu_to_le16(STA_KEY_FLG_EXT);
3443                 memcpy(u.cmd.common.key, key->key, key->keylen);
3444         }
3445
3446         if (mcast)
3447                 key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
3448         if (mfp)
3449                 key_flags |= cpu_to_le16(STA_KEY_MFP);
3450
3451         u.cmd.common.key_offset = key_offset;
3452         u.cmd.common.key_flags = key_flags;
3453         u.cmd.common.sta_id = sta_id;
3454
3455         if (key->cipher == WLAN_CIPHER_SUITE_TKIP)
3456                 i = 0;
3457         else
3458                 i = -1;
3459
3460         for (; i < IEEE80211_NUM_TIDS; i++) {
3461                 struct ieee80211_key_seq seq = {};
3462                 u8 _rx_pn[IEEE80211_MAX_PN_LEN] = {}, *rx_pn = _rx_pn;
3463                 int rx_pn_len = 8;
3464                 /* there's a hole at 2/3 in FW format depending on version */
3465                 int hole = api_ver >= 3 ? 0 : 2;
3466
3467                 ieee80211_get_key_rx_seq(key, i, &seq);
3468
3469                 if (key->cipher == WLAN_CIPHER_SUITE_TKIP) {
3470                         rx_pn[0] = seq.tkip.iv16;
3471                         rx_pn[1] = seq.tkip.iv16 >> 8;
3472                         rx_pn[2 + hole] = seq.tkip.iv32;
3473                         rx_pn[3 + hole] = seq.tkip.iv32 >> 8;
3474                         rx_pn[4 + hole] = seq.tkip.iv32 >> 16;
3475                         rx_pn[5 + hole] = seq.tkip.iv32 >> 24;
3476                 } else if (key_flags & cpu_to_le16(STA_KEY_FLG_EXT)) {
3477                         rx_pn = seq.hw.seq;
3478                         rx_pn_len = seq.hw.seq_len;
3479                 } else {
3480                         rx_pn[0] = seq.ccmp.pn[0];
3481                         rx_pn[1] = seq.ccmp.pn[1];
3482                         rx_pn[2 + hole] = seq.ccmp.pn[2];
3483                         rx_pn[3 + hole] = seq.ccmp.pn[3];
3484                         rx_pn[4 + hole] = seq.ccmp.pn[4];
3485                         rx_pn[5 + hole] = seq.ccmp.pn[5];
3486                 }
3487
3488                 if (iwl_mvm_pn_cmp(rx_pn, (u8 *)&u.cmd.common.rx_secur_seq_cnt,
3489                                    rx_pn_len) > 0)
3490                         memcpy(&u.cmd.common.rx_secur_seq_cnt, rx_pn,
3491                                rx_pn_len);
3492         }
3493
3494         if (api_ver >= 2) {
3495                 u.cmd.transmit_seq_cnt = cpu_to_le64(pn);
3496                 size = sizeof(u.cmd);
3497         } else {
3498                 size = sizeof(u.cmd_v1);
3499         }
3500
3501         status = ADD_STA_SUCCESS;
3502         if (cmd_flags & CMD_ASYNC)
3503                 ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA_KEY, CMD_ASYNC, size,
3504                                            &u.cmd);
3505         else
3506                 ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA_KEY, size,
3507                                                   &u.cmd, &status);
3508
3509         switch (status) {
3510         case ADD_STA_SUCCESS:
3511                 IWL_DEBUG_WEP(mvm, "MODIFY_STA: set dynamic key passed\n");
3512                 break;
3513         default:
3514                 ret = -EIO;
3515                 IWL_ERR(mvm, "MODIFY_STA: set dynamic key failed\n");
3516                 break;
3517         }
3518
3519         return ret;
3520 }
3521
3522 static int iwl_mvm_send_sta_igtk(struct iwl_mvm *mvm,
3523                                  struct ieee80211_key_conf *keyconf,
3524                                  u8 sta_id, bool remove_key)
3525 {
3526         struct iwl_mvm_mgmt_mcast_key_cmd igtk_cmd = {};
3527
3528         /* verify the key details match the required command's expectations */
3529         if (WARN_ON((keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE) ||
3530                     (keyconf->keyidx != 4 && keyconf->keyidx != 5 &&
3531                      keyconf->keyidx != 6 && keyconf->keyidx != 7) ||
3532                     (keyconf->cipher != WLAN_CIPHER_SUITE_AES_CMAC &&
3533                      keyconf->cipher != WLAN_CIPHER_SUITE_BIP_GMAC_128 &&
3534                      keyconf->cipher != WLAN_CIPHER_SUITE_BIP_GMAC_256)))
3535                 return -EINVAL;
3536
3537         if (WARN_ON(!iwl_mvm_has_new_rx_api(mvm) &&
3538                     keyconf->cipher != WLAN_CIPHER_SUITE_AES_CMAC))
3539                 return -EINVAL;
3540
3541         igtk_cmd.key_id = cpu_to_le32(keyconf->keyidx);
3542         igtk_cmd.sta_id = cpu_to_le32(sta_id);
3543
3544         if (remove_key) {
3545                 /* This is a valid situation for IGTK */
3546                 if (sta_id == IWL_MVM_INVALID_STA)
3547                         return 0;
3548
3549                 igtk_cmd.ctrl_flags |= cpu_to_le32(STA_KEY_NOT_VALID);
3550         } else {
3551                 struct ieee80211_key_seq seq;
3552                 const u8 *pn;
3553
3554                 switch (keyconf->cipher) {
3555                 case WLAN_CIPHER_SUITE_AES_CMAC:
3556                         igtk_cmd.ctrl_flags |= cpu_to_le32(STA_KEY_FLG_CCM);
3557                         break;
3558                 case WLAN_CIPHER_SUITE_BIP_GMAC_128:
3559                 case WLAN_CIPHER_SUITE_BIP_GMAC_256:
3560                         igtk_cmd.ctrl_flags |= cpu_to_le32(STA_KEY_FLG_GCMP);
3561                         break;
3562                 default:
3563                         return -EINVAL;
3564                 }
3565
3566                 memcpy(igtk_cmd.igtk, keyconf->key, keyconf->keylen);
3567                 if (keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_256)
3568                         igtk_cmd.ctrl_flags |=
3569                                 cpu_to_le32(STA_KEY_FLG_KEY_32BYTES);
3570                 ieee80211_get_key_rx_seq(keyconf, 0, &seq);
3571                 pn = seq.aes_cmac.pn;
3572                 igtk_cmd.receive_seq_cnt = cpu_to_le64(((u64) pn[5] << 0) |
3573                                                        ((u64) pn[4] << 8) |
3574                                                        ((u64) pn[3] << 16) |
3575                                                        ((u64) pn[2] << 24) |
3576                                                        ((u64) pn[1] << 32) |
3577                                                        ((u64) pn[0] << 40));
3578         }
3579
3580         IWL_DEBUG_INFO(mvm, "%s %sIGTK (%d) for sta %u\n",
3581                        remove_key ? "removing" : "installing",
3582                        keyconf->keyidx >= 6 ? "B" : "",
3583                        keyconf->keyidx, igtk_cmd.sta_id);
3584
3585         if (!iwl_mvm_has_new_rx_api(mvm)) {
3586                 struct iwl_mvm_mgmt_mcast_key_cmd_v1 igtk_cmd_v1 = {
3587                         .ctrl_flags = igtk_cmd.ctrl_flags,
3588                         .key_id = igtk_cmd.key_id,
3589                         .sta_id = igtk_cmd.sta_id,
3590                         .receive_seq_cnt = igtk_cmd.receive_seq_cnt
3591                 };
3592
3593                 memcpy(igtk_cmd_v1.igtk, igtk_cmd.igtk,
3594                        ARRAY_SIZE(igtk_cmd_v1.igtk));
3595                 return iwl_mvm_send_cmd_pdu(mvm, MGMT_MCAST_KEY, 0,
3596                                             sizeof(igtk_cmd_v1), &igtk_cmd_v1);
3597         }
3598         return iwl_mvm_send_cmd_pdu(mvm, MGMT_MCAST_KEY, 0,
3599                                     sizeof(igtk_cmd), &igtk_cmd);
3600 }
3601
3602
3603 static inline u8 *iwl_mvm_get_mac_addr(struct iwl_mvm *mvm,
3604                                        struct ieee80211_vif *vif,
3605                                        struct ieee80211_sta *sta)
3606 {
3607         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
3608
3609         if (sta)
3610                 return sta->addr;
3611
3612         if (vif->type == NL80211_IFTYPE_STATION &&
3613             mvmvif->ap_sta_id != IWL_MVM_INVALID_STA) {
3614                 u8 sta_id = mvmvif->ap_sta_id;
3615                 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
3616                                                 lockdep_is_held(&mvm->mutex));
3617                 return sta->addr;
3618         }
3619
3620
3621         return NULL;
3622 }
3623
3624 static int __iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
3625                                  struct ieee80211_vif *vif,
3626                                  struct ieee80211_sta *sta,
3627                                  struct ieee80211_key_conf *keyconf,
3628                                  u8 key_offset,
3629                                  bool mcast)
3630 {
3631         const u8 *addr;
3632         struct ieee80211_key_seq seq;
3633         u16 p1k[5];
3634         u32 sta_id;
3635         bool mfp = false;
3636
3637         if (sta) {
3638                 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
3639
3640                 sta_id = mvm_sta->sta_id;
3641                 mfp = sta->mfp;
3642         } else if (vif->type == NL80211_IFTYPE_AP &&
3643                    !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE)) {
3644                 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
3645
3646                 sta_id = mvmvif->mcast_sta.sta_id;
3647         } else {
3648                 IWL_ERR(mvm, "Failed to find station id\n");
3649                 return -EINVAL;
3650         }
3651
3652         if (keyconf->cipher == WLAN_CIPHER_SUITE_TKIP) {
3653                 addr = iwl_mvm_get_mac_addr(mvm, vif, sta);
3654                 /* get phase 1 key from mac80211 */
3655                 ieee80211_get_key_rx_seq(keyconf, 0, &seq);
3656                 ieee80211_get_tkip_rx_p1k(keyconf, addr, seq.tkip.iv32, p1k);
3657
3658                 return iwl_mvm_send_sta_key(mvm, sta_id, keyconf, mcast,
3659                                             seq.tkip.iv32, p1k, 0, key_offset,
3660                                             mfp);
3661         }
3662
3663         return iwl_mvm_send_sta_key(mvm, sta_id, keyconf, mcast,
3664                                     0, NULL, 0, key_offset, mfp);
3665 }
3666
3667 int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
3668                         struct ieee80211_vif *vif,
3669                         struct ieee80211_sta *sta,
3670                         struct ieee80211_key_conf *keyconf,
3671                         u8 key_offset)
3672 {
3673         bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
3674         struct iwl_mvm_sta *mvm_sta;
3675         u8 sta_id = IWL_MVM_INVALID_STA;
3676         int ret;
3677         static const u8 __maybe_unused zero_addr[ETH_ALEN] = {0};
3678
3679         lockdep_assert_held(&mvm->mutex);
3680
3681         if (vif->type != NL80211_IFTYPE_AP ||
3682             keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
3683                 /* Get the station id from the mvm local station table */
3684                 mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
3685                 if (!mvm_sta) {
3686                         IWL_ERR(mvm, "Failed to find station\n");
3687                         return -EINVAL;
3688                 }
3689                 sta_id = mvm_sta->sta_id;
3690
3691                 /*
3692                  * It is possible that the 'sta' parameter is NULL, and thus
3693                  * there is a need to retrieve the sta from the local station
3694                  * table.
3695                  */
3696                 if (!sta) {
3697                         sta = rcu_dereference_protected(
3698                                 mvm->fw_id_to_mac_id[sta_id],
3699                                 lockdep_is_held(&mvm->mutex));
3700                         if (IS_ERR_OR_NULL(sta)) {
3701                                 IWL_ERR(mvm, "Invalid station id\n");
3702                                 return -EINVAL;
3703                         }
3704                 }
3705
3706                 if (WARN_ON_ONCE(iwl_mvm_sta_from_mac80211(sta)->vif != vif))
3707                         return -EINVAL;
3708         } else {
3709                 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
3710
3711                 sta_id = mvmvif->mcast_sta.sta_id;
3712         }
3713
3714         if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC ||
3715             keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_128 ||
3716             keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_256) {
3717                 ret = iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, false);
3718                 goto end;
3719         }
3720
3721         /* If the key_offset is not pre-assigned, we need to find a
3722          * new offset to use.  In normal cases, the offset is not
3723          * pre-assigned, but during HW_RESTART we want to reuse the
3724          * same indices, so we pass them when this function is called.
3725          *
3726          * In D3 entry, we need to hardcoded the indices (because the
3727          * firmware hardcodes the PTK offset to 0).  In this case, we
3728          * need to make sure we don't overwrite the hw_key_idx in the
3729          * keyconf structure, because otherwise we cannot configure
3730          * the original ones back when resuming.
3731          */
3732         if (key_offset == STA_KEY_IDX_INVALID) {
3733                 key_offset  = iwl_mvm_set_fw_key_idx(mvm);
3734                 if (key_offset == STA_KEY_IDX_INVALID)
3735                         return -ENOSPC;
3736                 keyconf->hw_key_idx = key_offset;
3737         }
3738
3739         ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf, key_offset, mcast);
3740         if (ret)
3741                 goto end;
3742
3743         /*
3744          * For WEP, the same key is used for multicast and unicast. Upload it
3745          * again, using the same key offset, and now pointing the other one
3746          * to the same key slot (offset).
3747          * If this fails, remove the original as well.
3748          */
3749         if ((keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
3750              keyconf->cipher == WLAN_CIPHER_SUITE_WEP104) &&
3751             sta) {
3752                 ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf,
3753                                             key_offset, !mcast);
3754                 if (ret) {
3755                         __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
3756                         goto end;
3757                 }
3758         }
3759
3760         __set_bit(key_offset, mvm->fw_key_table);
3761
3762 end:
3763         IWL_DEBUG_WEP(mvm, "key: cipher=%x len=%d idx=%d sta=%pM ret=%d\n",
3764                       keyconf->cipher, keyconf->keylen, keyconf->keyidx,
3765                       sta ? sta->addr : zero_addr, ret);
3766         return ret;
3767 }
3768
3769 int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
3770                            struct ieee80211_vif *vif,
3771                            struct ieee80211_sta *sta,
3772                            struct ieee80211_key_conf *keyconf)
3773 {
3774         bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
3775         struct iwl_mvm_sta *mvm_sta;
3776         u8 sta_id = IWL_MVM_INVALID_STA;
3777         int ret, i;
3778
3779         lockdep_assert_held(&mvm->mutex);
3780
3781         /* Get the station from the mvm local station table */
3782         mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
3783         if (mvm_sta)
3784                 sta_id = mvm_sta->sta_id;
3785         else if (!sta && vif->type == NL80211_IFTYPE_AP && mcast)
3786                 sta_id = iwl_mvm_vif_from_mac80211(vif)->mcast_sta.sta_id;
3787
3788
3789         IWL_DEBUG_WEP(mvm, "mvm remove dynamic key: idx=%d sta=%d\n",
3790                       keyconf->keyidx, sta_id);
3791
3792         if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC ||
3793             keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_128 ||
3794             keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_256)
3795                 return iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, true);
3796
3797         if (!__test_and_clear_bit(keyconf->hw_key_idx, mvm->fw_key_table)) {
3798                 IWL_ERR(mvm, "offset %d not used in fw key table.\n",
3799                         keyconf->hw_key_idx);
3800                 return -ENOENT;
3801         }
3802
3803         /* track which key was deleted last */
3804         for (i = 0; i < STA_KEY_MAX_NUM; i++) {
3805                 if (mvm->fw_key_deleted[i] < U8_MAX)
3806                         mvm->fw_key_deleted[i]++;
3807         }
3808         mvm->fw_key_deleted[keyconf->hw_key_idx] = 0;
3809
3810         if (sta && !mvm_sta) {
3811                 IWL_DEBUG_WEP(mvm, "station non-existent, early return.\n");
3812                 return 0;
3813         }
3814
3815         ret = __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
3816         if (ret)
3817                 return ret;
3818
3819         /* delete WEP key twice to get rid of (now useless) offset */
3820         if (keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
3821             keyconf->cipher == WLAN_CIPHER_SUITE_WEP104)
3822                 ret = __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, !mcast);
3823
3824         return ret;
3825 }
3826
3827 void iwl_mvm_update_tkip_key(struct iwl_mvm *mvm,
3828                              struct ieee80211_vif *vif,
3829                              struct ieee80211_key_conf *keyconf,
3830                              struct ieee80211_sta *sta, u32 iv32,
3831                              u16 *phase1key)
3832 {
3833         struct iwl_mvm_sta *mvm_sta;
3834         bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
3835         bool mfp = sta ? sta->mfp : false;
3836
3837         rcu_read_lock();
3838
3839         mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
3840         if (WARN_ON_ONCE(!mvm_sta))
3841                 goto unlock;
3842         iwl_mvm_send_sta_key(mvm, mvm_sta->sta_id, keyconf, mcast,
3843                              iv32, phase1key, CMD_ASYNC, keyconf->hw_key_idx,
3844                              mfp);
3845
3846  unlock:
3847         rcu_read_unlock();
3848 }
3849
3850 void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
3851                                 struct ieee80211_sta *sta)
3852 {
3853         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
3854         struct iwl_mvm_add_sta_cmd cmd = {
3855                 .add_modify = STA_MODE_MODIFY,
3856                 .sta_id = mvmsta->sta_id,
3857                 .station_flags_msk = cpu_to_le32(STA_FLG_PS),
3858                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
3859         };
3860         int ret;
3861
3862         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
3863                                    iwl_mvm_add_sta_cmd_size(mvm), &cmd);
3864         if (ret)
3865                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
3866 }
3867
3868 void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
3869                                        struct ieee80211_sta *sta,
3870                                        enum ieee80211_frame_release_type reason,
3871                                        u16 cnt, u16 tids, bool more_data,
3872                                        bool single_sta_queue)
3873 {
3874         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
3875         struct iwl_mvm_add_sta_cmd cmd = {
3876                 .add_modify = STA_MODE_MODIFY,
3877                 .sta_id = mvmsta->sta_id,
3878                 .modify_mask = STA_MODIFY_SLEEPING_STA_TX_COUNT,
3879                 .sleep_tx_count = cpu_to_le16(cnt),
3880                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
3881         };
3882         int tid, ret;
3883         unsigned long _tids = tids;
3884
3885         /* convert TIDs to ACs - we don't support TSPEC so that's OK
3886          * Note that this field is reserved and unused by firmware not
3887          * supporting GO uAPSD, so it's safe to always do this.
3888          */
3889         for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT)
3890                 cmd.awake_acs |= BIT(tid_to_ucode_ac[tid]);
3891
3892         /* If we're releasing frames from aggregation or dqa queues then check
3893          * if all the queues that we're releasing frames from, combined, have:
3894          *  - more frames than the service period, in which case more_data
3895          *    needs to be set
3896          *  - fewer than 'cnt' frames, in which case we need to adjust the
3897          *    firmware command (but do that unconditionally)
3898          */
3899         if (single_sta_queue) {
3900                 int remaining = cnt;
3901                 int sleep_tx_count;
3902
3903                 spin_lock_bh(&mvmsta->lock);
3904                 for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT) {
3905                         struct iwl_mvm_tid_data *tid_data;
3906                         u16 n_queued;
3907
3908                         tid_data = &mvmsta->tid_data[tid];
3909
3910                         n_queued = iwl_mvm_tid_queued(mvm, tid_data);
3911                         if (n_queued > remaining) {
3912                                 more_data = true;
3913                                 remaining = 0;
3914                                 break;
3915                         }
3916                         remaining -= n_queued;
3917                 }
3918                 sleep_tx_count = cnt - remaining;
3919                 if (reason == IEEE80211_FRAME_RELEASE_UAPSD)
3920                         mvmsta->sleep_tx_count = sleep_tx_count;
3921                 spin_unlock_bh(&mvmsta->lock);
3922
3923                 cmd.sleep_tx_count = cpu_to_le16(sleep_tx_count);
3924                 if (WARN_ON(cnt - remaining == 0)) {
3925                         ieee80211_sta_eosp(sta);
3926                         return;
3927                 }
3928         }
3929
3930         /* Note: this is ignored by firmware not supporting GO uAPSD */
3931         if (more_data)
3932                 cmd.sleep_state_flags |= STA_SLEEP_STATE_MOREDATA;
3933
3934         if (reason == IEEE80211_FRAME_RELEASE_PSPOLL) {
3935                 mvmsta->next_status_eosp = true;
3936                 cmd.sleep_state_flags |= STA_SLEEP_STATE_PS_POLL;
3937         } else {
3938                 cmd.sleep_state_flags |= STA_SLEEP_STATE_UAPSD;
3939         }
3940
3941         /* block the Tx queues until the FW updated the sleep Tx count */
3942         iwl_trans_block_txq_ptrs(mvm->trans, true);
3943
3944         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA,
3945                                    CMD_ASYNC | CMD_WANT_ASYNC_CALLBACK,
3946                                    iwl_mvm_add_sta_cmd_size(mvm), &cmd);
3947         if (ret)
3948                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
3949 }
3950
3951 void iwl_mvm_rx_eosp_notif(struct iwl_mvm *mvm,
3952                            struct iwl_rx_cmd_buffer *rxb)
3953 {
3954         struct iwl_rx_packet *pkt = rxb_addr(rxb);
3955         struct iwl_mvm_eosp_notification *notif = (void *)pkt->data;
3956         struct ieee80211_sta *sta;
3957         u32 sta_id = le32_to_cpu(notif->sta_id);
3958
3959         if (WARN_ON_ONCE(sta_id >= mvm->fw->ucode_capa.num_stations))
3960                 return;
3961
3962         rcu_read_lock();
3963         sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
3964         if (!IS_ERR_OR_NULL(sta))
3965                 ieee80211_sta_eosp(sta);
3966         rcu_read_unlock();
3967 }
3968
3969 void iwl_mvm_sta_modify_disable_tx(struct iwl_mvm *mvm,
3970                                    struct iwl_mvm_sta *mvmsta, bool disable)
3971 {
3972         struct iwl_mvm_add_sta_cmd cmd = {
3973                 .add_modify = STA_MODE_MODIFY,
3974                 .sta_id = mvmsta->sta_id,
3975                 .station_flags = disable ? cpu_to_le32(STA_FLG_DISABLE_TX) : 0,
3976                 .station_flags_msk = cpu_to_le32(STA_FLG_DISABLE_TX),
3977                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
3978         };
3979         int ret;
3980
3981         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
3982                                    iwl_mvm_add_sta_cmd_size(mvm), &cmd);
3983         if (ret)
3984                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
3985 }
3986
3987 void iwl_mvm_sta_modify_disable_tx_ap(struct iwl_mvm *mvm,
3988                                       struct ieee80211_sta *sta,
3989                                       bool disable)
3990 {
3991         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
3992
3993         spin_lock_bh(&mvm_sta->lock);
3994
3995         if (mvm_sta->disable_tx == disable) {
3996                 spin_unlock_bh(&mvm_sta->lock);
3997                 return;
3998         }
3999
4000         mvm_sta->disable_tx = disable;
4001
4002         /*
4003          * If sta PS state is handled by mac80211, tell it to start/stop
4004          * queuing tx for this station.
4005          */
4006         if (!ieee80211_hw_check(mvm->hw, AP_LINK_PS))
4007                 ieee80211_sta_block_awake(mvm->hw, sta, disable);
4008
4009         iwl_mvm_sta_modify_disable_tx(mvm, mvm_sta, disable);
4010
4011         spin_unlock_bh(&mvm_sta->lock);
4012 }
4013
4014 static void iwl_mvm_int_sta_modify_disable_tx(struct iwl_mvm *mvm,
4015                                               struct iwl_mvm_vif *mvmvif,
4016                                               struct iwl_mvm_int_sta *sta,
4017                                               bool disable)
4018 {
4019         u32 id = FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color);
4020         struct iwl_mvm_add_sta_cmd cmd = {
4021                 .add_modify = STA_MODE_MODIFY,
4022                 .sta_id = sta->sta_id,
4023                 .station_flags = disable ? cpu_to_le32(STA_FLG_DISABLE_TX) : 0,
4024                 .station_flags_msk = cpu_to_le32(STA_FLG_DISABLE_TX),
4025                 .mac_id_n_color = cpu_to_le32(id),
4026         };
4027         int ret;
4028
4029         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
4030                                    iwl_mvm_add_sta_cmd_size(mvm), &cmd);
4031         if (ret)
4032                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
4033 }
4034
4035 void iwl_mvm_modify_all_sta_disable_tx(struct iwl_mvm *mvm,
4036                                        struct iwl_mvm_vif *mvmvif,
4037                                        bool disable)
4038 {
4039         struct ieee80211_sta *sta;
4040         struct iwl_mvm_sta *mvm_sta;
4041         int i;
4042
4043         rcu_read_lock();
4044
4045         /* Block/unblock all the stations of the given mvmvif */
4046         for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++) {
4047                 sta = rcu_dereference(mvm->fw_id_to_mac_id[i]);
4048                 if (IS_ERR_OR_NULL(sta))
4049                         continue;
4050
4051                 mvm_sta = iwl_mvm_sta_from_mac80211(sta);
4052                 if (mvm_sta->mac_id_n_color !=
4053                     FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color))
4054                         continue;
4055
4056                 iwl_mvm_sta_modify_disable_tx_ap(mvm, sta, disable);
4057         }
4058
4059         rcu_read_unlock();
4060
4061         if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE))
4062                 return;
4063
4064         /* Need to block/unblock also multicast station */
4065         if (mvmvif->mcast_sta.sta_id != IWL_MVM_INVALID_STA)
4066                 iwl_mvm_int_sta_modify_disable_tx(mvm, mvmvif,
4067                                                   &mvmvif->mcast_sta, disable);
4068
4069         /*
4070          * Only unblock the broadcast station (FW blocks it for immediate
4071          * quiet, not the driver)
4072          */
4073         if (!disable && mvmvif->bcast_sta.sta_id != IWL_MVM_INVALID_STA)
4074                 iwl_mvm_int_sta_modify_disable_tx(mvm, mvmvif,
4075                                                   &mvmvif->bcast_sta, disable);
4076 }
4077
4078 void iwl_mvm_csa_client_absent(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
4079 {
4080         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
4081         struct iwl_mvm_sta *mvmsta;
4082
4083         rcu_read_lock();
4084
4085         mvmsta = iwl_mvm_sta_from_staid_rcu(mvm, mvmvif->ap_sta_id);
4086
4087         if (mvmsta)
4088                 iwl_mvm_sta_modify_disable_tx(mvm, mvmsta, true);
4089
4090         rcu_read_unlock();
4091 }
4092
4093 u16 iwl_mvm_tid_queued(struct iwl_mvm *mvm, struct iwl_mvm_tid_data *tid_data)
4094 {
4095         u16 sn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
4096
4097         /*
4098          * In 22000 HW, the next_reclaimed index is only 8 bit, so we'll need
4099          * to align the wrap around of ssn so we compare relevant values.
4100          */
4101         if (mvm->trans->trans_cfg->gen2)
4102                 sn &= 0xff;
4103
4104         return ieee80211_sn_sub(sn, tid_data->next_reclaimed);
4105 }
4106
4107 int iwl_mvm_add_pasn_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
4108                          struct iwl_mvm_int_sta *sta, u8 *addr, u32 cipher,
4109                          u8 *key, u32 key_len)
4110 {
4111         int ret;
4112         u16 queue;
4113         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
4114         struct ieee80211_key_conf *keyconf;
4115
4116         ret = iwl_mvm_allocate_int_sta(mvm, sta, 0,
4117                                        NL80211_IFTYPE_UNSPECIFIED,
4118                                        IWL_STA_LINK);
4119         if (ret)
4120                 return ret;
4121
4122         ret = iwl_mvm_add_int_sta_with_queue(mvm, mvmvif->id, mvmvif->color,
4123                                              addr, sta, &queue,
4124                                              IWL_MVM_TX_FIFO_BE);
4125         if (ret)
4126                 goto out;
4127
4128         keyconf = kzalloc(sizeof(*keyconf) + key_len, GFP_KERNEL);
4129         if (!keyconf) {
4130                 ret = -ENOBUFS;
4131                 goto out;
4132         }
4133
4134         keyconf->cipher = cipher;
4135         memcpy(keyconf->key, key, key_len);
4136         keyconf->keylen = key_len;
4137
4138         ret = iwl_mvm_send_sta_key(mvm, sta->sta_id, keyconf, false,
4139                                    0, NULL, 0, 0, true);
4140         kfree(keyconf);
4141         return 0;
4142 out:
4143         iwl_mvm_dealloc_int_sta(mvm, sta);
4144         return ret;
4145 }
4146
4147 void iwl_mvm_cancel_channel_switch(struct iwl_mvm *mvm,
4148                                    struct ieee80211_vif *vif,
4149                                    u32 mac_id)
4150 {
4151         struct iwl_cancel_channel_switch_cmd cancel_channel_switch_cmd = {
4152                 .mac_id = cpu_to_le32(mac_id),
4153         };
4154         int ret;
4155
4156         ret = iwl_mvm_send_cmd_pdu(mvm,
4157                                    WIDE_ID(MAC_CONF_GROUP, CANCEL_CHANNEL_SWITCH_CMD),
4158                                    CMD_ASYNC,
4159                                    sizeof(cancel_channel_switch_cmd),
4160                                    &cancel_channel_switch_cmd);
4161         if (ret)
4162                 IWL_ERR(mvm, "Failed to cancel the channel switch\n");
4163 }