sta.c

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