drivers/net/wireless/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  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of version 2 of the GNU General Public License as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful, but
  16  * WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18  * General Public License for more details.
  19  *
  20  * You should have received a copy of the GNU General Public License
  21  * along with this program; if not, write to the Free Software
  22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
  23  * USA
  24  *
  25  * The full GNU General Public License is included in this distribution
  26  * in the file called COPYING.
  27  *
  28  * Contact Information:
  29  *  Intel Linux Wireless <ilw@linux.intel.com>
  30  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  31  *
  32  * BSD LICENSE
  33  *
  34  * Copyright(c) 2012 - 2015 Intel Corporation. All rights reserved.
  35  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  36  * All rights reserved.
  37  *
  38  * Redistribution and use in source and binary forms, with or without
  39  * modification, are permitted provided that the following conditions
  40  * are met:
  41  *
  42  *  * Redistributions of source code must retain the above copyright
  43  *    notice, this list of conditions and the following disclaimer.
  44  *  * Redistributions in binary form must reproduce the above copyright
  45  *    notice, this list of conditions and the following disclaimer in
  46  *    the documentation and/or other materials provided with the
  47  *    distribution.
  48  *  * Neither the name Intel Corporation nor the names of its
  49  *    contributors may be used to endorse or promote products derived
  50  *    from this software without specific prior written permission.
  51  *
  52  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  53  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  54  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  55  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  56  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  57  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  58  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  59  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  60  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  61  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  62  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  63  *
  64  *****************************************************************************/
  65 #include <net/mac80211.h>
  66
  67 #include "mvm.h"
  68 #include "sta.h"
  69 #include "rs.h"
  70
  71 static int iwl_mvm_find_free_sta_id(struct iwl_mvm *mvm,
  72                                     enum nl80211_iftype iftype)
  73 {
  74         int sta_id;
  75         u32 reserved_ids = 0;
  76
  77         BUILD_BUG_ON(IWL_MVM_STATION_COUNT > 32);
  78         WARN_ON_ONCE(test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status));
  79
  80         lockdep_assert_held(&mvm->mutex);
  81
  82         /* d0i3/d3 assumes the AP's sta_id (of sta vif) is 0. reserve it. */
  83         if (iftype != NL80211_IFTYPE_STATION)
  84                 reserved_ids = BIT(0);
  85
  86         /* Don't take rcu_read_lock() since we are protected by mvm->mutex */
  87         for (sta_id = 0; sta_id < IWL_MVM_STATION_COUNT; sta_id++) {
  88                 if (BIT(sta_id) & reserved_ids)
  89                         continue;
  90
  91                 if (!rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
  92                                                lockdep_is_held(&mvm->mutex)))
  93                         return sta_id;
  94         }
  95         return IWL_MVM_STATION_COUNT;
  96 }
  97
  98 /* send station add/update command to firmware */
  99 int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 100                            bool update)
 101 {
 102         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 103         struct iwl_mvm_add_sta_cmd add_sta_cmd = {
 104                 .sta_id = mvm_sta->sta_id,
 105                 .mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color),
 106                 .add_modify = update ? 1 : 0,
 107                 .station_flags_msk = cpu_to_le32(STA_FLG_FAT_EN_MSK |
 108                                                  STA_FLG_MIMO_EN_MSK),
 109         };
 110         int ret;
 111         u32 status;
 112         u32 agg_size = 0, mpdu_dens = 0;
 113
 114         if (!update) {
 115                 add_sta_cmd.tfd_queue_msk = cpu_to_le32(mvm_sta->tfd_queue_msk);
 116                 memcpy(&add_sta_cmd.addr, sta->addr, ETH_ALEN);
 117         }
 118
 119         switch (sta->bandwidth) {
 120         case IEEE80211_STA_RX_BW_160:
 121                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_160MHZ);
 122                 /* fall through */
 123         case IEEE80211_STA_RX_BW_80:
 124                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_80MHZ);
 125                 /* fall through */
 126         case IEEE80211_STA_RX_BW_40:
 127                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_40MHZ);
 128                 /* fall through */
 129         case IEEE80211_STA_RX_BW_20:
 130                 if (sta->ht_cap.ht_supported)
 131                         add_sta_cmd.station_flags |=
 132                                 cpu_to_le32(STA_FLG_FAT_EN_20MHZ);
 133                 break;
 134         }
 135
 136         switch (sta->rx_nss) {
 137         case 1:
 138                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_SISO);
 139                 break;
 140         case 2:
 141                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_MIMO2);
 142                 break;
 143         case 3 ... 8:
 144                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_MIMO3);
 145                 break;
 146         }
 147
 148         switch (sta->smps_mode) {
 149         case IEEE80211_SMPS_AUTOMATIC:
 150         case IEEE80211_SMPS_NUM_MODES:
 151                 WARN_ON(1);
 152                 break;
 153         case IEEE80211_SMPS_STATIC:
 154                 /* override NSS */
 155                 add_sta_cmd.station_flags &= ~cpu_to_le32(STA_FLG_MIMO_EN_MSK);
 156                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_SISO);
 157                 break;
 158         case IEEE80211_SMPS_DYNAMIC:
 159                 add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_RTS_MIMO_PROT);
 160                 break;
 161         case IEEE80211_SMPS_OFF:
 162                 /* nothing */
 163                 break;
 164         }
 165
 166         if (sta->ht_cap.ht_supported) {
 167                 add_sta_cmd.station_flags_msk |=
 168                         cpu_to_le32(STA_FLG_MAX_AGG_SIZE_MSK |
 169                                     STA_FLG_AGG_MPDU_DENS_MSK);
 170
 171                 mpdu_dens = sta->ht_cap.ampdu_density;
 172         }
 173
 174         if (sta->vht_cap.vht_supported) {
 175                 agg_size = sta->vht_cap.cap &
 176                         IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK;
 177                 agg_size >>=
 178                         IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_SHIFT;
 179         } else if (sta->ht_cap.ht_supported) {
 180                 agg_size = sta->ht_cap.ampdu_factor;
 181         }
 182
 183         add_sta_cmd.station_flags |=
 184                 cpu_to_le32(agg_size << STA_FLG_MAX_AGG_SIZE_SHIFT);
 185         add_sta_cmd.station_flags |=
 186                 cpu_to_le32(mpdu_dens << STA_FLG_AGG_MPDU_DENS_SHIFT);
 187
 188         status = ADD_STA_SUCCESS;
 189         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(add_sta_cmd),
 190                                           &add_sta_cmd, &status);
 191         if (ret)
 192                 return ret;
 193
 194         switch (status) {
 195         case ADD_STA_SUCCESS:
 196                 IWL_DEBUG_ASSOC(mvm, "ADD_STA PASSED\n");
 197                 break;
 198         default:
 199                 ret = -EIO;
 200                 IWL_ERR(mvm, "ADD_STA failed\n");
 201                 break;
 202         }
 203
 204         return ret;
 205 }
 206
 207 static int iwl_mvm_tdls_sta_init(struct iwl_mvm *mvm,
 208                                  struct ieee80211_sta *sta)
 209 {
 210         unsigned long used_hw_queues;
 211         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 212         unsigned int wdg_timeout =
 213                 iwl_mvm_get_wd_timeout(mvm, NULL, true, false);
 214         u32 ac;
 215
 216         lockdep_assert_held(&mvm->mutex);
 217
 218         used_hw_queues = iwl_mvm_get_used_hw_queues(mvm, NULL);
 219
 220         /* Find available queues, and allocate them to the ACs */
 221         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
 222                 u8 queue = find_first_zero_bit(&used_hw_queues,
 223                                                mvm->first_agg_queue);
 224
 225                 if (queue >= mvm->first_agg_queue) {
 226                         IWL_ERR(mvm, "Failed to allocate STA queue\n");
 227                         return -EBUSY;
 228                 }
 229
 230                 __set_bit(queue, &used_hw_queues);
 231                 mvmsta->hw_queue[ac] = queue;
 232         }
 233
 234         /* Found a place for all queues - enable them */
 235         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
 236                 iwl_mvm_enable_ac_txq(mvm, mvmsta->hw_queue[ac],
 237                                       iwl_mvm_ac_to_tx_fifo[ac], wdg_timeout);
 238                 mvmsta->tfd_queue_msk |= BIT(mvmsta->hw_queue[ac]);
 239         }
 240
 241         return 0;
 242 }
 243
 244 static void iwl_mvm_tdls_sta_deinit(struct iwl_mvm *mvm,
 245                                     struct ieee80211_sta *sta)
 246 {
 247         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 248         unsigned long sta_msk;
 249         int i;
 250
 251         lockdep_assert_held(&mvm->mutex);
 252
 253         /* disable the TDLS STA-specific queues */
 254         sta_msk = mvmsta->tfd_queue_msk;
 255         for_each_set_bit(i, &sta_msk, sizeof(sta_msk) * BITS_PER_BYTE)
 256                 iwl_mvm_disable_txq(mvm, i, 0);
 257 }
 258
 259 int iwl_mvm_add_sta(struct iwl_mvm *mvm,
 260                     struct ieee80211_vif *vif,
 261                     struct ieee80211_sta *sta)
 262 {
 263         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 264         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 265         int i, ret, sta_id;
 266
 267         lockdep_assert_held(&mvm->mutex);
 268
 269         if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
 270                 sta_id = iwl_mvm_find_free_sta_id(mvm,
 271                                                   ieee80211_vif_type_p2p(vif));
 272         else
 273                 sta_id = mvm_sta->sta_id;
 274
 275         if (sta_id == IWL_MVM_STATION_COUNT)
 276                 return -ENOSPC;
 277
 278         spin_lock_init(&mvm_sta->lock);
 279
 280         mvm_sta->sta_id = sta_id;
 281         mvm_sta->mac_id_n_color = FW_CMD_ID_AND_COLOR(mvmvif->id,
 282                                                       mvmvif->color);
 283         mvm_sta->vif = vif;
 284         mvm_sta->max_agg_bufsize = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
 285         mvm_sta->tx_protection = 0;
 286         mvm_sta->tt_tx_protection = false;
 287
 288         /* HW restart, don't assume the memory has been zeroed */
 289         atomic_set(&mvm->pending_frames[sta_id], 0);
 290         mvm_sta->tid_disable_agg = 0;
 291         mvm_sta->tfd_queue_msk = 0;
 292
 293         /* allocate new queues for a TDLS station */
 294         if (sta->tdls) {
 295                 ret = iwl_mvm_tdls_sta_init(mvm, sta);
 296                 if (ret)
 297                         return ret;
 298         } else {
 299                 for (i = 0; i < IEEE80211_NUM_ACS; i++)
 300                         if (vif->hw_queue[i] != IEEE80211_INVAL_HW_QUEUE)
 301                                 mvm_sta->tfd_queue_msk |= BIT(vif->hw_queue[i]);
 302         }
 303
 304         /* for HW restart - reset everything but the sequence number */
 305         for (i = 0; i < IWL_MAX_TID_COUNT; i++) {
 306                 u16 seq = mvm_sta->tid_data[i].seq_number;
 307                 memset(&mvm_sta->tid_data[i], 0, sizeof(mvm_sta->tid_data[i]));
 308                 mvm_sta->tid_data[i].seq_number = seq;
 309         }
 310         mvm_sta->agg_tids = 0;
 311
 312         ret = iwl_mvm_sta_send_to_fw(mvm, sta, false);
 313         if (ret)
 314                 goto err;
 315
 316         if (vif->type == NL80211_IFTYPE_STATION) {
 317                 if (!sta->tdls) {
 318                         WARN_ON(mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT);
 319                         mvmvif->ap_sta_id = sta_id;
 320                 } else {
 321                         WARN_ON(mvmvif->ap_sta_id == IWL_MVM_STATION_COUNT);
 322                 }
 323         }
 324
 325         rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);
 326
 327         return 0;
 328
 329 err:
 330         iwl_mvm_tdls_sta_deinit(mvm, sta);
 331         return ret;
 332 }
 333
 334 int iwl_mvm_update_sta(struct iwl_mvm *mvm,
 335                        struct ieee80211_vif *vif,
 336                        struct ieee80211_sta *sta)
 337 {
 338         return iwl_mvm_sta_send_to_fw(mvm, sta, true);
 339 }
 340
 341 int iwl_mvm_drain_sta(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvmsta,
 342                       bool drain)
 343 {
 344         struct iwl_mvm_add_sta_cmd cmd = {};
 345         int ret;
 346         u32 status;
 347
 348         lockdep_assert_held(&mvm->mutex);
 349
 350         cmd.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color);
 351         cmd.sta_id = mvmsta->sta_id;
 352         cmd.add_modify = STA_MODE_MODIFY;
 353         cmd.station_flags = drain ? cpu_to_le32(STA_FLG_DRAIN_FLOW) : 0;
 354         cmd.station_flags_msk = cpu_to_le32(STA_FLG_DRAIN_FLOW);
 355
 356         status = ADD_STA_SUCCESS;
 357         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
 358                                           &cmd, &status);
 359         if (ret)
 360                 return ret;
 361
 362         switch (status) {
 363         case ADD_STA_SUCCESS:
 364                 IWL_DEBUG_INFO(mvm, "Frames for staid %d will drained in fw\n",
 365                                mvmsta->sta_id);
 366                 break;
 367         default:
 368                 ret = -EIO;
 369                 IWL_ERR(mvm, "Couldn't drain frames for staid %d\n",
 370                         mvmsta->sta_id);
 371                 break;
 372         }
 373
 374         return ret;
 375 }
 376
 377 /*
 378  * Remove a station from the FW table. Before sending the command to remove
 379  * the station validate that the station is indeed known to the driver (sanity
 380  * only).
 381  */
 382 static int iwl_mvm_rm_sta_common(struct iwl_mvm *mvm, u8 sta_id)
 383 {
 384         struct ieee80211_sta *sta;
 385         struct iwl_mvm_rm_sta_cmd rm_sta_cmd = {
 386                 .sta_id = sta_id,
 387         };
 388         int ret;
 389
 390         sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
 391                                         lockdep_is_held(&mvm->mutex));
 392
 393         /* Note: internal stations are marked as error values */
 394         if (!sta) {
 395                 IWL_ERR(mvm, "Invalid station id\n");
 396                 return -EINVAL;
 397         }
 398
 399         ret = iwl_mvm_send_cmd_pdu(mvm, REMOVE_STA, 0,
 400                                    sizeof(rm_sta_cmd), &rm_sta_cmd);
 401         if (ret) {
 402                 IWL_ERR(mvm, "Failed to remove station. Id=%d\n", sta_id);
 403                 return ret;
 404         }
 405
 406         return 0;
 407 }
 408
 409 void iwl_mvm_sta_drained_wk(struct work_struct *wk)
 410 {
 411         struct iwl_mvm *mvm = container_of(wk, struct iwl_mvm, sta_drained_wk);
 412         u8 sta_id;
 413
 414         /*
 415          * The mutex is needed because of the SYNC cmd, but not only: if the
 416          * work would run concurrently with iwl_mvm_rm_sta, it would run before
 417          * iwl_mvm_rm_sta sets the station as busy, and exit. Then
 418          * iwl_mvm_rm_sta would set the station as busy, and nobody will clean
 419          * that later.
 420          */
 421         mutex_lock(&mvm->mutex);
 422
 423         for_each_set_bit(sta_id, mvm->sta_drained, IWL_MVM_STATION_COUNT) {
 424                 int ret;
 425                 struct ieee80211_sta *sta =
 426                         rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
 427                                                   lockdep_is_held(&mvm->mutex));
 428
 429                 /*
 430                  * This station is in use or RCU-removed; the latter happens in
 431                  * managed mode, where mac80211 removes the station before we
 432                  * can remove it from firmware (we can only do that after the
 433                  * MAC is marked unassociated), and possibly while the deauth
 434                  * frame to disconnect from the AP is still queued. Then, the
 435                  * station pointer is -ENOENT when the last skb is reclaimed.
 436                  */
 437                 if (!IS_ERR(sta) || PTR_ERR(sta) == -ENOENT)
 438                         continue;
 439
 440                 if (PTR_ERR(sta) == -EINVAL) {
 441                         IWL_ERR(mvm, "Drained sta %d, but it is internal?\n",
 442                                 sta_id);
 443                         continue;
 444                 }
 445
 446                 if (!sta) {
 447                         IWL_ERR(mvm, "Drained sta %d, but it was NULL?\n",
 448                                 sta_id);
 449                         continue;
 450                 }
 451
 452                 WARN_ON(PTR_ERR(sta) != -EBUSY);
 453                 /* This station was removed and we waited until it got drained,
 454                  * we can now proceed and remove it.
 455                  */
 456                 ret = iwl_mvm_rm_sta_common(mvm, sta_id);
 457                 if (ret) {
 458                         IWL_ERR(mvm,
 459                                 "Couldn't remove sta %d after it was drained\n",
 460                                 sta_id);
 461                         continue;
 462                 }
 463                 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL);
 464                 clear_bit(sta_id, mvm->sta_drained);
 465
 466                 if (mvm->tfd_drained[sta_id]) {
 467                         unsigned long i, msk = mvm->tfd_drained[sta_id];
 468
 469                         for_each_set_bit(i, &msk, sizeof(msk) * BITS_PER_BYTE)
 470                                 iwl_mvm_disable_txq(mvm, i, 0);
 471
 472                         mvm->tfd_drained[sta_id] = 0;
 473                         IWL_DEBUG_TDLS(mvm, "Drained sta %d, with queues %ld\n",
 474                                        sta_id, msk);
 475                 }
 476         }
 477
 478         mutex_unlock(&mvm->mutex);
 479 }
 480
 481 int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 482                    struct ieee80211_vif *vif,
 483                    struct ieee80211_sta *sta)
 484 {
 485         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 486         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 487         int ret;
 488
 489         lockdep_assert_held(&mvm->mutex);
 490
 491         if (vif->type == NL80211_IFTYPE_STATION &&
 492             mvmvif->ap_sta_id == mvm_sta->sta_id) {
 493                 ret = iwl_mvm_drain_sta(mvm, mvm_sta, true);
 494                 if (ret)
 495                         return ret;
 496                 /* flush its queues here since we are freeing mvm_sta */
 497                 ret = iwl_mvm_flush_tx_path(mvm, mvm_sta->tfd_queue_msk, true);
 498                 if (ret)
 499                         return ret;
 500                 ret = iwl_trans_wait_tx_queue_empty(mvm->trans,
 501                                                     mvm_sta->tfd_queue_msk);
 502                 if (ret)
 503                         return ret;
 504                 ret = iwl_mvm_drain_sta(mvm, mvm_sta, false);
 505
 506                 /* if we are associated - we can't remove the AP STA now */
 507                 if (vif->bss_conf.assoc)
 508                         return ret;
 509
 510                 /* unassoc - go ahead - remove the AP STA now */
 511                 mvmvif->ap_sta_id = IWL_MVM_STATION_COUNT;
 512
 513                 /* clear d0i3_ap_sta_id if no longer relevant */
 514                 if (mvm->d0i3_ap_sta_id == mvm_sta->sta_id)
 515                         mvm->d0i3_ap_sta_id = IWL_MVM_STATION_COUNT;
 516         }
 517
 518         /*
 519          * This shouldn't happen - the TDLS channel switch should be canceled
 520          * before the STA is removed.
 521          */
 522         if (WARN_ON_ONCE(mvm->tdls_cs.peer.sta_id == mvm_sta->sta_id)) {
 523                 mvm->tdls_cs.peer.sta_id = IWL_MVM_STATION_COUNT;
 524                 cancel_delayed_work(&mvm->tdls_cs.dwork);
 525         }
 526
 527         /*
 528          * Make sure that the tx response code sees the station as -EBUSY and
 529          * calls the drain worker.
 530          */
 531         spin_lock_bh(&mvm_sta->lock);
 532         /*
 533          * There are frames pending on the AC queues for this station.
 534          * We need to wait until all the frames are drained...
 535          */
 536         if (atomic_read(&mvm->pending_frames[mvm_sta->sta_id])) {
 537                 rcu_assign_pointer(mvm->fw_id_to_mac_id[mvm_sta->sta_id],
 538                                    ERR_PTR(-EBUSY));
 539                 spin_unlock_bh(&mvm_sta->lock);
 540
 541                 /* disable TDLS sta queues on drain complete */
 542                 if (sta->tdls) {
 543                         mvm->tfd_drained[mvm_sta->sta_id] =
 544                                                         mvm_sta->tfd_queue_msk;
 545                         IWL_DEBUG_TDLS(mvm, "Draining TDLS sta %d\n",
 546                                        mvm_sta->sta_id);
 547                 }
 548
 549                 ret = iwl_mvm_drain_sta(mvm, mvm_sta, true);
 550         } else {
 551                 spin_unlock_bh(&mvm_sta->lock);
 552
 553                 if (sta->tdls)
 554                         iwl_mvm_tdls_sta_deinit(mvm, sta);
 555
 556                 ret = iwl_mvm_rm_sta_common(mvm, mvm_sta->sta_id);
 557                 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta->sta_id], NULL);
 558         }
 559
 560         return ret;
 561 }
 562
 563 int iwl_mvm_rm_sta_id(struct iwl_mvm *mvm,
 564                       struct ieee80211_vif *vif,
 565                       u8 sta_id)
 566 {
 567         int ret = iwl_mvm_rm_sta_common(mvm, sta_id);
 568
 569         lockdep_assert_held(&mvm->mutex);
 570
 571         RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL);
 572         return ret;
 573 }
 574
 575 static int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm,
 576                                     struct iwl_mvm_int_sta *sta,
 577                                     u32 qmask, enum nl80211_iftype iftype)
 578 {
 579         if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
 580                 sta->sta_id = iwl_mvm_find_free_sta_id(mvm, iftype);
 581                 if (WARN_ON_ONCE(sta->sta_id == IWL_MVM_STATION_COUNT))
 582                         return -ENOSPC;
 583         }
 584
 585         sta->tfd_queue_msk = qmask;
 586
 587         /* put a non-NULL value so iterating over the stations won't stop */
 588         rcu_assign_pointer(mvm->fw_id_to_mac_id[sta->sta_id], ERR_PTR(-EINVAL));
 589         return 0;
 590 }
 591
 592 static void iwl_mvm_dealloc_int_sta(struct iwl_mvm *mvm,
 593                                     struct iwl_mvm_int_sta *sta)
 594 {
 595         RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta->sta_id], NULL);
 596         memset(sta, 0, sizeof(struct iwl_mvm_int_sta));
 597         sta->sta_id = IWL_MVM_STATION_COUNT;
 598 }
 599
 600 static int iwl_mvm_add_int_sta_common(struct iwl_mvm *mvm,
 601                                       struct iwl_mvm_int_sta *sta,
 602                                       const u8 *addr,
 603                                       u16 mac_id, u16 color)
 604 {
 605         struct iwl_mvm_add_sta_cmd cmd;
 606         int ret;
 607         u32 status;
 608
 609         lockdep_assert_held(&mvm->mutex);
 610
 611         memset(&cmd, 0, sizeof(cmd));
 612         cmd.sta_id = sta->sta_id;
 613         cmd.mac_id_n_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mac_id,
 614                                                              color));
 615
 616         cmd.tfd_queue_msk = cpu_to_le32(sta->tfd_queue_msk);
 617
 618         if (addr)
 619                 memcpy(cmd.addr, addr, ETH_ALEN);
 620
 621         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
 622                                           &cmd, &status);
 623         if (ret)
 624                 return ret;
 625
 626         switch (status) {
 627         case ADD_STA_SUCCESS:
 628                 IWL_DEBUG_INFO(mvm, "Internal station added.\n");
 629                 return 0;
 630         default:
 631                 ret = -EIO;
 632                 IWL_ERR(mvm, "Add internal station failed, status=0x%x\n",
 633                         status);
 634                 break;
 635         }
 636         return ret;
 637 }
 638
 639 int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm)
 640 {
 641         unsigned int wdg_timeout = iwlmvm_mod_params.tfd_q_hang_detect ?
 642                                         mvm->cfg->base_params->wd_timeout :
 643                                         IWL_WATCHDOG_DISABLED;
 644         int ret;
 645
 646         lockdep_assert_held(&mvm->mutex);
 647
 648         /* Map Aux queue to fifo - needs to happen before adding Aux station */
 649         iwl_mvm_enable_ac_txq(mvm, mvm->aux_queue,
 650                               IWL_MVM_TX_FIFO_MCAST, wdg_timeout);
 651
 652         /* Allocate aux station and assign to it the aux queue */
 653         ret = iwl_mvm_allocate_int_sta(mvm, &mvm->aux_sta, BIT(mvm->aux_queue),
 654                                        NL80211_IFTYPE_UNSPECIFIED);
 655         if (ret)
 656                 return ret;
 657
 658         ret = iwl_mvm_add_int_sta_common(mvm, &mvm->aux_sta, NULL,
 659                                          MAC_INDEX_AUX, 0);
 660
 661         if (ret)
 662                 iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);
 663         return ret;
 664 }
 665
 666 void iwl_mvm_del_aux_sta(struct iwl_mvm *mvm)
 667 {
 668         lockdep_assert_held(&mvm->mutex);
 669
 670         iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);
 671 }
 672
 673 /*
 674  * Send the add station command for the vif's broadcast station.
 675  * Assumes that the station was already allocated.
 676  *
 677  * @mvm: the mvm component
 678  * @vif: the interface to which the broadcast station is added
 679  * @bsta: the broadcast station to add.
 680  */
 681 int iwl_mvm_send_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 682 {
 683         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 684         struct iwl_mvm_int_sta *bsta = &mvmvif->bcast_sta;
 685         static const u8 _baddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 686         const u8 *baddr = _baddr;
 687
 688         lockdep_assert_held(&mvm->mutex);
 689
 690         if (vif->type == NL80211_IFTYPE_ADHOC)
 691                 baddr = vif->bss_conf.bssid;
 692
 693         if (WARN_ON_ONCE(bsta->sta_id == IWL_MVM_STATION_COUNT))
 694                 return -ENOSPC;
 695
 696         return iwl_mvm_add_int_sta_common(mvm, bsta, baddr,
 697                                           mvmvif->id, mvmvif->color);
 698 }
 699
 700 /* Send the FW a request to remove the station from it's internal data
 701  * structures, but DO NOT remove the entry from the local data structures. */
 702 int iwl_mvm_send_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 703 {
 704         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 705         int ret;
 706
 707         lockdep_assert_held(&mvm->mutex);
 708
 709         ret = iwl_mvm_rm_sta_common(mvm, mvmvif->bcast_sta.sta_id);
 710         if (ret)
 711                 IWL_WARN(mvm, "Failed sending remove station\n");
 712         return ret;
 713 }
 714
 715 int iwl_mvm_alloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 716 {
 717         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 718         u32 qmask;
 719
 720         lockdep_assert_held(&mvm->mutex);
 721
 722         qmask = iwl_mvm_mac_get_queues_mask(vif);
 723
 724         /*
 725          * The firmware defines the TFD queue mask to only be relevant
 726          * for *unicast* queues, so the multicast (CAB) queue shouldn't
 727          * be included.
 728          */
 729         if (vif->type == NL80211_IFTYPE_AP)
 730                 qmask &= ~BIT(vif->cab_queue);
 731
 732         return iwl_mvm_allocate_int_sta(mvm, &mvmvif->bcast_sta, qmask,
 733                                         ieee80211_vif_type_p2p(vif));
 734 }
 735
 736 /* Allocate a new station entry for the broadcast station to the given vif,
 737  * and send it to the FW.
 738  * Note that each P2P mac should have its own broadcast station.
 739  *
 740  * @mvm: the mvm component
 741  * @vif: the interface to which the broadcast station is added
 742  * @bsta: the broadcast station to add. */
 743 int iwl_mvm_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 744 {
 745         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 746         struct iwl_mvm_int_sta *bsta = &mvmvif->bcast_sta;
 747         int ret;
 748
 749         lockdep_assert_held(&mvm->mutex);
 750
 751         ret = iwl_mvm_alloc_bcast_sta(mvm, vif);
 752         if (ret)
 753                 return ret;
 754
 755         ret = iwl_mvm_send_add_bcast_sta(mvm, vif);
 756
 757         if (ret)
 758                 iwl_mvm_dealloc_int_sta(mvm, bsta);
 759
 760         return ret;
 761 }
 762
 763 void iwl_mvm_dealloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 764 {
 765         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 766
 767         iwl_mvm_dealloc_int_sta(mvm, &mvmvif->bcast_sta);
 768 }
 769
 770 /*
 771  * Send the FW a request to remove the station from it's internal data
 772  * structures, and in addition remove it from the local data structure.
 773  */
 774 int iwl_mvm_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 775 {
 776         int ret;
 777
 778         lockdep_assert_held(&mvm->mutex);
 779
 780         ret = iwl_mvm_send_rm_bcast_sta(mvm, vif);
 781
 782         iwl_mvm_dealloc_bcast_sta(mvm, vif);
 783
 784         return ret;
 785 }
 786
 787 #define IWL_MAX_RX_BA_SESSIONS 16
 788
 789 int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 790                        int tid, u16 ssn, bool start)
 791 {
 792         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 793         struct iwl_mvm_add_sta_cmd cmd = {};
 794         int ret;
 795         u32 status;
 796
 797         lockdep_assert_held(&mvm->mutex);
 798
 799         if (start && mvm->rx_ba_sessions >= IWL_MAX_RX_BA_SESSIONS) {
 800                 IWL_WARN(mvm, "Not enough RX BA SESSIONS\n");
 801                 return -ENOSPC;
 802         }
 803
 804         cmd.mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color);
 805         cmd.sta_id = mvm_sta->sta_id;
 806         cmd.add_modify = STA_MODE_MODIFY;
 807         if (start) {
 808                 cmd.add_immediate_ba_tid = (u8) tid;
 809                 cmd.add_immediate_ba_ssn = cpu_to_le16(ssn);
 810         } else {
 811                 cmd.remove_immediate_ba_tid = (u8) tid;
 812         }
 813         cmd.modify_mask = start ? STA_MODIFY_ADD_BA_TID :
 814                                   STA_MODIFY_REMOVE_BA_TID;
 815
 816         status = ADD_STA_SUCCESS;
 817         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
 818                                           &cmd, &status);
 819         if (ret)
 820                 return ret;
 821
 822         switch (status) {
 823         case ADD_STA_SUCCESS:
 824                 IWL_DEBUG_INFO(mvm, "RX BA Session %sed in fw\n",
 825                                start ? "start" : "stopp");
 826                 break;
 827         case ADD_STA_IMMEDIATE_BA_FAILURE:
 828                 IWL_WARN(mvm, "RX BA Session refused by fw\n");
 829                 ret = -ENOSPC;
 830                 break;
 831         default:
 832                 ret = -EIO;
 833                 IWL_ERR(mvm, "RX BA Session failed %sing, status 0x%x\n",
 834                         start ? "start" : "stopp", status);
 835                 break;
 836         }
 837
 838         if (!ret) {
 839                 if (start)
 840                         mvm->rx_ba_sessions++;
 841                 else if (mvm->rx_ba_sessions > 0)
 842                         /* check that restart flow didn't zero the counter */
 843                         mvm->rx_ba_sessions--;
 844         }
 845
 846         return ret;
 847 }
 848
 849 static int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 850                               int tid, u8 queue, bool start)
 851 {
 852         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 853         struct iwl_mvm_add_sta_cmd cmd = {};
 854         int ret;
 855         u32 status;
 856
 857         lockdep_assert_held(&mvm->mutex);
 858
 859         if (start) {
 860                 mvm_sta->tfd_queue_msk |= BIT(queue);
 861                 mvm_sta->tid_disable_agg &= ~BIT(tid);
 862         } else {
 863                 mvm_sta->tfd_queue_msk &= ~BIT(queue);
 864                 mvm_sta->tid_disable_agg |= BIT(tid);
 865         }
 866
 867         cmd.mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color);
 868         cmd.sta_id = mvm_sta->sta_id;
 869         cmd.add_modify = STA_MODE_MODIFY;
 870         cmd.modify_mask = STA_MODIFY_QUEUES | STA_MODIFY_TID_DISABLE_TX;
 871         cmd.tfd_queue_msk = cpu_to_le32(mvm_sta->tfd_queue_msk);
 872         cmd.tid_disable_tx = cpu_to_le16(mvm_sta->tid_disable_agg);
 873
 874         status = ADD_STA_SUCCESS;
 875         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
 876                                           &cmd, &status);
 877         if (ret)
 878                 return ret;
 879
 880         switch (status) {
 881         case ADD_STA_SUCCESS:
 882                 break;
 883         default:
 884                 ret = -EIO;
 885                 IWL_ERR(mvm, "TX BA Session failed %sing, status 0x%x\n",
 886                         start ? "start" : "stopp", status);
 887                 break;
 888         }
 889
 890         return ret;
 891 }
 892
 893 const u8 tid_to_mac80211_ac[] = {
 894         IEEE80211_AC_BE,
 895         IEEE80211_AC_BK,
 896         IEEE80211_AC_BK,
 897         IEEE80211_AC_BE,
 898         IEEE80211_AC_VI,
 899         IEEE80211_AC_VI,
 900         IEEE80211_AC_VO,
 901         IEEE80211_AC_VO,
 902 };
 903
 904 static const u8 tid_to_ucode_ac[] = {
 905         AC_BE,
 906         AC_BK,
 907         AC_BK,
 908         AC_BE,
 909         AC_VI,
 910         AC_VI,
 911         AC_VO,
 912         AC_VO,
 913 };
 914
 915 int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 916                              struct ieee80211_sta *sta, u16 tid, u16 *ssn)
 917 {
 918         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 919         struct iwl_mvm_tid_data *tid_data;
 920         int txq_id;
 921
 922         if (WARN_ON_ONCE(tid >= IWL_MAX_TID_COUNT))
 923                 return -EINVAL;
 924
 925         if (mvmsta->tid_data[tid].state != IWL_AGG_OFF) {
 926                 IWL_ERR(mvm, "Start AGG when state is not IWL_AGG_OFF %d!\n",
 927                         mvmsta->tid_data[tid].state);
 928                 return -ENXIO;
 929         }
 930
 931         lockdep_assert_held(&mvm->mutex);
 932
 933         for (txq_id = mvm->first_agg_queue;
 934              txq_id <= mvm->last_agg_queue; txq_id++)
 935                 if (mvm->queue_to_mac80211[txq_id] ==
 936                     IWL_INVALID_MAC80211_QUEUE)
 937                         break;
 938
 939         if (txq_id > mvm->last_agg_queue) {
 940                 IWL_ERR(mvm, "Failed to allocate agg queue\n");
 941                 return -EIO;
 942         }
 943
 944         spin_lock_bh(&mvmsta->lock);
 945
 946         /* possible race condition - we entered D0i3 while starting agg */
 947         if (test_bit(IWL_MVM_STATUS_IN_D0I3, &mvm->status)) {
 948                 spin_unlock_bh(&mvmsta->lock);
 949                 IWL_ERR(mvm, "Entered D0i3 while starting Tx agg\n");
 950                 return -EIO;
 951         }
 952
 953         /* the new tx queue is still connected to the same mac80211 queue */
 954         mvm->queue_to_mac80211[txq_id] = vif->hw_queue[tid_to_mac80211_ac[tid]];
 955
 956         tid_data = &mvmsta->tid_data[tid];
 957         tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
 958         tid_data->txq_id = txq_id;
 959         *ssn = tid_data->ssn;
 960
 961         IWL_DEBUG_TX_QUEUES(mvm,
 962                             "Start AGG: sta %d tid %d queue %d - ssn = %d, next_recl = %d\n",
 963                             mvmsta->sta_id, tid, txq_id, tid_data->ssn,
 964                             tid_data->next_reclaimed);
 965
 966         if (tid_data->ssn == tid_data->next_reclaimed) {
 967                 tid_data->state = IWL_AGG_STARTING;
 968                 ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
 969         } else {
 970                 tid_data->state = IWL_EMPTYING_HW_QUEUE_ADDBA;
 971         }
 972
 973         spin_unlock_bh(&mvmsta->lock);
 974
 975         return 0;
 976 }
 977
 978 int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 979                             struct ieee80211_sta *sta, u16 tid, u8 buf_size)
 980 {
 981         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 982         struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
 983         unsigned int wdg_timeout =
 984                 iwl_mvm_get_wd_timeout(mvm, vif, sta->tdls, false);
 985         int queue, fifo, ret;
 986         u16 ssn;
 987
 988         BUILD_BUG_ON((sizeof(mvmsta->agg_tids) * BITS_PER_BYTE)
 989                      != IWL_MAX_TID_COUNT);
 990
 991         buf_size = min_t(int, buf_size, LINK_QUAL_AGG_FRAME_LIMIT_DEF);
 992
 993         spin_lock_bh(&mvmsta->lock);
 994         ssn = tid_data->ssn;
 995         queue = tid_data->txq_id;
 996         tid_data->state = IWL_AGG_ON;
 997         mvmsta->agg_tids |= BIT(tid);
 998         tid_data->ssn = 0xffff;
 999         spin_unlock_bh(&mvmsta->lock);
1000
1001         fifo = iwl_mvm_ac_to_tx_fifo[tid_to_mac80211_ac[tid]];
1002
1003         iwl_mvm_enable_agg_txq(mvm, queue, fifo, mvmsta->sta_id, tid,
1004                                buf_size, ssn, wdg_timeout);
1005
1006         ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
1007         if (ret)
1008                 return -EIO;
1009
1010         /*
1011          * Even though in theory the peer could have different
1012          * aggregation reorder buffer sizes for different sessions,
1013          * our ucode doesn't allow for that and has a global limit
1014          * for each station. Therefore, use the minimum of all the
1015          * aggregation sessions and our default value.
1016          */
1017         mvmsta->max_agg_bufsize =
1018                 min(mvmsta->max_agg_bufsize, buf_size);
1019         mvmsta->lq_sta.lq.agg_frame_cnt_limit = mvmsta->max_agg_bufsize;
1020
1021         IWL_DEBUG_HT(mvm, "Tx aggregation enabled on ra = %pM tid = %d\n",
1022                      sta->addr, tid);
1023
1024         return iwl_mvm_send_lq_cmd(mvm, &mvmsta->lq_sta.lq, false);
1025 }
1026
1027 int iwl_mvm_sta_tx_agg_stop(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
1028                             struct ieee80211_sta *sta, u16 tid)
1029 {
1030         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1031         struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
1032         u16 txq_id;
1033         int err;
1034
1035
1036         /*
1037          * If mac80211 is cleaning its state, then say that we finished since
1038          * our state has been cleared anyway.
1039          */
1040         if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
1041                 ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
1042                 return 0;
1043         }
1044
1045         spin_lock_bh(&mvmsta->lock);
1046
1047         txq_id = tid_data->txq_id;
1048
1049         IWL_DEBUG_TX_QUEUES(mvm, "Stop AGG: sta %d tid %d q %d state %d\n",
1050                             mvmsta->sta_id, tid, txq_id, tid_data->state);
1051
1052         mvmsta->agg_tids &= ~BIT(tid);
1053
1054         switch (tid_data->state) {
1055         case IWL_AGG_ON:
1056                 tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
1057
1058                 IWL_DEBUG_TX_QUEUES(mvm,
1059                                     "ssn = %d, next_recl = %d\n",
1060                                     tid_data->ssn, tid_data->next_reclaimed);
1061
1062                 /* There are still packets for this RA / TID in the HW */
1063                 if (tid_data->ssn != tid_data->next_reclaimed) {
1064                         tid_data->state = IWL_EMPTYING_HW_QUEUE_DELBA;
1065                         err = 0;
1066                         break;
1067                 }
1068
1069                 tid_data->ssn = 0xffff;
1070                 tid_data->state = IWL_AGG_OFF;
1071                 mvm->queue_to_mac80211[txq_id] = IWL_INVALID_MAC80211_QUEUE;
1072                 spin_unlock_bh(&mvmsta->lock);
1073
1074                 ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
1075
1076                 iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
1077
1078                 iwl_mvm_disable_txq(mvm, txq_id, 0);
1079                 return 0;
1080         case IWL_AGG_STARTING:
1081         case IWL_EMPTYING_HW_QUEUE_ADDBA:
1082                 /*
1083                  * The agg session has been stopped before it was set up. This
1084                  * can happen when the AddBA timer times out for example.
1085                  */
1086
1087                 /* No barriers since we are under mutex */
1088                 lockdep_assert_held(&mvm->mutex);
1089                 mvm->queue_to_mac80211[txq_id] = IWL_INVALID_MAC80211_QUEUE;
1090
1091                 ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
1092                 tid_data->state = IWL_AGG_OFF;
1093                 err = 0;
1094                 break;
1095         default:
1096                 IWL_ERR(mvm,
1097                         "Stopping AGG while state not ON or starting for %d on %d (%d)\n",
1098                         mvmsta->sta_id, tid, tid_data->state);
1099                 IWL_ERR(mvm,
1100                         "\ttid_data->txq_id = %d\n", tid_data->txq_id);
1101                 err = -EINVAL;
1102         }
1103
1104         spin_unlock_bh(&mvmsta->lock);
1105
1106         return err;
1107 }
1108
1109 int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
1110                             struct ieee80211_sta *sta, u16 tid)
1111 {
1112         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1113         struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
1114         u16 txq_id;
1115         enum iwl_mvm_agg_state old_state;
1116
1117         /*
1118          * First set the agg state to OFF to avoid calling
1119          * ieee80211_stop_tx_ba_cb in iwl_mvm_check_ratid_empty.
1120          */
1121         spin_lock_bh(&mvmsta->lock);
1122         txq_id = tid_data->txq_id;
1123         IWL_DEBUG_TX_QUEUES(mvm, "Flush AGG: sta %d tid %d q %d state %d\n",
1124                             mvmsta->sta_id, tid, txq_id, tid_data->state);
1125         old_state = tid_data->state;
1126         tid_data->state = IWL_AGG_OFF;
1127         mvmsta->agg_tids &= ~BIT(tid);
1128         spin_unlock_bh(&mvmsta->lock);
1129
1130         if (old_state >= IWL_AGG_ON) {
1131                 iwl_mvm_drain_sta(mvm, mvmsta, true);
1132                 if (iwl_mvm_flush_tx_path(mvm, BIT(txq_id), true))
1133                         IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
1134                 iwl_trans_wait_tx_queue_empty(mvm->trans,
1135                                               mvmsta->tfd_queue_msk);
1136                 iwl_mvm_drain_sta(mvm, mvmsta, false);
1137
1138                 iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
1139
1140                 iwl_mvm_disable_txq(mvm, tid_data->txq_id, 0);
1141         }
1142
1143         mvm->queue_to_mac80211[tid_data->txq_id] =
1144                                 IWL_INVALID_MAC80211_QUEUE;
1145
1146         return 0;
1147 }
1148
1149 static int iwl_mvm_set_fw_key_idx(struct iwl_mvm *mvm)
1150 {
1151         int i, max = -1, max_offs = -1;
1152
1153         lockdep_assert_held(&mvm->mutex);
1154
1155         /* Pick the unused key offset with the highest 'deleted'
1156          * counter. Every time a key is deleted, all the counters
1157          * are incremented and the one that was just deleted is
1158          * reset to zero. Thus, the highest counter is the one
1159          * that was deleted longest ago. Pick that one.
1160          */
1161         for (i = 0; i < STA_KEY_MAX_NUM; i++) {
1162                 if (test_bit(i, mvm->fw_key_table))
1163                         continue;
1164                 if (mvm->fw_key_deleted[i] > max) {
1165                         max = mvm->fw_key_deleted[i];
1166                         max_offs = i;
1167                 }
1168         }
1169
1170         if (max_offs < 0)
1171                 return STA_KEY_IDX_INVALID;
1172
1173         __set_bit(max_offs, mvm->fw_key_table);
1174
1175         return max_offs;
1176 }
1177
1178 static u8 iwl_mvm_get_key_sta_id(struct ieee80211_vif *vif,
1179                                  struct ieee80211_sta *sta)
1180 {
1181         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1182
1183         if (sta) {
1184                 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1185
1186                 return mvm_sta->sta_id;
1187         }
1188
1189         /*
1190          * The device expects GTKs for station interfaces to be
1191          * installed as GTKs for the AP station. If we have no
1192          * station ID, then use AP's station ID.
1193          */
1194         if (vif->type == NL80211_IFTYPE_STATION &&
1195             mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT)
1196                 return mvmvif->ap_sta_id;
1197
1198         return IWL_MVM_STATION_COUNT;
1199 }
1200
1201 static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
1202                                 struct iwl_mvm_sta *mvm_sta,
1203                                 struct ieee80211_key_conf *keyconf, bool mcast,
1204                                 u32 tkip_iv32, u16 *tkip_p1k, u32 cmd_flags)
1205 {
1206         struct iwl_mvm_add_sta_key_cmd cmd = {};
1207         __le16 key_flags;
1208         int ret;
1209         u32 status;
1210         u16 keyidx;
1211         int i;
1212         u8 sta_id = mvm_sta->sta_id;
1213
1214         keyidx = (keyconf->keyidx << STA_KEY_FLG_KEYID_POS) &
1215                  STA_KEY_FLG_KEYID_MSK;
1216         key_flags = cpu_to_le16(keyidx);
1217         key_flags |= cpu_to_le16(STA_KEY_FLG_WEP_KEY_MAP);
1218
1219         switch (keyconf->cipher) {
1220         case WLAN_CIPHER_SUITE_TKIP:
1221                 key_flags |= cpu_to_le16(STA_KEY_FLG_TKIP);
1222                 cmd.tkip_rx_tsc_byte2 = tkip_iv32;
1223                 for (i = 0; i < 5; i++)
1224                         cmd.tkip_rx_ttak[i] = cpu_to_le16(tkip_p1k[i]);
1225                 memcpy(cmd.key, keyconf->key, keyconf->keylen);
1226                 break;
1227         case WLAN_CIPHER_SUITE_CCMP:
1228                 key_flags |= cpu_to_le16(STA_KEY_FLG_CCM);
1229                 memcpy(cmd.key, keyconf->key, keyconf->keylen);
1230                 break;
1231         case WLAN_CIPHER_SUITE_WEP104:
1232                 key_flags |= cpu_to_le16(STA_KEY_FLG_WEP_13BYTES);
1233                 /* fall through */
1234         case WLAN_CIPHER_SUITE_WEP40:
1235                 key_flags |= cpu_to_le16(STA_KEY_FLG_WEP);
1236                 memcpy(cmd.key + 3, keyconf->key, keyconf->keylen);
1237                 break;
1238         default:
1239                 key_flags |= cpu_to_le16(STA_KEY_FLG_EXT);
1240                 memcpy(cmd.key, keyconf->key, keyconf->keylen);
1241         }
1242
1243         if (mcast)
1244                 key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
1245
1246         cmd.key_offset = keyconf->hw_key_idx;
1247         cmd.key_flags = key_flags;
1248         cmd.sta_id = sta_id;
1249
1250         status = ADD_STA_SUCCESS;
1251         if (cmd_flags & CMD_ASYNC)
1252                 ret =  iwl_mvm_send_cmd_pdu(mvm, ADD_STA_KEY, CMD_ASYNC,
1253                                             sizeof(cmd), &cmd);
1254         else
1255                 ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA_KEY, sizeof(cmd),
1256                                                   &cmd, &status);
1257
1258         switch (status) {
1259         case ADD_STA_SUCCESS:
1260                 IWL_DEBUG_WEP(mvm, "MODIFY_STA: set dynamic key passed\n");
1261                 break;
1262         default:
1263                 ret = -EIO;
1264                 IWL_ERR(mvm, "MODIFY_STA: set dynamic key failed\n");
1265                 break;
1266         }
1267
1268         return ret;
1269 }
1270
1271 static int iwl_mvm_send_sta_igtk(struct iwl_mvm *mvm,
1272                                  struct ieee80211_key_conf *keyconf,
1273                                  u8 sta_id, bool remove_key)
1274 {
1275         struct iwl_mvm_mgmt_mcast_key_cmd igtk_cmd = {};
1276
1277         /* verify the key details match the required command's expectations */
1278         if (WARN_ON((keyconf->cipher != WLAN_CIPHER_SUITE_AES_CMAC) ||
1279                     (keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE) ||
1280                     (keyconf->keyidx != 4 && keyconf->keyidx != 5)))
1281                 return -EINVAL;
1282
1283         igtk_cmd.key_id = cpu_to_le32(keyconf->keyidx);
1284         igtk_cmd.sta_id = cpu_to_le32(sta_id);
1285
1286         if (remove_key) {
1287                 igtk_cmd.ctrl_flags |= cpu_to_le32(STA_KEY_NOT_VALID);
1288         } else {
1289                 struct ieee80211_key_seq seq;
1290                 const u8 *pn;
1291
1292                 memcpy(igtk_cmd.IGTK, keyconf->key, keyconf->keylen);
1293                 ieee80211_aes_cmac_calculate_k1_k2(keyconf,
1294                                                    igtk_cmd.K1, igtk_cmd.K2);
1295                 ieee80211_get_key_rx_seq(keyconf, 0, &seq);
1296                 pn = seq.aes_cmac.pn;
1297                 igtk_cmd.receive_seq_cnt = cpu_to_le64(((u64) pn[5] << 0) |
1298                                                        ((u64) pn[4] << 8) |
1299                                                        ((u64) pn[3] << 16) |
1300                                                        ((u64) pn[2] << 24) |
1301                                                        ((u64) pn[1] << 32) |
1302                                                        ((u64) pn[0] << 40));
1303         }
1304
1305         IWL_DEBUG_INFO(mvm, "%s igtk for sta %u\n",
1306                        remove_key ? "removing" : "installing",
1307                        igtk_cmd.sta_id);
1308
1309         return iwl_mvm_send_cmd_pdu(mvm, MGMT_MCAST_KEY, 0,
1310                                     sizeof(igtk_cmd), &igtk_cmd);
1311 }
1312
1313
1314 static inline u8 *iwl_mvm_get_mac_addr(struct iwl_mvm *mvm,
1315                                        struct ieee80211_vif *vif,
1316                                        struct ieee80211_sta *sta)
1317 {
1318         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1319
1320         if (sta)
1321                 return sta->addr;
1322
1323         if (vif->type == NL80211_IFTYPE_STATION &&
1324             mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT) {
1325                 u8 sta_id = mvmvif->ap_sta_id;
1326                 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1327                                                 lockdep_is_held(&mvm->mutex));
1328                 return sta->addr;
1329         }
1330
1331
1332         return NULL;
1333 }
1334
1335 static int __iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
1336                                  struct ieee80211_vif *vif,
1337                                  struct ieee80211_sta *sta,
1338                                  struct ieee80211_key_conf *keyconf,
1339                                  bool mcast)
1340 {
1341         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1342         int ret;
1343         const u8 *addr;
1344         struct ieee80211_key_seq seq;
1345         u16 p1k[5];
1346
1347         switch (keyconf->cipher) {
1348         case WLAN_CIPHER_SUITE_TKIP:
1349                 addr = iwl_mvm_get_mac_addr(mvm, vif, sta);
1350                 /* get phase 1 key from mac80211 */
1351                 ieee80211_get_key_rx_seq(keyconf, 0, &seq);
1352                 ieee80211_get_tkip_rx_p1k(keyconf, addr, seq.tkip.iv32, p1k);
1353                 ret = iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
1354                                            seq.tkip.iv32, p1k, 0);
1355                 break;
1356         case WLAN_CIPHER_SUITE_CCMP:
1357         case WLAN_CIPHER_SUITE_WEP40:
1358         case WLAN_CIPHER_SUITE_WEP104:
1359                 ret = iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
1360                                            0, NULL, 0);
1361                 break;
1362         default:
1363                 ret = iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
1364                                            0, NULL, 0);
1365         }
1366
1367         return ret;
1368 }
1369
1370 static int __iwl_mvm_remove_sta_key(struct iwl_mvm *mvm, u8 sta_id,
1371                                     struct ieee80211_key_conf *keyconf,
1372                                     bool mcast)
1373 {
1374         struct iwl_mvm_add_sta_key_cmd cmd = {};
1375         __le16 key_flags;
1376         int ret;
1377         u32 status;
1378
1379         key_flags = cpu_to_le16((keyconf->keyidx << STA_KEY_FLG_KEYID_POS) &
1380                                  STA_KEY_FLG_KEYID_MSK);
1381         key_flags |= cpu_to_le16(STA_KEY_FLG_NO_ENC | STA_KEY_FLG_WEP_KEY_MAP);
1382         key_flags |= cpu_to_le16(STA_KEY_NOT_VALID);
1383
1384         if (mcast)
1385                 key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
1386
1387         cmd.key_flags = key_flags;
1388         cmd.key_offset = keyconf->hw_key_idx;
1389         cmd.sta_id = sta_id;
1390
1391         status = ADD_STA_SUCCESS;
1392         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA_KEY, sizeof(cmd),
1393                                           &cmd, &status);
1394
1395         switch (status) {
1396         case ADD_STA_SUCCESS:
1397                 IWL_DEBUG_WEP(mvm, "MODIFY_STA: remove sta key passed\n");
1398                 break;
1399         default:
1400                 ret = -EIO;
1401                 IWL_ERR(mvm, "MODIFY_STA: remove sta key failed\n");
1402                 break;
1403         }
1404
1405         return ret;
1406 }
1407
1408 int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
1409                         struct ieee80211_vif *vif,
1410                         struct ieee80211_sta *sta,
1411                         struct ieee80211_key_conf *keyconf,
1412                         bool have_key_offset)
1413 {
1414         bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
1415         u8 sta_id;
1416         int ret;
1417         static const u8 __maybe_unused zero_addr[ETH_ALEN] = {0};
1418
1419         lockdep_assert_held(&mvm->mutex);
1420
1421         /* Get the station id from the mvm local station table */
1422         sta_id = iwl_mvm_get_key_sta_id(vif, sta);
1423         if (sta_id == IWL_MVM_STATION_COUNT) {
1424                 IWL_ERR(mvm, "Failed to find station id\n");
1425                 return -EINVAL;
1426         }
1427
1428         if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC) {
1429                 ret = iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, false);
1430                 goto end;
1431         }
1432
1433         /*
1434          * It is possible that the 'sta' parameter is NULL, and thus
1435          * there is a need to retrieve  the sta from the local station table.
1436          */
1437         if (!sta) {
1438                 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1439                                                 lockdep_is_held(&mvm->mutex));
1440                 if (IS_ERR_OR_NULL(sta)) {
1441                         IWL_ERR(mvm, "Invalid station id\n");
1442                         return -EINVAL;
1443                 }
1444         }
1445
1446         if (WARN_ON_ONCE(iwl_mvm_sta_from_mac80211(sta)->vif != vif))
1447                 return -EINVAL;
1448
1449         if (!have_key_offset) {
1450                 /*
1451                  * The D3 firmware hardcodes the PTK offset to 0, so we have to
1452                  * configure it there. As a result, this workaround exists to
1453                  * let the caller set the key offset (hw_key_idx), see d3.c.
1454                  */
1455                 keyconf->hw_key_idx = iwl_mvm_set_fw_key_idx(mvm);
1456                 if (keyconf->hw_key_idx == STA_KEY_IDX_INVALID)
1457                         return -ENOSPC;
1458         }
1459
1460         ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf, mcast);
1461         if (ret) {
1462                 __clear_bit(keyconf->hw_key_idx, mvm->fw_key_table);
1463                 goto end;
1464         }
1465
1466         /*
1467          * For WEP, the same key is used for multicast and unicast. Upload it
1468          * again, using the same key offset, and now pointing the other one
1469          * to the same key slot (offset).
1470          * If this fails, remove the original as well.
1471          */
1472         if (keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
1473             keyconf->cipher == WLAN_CIPHER_SUITE_WEP104) {
1474                 ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf, !mcast);
1475                 if (ret) {
1476                         __clear_bit(keyconf->hw_key_idx, mvm->fw_key_table);
1477                         __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
1478                 }
1479         }
1480
1481 end:
1482         IWL_DEBUG_WEP(mvm, "key: cipher=%x len=%d idx=%d sta=%pM ret=%d\n",
1483                       keyconf->cipher, keyconf->keylen, keyconf->keyidx,
1484                       sta ? sta->addr : zero_addr, ret);
1485         return ret;
1486 }
1487
1488 int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
1489                            struct ieee80211_vif *vif,
1490                            struct ieee80211_sta *sta,
1491                            struct ieee80211_key_conf *keyconf)
1492 {
1493         bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
1494         u8 sta_id;
1495         int ret, i;
1496
1497         lockdep_assert_held(&mvm->mutex);
1498
1499         /* Get the station id from the mvm local station table */
1500         sta_id = iwl_mvm_get_key_sta_id(vif, sta);
1501
1502         IWL_DEBUG_WEP(mvm, "mvm remove dynamic key: idx=%d sta=%d\n",
1503                       keyconf->keyidx, sta_id);
1504
1505         if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC)
1506                 return iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, true);
1507
1508         if (!__test_and_clear_bit(keyconf->hw_key_idx, mvm->fw_key_table)) {
1509                 IWL_ERR(mvm, "offset %d not used in fw key table.\n",
1510                         keyconf->hw_key_idx);
1511                 return -ENOENT;
1512         }
1513
1514         /* track which key was deleted last */
1515         for (i = 0; i < STA_KEY_MAX_NUM; i++) {
1516                 if (mvm->fw_key_deleted[i] < U8_MAX)
1517                         mvm->fw_key_deleted[i]++;
1518         }
1519         mvm->fw_key_deleted[keyconf->hw_key_idx] = 0;
1520
1521         if (sta_id == IWL_MVM_STATION_COUNT) {
1522                 IWL_DEBUG_WEP(mvm, "station non-existent, early return.\n");
1523                 return 0;
1524         }
1525
1526         /*
1527          * It is possible that the 'sta' parameter is NULL, and thus
1528          * there is a need to retrieve the sta from the local station table,
1529          * for example when a GTK is removed (where the sta_id will then be
1530          * the AP ID, and no station was passed by mac80211.)
1531          */
1532         if (!sta) {
1533                 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1534                                                 lockdep_is_held(&mvm->mutex));
1535                 if (!sta) {
1536                         IWL_ERR(mvm, "Invalid station id\n");
1537                         return -EINVAL;
1538                 }
1539         }
1540
1541         if (WARN_ON_ONCE(iwl_mvm_sta_from_mac80211(sta)->vif != vif))
1542                 return -EINVAL;
1543
1544         ret = __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
1545         if (ret)
1546                 return ret;
1547
1548         /* delete WEP key twice to get rid of (now useless) offset */
1549         if (keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
1550             keyconf->cipher == WLAN_CIPHER_SUITE_WEP104)
1551                 ret = __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, !mcast);
1552
1553         return ret;
1554 }
1555
1556 void iwl_mvm_update_tkip_key(struct iwl_mvm *mvm,
1557                              struct ieee80211_vif *vif,
1558                              struct ieee80211_key_conf *keyconf,
1559                              struct ieee80211_sta *sta, u32 iv32,
1560                              u16 *phase1key)
1561 {
1562         struct iwl_mvm_sta *mvm_sta;
1563         u8 sta_id = iwl_mvm_get_key_sta_id(vif, sta);
1564         bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
1565
1566         if (WARN_ON_ONCE(sta_id == IWL_MVM_STATION_COUNT))
1567                 return;
1568
1569         rcu_read_lock();
1570
1571         if (!sta) {
1572                 sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
1573                 if (WARN_ON(IS_ERR_OR_NULL(sta))) {
1574                         rcu_read_unlock();
1575                         return;
1576                 }
1577         }
1578
1579         mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1580         iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
1581                              iv32, phase1key, CMD_ASYNC);
1582         rcu_read_unlock();
1583 }
1584
1585 void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
1586                                 struct ieee80211_sta *sta)
1587 {
1588         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1589         struct iwl_mvm_add_sta_cmd cmd = {
1590                 .add_modify = STA_MODE_MODIFY,
1591                 .sta_id = mvmsta->sta_id,
1592                 .station_flags_msk = cpu_to_le32(STA_FLG_PS),
1593                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
1594         };
1595         int ret;
1596
1597         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
1598         if (ret)
1599                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
1600 }
1601
1602 void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
1603                                        struct ieee80211_sta *sta,
1604                                        enum ieee80211_frame_release_type reason,
1605                                        u16 cnt, u16 tids, bool more_data,
1606                                        bool agg)
1607 {
1608         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1609         struct iwl_mvm_add_sta_cmd cmd = {
1610                 .add_modify = STA_MODE_MODIFY,
1611                 .sta_id = mvmsta->sta_id,
1612                 .modify_mask = STA_MODIFY_SLEEPING_STA_TX_COUNT,
1613                 .sleep_tx_count = cpu_to_le16(cnt),
1614                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
1615         };
1616         int tid, ret;
1617         unsigned long _tids = tids;
1618
1619         /* convert TIDs to ACs - we don't support TSPEC so that's OK
1620          * Note that this field is reserved and unused by firmware not
1621          * supporting GO uAPSD, so it's safe to always do this.
1622          */
1623         for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT)
1624                 cmd.awake_acs |= BIT(tid_to_ucode_ac[tid]);
1625
1626         /* If we're releasing frames from aggregation queues then check if the
1627          * all queues combined that we're releasing frames from have
1628          *  - more frames than the service period, in which case more_data
1629          *    needs to be set
1630          *  - fewer than 'cnt' frames, in which case we need to adjust the
1631          *    firmware command (but do that unconditionally)
1632          */
1633         if (agg) {
1634                 int remaining = cnt;
1635
1636                 spin_lock_bh(&mvmsta->lock);
1637                 for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT) {
1638                         struct iwl_mvm_tid_data *tid_data;
1639                         u16 n_queued;
1640
1641                         tid_data = &mvmsta->tid_data[tid];
1642                         if (WARN(tid_data->state != IWL_AGG_ON &&
1643                                  tid_data->state != IWL_EMPTYING_HW_QUEUE_DELBA,
1644                                  "TID %d state is %d\n",
1645                                  tid, tid_data->state)) {
1646                                 spin_unlock_bh(&mvmsta->lock);
1647                                 ieee80211_sta_eosp(sta);
1648                                 return;
1649                         }
1650
1651                         n_queued = iwl_mvm_tid_queued(tid_data);
1652                         if (n_queued > remaining) {
1653                                 more_data = true;
1654                                 remaining = 0;
1655                                 break;
1656                         }
1657                         remaining -= n_queued;
1658                 }
1659                 spin_unlock_bh(&mvmsta->lock);
1660
1661                 cmd.sleep_tx_count = cpu_to_le16(cnt - remaining);
1662                 if (WARN_ON(cnt - remaining == 0)) {
1663                         ieee80211_sta_eosp(sta);
1664                         return;
1665                 }
1666         }
1667
1668         /* Note: this is ignored by firmware not supporting GO uAPSD */
1669         if (more_data)
1670                 cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_MOREDATA);
1671
1672         if (reason == IEEE80211_FRAME_RELEASE_PSPOLL) {
1673                 mvmsta->next_status_eosp = true;
1674                 cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_PS_POLL);
1675         } else {
1676                 cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_UAPSD);
1677         }
1678
1679         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
1680         if (ret)
1681                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
1682 }
1683
1684 void iwl_mvm_rx_eosp_notif(struct iwl_mvm *mvm,
1685                            struct iwl_rx_cmd_buffer *rxb)
1686 {
1687         struct iwl_rx_packet *pkt = rxb_addr(rxb);
1688         struct iwl_mvm_eosp_notification *notif = (void *)pkt->data;
1689         struct ieee80211_sta *sta;
1690         u32 sta_id = le32_to_cpu(notif->sta_id);
1691
1692         if (WARN_ON_ONCE(sta_id >= IWL_MVM_STATION_COUNT))
1693                 return;
1694
1695         rcu_read_lock();
1696         sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
1697         if (!IS_ERR_OR_NULL(sta))
1698                 ieee80211_sta_eosp(sta);
1699         rcu_read_unlock();
1700 }
1701
1702 void iwl_mvm_sta_modify_disable_tx(struct iwl_mvm *mvm,
1703                                    struct iwl_mvm_sta *mvmsta, bool disable)
1704 {
1705         struct iwl_mvm_add_sta_cmd cmd = {
1706                 .add_modify = STA_MODE_MODIFY,
1707                 .sta_id = mvmsta->sta_id,
1708                 .station_flags = disable ? cpu_to_le32(STA_FLG_DISABLE_TX) : 0,
1709                 .station_flags_msk = cpu_to_le32(STA_FLG_DISABLE_TX),
1710                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
1711         };
1712         int ret;
1713
1714         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
1715         if (ret)
1716                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
1717 }
1718
1719 void iwl_mvm_sta_modify_disable_tx_ap(struct iwl_mvm *mvm,
1720                                       struct ieee80211_sta *sta,
1721                                       bool disable)
1722 {
1723         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1724
1725         spin_lock_bh(&mvm_sta->lock);
1726
1727         if (mvm_sta->disable_tx == disable) {
1728                 spin_unlock_bh(&mvm_sta->lock);
1729                 return;
1730         }
1731
1732         mvm_sta->disable_tx = disable;
1733
1734         /*
1735          * Tell mac80211 to start/stop queuing tx for this station,
1736          * but don't stop queuing if there are still pending frames
1737          * for this station.
1738          */
1739         if (disable || !atomic_read(&mvm->pending_frames[mvm_sta->sta_id]))
1740                 ieee80211_sta_block_awake(mvm->hw, sta, disable);
1741
1742         iwl_mvm_sta_modify_disable_tx(mvm, mvm_sta, disable);
1743
1744         spin_unlock_bh(&mvm_sta->lock);
1745 }
1746
1747 void iwl_mvm_modify_all_sta_disable_tx(struct iwl_mvm *mvm,
1748                                        struct iwl_mvm_vif *mvmvif,
1749                                        bool disable)
1750 {
1751         struct ieee80211_sta *sta;
1752         struct iwl_mvm_sta *mvm_sta;
1753         int i;
1754
1755         lockdep_assert_held(&mvm->mutex);
1756
1757         /* Block/unblock all the stations of the given mvmvif */
1758         for (i = 0; i < IWL_MVM_STATION_COUNT; i++) {
1759                 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[i],
1760                                                 lockdep_is_held(&mvm->mutex));
1761                 if (IS_ERR_OR_NULL(sta))
1762                         continue;
1763
1764                 mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1765                 if (mvm_sta->mac_id_n_color !=
1766                     FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color))
1767                         continue;
1768
1769                 iwl_mvm_sta_modify_disable_tx_ap(mvm, sta, disable);
1770         }
1771 }
1772
1773 void iwl_mvm_csa_client_absent(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
1774 {
1775         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1776         struct iwl_mvm_sta *mvmsta;
1777
1778         rcu_read_lock();
1779
1780         mvmsta = iwl_mvm_sta_from_staid_rcu(mvm, mvmvif->ap_sta_id);
1781
1782         if (!WARN_ON(!mvmsta))
1783                 iwl_mvm_sta_modify_disable_tx(mvm, mvmsta, true);
1784
1785         rcu_read_unlock();
1786 }