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

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