kernel/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 - 2014 Intel Corporation. All rights reserved.
   9  * Copyright(c) 2013 - 2014 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 - 2014 Intel Corporation. All rights reserved.
  35  * Copyright(c) 2013 - 2014 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         ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
1004         if (ret)
1005                 return -EIO;
1006
1007         iwl_mvm_enable_agg_txq(mvm, queue, fifo, mvmsta->sta_id, tid,
1008                                buf_size, ssn, wdg_timeout);
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;
1152
1153         lockdep_assert_held(&mvm->mutex);
1154
1155         i = find_first_zero_bit(mvm->fw_key_table, STA_KEY_MAX_NUM);
1156
1157         if (i == STA_KEY_MAX_NUM)
1158                 return STA_KEY_IDX_INVALID;
1159
1160         __set_bit(i, mvm->fw_key_table);
1161
1162         return i;
1163 }
1164
1165 static u8 iwl_mvm_get_key_sta_id(struct ieee80211_vif *vif,
1166                                  struct ieee80211_sta *sta)
1167 {
1168         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1169
1170         if (sta) {
1171                 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1172
1173                 return mvm_sta->sta_id;
1174         }
1175
1176         /*
1177          * The device expects GTKs for station interfaces to be
1178          * installed as GTKs for the AP station. If we have no
1179          * station ID, then use AP's station ID.
1180          */
1181         if (vif->type == NL80211_IFTYPE_STATION &&
1182             mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT)
1183                 return mvmvif->ap_sta_id;
1184
1185         return IWL_MVM_STATION_COUNT;
1186 }
1187
1188 static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
1189                                 struct iwl_mvm_sta *mvm_sta,
1190                                 struct ieee80211_key_conf *keyconf, bool mcast,
1191                                 u32 tkip_iv32, u16 *tkip_p1k, u32 cmd_flags)
1192 {
1193         struct iwl_mvm_add_sta_key_cmd cmd = {};
1194         __le16 key_flags;
1195         int ret;
1196         u32 status;
1197         u16 keyidx;
1198         int i;
1199         u8 sta_id = mvm_sta->sta_id;
1200
1201         keyidx = (keyconf->keyidx << STA_KEY_FLG_KEYID_POS) &
1202                  STA_KEY_FLG_KEYID_MSK;
1203         key_flags = cpu_to_le16(keyidx);
1204         key_flags |= cpu_to_le16(STA_KEY_FLG_WEP_KEY_MAP);
1205
1206         switch (keyconf->cipher) {
1207         case WLAN_CIPHER_SUITE_TKIP:
1208                 key_flags |= cpu_to_le16(STA_KEY_FLG_TKIP);
1209                 cmd.tkip_rx_tsc_byte2 = tkip_iv32;
1210                 for (i = 0; i < 5; i++)
1211                         cmd.tkip_rx_ttak[i] = cpu_to_le16(tkip_p1k[i]);
1212                 memcpy(cmd.key, keyconf->key, keyconf->keylen);
1213                 break;
1214         case WLAN_CIPHER_SUITE_CCMP:
1215                 key_flags |= cpu_to_le16(STA_KEY_FLG_CCM);
1216                 memcpy(cmd.key, keyconf->key, keyconf->keylen);
1217                 break;
1218         case WLAN_CIPHER_SUITE_WEP104:
1219                 key_flags |= cpu_to_le16(STA_KEY_FLG_WEP_13BYTES);
1220                 /* fall through */
1221         case WLAN_CIPHER_SUITE_WEP40:
1222                 key_flags |= cpu_to_le16(STA_KEY_FLG_WEP);
1223                 memcpy(cmd.key + 3, keyconf->key, keyconf->keylen);
1224                 break;
1225         default:
1226                 key_flags |= cpu_to_le16(STA_KEY_FLG_EXT);
1227                 memcpy(cmd.key, keyconf->key, keyconf->keylen);
1228         }
1229
1230         if (mcast)
1231                 key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
1232
1233         cmd.key_offset = keyconf->hw_key_idx;
1234         cmd.key_flags = key_flags;
1235         cmd.sta_id = sta_id;
1236
1237         status = ADD_STA_SUCCESS;
1238         if (cmd_flags & CMD_ASYNC)
1239                 ret =  iwl_mvm_send_cmd_pdu(mvm, ADD_STA_KEY, CMD_ASYNC,
1240                                             sizeof(cmd), &cmd);
1241         else
1242                 ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA_KEY, sizeof(cmd),
1243                                                   &cmd, &status);
1244
1245         switch (status) {
1246         case ADD_STA_SUCCESS:
1247                 IWL_DEBUG_WEP(mvm, "MODIFY_STA: set dynamic key passed\n");
1248                 break;
1249         default:
1250                 ret = -EIO;
1251                 IWL_ERR(mvm, "MODIFY_STA: set dynamic key failed\n");
1252                 break;
1253         }
1254
1255         return ret;
1256 }
1257
1258 static int iwl_mvm_send_sta_igtk(struct iwl_mvm *mvm,
1259                                  struct ieee80211_key_conf *keyconf,
1260                                  u8 sta_id, bool remove_key)
1261 {
1262         struct iwl_mvm_mgmt_mcast_key_cmd igtk_cmd = {};
1263
1264         /* verify the key details match the required command's expectations */
1265         if (WARN_ON((keyconf->cipher != WLAN_CIPHER_SUITE_AES_CMAC) ||
1266                     (keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE) ||
1267                     (keyconf->keyidx != 4 && keyconf->keyidx != 5)))
1268                 return -EINVAL;
1269
1270         igtk_cmd.key_id = cpu_to_le32(keyconf->keyidx);
1271         igtk_cmd.sta_id = cpu_to_le32(sta_id);
1272
1273         if (remove_key) {
1274                 igtk_cmd.ctrl_flags |= cpu_to_le32(STA_KEY_NOT_VALID);
1275         } else {
1276                 struct ieee80211_key_seq seq;
1277                 const u8 *pn;
1278
1279                 memcpy(igtk_cmd.IGTK, keyconf->key, keyconf->keylen);
1280                 ieee80211_aes_cmac_calculate_k1_k2(keyconf,
1281                                                    igtk_cmd.K1, igtk_cmd.K2);
1282                 ieee80211_get_key_rx_seq(keyconf, 0, &seq);
1283                 pn = seq.aes_cmac.pn;
1284                 igtk_cmd.receive_seq_cnt = cpu_to_le64(((u64) pn[5] << 0) |
1285                                                        ((u64) pn[4] << 8) |
1286                                                        ((u64) pn[3] << 16) |
1287                                                        ((u64) pn[2] << 24) |
1288                                                        ((u64) pn[1] << 32) |
1289                                                        ((u64) pn[0] << 40));
1290         }
1291
1292         IWL_DEBUG_INFO(mvm, "%s igtk for sta %u\n",
1293                        remove_key ? "removing" : "installing",
1294                        igtk_cmd.sta_id);
1295
1296         return iwl_mvm_send_cmd_pdu(mvm, MGMT_MCAST_KEY, 0,
1297                                     sizeof(igtk_cmd), &igtk_cmd);
1298 }
1299
1300
1301 static inline u8 *iwl_mvm_get_mac_addr(struct iwl_mvm *mvm,
1302                                        struct ieee80211_vif *vif,
1303                                        struct ieee80211_sta *sta)
1304 {
1305         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1306
1307         if (sta)
1308                 return sta->addr;
1309
1310         if (vif->type == NL80211_IFTYPE_STATION &&
1311             mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT) {
1312                 u8 sta_id = mvmvif->ap_sta_id;
1313                 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1314                                                 lockdep_is_held(&mvm->mutex));
1315                 return sta->addr;
1316         }
1317
1318
1319         return NULL;
1320 }
1321
1322 static int __iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
1323                                  struct ieee80211_vif *vif,
1324                                  struct ieee80211_sta *sta,
1325                                  struct ieee80211_key_conf *keyconf,
1326                                  bool mcast)
1327 {
1328         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1329         int ret;
1330         const u8 *addr;
1331         struct ieee80211_key_seq seq;
1332         u16 p1k[5];
1333
1334         switch (keyconf->cipher) {
1335         case WLAN_CIPHER_SUITE_TKIP:
1336                 addr = iwl_mvm_get_mac_addr(mvm, vif, sta);
1337                 /* get phase 1 key from mac80211 */
1338                 ieee80211_get_key_rx_seq(keyconf, 0, &seq);
1339                 ieee80211_get_tkip_rx_p1k(keyconf, addr, seq.tkip.iv32, p1k);
1340                 ret = iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
1341                                            seq.tkip.iv32, p1k, 0);
1342                 break;
1343         case WLAN_CIPHER_SUITE_CCMP:
1344         case WLAN_CIPHER_SUITE_WEP40:
1345         case WLAN_CIPHER_SUITE_WEP104:
1346                 ret = iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
1347                                            0, NULL, 0);
1348                 break;
1349         default:
1350                 ret = iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
1351                                            0, NULL, 0);
1352         }
1353
1354         return ret;
1355 }
1356
1357 static int __iwl_mvm_remove_sta_key(struct iwl_mvm *mvm, u8 sta_id,
1358                                     struct ieee80211_key_conf *keyconf,
1359                                     bool mcast)
1360 {
1361         struct iwl_mvm_add_sta_key_cmd cmd = {};
1362         __le16 key_flags;
1363         int ret;
1364         u32 status;
1365
1366         key_flags = cpu_to_le16((keyconf->keyidx << STA_KEY_FLG_KEYID_POS) &
1367                                  STA_KEY_FLG_KEYID_MSK);
1368         key_flags |= cpu_to_le16(STA_KEY_FLG_NO_ENC | STA_KEY_FLG_WEP_KEY_MAP);
1369         key_flags |= cpu_to_le16(STA_KEY_NOT_VALID);
1370
1371         if (mcast)
1372                 key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
1373
1374         cmd.key_flags = key_flags;
1375         cmd.key_offset = keyconf->hw_key_idx;
1376         cmd.sta_id = sta_id;
1377
1378         status = ADD_STA_SUCCESS;
1379         ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA_KEY, sizeof(cmd),
1380                                           &cmd, &status);
1381
1382         switch (status) {
1383         case ADD_STA_SUCCESS:
1384                 IWL_DEBUG_WEP(mvm, "MODIFY_STA: remove sta key passed\n");
1385                 break;
1386         default:
1387                 ret = -EIO;
1388                 IWL_ERR(mvm, "MODIFY_STA: remove sta key failed\n");
1389                 break;
1390         }
1391
1392         return ret;
1393 }
1394
1395 int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
1396                         struct ieee80211_vif *vif,
1397                         struct ieee80211_sta *sta,
1398                         struct ieee80211_key_conf *keyconf,
1399                         bool have_key_offset)
1400 {
1401         bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
1402         u8 sta_id;
1403         int ret;
1404
1405         lockdep_assert_held(&mvm->mutex);
1406
1407         /* Get the station id from the mvm local station table */
1408         sta_id = iwl_mvm_get_key_sta_id(vif, sta);
1409         if (sta_id == IWL_MVM_STATION_COUNT) {
1410                 IWL_ERR(mvm, "Failed to find station id\n");
1411                 return -EINVAL;
1412         }
1413
1414         if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC) {
1415                 ret = iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, false);
1416                 goto end;
1417         }
1418
1419         /*
1420          * It is possible that the 'sta' parameter is NULL, and thus
1421          * there is a need to retrieve  the sta from the local station table.
1422          */
1423         if (!sta) {
1424                 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1425                                                 lockdep_is_held(&mvm->mutex));
1426                 if (IS_ERR_OR_NULL(sta)) {
1427                         IWL_ERR(mvm, "Invalid station id\n");
1428                         return -EINVAL;
1429                 }
1430         }
1431
1432         if (WARN_ON_ONCE(iwl_mvm_sta_from_mac80211(sta)->vif != vif))
1433                 return -EINVAL;
1434
1435         if (!have_key_offset) {
1436                 /*
1437                  * The D3 firmware hardcodes the PTK offset to 0, so we have to
1438                  * configure it there. As a result, this workaround exists to
1439                  * let the caller set the key offset (hw_key_idx), see d3.c.
1440                  */
1441                 keyconf->hw_key_idx = iwl_mvm_set_fw_key_idx(mvm);
1442                 if (keyconf->hw_key_idx == STA_KEY_IDX_INVALID)
1443                         return -ENOSPC;
1444         }
1445
1446         ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf, mcast);
1447         if (ret) {
1448                 __clear_bit(keyconf->hw_key_idx, mvm->fw_key_table);
1449                 goto end;
1450         }
1451
1452         /*
1453          * For WEP, the same key is used for multicast and unicast. Upload it
1454          * again, using the same key offset, and now pointing the other one
1455          * to the same key slot (offset).
1456          * If this fails, remove the original as well.
1457          */
1458         if (keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
1459             keyconf->cipher == WLAN_CIPHER_SUITE_WEP104) {
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                         __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
1464                 }
1465         }
1466
1467 end:
1468         IWL_DEBUG_WEP(mvm, "key: cipher=%x len=%d idx=%d sta=%pM ret=%d\n",
1469                       keyconf->cipher, keyconf->keylen, keyconf->keyidx,
1470                       sta->addr, ret);
1471         return ret;
1472 }
1473
1474 int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
1475                            struct ieee80211_vif *vif,
1476                            struct ieee80211_sta *sta,
1477                            struct ieee80211_key_conf *keyconf)
1478 {
1479         bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
1480         u8 sta_id;
1481         int ret;
1482
1483         lockdep_assert_held(&mvm->mutex);
1484
1485         /* Get the station id from the mvm local station table */
1486         sta_id = iwl_mvm_get_key_sta_id(vif, sta);
1487
1488         IWL_DEBUG_WEP(mvm, "mvm remove dynamic key: idx=%d sta=%d\n",
1489                       keyconf->keyidx, sta_id);
1490
1491         if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC)
1492                 return iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, true);
1493
1494         if (!__test_and_clear_bit(keyconf->hw_key_idx, mvm->fw_key_table)) {
1495                 IWL_ERR(mvm, "offset %d not used in fw key table.\n",
1496                         keyconf->hw_key_idx);
1497                 return -ENOENT;
1498         }
1499
1500         if (sta_id == IWL_MVM_STATION_COUNT) {
1501                 IWL_DEBUG_WEP(mvm, "station non-existent, early return.\n");
1502                 return 0;
1503         }
1504
1505         /*
1506          * It is possible that the 'sta' parameter is NULL, and thus
1507          * there is a need to retrieve the sta from the local station table,
1508          * for example when a GTK is removed (where the sta_id will then be
1509          * the AP ID, and no station was passed by mac80211.)
1510          */
1511         if (!sta) {
1512                 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1513                                                 lockdep_is_held(&mvm->mutex));
1514                 if (!sta) {
1515                         IWL_ERR(mvm, "Invalid station id\n");
1516                         return -EINVAL;
1517                 }
1518         }
1519
1520         if (WARN_ON_ONCE(iwl_mvm_sta_from_mac80211(sta)->vif != vif))
1521                 return -EINVAL;
1522
1523         ret = __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
1524         if (ret)
1525                 return ret;
1526
1527         /* delete WEP key twice to get rid of (now useless) offset */
1528         if (keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
1529             keyconf->cipher == WLAN_CIPHER_SUITE_WEP104)
1530                 ret = __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, !mcast);
1531
1532         return ret;
1533 }
1534
1535 void iwl_mvm_update_tkip_key(struct iwl_mvm *mvm,
1536                              struct ieee80211_vif *vif,
1537                              struct ieee80211_key_conf *keyconf,
1538                              struct ieee80211_sta *sta, u32 iv32,
1539                              u16 *phase1key)
1540 {
1541         struct iwl_mvm_sta *mvm_sta;
1542         u8 sta_id = iwl_mvm_get_key_sta_id(vif, sta);
1543         bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
1544
1545         if (WARN_ON_ONCE(sta_id == IWL_MVM_STATION_COUNT))
1546                 return;
1547
1548         rcu_read_lock();
1549
1550         if (!sta) {
1551                 sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
1552                 if (WARN_ON(IS_ERR_OR_NULL(sta))) {
1553                         rcu_read_unlock();
1554                         return;
1555                 }
1556         }
1557
1558         mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1559         iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
1560                              iv32, phase1key, CMD_ASYNC);
1561         rcu_read_unlock();
1562 }
1563
1564 void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
1565                                 struct ieee80211_sta *sta)
1566 {
1567         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1568         struct iwl_mvm_add_sta_cmd cmd = {
1569                 .add_modify = STA_MODE_MODIFY,
1570                 .sta_id = mvmsta->sta_id,
1571                 .station_flags_msk = cpu_to_le32(STA_FLG_PS),
1572                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
1573         };
1574         int ret;
1575
1576         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
1577         if (ret)
1578                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
1579 }
1580
1581 void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
1582                                        struct ieee80211_sta *sta,
1583                                        enum ieee80211_frame_release_type reason,
1584                                        u16 cnt, u16 tids, bool more_data,
1585                                        bool agg)
1586 {
1587         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1588         struct iwl_mvm_add_sta_cmd cmd = {
1589                 .add_modify = STA_MODE_MODIFY,
1590                 .sta_id = mvmsta->sta_id,
1591                 .modify_mask = STA_MODIFY_SLEEPING_STA_TX_COUNT,
1592                 .sleep_tx_count = cpu_to_le16(cnt),
1593                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
1594         };
1595         int tid, ret;
1596         unsigned long _tids = tids;
1597
1598         /* convert TIDs to ACs - we don't support TSPEC so that's OK
1599          * Note that this field is reserved and unused by firmware not
1600          * supporting GO uAPSD, so it's safe to always do this.
1601          */
1602         for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT)
1603                 cmd.awake_acs |= BIT(tid_to_ucode_ac[tid]);
1604
1605         /* If we're releasing frames from aggregation queues then check if the
1606          * all queues combined that we're releasing frames from have
1607          *  - more frames than the service period, in which case more_data
1608          *    needs to be set
1609          *  - fewer than 'cnt' frames, in which case we need to adjust the
1610          *    firmware command (but do that unconditionally)
1611          */
1612         if (agg) {
1613                 int remaining = cnt;
1614
1615                 spin_lock_bh(&mvmsta->lock);
1616                 for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT) {
1617                         struct iwl_mvm_tid_data *tid_data;
1618                         u16 n_queued;
1619
1620                         tid_data = &mvmsta->tid_data[tid];
1621                         if (WARN(tid_data->state != IWL_AGG_ON &&
1622                                  tid_data->state != IWL_EMPTYING_HW_QUEUE_DELBA,
1623                                  "TID %d state is %d\n",
1624                                  tid, tid_data->state)) {
1625                                 spin_unlock_bh(&mvmsta->lock);
1626                                 ieee80211_sta_eosp(sta);
1627                                 return;
1628                         }
1629
1630                         n_queued = iwl_mvm_tid_queued(tid_data);
1631                         if (n_queued > remaining) {
1632                                 more_data = true;
1633                                 remaining = 0;
1634                                 break;
1635                         }
1636                         remaining -= n_queued;
1637                 }
1638                 spin_unlock_bh(&mvmsta->lock);
1639
1640                 cmd.sleep_tx_count = cpu_to_le16(cnt - remaining);
1641                 if (WARN_ON(cnt - remaining == 0)) {
1642                         ieee80211_sta_eosp(sta);
1643                         return;
1644                 }
1645         }
1646
1647         /* Note: this is ignored by firmware not supporting GO uAPSD */
1648         if (more_data)
1649                 cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_MOREDATA);
1650
1651         if (reason == IEEE80211_FRAME_RELEASE_PSPOLL) {
1652                 mvmsta->next_status_eosp = true;
1653                 cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_PS_POLL);
1654         } else {
1655                 cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_UAPSD);
1656         }
1657
1658         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
1659         if (ret)
1660                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
1661 }
1662
1663 int iwl_mvm_rx_eosp_notif(struct iwl_mvm *mvm,
1664                           struct iwl_rx_cmd_buffer *rxb,
1665                           struct iwl_device_cmd *cmd)
1666 {
1667         struct iwl_rx_packet *pkt = rxb_addr(rxb);
1668         struct iwl_mvm_eosp_notification *notif = (void *)pkt->data;
1669         struct ieee80211_sta *sta;
1670         u32 sta_id = le32_to_cpu(notif->sta_id);
1671
1672         if (WARN_ON_ONCE(sta_id >= IWL_MVM_STATION_COUNT))
1673                 return 0;
1674
1675         rcu_read_lock();
1676         sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
1677         if (!IS_ERR_OR_NULL(sta))
1678                 ieee80211_sta_eosp(sta);
1679         rcu_read_unlock();
1680
1681         return 0;
1682 }
1683
1684 void iwl_mvm_sta_modify_disable_tx(struct iwl_mvm *mvm,
1685                                    struct iwl_mvm_sta *mvmsta, bool disable)
1686 {
1687         struct iwl_mvm_add_sta_cmd cmd = {
1688                 .add_modify = STA_MODE_MODIFY,
1689                 .sta_id = mvmsta->sta_id,
1690                 .station_flags = disable ? cpu_to_le32(STA_FLG_DISABLE_TX) : 0,
1691                 .station_flags_msk = cpu_to_le32(STA_FLG_DISABLE_TX),
1692                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
1693         };
1694         int ret;
1695
1696         ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
1697         if (ret)
1698                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
1699 }
1700
1701 void iwl_mvm_sta_modify_disable_tx_ap(struct iwl_mvm *mvm,
1702                                       struct ieee80211_sta *sta,
1703                                       bool disable)
1704 {
1705         struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1706
1707         spin_lock_bh(&mvm_sta->lock);
1708
1709         if (mvm_sta->disable_tx == disable) {
1710                 spin_unlock_bh(&mvm_sta->lock);
1711                 return;
1712         }
1713
1714         mvm_sta->disable_tx = disable;
1715
1716         /*
1717          * Tell mac80211 to start/stop queuing tx for this station,
1718          * but don't stop queuing if there are still pending frames
1719          * for this station.
1720          */
1721         if (disable || !atomic_read(&mvm->pending_frames[mvm_sta->sta_id]))
1722                 ieee80211_sta_block_awake(mvm->hw, sta, disable);
1723
1724         iwl_mvm_sta_modify_disable_tx(mvm, mvm_sta, disable);
1725
1726         spin_unlock_bh(&mvm_sta->lock);
1727 }
1728
1729 void iwl_mvm_modify_all_sta_disable_tx(struct iwl_mvm *mvm,
1730                                        struct iwl_mvm_vif *mvmvif,
1731                                        bool disable)
1732 {
1733         struct ieee80211_sta *sta;
1734         struct iwl_mvm_sta *mvm_sta;
1735         int i;
1736
1737         lockdep_assert_held(&mvm->mutex);
1738
1739         /* Block/unblock all the stations of the given mvmvif */
1740         for (i = 0; i < IWL_MVM_STATION_COUNT; i++) {
1741                 sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[i],
1742                                                 lockdep_is_held(&mvm->mutex));
1743                 if (IS_ERR_OR_NULL(sta))
1744                         continue;
1745
1746                 mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1747                 if (mvm_sta->mac_id_n_color !=
1748                     FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color))
1749                         continue;
1750
1751                 iwl_mvm_sta_modify_disable_tx_ap(mvm, sta, disable);
1752         }
1753 }
1754
1755 void iwl_mvm_csa_client_absent(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
1756 {
1757         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1758         struct iwl_mvm_sta *mvmsta;
1759
1760         rcu_read_lock();
1761
1762         mvmsta = iwl_mvm_sta_from_staid_rcu(mvm, mvmvif->ap_sta_id);
1763
1764         if (!WARN_ON(!mvmsta))
1765                 iwl_mvm_sta_modify_disable_tx(mvm, mvmsta, true);
1766
1767         rcu_read_unlock();
1768 }