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