1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
   2/*
   3 * Copyright (C) 2012-2015, 2018-2023 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_MVM_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_MVM_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->chandef.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.chandef.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		from_timer(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->trans_cfg->base_params->num_of_queues,
 795		 "max queue %d >= num_of_queues (%d)", maxq,
 796		 mvm->trans->trans_cfg->base_params->num_of_queues))
 797		maxq = mvm->trans->trans_cfg->base_params->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->cfg->min_txq_size);
 856	} else {
 857		size = iwl_mvm_get_queue_size(sta);
 858	}
 859
 860	/* take the min with bc tbl entries allowed */
 861	size = min_t(u32, size, mvm->trans->txqs.bc_tbl_size / sizeof(u16));
 862
 863	/* size needs to be power of 2 values for calculating read/write pointers */
 864	size = rounddown_pow_of_two(size);
 865
 866	if (sta) {
 867		struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 868		struct ieee80211_link_sta *link_sta;
 869		unsigned int link_id;
 870
 871		rcu_read_lock();
 872		for_each_sta_active_link(mvmsta->vif, sta, link_sta, link_id) {
 873			struct iwl_mvm_link_sta *link =
 874				rcu_dereference_protected(mvmsta->link[link_id],
 875							  lockdep_is_held(&mvm->mutex));
 876
 877			if (!link)
 878				continue;
 879
 880			sta_mask |= BIT(link->sta_id);
 881		}
 882		rcu_read_unlock();
 883	} else {
 884		sta_mask |= BIT(sta_id);
 885	}
 886
 887	if (!sta_mask)
 888		return -EINVAL;
 889
 890	do {
 891		queue = iwl_trans_txq_alloc(mvm->trans, 0, sta_mask,
 892					    tid, size, timeout);
 893
 894		if (queue < 0)
 895			IWL_DEBUG_TX_QUEUES(mvm,
 896					    "Failed allocating TXQ of size %d for sta mask %x tid %d, ret: %d\n",
 897					    size, sta_mask, tid, queue);
 898		size /= 2;
 899	} while (queue < 0 && size >= 16);
 900
 901	if (queue < 0)
 902		return queue;
 903
 904	IWL_DEBUG_TX_QUEUES(mvm, "Enabling TXQ #%d for sta mask 0x%x tid %d\n",
 905			    queue, sta_mask, tid);
 906
 907	return queue;
 908}
 909
 910static int iwl_mvm_sta_alloc_queue_tvqm(struct iwl_mvm *mvm,
 911					struct ieee80211_sta *sta, u8 ac,
 912					int tid)
 913{
 914	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 915	struct iwl_mvm_txq *mvmtxq =
 916		iwl_mvm_txq_from_tid(sta, tid);
 917	unsigned int wdg_timeout =
 918		iwl_mvm_get_wd_timeout(mvm, mvmsta->vif, false, false);
 919	int queue = -1;
 920
 921	lockdep_assert_held(&mvm->mutex);
 922
 923	IWL_DEBUG_TX_QUEUES(mvm,
 924			    "Allocating queue for sta %d on tid %d\n",
 925			    mvmsta->deflink.sta_id, tid);
 926	queue = iwl_mvm_tvqm_enable_txq(mvm, sta, mvmsta->deflink.sta_id,
 927					tid, wdg_timeout);
 928	if (queue < 0)
 929		return queue;
 930
 931	mvmtxq->txq_id = queue;
 932	mvm->tvqm_info[queue].txq_tid = tid;
 933	mvm->tvqm_info[queue].sta_id = mvmsta->deflink.sta_id;
 934
 935	IWL_DEBUG_TX_QUEUES(mvm, "Allocated queue is %d\n", queue);
 936
 937	spin_lock_bh(&mvmsta->lock);
 938	mvmsta->tid_data[tid].txq_id = queue;
 939	spin_unlock_bh(&mvmsta->lock);
 940
 941	return 0;
 942}
 943
 944static bool iwl_mvm_update_txq_mapping(struct iwl_mvm *mvm,
 945				       struct ieee80211_sta *sta,
 946				       int queue, u8 sta_id, u8 tid)
 947{
 948	bool enable_queue = true;
 949
 950	/* Make sure this TID isn't already enabled */
 951	if (mvm->queue_info[queue].tid_bitmap & BIT(tid)) {
 952		IWL_ERR(mvm, "Trying to enable TXQ %d with existing TID %d\n",
 953			queue, tid);
 954		return false;
 955	}
 956
 957	/* Update mappings and refcounts */
 958	if (mvm->queue_info[queue].tid_bitmap)
 959		enable_queue = false;
 960
 961	mvm->queue_info[queue].tid_bitmap |= BIT(tid);
 962	mvm->queue_info[queue].ra_sta_id = sta_id;
 963
 964	if (enable_queue) {
 965		if (tid != IWL_MAX_TID_COUNT)
 966			mvm->queue_info[queue].mac80211_ac =
 967				tid_to_mac80211_ac[tid];
 968		else
 969			mvm->queue_info[queue].mac80211_ac = IEEE80211_AC_VO;
 970
 971		mvm->queue_info[queue].txq_tid = tid;
 972	}
 973
 974	if (sta) {
 975		struct iwl_mvm_txq *mvmtxq =
 976			iwl_mvm_txq_from_tid(sta, tid);
 977
 978		mvmtxq->txq_id = queue;
 979	}
 980
 981	IWL_DEBUG_TX_QUEUES(mvm,
 982			    "Enabling TXQ #%d tids=0x%x\n",
 983			    queue, mvm->queue_info[queue].tid_bitmap);
 984
 985	return enable_queue;
 986}
 987
 988static bool iwl_mvm_enable_txq(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 989			       int queue, u16 ssn,
 990			       const struct iwl_trans_txq_scd_cfg *cfg,
 991			       unsigned int wdg_timeout)
 992{
 993	struct iwl_scd_txq_cfg_cmd cmd = {
 994		.scd_queue = queue,
 995		.action = SCD_CFG_ENABLE_QUEUE,
 996		.window = cfg->frame_limit,
 997		.sta_id = cfg->sta_id,
 998		.ssn = cpu_to_le16(ssn),
 999		.tx_fifo = cfg->fifo,
1000		.aggregate = cfg->aggregate,
1001		.tid = cfg->tid,
1002	};
1003	bool inc_ssn;
1004
1005	if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
1006		return false;
1007
1008	/* Send the enabling command if we need to */
1009	if (!iwl_mvm_update_txq_mapping(mvm, sta, queue, cfg->sta_id, cfg->tid))
1010		return false;
1011
1012	inc_ssn = iwl_trans_txq_enable_cfg(mvm->trans, queue, ssn,
1013					   NULL, wdg_timeout);
1014	if (inc_ssn)
1015		le16_add_cpu(&cmd.ssn, 1);
1016
1017	WARN(iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd),
1018	     "Failed to configure queue %d on FIFO %d\n", queue, cfg->fifo);
1019
1020	return inc_ssn;
1021}
1022
1023static void iwl_mvm_change_queue_tid(struct iwl_mvm *mvm, int queue)
1024{
1025	struct iwl_scd_txq_cfg_cmd cmd = {
1026		.scd_queue = queue,
1027		.action = SCD_CFG_UPDATE_QUEUE_TID,
1028	};
1029	int tid;
1030	unsigned long tid_bitmap;
1031	int ret;
1032
1033	lockdep_assert_held(&mvm->mutex);
1034
1035	if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
1036		return;
1037
1038	tid_bitmap = mvm->queue_info[queue].tid_bitmap;
1039
1040	if (WARN(!tid_bitmap, "TXQ %d has no tids assigned to it\n", queue))
1041		return;
1042
1043	/* Find any TID for queue */
1044	tid = find_first_bit(&tid_bitmap, IWL_MAX_TID_COUNT + 1);
1045	cmd.tid = tid;
1046	cmd.tx_fifo = iwl_mvm_ac_to_tx_fifo[tid_to_mac80211_ac[tid]];
1047
1048	ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd);
1049	if (ret) {
1050		IWL_ERR(mvm, "Failed to update owner of TXQ %d (ret=%d)\n",
1051			queue, ret);
1052		return;
1053	}
1054
1055	mvm->queue_info[queue].txq_tid = tid;
1056	IWL_DEBUG_TX_QUEUES(mvm, "Changed TXQ %d ownership to tid %d\n",
1057			    queue, tid);
1058}
1059
1060static void iwl_mvm_unshare_queue(struct iwl_mvm *mvm, int queue)
1061{
1062	struct ieee80211_sta *sta;
1063	struct iwl_mvm_sta *mvmsta;
1064	u8 sta_id;
1065	int tid = -1;
1066	unsigned long tid_bitmap;
1067	unsigned int wdg_timeout;
1068	int ssn;
1069	int ret = true;
1070
1071	/* queue sharing is disabled on new TX path */
1072	if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
1073		return;
1074
1075	lockdep_assert_held(&mvm->mutex);
1076
1077	sta_id = mvm->queue_info[queue].ra_sta_id;
1078	tid_bitmap = mvm->queue_info[queue].tid_bitmap;
1079
1080	/* Find TID for queue, and make sure it is the only one on the queue */
1081	tid = find_first_bit(&tid_bitmap, IWL_MAX_TID_COUNT + 1);
1082	if (tid_bitmap != BIT(tid)) {
1083		IWL_ERR(mvm, "Failed to unshare q %d, active tids=0x%lx\n",
1084			queue, tid_bitmap);
1085		return;
1086	}
1087
1088	IWL_DEBUG_TX_QUEUES(mvm, "Unsharing TXQ %d, keeping tid %d\n", queue,
1089			    tid);
1090
1091	sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1092					lockdep_is_held(&mvm->mutex));
1093
1094	if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta)))
1095		return;
1096
1097	mvmsta = iwl_mvm_sta_from_mac80211(sta);
1098	wdg_timeout = iwl_mvm_get_wd_timeout(mvm, mvmsta->vif, false, false);
1099
1100	ssn = IEEE80211_SEQ_TO_SN(mvmsta->tid_data[tid].seq_number);
1101
1102	ret = iwl_mvm_redirect_queue(mvm, queue, tid,
1103				     tid_to_mac80211_ac[tid], ssn,
1104				     wdg_timeout, true,
1105				     iwl_mvm_txq_from_tid(sta, tid));
1106	if (ret) {
1107		IWL_ERR(mvm, "Failed to redirect TXQ %d\n", queue);
1108		return;
1109	}
1110
1111	/* If aggs should be turned back on - do it */
1112	if (mvmsta->tid_data[tid].state == IWL_AGG_ON) {
1113		struct iwl_mvm_add_sta_cmd cmd = {0};
1114
1115		mvmsta->tid_disable_agg &= ~BIT(tid);
1116
1117		cmd.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color);
1118		cmd.sta_id = mvmsta->deflink.sta_id;
1119		cmd.add_modify = STA_MODE_MODIFY;
1120		cmd.modify_mask = STA_MODIFY_TID_DISABLE_TX;
1121		cmd.tfd_queue_msk = cpu_to_le32(mvmsta->tfd_queue_msk);
1122		cmd.tid_disable_tx = cpu_to_le16(mvmsta->tid_disable_agg);
1123
1124		ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
1125					   iwl_mvm_add_sta_cmd_size(mvm), &cmd);
1126		if (!ret) {
1127			IWL_DEBUG_TX_QUEUES(mvm,
1128					    "TXQ #%d is now aggregated again\n",
1129					    queue);
1130
1131			/* Mark queue intenally as aggregating again */
1132			iwl_trans_txq_set_shared_mode(mvm->trans, queue, false);
1133		}
1134	}
1135
1136	mvm->queue_info[queue].status = IWL_MVM_QUEUE_READY;
1137}
1138
1139/*
1140 * Remove inactive TIDs of a given queue.
1141 * If all queue TIDs are inactive - mark the queue as inactive
1142 * If only some the queue TIDs are inactive - unmap them from the queue
1143 *
1144 * Returns %true if all TIDs were removed and the queue could be reused.
1145 */
1146static bool iwl_mvm_remove_inactive_tids(struct iwl_mvm *mvm,
1147					 struct iwl_mvm_sta *mvmsta, int queue,
1148					 unsigned long tid_bitmap,
1149					 unsigned long *unshare_queues,
1150					 unsigned long *changetid_queues)
1151{
1152	unsigned int tid;
1153
1154	lockdep_assert_held(&mvmsta->lock);
1155	lockdep_assert_held(&mvm->mutex);
1156
1157	if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
1158		return false;
1159
1160	/* Go over all non-active TIDs, incl. IWL_MAX_TID_COUNT (for mgmt) */
1161	for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
1162		/* If some TFDs are still queued - don't mark TID as inactive */
1163		if (iwl_mvm_tid_queued(mvm, &mvmsta->tid_data[tid]))
1164			tid_bitmap &= ~BIT(tid);
1165
1166		/* Don't mark as inactive any TID that has an active BA */
1167		if (mvmsta->tid_data[tid].state != IWL_AGG_OFF)
1168			tid_bitmap &= ~BIT(tid);
1169	}
1170
1171	/* If all TIDs in the queue are inactive - return it can be reused */
1172	if (tid_bitmap == mvm->queue_info[queue].tid_bitmap) {
1173		IWL_DEBUG_TX_QUEUES(mvm, "Queue %d is inactive\n", queue);
1174		return true;
1175	}
1176
1177	/*
1178	 * If we are here, this is a shared queue and not all TIDs timed-out.
1179	 * Remove the ones that did.
1180	 */
1181	for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
1182		u16 q_tid_bitmap;
1183
1184		mvmsta->tid_data[tid].txq_id = IWL_MVM_INVALID_QUEUE;
1185		mvm->queue_info[queue].tid_bitmap &= ~BIT(tid);
1186
1187		q_tid_bitmap = mvm->queue_info[queue].tid_bitmap;
1188
1189		/*
1190		 * We need to take into account a situation in which a TXQ was
1191		 * allocated to TID x, and then turned shared by adding TIDs y
1192		 * and z. If TID x becomes inactive and is removed from the TXQ,
1193		 * ownership must be given to one of the remaining TIDs.
1194		 * This is mainly because if TID x continues - a new queue can't
1195		 * be allocated for it as long as it is an owner of another TXQ.
1196		 *
1197		 * Mark this queue in the right bitmap, we'll send the command
1198		 * to the firmware later.
1199		 */
1200		if (!(q_tid_bitmap & BIT(mvm->queue_info[queue].txq_tid)))
1201			set_bit(queue, changetid_queues);
1202
1203		IWL_DEBUG_TX_QUEUES(mvm,
1204				    "Removing inactive TID %d from shared Q:%d\n",
1205				    tid, queue);
1206	}
1207
1208	IWL_DEBUG_TX_QUEUES(mvm,
1209			    "TXQ #%d left with tid bitmap 0x%x\n", queue,
1210			    mvm->queue_info[queue].tid_bitmap);
1211
1212	/*
1213	 * There may be different TIDs with the same mac queues, so make
1214	 * sure all TIDs have existing corresponding mac queues enabled
1215	 */
1216	tid_bitmap = mvm->queue_info[queue].tid_bitmap;
1217
1218	/* If the queue is marked as shared - "unshare" it */
1219	if (hweight16(mvm->queue_info[queue].tid_bitmap) == 1 &&
1220	    mvm->queue_info[queue].status == IWL_MVM_QUEUE_SHARED) {
1221		IWL_DEBUG_TX_QUEUES(mvm, "Marking Q:%d for reconfig\n",
1222				    queue);
1223		set_bit(queue, unshare_queues);
1224	}
1225
1226	return false;
1227}
1228
1229/*
1230 * Check for inactivity - this includes checking if any queue
1231 * can be unshared and finding one (and only one) that can be
1232 * reused.
1233 * This function is also invoked as a sort of clean-up task,
1234 * in which case @alloc_for_sta is IWL_MVM_INVALID_STA.
1235 *
1236 * Returns the queue number, or -ENOSPC.
1237 */
1238static int iwl_mvm_inactivity_check(struct iwl_mvm *mvm, u8 alloc_for_sta)
1239{
1240	unsigned long now = jiffies;
1241	unsigned long unshare_queues = 0;
1242	unsigned long changetid_queues = 0;
1243	int i, ret, free_queue = -ENOSPC;
1244	struct ieee80211_sta *queue_owner  = NULL;
1245
1246	lockdep_assert_held(&mvm->mutex);
1247
1248	if (iwl_mvm_has_new_tx_api(mvm))
1249		return -ENOSPC;
1250
1251	rcu_read_lock();
1252
1253	/* we skip the CMD queue below by starting at 1 */
1254	BUILD_BUG_ON(IWL_MVM_DQA_CMD_QUEUE != 0);
1255
1256	for (i = 1; i < IWL_MAX_HW_QUEUES; i++) {
1257		struct ieee80211_sta *sta;
1258		struct iwl_mvm_sta *mvmsta;
1259		u8 sta_id;
1260		int tid;
1261		unsigned long inactive_tid_bitmap = 0;
1262		unsigned long queue_tid_bitmap;
1263
1264		queue_tid_bitmap = mvm->queue_info[i].tid_bitmap;
1265		if (!queue_tid_bitmap)
1266			continue;
1267
1268		/* If TXQ isn't in active use anyway - nothing to do here... */
1269		if (mvm->queue_info[i].status != IWL_MVM_QUEUE_READY &&
1270		    mvm->queue_info[i].status != IWL_MVM_QUEUE_SHARED)
1271			continue;
1272
1273		/* Check to see if there are inactive TIDs on this queue */
1274		for_each_set_bit(tid, &queue_tid_bitmap,
1275				 IWL_MAX_TID_COUNT + 1) {
1276			if (time_after(mvm->queue_info[i].last_frame_time[tid] +
1277				       IWL_MVM_DQA_QUEUE_TIMEOUT, now))
1278				continue;
1279
1280			inactive_tid_bitmap |= BIT(tid);
1281		}
1282
1283		/* If all TIDs are active - finish check on this queue */
1284		if (!inactive_tid_bitmap)
1285			continue;
1286
1287		/*
1288		 * If we are here - the queue hadn't been served recently and is
1289		 * in use
1290		 */
1291
1292		sta_id = mvm->queue_info[i].ra_sta_id;
1293		sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
1294
1295		/*
1296		 * If the STA doesn't exist anymore, it isn't an error. It could
1297		 * be that it was removed since getting the queues, and in this
1298		 * case it should've inactivated its queues anyway.
1299		 */
1300		if (IS_ERR_OR_NULL(sta))
1301			continue;
1302
1303		mvmsta = iwl_mvm_sta_from_mac80211(sta);
1304
1305		spin_lock_bh(&mvmsta->lock);
1306		ret = iwl_mvm_remove_inactive_tids(mvm, mvmsta, i,
1307						   inactive_tid_bitmap,
1308						   &unshare_queues,
1309						   &changetid_queues);
1310		if (ret && free_queue < 0) {
1311			queue_owner = sta;
1312			free_queue = i;
1313		}
1314		/* only unlock sta lock - we still need the queue info lock */
1315		spin_unlock_bh(&mvmsta->lock);
1316	}
1317
1318
1319	/* Reconfigure queues requiring reconfiguation */
1320	for_each_set_bit(i, &unshare_queues, IWL_MAX_HW_QUEUES)
1321		iwl_mvm_unshare_queue(mvm, i);
1322	for_each_set_bit(i, &changetid_queues, IWL_MAX_HW_QUEUES)
1323		iwl_mvm_change_queue_tid(mvm, i);
1324
1325	rcu_read_unlock();
1326
1327	if (free_queue >= 0 && alloc_for_sta != IWL_MVM_INVALID_STA) {
1328		ret = iwl_mvm_free_inactive_queue(mvm, free_queue, queue_owner,
1329						  alloc_for_sta);
1330		if (ret)
1331			return ret;
1332	}
1333
1334	return free_queue;
1335}
1336
1337static int iwl_mvm_sta_alloc_queue(struct iwl_mvm *mvm,
1338				   struct ieee80211_sta *sta, u8 ac, int tid)
1339{
1340	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1341	struct iwl_trans_txq_scd_cfg cfg = {
1342		.fifo = iwl_mvm_mac_ac_to_tx_fifo(mvm, ac),
1343		.sta_id = mvmsta->deflink.sta_id,
1344		.tid = tid,
1345		.frame_limit = IWL_FRAME_LIMIT,
1346	};
1347	unsigned int wdg_timeout =
1348		iwl_mvm_get_wd_timeout(mvm, mvmsta->vif, false, false);
1349	int queue = -1;
1350	u16 queue_tmp;
1351	unsigned long disable_agg_tids = 0;
1352	enum iwl_mvm_agg_state queue_state;
1353	bool shared_queue = false, inc_ssn;
1354	int ssn;
1355	unsigned long tfd_queue_mask;
1356	int ret;
1357
1358	lockdep_assert_held(&mvm->mutex);
1359
1360	if (iwl_mvm_has_new_tx_api(mvm))
1361		return iwl_mvm_sta_alloc_queue_tvqm(mvm, sta, ac, tid);
1362
1363	spin_lock_bh(&mvmsta->lock);
1364	tfd_queue_mask = mvmsta->tfd_queue_msk;
1365	ssn = IEEE80211_SEQ_TO_SN(mvmsta->tid_data[tid].seq_number);
1366	spin_unlock_bh(&mvmsta->lock);
1367
1368	if (tid == IWL_MAX_TID_COUNT) {
1369		queue = iwl_mvm_find_free_queue(mvm, mvmsta->deflink.sta_id,
1370						IWL_MVM_DQA_MIN_MGMT_QUEUE,
1371						IWL_MVM_DQA_MAX_MGMT_QUEUE);
1372		if (queue >= IWL_MVM_DQA_MIN_MGMT_QUEUE)
1373			IWL_DEBUG_TX_QUEUES(mvm, "Found free MGMT queue #%d\n",
1374					    queue);
1375
1376		/* If no such queue is found, we'll use a DATA queue instead */
1377	}
1378
1379	if ((queue < 0 && mvmsta->reserved_queue != IEEE80211_INVAL_HW_QUEUE) &&
1380	    (mvm->queue_info[mvmsta->reserved_queue].status ==
1381			IWL_MVM_QUEUE_RESERVED)) {
1382		queue = mvmsta->reserved_queue;
1383		mvm->queue_info[queue].reserved = true;
1384		IWL_DEBUG_TX_QUEUES(mvm, "Using reserved queue #%d\n", queue);
1385	}
1386
1387	if (queue < 0)
1388		queue = iwl_mvm_find_free_queue(mvm, mvmsta->deflink.sta_id,
1389						IWL_MVM_DQA_MIN_DATA_QUEUE,
1390						IWL_MVM_DQA_MAX_DATA_QUEUE);
1391	if (queue < 0) {
1392		/* try harder - perhaps kill an inactive queue */
1393		queue = iwl_mvm_inactivity_check(mvm, mvmsta->deflink.sta_id);
1394	}
1395
1396	/* No free queue - we'll have to share */
1397	if (queue <= 0) {
1398		queue = iwl_mvm_get_shared_queue(mvm, tfd_queue_mask, ac);
1399		if (queue > 0) {
1400			shared_queue = true;
1401			mvm->queue_info[queue].status = IWL_MVM_QUEUE_SHARED;
1402		}
1403	}
1404
1405	/*
1406	 * Mark TXQ as ready, even though it hasn't been fully configured yet,
1407	 * to make sure no one else takes it.
1408	 * This will allow avoiding re-acquiring the lock at the end of the
1409	 * configuration. On error we'll mark it back as free.
1410	 */
1411	if (queue > 0 && !shared_queue)
1412		mvm->queue_info[queue].status = IWL_MVM_QUEUE_READY;
1413
1414	/* This shouldn't happen - out of queues */
1415	if (WARN_ON(queue <= 0)) {
1416		IWL_ERR(mvm, "No available queues for tid %d on sta_id %d\n",
1417			tid, cfg.sta_id);
1418		return queue;
1419	}
1420
1421	/*
1422	 * Actual en/disablement of aggregations is through the ADD_STA HCMD,
1423	 * but for configuring the SCD to send A-MPDUs we need to mark the queue
1424	 * as aggregatable.
1425	 * Mark all DATA queues as allowing to be aggregated at some point
1426	 */
1427	cfg.aggregate = (queue >= IWL_MVM_DQA_MIN_DATA_QUEUE ||
1428			 queue == IWL_MVM_DQA_BSS_CLIENT_QUEUE);
1429
1430	IWL_DEBUG_TX_QUEUES(mvm,
1431			    "Allocating %squeue #%d to sta %d on tid %d\n",
1432			    shared_queue ? "shared " : "", queue,
1433			    mvmsta->deflink.sta_id, tid);
1434
1435	if (shared_queue) {
1436		/* Disable any open aggs on this queue */
1437		disable_agg_tids = iwl_mvm_get_queue_agg_tids(mvm, queue);
1438
1439		if (disable_agg_tids) {
1440			IWL_DEBUG_TX_QUEUES(mvm, "Disabling aggs on queue %d\n",
1441					    queue);
1442			iwl_mvm_invalidate_sta_queue(mvm, queue,
1443						     disable_agg_tids, false);
1444		}
1445	}
1446
1447	inc_ssn = iwl_mvm_enable_txq(mvm, sta, queue, ssn, &cfg, wdg_timeout);
1448
1449	/*
1450	 * Mark queue as shared in transport if shared
1451	 * Note this has to be done after queue enablement because enablement
1452	 * can also set this value, and there is no indication there to shared
1453	 * queues
1454	 */
1455	if (shared_queue)
1456		iwl_trans_txq_set_shared_mode(mvm->trans, queue, true);
1457
1458	spin_lock_bh(&mvmsta->lock);
1459	/*
1460	 * This looks racy, but it is not. We have only one packet for
1461	 * this ra/tid in our Tx path since we stop the Qdisc when we
1462	 * need to allocate a new TFD queue.
1463	 */
1464	if (inc_ssn) {
1465		mvmsta->tid_data[tid].seq_number += 0x10;
1466		ssn = (ssn + 1) & IEEE80211_SCTL_SEQ;
1467	}
1468	mvmsta->tid_data[tid].txq_id = queue;
1469	mvmsta->tfd_queue_msk |= BIT(queue);
1470	queue_state = mvmsta->tid_data[tid].state;
1471
1472	if (mvmsta->reserved_queue == queue)
1473		mvmsta->reserved_queue = IEEE80211_INVAL_HW_QUEUE;
1474	spin_unlock_bh(&mvmsta->lock);
1475
1476	if (!shared_queue) {
1477		ret = iwl_mvm_sta_send_to_fw(mvm, sta, true, STA_MODIFY_QUEUES);
1478		if (ret)
1479			goto out_err;
1480
1481		/* If we need to re-enable aggregations... */
1482		if (queue_state == IWL_AGG_ON) {
1483			ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
1484			if (ret)
1485				goto out_err;
1486		}
1487	} else {
1488		/* Redirect queue, if needed */
1489		ret = iwl_mvm_redirect_queue(mvm, queue, tid, ac, ssn,
1490					     wdg_timeout, false,
1491					     iwl_mvm_txq_from_tid(sta, tid));
1492		if (ret)
1493			goto out_err;
1494	}
1495
1496	return 0;
1497
1498out_err:
1499	queue_tmp = queue;
1500	iwl_mvm_disable_txq(mvm, sta, mvmsta->deflink.sta_id, &queue_tmp, tid);
1501
1502	return ret;
1503}
1504
1505int iwl_mvm_sta_ensure_queue(struct iwl_mvm *mvm,
1506			     struct ieee80211_txq *txq)
1507{
1508	struct iwl_mvm_txq *mvmtxq = iwl_mvm_txq_from_mac80211(txq);
1509	int ret = -EINVAL;
1510
1511	lockdep_assert_held(&mvm->mutex);
1512
1513	if (likely(test_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state)) ||
1514	    !txq->sta) {
1515		return 0;
1516	}
1517
1518	if (!iwl_mvm_sta_alloc_queue(mvm, txq->sta, txq->ac, txq->tid)) {
1519		set_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state);
1520		ret = 0;
1521	}
1522
1523	local_bh_disable();
1524	spin_lock(&mvm->add_stream_lock);
1525	if (!list_empty(&mvmtxq->list))
1526		list_del_init(&mvmtxq->list);
1527	spin_unlock(&mvm->add_stream_lock);
1528	local_bh_enable();
1529
1530	return ret;
1531}
1532
1533void iwl_mvm_add_new_dqa_stream_wk(struct work_struct *wk)
1534{
1535	struct iwl_mvm *mvm = container_of(wk, struct iwl_mvm,
1536					   add_stream_wk);
1537
1538	mutex_lock(&mvm->mutex);
1539
1540	iwl_mvm_inactivity_check(mvm, IWL_MVM_INVALID_STA);
1541
1542	while (!list_empty(&mvm->add_stream_txqs)) {
1543		struct iwl_mvm_txq *mvmtxq;
1544		struct ieee80211_txq *txq;
1545		u8 tid;
1546
1547		mvmtxq = list_first_entry(&mvm->add_stream_txqs,
1548					  struct iwl_mvm_txq, list);
1549
1550		txq = container_of((void *)mvmtxq, struct ieee80211_txq,
1551				   drv_priv);
1552		tid = txq->tid;
1553		if (tid == IEEE80211_NUM_TIDS)
1554			tid = IWL_MAX_TID_COUNT;
1555
1556		/*
1557		 * We can't really do much here, but if this fails we can't
1558		 * transmit anyway - so just don't transmit the frame etc.
1559		 * and let them back up ... we've tried our best to allocate
1560		 * a queue in the function itself.
1561		 */
1562		if (iwl_mvm_sta_alloc_queue(mvm, txq->sta, txq->ac, tid)) {
1563			spin_lock_bh(&mvm->add_stream_lock);
1564			list_del_init(&mvmtxq->list);
1565			spin_unlock_bh(&mvm->add_stream_lock);
1566			continue;
1567		}
1568
1569		/* now we're ready, any remaining races/concurrency will be
1570		 * handled in iwl_mvm_mac_itxq_xmit()
1571		 */
1572		set_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state);
1573
1574		local_bh_disable();
1575		spin_lock(&mvm->add_stream_lock);
1576		list_del_init(&mvmtxq->list);
1577		spin_unlock(&mvm->add_stream_lock);
1578
1579		iwl_mvm_mac_itxq_xmit(mvm->hw, txq);
1580		local_bh_enable();
1581	}
1582
1583	mutex_unlock(&mvm->mutex);
1584}
1585
1586static int iwl_mvm_reserve_sta_stream(struct iwl_mvm *mvm,
1587				      struct ieee80211_sta *sta,
1588				      enum nl80211_iftype vif_type)
1589{
1590	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
1591	int queue;
1592
1593	/* queue reserving is disabled on new TX path */
1594	if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
1595		return 0;
1596
1597	/* run the general cleanup/unsharing of queues */
1598	iwl_mvm_inactivity_check(mvm, IWL_MVM_INVALID_STA);
1599
1600	/* Make sure we have free resources for this STA */
1601	if (vif_type == NL80211_IFTYPE_STATION && !sta->tdls &&
1602	    !mvm->queue_info[IWL_MVM_DQA_BSS_CLIENT_QUEUE].tid_bitmap &&
1603	    (mvm->queue_info[IWL_MVM_DQA_BSS_CLIENT_QUEUE].status ==
1604	     IWL_MVM_QUEUE_FREE))
1605		queue = IWL_MVM_DQA_BSS_CLIENT_QUEUE;
1606	else
1607		queue = iwl_mvm_find_free_queue(mvm, mvmsta->deflink.sta_id,
1608						IWL_MVM_DQA_MIN_DATA_QUEUE,
1609						IWL_MVM_DQA_MAX_DATA_QUEUE);
1610	if (queue < 0) {
1611		/* try again - this time kick out a queue if needed */
1612		queue = iwl_mvm_inactivity_check(mvm, mvmsta->deflink.sta_id);
1613		if (queue < 0) {
1614			IWL_ERR(mvm, "No available queues for new station\n");
1615			return -ENOSPC;
1616		}
1617	}
1618	mvm->queue_info[queue].status = IWL_MVM_QUEUE_RESERVED;
1619
1620	mvmsta->reserved_queue = queue;
1621
1622	IWL_DEBUG_TX_QUEUES(mvm, "Reserving data queue #%d for sta_id %d\n",
1623			    queue, mvmsta->deflink.sta_id);
1624
1625	return 0;
1626}
1627
1628/*
1629 * In DQA mode, after a HW restart the queues should be allocated as before, in
1630 * order to avoid race conditions when there are shared queues. This function
1631 * does the re-mapping and queue allocation.
1632 *
1633 * Note that re-enabling aggregations isn't done in this function.
1634 */
1635void iwl_mvm_realloc_queues_after_restart(struct iwl_mvm *mvm,
1636					  struct ieee80211_sta *sta)
1637{
1638	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1639	unsigned int wdg =
1640		iwl_mvm_get_wd_timeout(mvm, mvm_sta->vif, false, false);
1641	int i;
1642	struct iwl_trans_txq_scd_cfg cfg = {
1643		.sta_id = mvm_sta->deflink.sta_id,
1644		.frame_limit = IWL_FRAME_LIMIT,
1645	};
1646
1647	/* Make sure reserved queue is still marked as such (if allocated) */
1648	if (mvm_sta->reserved_queue != IEEE80211_INVAL_HW_QUEUE)
1649		mvm->queue_info[mvm_sta->reserved_queue].status =
1650			IWL_MVM_QUEUE_RESERVED;
1651
1652	for (i = 0; i <= IWL_MAX_TID_COUNT; i++) {
1653		struct iwl_mvm_tid_data *tid_data = &mvm_sta->tid_data[i];
1654		int txq_id = tid_data->txq_id;
1655		int ac;
1656
1657		if (txq_id == IWL_MVM_INVALID_QUEUE)
1658			continue;
1659
1660		ac = tid_to_mac80211_ac[i];
1661
1662		if (iwl_mvm_has_new_tx_api(mvm)) {
1663			IWL_DEBUG_TX_QUEUES(mvm,
1664					    "Re-mapping sta %d tid %d\n",
1665					    mvm_sta->deflink.sta_id, i);
1666			txq_id = iwl_mvm_tvqm_enable_txq(mvm, sta,
1667							 mvm_sta->deflink.sta_id,
1668							 i, wdg);
1669			/*
1670			 * on failures, just set it to IWL_MVM_INVALID_QUEUE
1671			 * to try again later, we have no other good way of
1672			 * failing here
1673			 */
1674			if (txq_id < 0)
1675				txq_id = IWL_MVM_INVALID_QUEUE;
1676			tid_data->txq_id = txq_id;
1677
1678			/*
1679			 * Since we don't set the seq number after reset, and HW
1680			 * sets it now, FW reset will cause the seq num to start
1681			 * at 0 again, so driver will need to update it
1682			 * internally as well, so it keeps in sync with real val
1683			 */
1684			tid_data->seq_number = 0;
1685		} else {
1686			u16 seq = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
1687
1688			cfg.tid = i;
1689			cfg.fifo = iwl_mvm_mac_ac_to_tx_fifo(mvm, ac);
1690			cfg.aggregate = (txq_id >= IWL_MVM_DQA_MIN_DATA_QUEUE ||
1691					 txq_id ==
1692					 IWL_MVM_DQA_BSS_CLIENT_QUEUE);
1693
1694			IWL_DEBUG_TX_QUEUES(mvm,
1695					    "Re-mapping sta %d tid %d to queue %d\n",
1696					    mvm_sta->deflink.sta_id, i,
1697					    txq_id);
1698
1699			iwl_mvm_enable_txq(mvm, sta, txq_id, seq, &cfg, wdg);
1700			mvm->queue_info[txq_id].status = IWL_MVM_QUEUE_READY;
1701		}
1702	}
1703}
1704
1705static int iwl_mvm_add_int_sta_common(struct iwl_mvm *mvm,
1706				      struct iwl_mvm_int_sta *sta,
1707				      const u8 *addr,
1708				      u16 mac_id, u16 color)
1709{
1710	struct iwl_mvm_add_sta_cmd cmd;
1711	int ret;
1712	u32 status = ADD_STA_SUCCESS;
1713
1714	lockdep_assert_held(&mvm->mutex);
1715
1716	memset(&cmd, 0, sizeof(cmd));
1717	cmd.sta_id = sta->sta_id;
1718
1719	if (iwl_mvm_has_new_station_api(mvm->fw) &&
1720	    sta->type == IWL_STA_AUX_ACTIVITY)
1721		cmd.mac_id_n_color = cpu_to_le32(mac_id);
1722	else
1723		cmd.mac_id_n_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mac_id,
1724								     color));
1725
1726	if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE))
1727		cmd.station_type = sta->type;
1728
1729	if (!iwl_mvm_has_new_tx_api(mvm))
1730		cmd.tfd_queue_msk = cpu_to_le32(sta->tfd_queue_msk);
1731	cmd.tid_disable_tx = cpu_to_le16(0xffff);
1732
1733	if (addr)
1734		memcpy(cmd.addr, addr, ETH_ALEN);
1735
1736	ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
1737					  iwl_mvm_add_sta_cmd_size(mvm),
1738					  &cmd, &status);
1739	if (ret)
1740		return ret;
1741
1742	switch (status & IWL_ADD_STA_STATUS_MASK) {
1743	case ADD_STA_SUCCESS:
1744		IWL_DEBUG_INFO(mvm, "Internal station added.\n");
1745		return 0;
1746	default:
1747		ret = -EIO;
1748		IWL_ERR(mvm, "Add internal station failed, status=0x%x\n",
1749			status);
1750		break;
1751	}
1752	return ret;
1753}
1754
1755/* Initialize driver data of a new sta */
1756int iwl_mvm_sta_init(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
1757		     struct ieee80211_sta *sta, int sta_id, u8 sta_type)
1758{
1759	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1760	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1761	struct iwl_mvm_rxq_dup_data *dup_data;
1762	int i, ret = 0;
1763
1764	lockdep_assert_held(&mvm->mutex);
1765
1766	mvm_sta->mac_id_n_color = FW_CMD_ID_AND_COLOR(mvmvif->id,
1767						      mvmvif->color);
1768	mvm_sta->vif = vif;
1769
1770	/* for MLD sta_id(s) should be allocated for each link before calling
1771	 * this function
1772	 */
1773	if (!mvm->mld_api_is_used) {
1774		if (WARN_ON(sta_id == IWL_MVM_INVALID_STA))
1775			return -EINVAL;
1776
1777		mvm_sta->deflink.sta_id = sta_id;
1778		rcu_assign_pointer(mvm_sta->link[0], &mvm_sta->deflink);
1779
1780		if (!mvm->trans->trans_cfg->gen2)
1781			mvm_sta->deflink.lq_sta.rs_drv.pers.max_agg_bufsize =
1782				LINK_QUAL_AGG_FRAME_LIMIT_DEF;
1783		else
1784			mvm_sta->deflink.lq_sta.rs_drv.pers.max_agg_bufsize =
1785				LINK_QUAL_AGG_FRAME_LIMIT_GEN2_DEF;
1786	}
1787
1788	mvm_sta->tt_tx_protection = false;
1789	mvm_sta->sta_type = sta_type;
1790
1791	mvm_sta->tid_disable_agg = 0xffff; /* No aggs at first */
1792
1793	for (i = 0; i <= IWL_MAX_TID_COUNT; i++) {
1794		/*
1795		 * Mark all queues for this STA as unallocated and defer TX
1796		 * frames until the queue is allocated
1797		 */
1798		mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE;
1799	}
1800
1801	for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
1802		struct iwl_mvm_txq *mvmtxq =
1803			iwl_mvm_txq_from_mac80211(sta->txq[i]);
1804
1805		mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
1806		INIT_LIST_HEAD(&mvmtxq->list);
1807		atomic_set(&mvmtxq->tx_request, 0);
1808	}
1809
1810	if (iwl_mvm_has_new_rx_api(mvm)) {
1811		int q;
1812
1813		dup_data = kcalloc(mvm->trans->num_rx_queues,
1814				   sizeof(*dup_data), GFP_KERNEL);
1815		if (!dup_data)
1816			return -ENOMEM;
1817		/*
1818		 * Initialize all the last_seq values to 0xffff which can never
1819		 * compare equal to the frame's seq_ctrl in the check in
1820		 * iwl_mvm_is_dup() since the lower 4 bits are the fragment
1821		 * number and fragmented packets don't reach that function.
1822		 *
1823		 * This thus allows receiving a packet with seqno 0 and the
1824		 * retry bit set as the very first packet on a new TID.
1825		 */
1826		for (q = 0; q < mvm->trans->num_rx_queues; q++)
1827			memset(dup_data[q].last_seq, 0xff,
1828			       sizeof(dup_data[q].last_seq));
1829		mvm_sta->dup_data = dup_data;
1830	}
1831
1832	if (!iwl_mvm_has_new_tx_api(mvm)) {
1833		ret = iwl_mvm_reserve_sta_stream(mvm, sta,
1834						 ieee80211_vif_type_p2p(vif));
1835		if (ret)
1836			return ret;
1837	}
1838
1839	/*
1840	 * if rs is registered with mac80211, then "add station" will be handled
1841	 * via the corresponding ops, otherwise need to notify rate scaling here
1842	 */
1843	if (iwl_mvm_has_tlc_offload(mvm))
1844		iwl_mvm_rs_add_sta(mvm, mvm_sta);
1845	else
1846		spin_lock_init(&mvm_sta->deflink.lq_sta.rs_drv.pers.lock);
1847
1848	iwl_mvm_toggle_tx_ant(mvm, &mvm_sta->tx_ant);
1849
1850	return 0;
1851}
1852
1853int iwl_mvm_add_sta(struct iwl_mvm *mvm,
1854		    struct ieee80211_vif *vif,
1855		    struct ieee80211_sta *sta)
1856{
1857	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1858	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1859	int ret, sta_id;
1860	bool sta_update = false;
1861	unsigned int sta_flags = 0;
1862
1863	lockdep_assert_held(&mvm->mutex);
1864
1865	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
1866		sta_id = iwl_mvm_find_free_sta_id(mvm,
1867						  ieee80211_vif_type_p2p(vif));
1868	else
1869		sta_id = mvm_sta->deflink.sta_id;
1870
1871	if (sta_id == IWL_MVM_INVALID_STA)
1872		return -ENOSPC;
1873
1874	spin_lock_init(&mvm_sta->lock);
1875
1876	/* if this is a HW restart re-alloc existing queues */
1877	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
1878		struct iwl_mvm_int_sta tmp_sta = {
1879			.sta_id = sta_id,
1880			.type = mvm_sta->sta_type,
1881		};
1882
1883		/* First add an empty station since allocating
1884		 * a queue requires a valid station
1885		 */
1886		ret = iwl_mvm_add_int_sta_common(mvm, &tmp_sta, sta->addr,
1887						 mvmvif->id, mvmvif->color);
1888		if (ret)
1889			goto err;
1890
1891		iwl_mvm_realloc_queues_after_restart(mvm, sta);
1892		sta_update = true;
1893		sta_flags = iwl_mvm_has_new_tx_api(mvm) ? 0 : STA_MODIFY_QUEUES;
1894		goto update_fw;
1895	}
1896
1897	ret = iwl_mvm_sta_init(mvm, vif, sta, sta_id,
1898			       sta->tdls ? IWL_STA_TDLS_LINK : IWL_STA_LINK);
1899	if (ret)
1900		goto err;
1901
1902update_fw:
1903	ret = iwl_mvm_sta_send_to_fw(mvm, sta, sta_update, sta_flags);
1904	if (ret)
1905		goto err;
1906
1907	if (vif->type == NL80211_IFTYPE_STATION) {
1908		if (!sta->tdls) {
1909			WARN_ON(mvmvif->deflink.ap_sta_id != IWL_MVM_INVALID_STA);
1910			mvmvif->deflink.ap_sta_id = sta_id;
1911		} else {
1912			WARN_ON(mvmvif->deflink.ap_sta_id == IWL_MVM_INVALID_STA);
1913		}
1914	}
1915
1916	rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);
1917
1918	return 0;
1919
1920err:
1921	return ret;
1922}
1923
1924int iwl_mvm_drain_sta(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvmsta,
1925		      bool drain)
1926{
1927	struct iwl_mvm_add_sta_cmd cmd = {};
1928	int ret;
1929	u32 status;
1930
1931	lockdep_assert_held(&mvm->mutex);
1932
1933	cmd.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color);
1934	cmd.sta_id = mvmsta->deflink.sta_id;
1935	cmd.add_modify = STA_MODE_MODIFY;
1936	cmd.station_flags = drain ? cpu_to_le32(STA_FLG_DRAIN_FLOW) : 0;
1937	cmd.station_flags_msk = cpu_to_le32(STA_FLG_DRAIN_FLOW);
1938
1939	status = ADD_STA_SUCCESS;
1940	ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
1941					  iwl_mvm_add_sta_cmd_size(mvm),
1942					  &cmd, &status);
1943	if (ret)
1944		return ret;
1945
1946	switch (status & IWL_ADD_STA_STATUS_MASK) {
1947	case ADD_STA_SUCCESS:
1948		IWL_DEBUG_INFO(mvm, "Frames for staid %d will drained in fw\n",
1949			       mvmsta->deflink.sta_id);
1950		break;
1951	default:
1952		ret = -EIO;
1953		IWL_ERR(mvm, "Couldn't drain frames for staid %d\n",
1954			mvmsta->deflink.sta_id);
1955		break;
1956	}
1957
1958	return ret;
1959}
1960
1961/*
1962 * Remove a station from the FW table. Before sending the command to remove
1963 * the station validate that the station is indeed known to the driver (sanity
1964 * only).
1965 */
1966static int iwl_mvm_rm_sta_common(struct iwl_mvm *mvm, u8 sta_id)
1967{
1968	struct ieee80211_sta *sta;
1969	struct iwl_mvm_rm_sta_cmd rm_sta_cmd = {
1970		.sta_id = sta_id,
1971	};
1972	int ret;
1973
1974	sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
1975					lockdep_is_held(&mvm->mutex));
1976
1977	/* Note: internal stations are marked as error values */
1978	if (!sta) {
1979		IWL_ERR(mvm, "Invalid station id\n");
1980		return -EINVAL;
1981	}
1982
1983	ret = iwl_mvm_send_cmd_pdu(mvm, REMOVE_STA, 0,
1984				   sizeof(rm_sta_cmd), &rm_sta_cmd);
1985	if (ret) {
1986		IWL_ERR(mvm, "Failed to remove station. Id=%d\n", sta_id);
1987		return ret;
1988	}
1989
1990	return 0;
1991}
1992
1993static void iwl_mvm_disable_sta_queues(struct iwl_mvm *mvm,
1994				       struct ieee80211_vif *vif,
1995				       struct ieee80211_sta *sta)
1996{
1997	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1998	int i;
1999
2000	lockdep_assert_held(&mvm->mutex);
2001
2002	for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
2003		if (mvm_sta->tid_data[i].txq_id == IWL_MVM_INVALID_QUEUE)
2004			continue;
2005
2006		iwl_mvm_disable_txq(mvm, sta, mvm_sta->deflink.sta_id,
2007				    &mvm_sta->tid_data[i].txq_id, i);
2008		mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE;
2009	}
2010
2011	for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
2012		struct iwl_mvm_txq *mvmtxq =
2013			iwl_mvm_txq_from_mac80211(sta->txq[i]);
2014
2015		spin_lock_bh(&mvm->add_stream_lock);
2016		mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
2017		list_del_init(&mvmtxq->list);
2018		clear_bit(IWL_MVM_TXQ_STATE_READY, &mvmtxq->state);
2019		spin_unlock_bh(&mvm->add_stream_lock);
2020	}
2021}
2022
2023int iwl_mvm_wait_sta_queues_empty(struct iwl_mvm *mvm,
2024				  struct iwl_mvm_sta *mvm_sta)
2025{
2026	int i;
2027
2028	for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
2029		u16 txq_id;
2030		int ret;
2031
2032		spin_lock_bh(&mvm_sta->lock);
2033		txq_id = mvm_sta->tid_data[i].txq_id;
2034		spin_unlock_bh(&mvm_sta->lock);
2035
2036		if (txq_id == IWL_MVM_INVALID_QUEUE)
2037			continue;
2038
2039		ret = iwl_trans_wait_txq_empty(mvm->trans, txq_id);
2040		if (ret)
2041			return ret;
2042	}
2043
2044	return 0;
2045}
2046
2047/* Execute the common part for both MLD and non-MLD modes.
2048 * Returns if we're done with removing the station, either
2049 * with error or success
2050 */
2051bool iwl_mvm_sta_del(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
2052		     struct ieee80211_sta *sta,
2053		     struct ieee80211_link_sta *link_sta, int *ret)
2054{
2055	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2056	struct iwl_mvm_vif_link_info *mvm_link =
2057		mvmvif->link[link_sta->link_id];
2058	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
2059	struct iwl_mvm_link_sta *mvm_link_sta;
2060	u8 sta_id;
2061
2062	lockdep_assert_held(&mvm->mutex);
2063
2064	mvm_link_sta =
2065		rcu_dereference_protected(mvm_sta->link[link_sta->link_id],
2066					  lockdep_is_held(&mvm->mutex));
2067	sta_id = mvm_link_sta->sta_id;
2068
2069	/* If there is a TXQ still marked as reserved - free it */
2070	if (mvm_sta->reserved_queue != IEEE80211_INVAL_HW_QUEUE) {
2071		u8 reserved_txq = mvm_sta->reserved_queue;
2072		enum iwl_mvm_queue_status *status;
2073
2074		/*
2075		 * If no traffic has gone through the reserved TXQ - it
2076		 * is still marked as IWL_MVM_QUEUE_RESERVED, and
2077		 * should be manually marked as free again
2078		 */
2079		status = &mvm->queue_info[reserved_txq].status;
2080		if (WARN((*status != IWL_MVM_QUEUE_RESERVED) &&
2081			 (*status != IWL_MVM_QUEUE_FREE),
2082			 "sta_id %d reserved txq %d status %d",
2083			 sta_id, reserved_txq, *status)) {
2084			*ret = -EINVAL;
2085			return true;
2086		}
2087
2088		*status = IWL_MVM_QUEUE_FREE;
2089	}
2090
2091	if (vif->type == NL80211_IFTYPE_STATION &&
2092	    mvm_link->ap_sta_id == sta_id) {
2093		/* if associated - we can't remove the AP STA now */
2094		if (vif->cfg.assoc)
2095			return true;
2096
2097		/* first remove remaining keys */
2098		iwl_mvm_sec_key_remove_ap(mvm, vif, mvm_link, 0);
2099
2100		/* unassoc - go ahead - remove the AP STA now */
2101		mvm_link->ap_sta_id = IWL_MVM_INVALID_STA;
2102	}
2103
2104	/*
2105	 * This shouldn't happen - the TDLS channel switch should be canceled
2106	 * before the STA is removed.
2107	 */
2108	if (WARN_ON_ONCE(mvm->tdls_cs.peer.sta_id == sta_id)) {
2109		mvm->tdls_cs.peer.sta_id = IWL_MVM_INVALID_STA;
2110		cancel_delayed_work(&mvm->tdls_cs.dwork);
2111	}
2112
2113	return false;
2114}
2115
2116int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
2117		   struct ieee80211_vif *vif,
2118		   struct ieee80211_sta *sta)
2119{
2120	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
2121	int ret;
2122
2123	lockdep_assert_held(&mvm->mutex);
2124
2125	ret = iwl_mvm_drain_sta(mvm, mvm_sta, true);
2126	if (ret)
2127		return ret;
2128
2129	/* flush its queues here since we are freeing mvm_sta */
2130	ret = iwl_mvm_flush_sta(mvm, mvm_sta->deflink.sta_id,
2131				mvm_sta->tfd_queue_msk);
2132	if (ret)
2133		return ret;
2134	if (iwl_mvm_has_new_tx_api(mvm)) {
2135		ret = iwl_mvm_wait_sta_queues_empty(mvm, mvm_sta);
2136	} else {
2137		u32 q_mask = mvm_sta->tfd_queue_msk;
2138
2139		ret = iwl_trans_wait_tx_queues_empty(mvm->trans,
2140						     q_mask);
2141	}
2142	if (ret)
2143		return ret;
2144
2145	ret = iwl_mvm_drain_sta(mvm, mvm_sta, false);
2146
2147	iwl_mvm_disable_sta_queues(mvm, vif, sta);
2148
2149	if (iwl_mvm_sta_del(mvm, vif, sta, &sta->deflink, &ret))
2150		return ret;
2151
2152	ret = iwl_mvm_rm_sta_common(mvm, mvm_sta->deflink.sta_id);
2153	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta->deflink.sta_id], NULL);
2154
2155	return ret;
2156}
2157
2158int iwl_mvm_rm_sta_id(struct iwl_mvm *mvm,
2159		      struct ieee80211_vif *vif,
2160		      u8 sta_id)
2161{
2162	int ret = iwl_mvm_rm_sta_common(mvm, sta_id);
2163
2164	lockdep_assert_held(&mvm->mutex);
2165
2166	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL);
2167	return ret;
2168}
2169
2170int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm,
2171			     struct iwl_mvm_int_sta *sta,
2172			     u32 qmask, enum nl80211_iftype iftype,
2173			     u8 type)
2174{
2175	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status) ||
2176	    sta->sta_id == IWL_MVM_INVALID_STA) {
2177		sta->sta_id = iwl_mvm_find_free_sta_id(mvm, iftype);
2178		if (WARN_ON_ONCE(sta->sta_id == IWL_MVM_INVALID_STA))
2179			return -ENOSPC;
2180	}
2181
2182	sta->tfd_queue_msk = qmask;
2183	sta->type = type;
2184
2185	/* put a non-NULL value so iterating over the stations won't stop */
2186	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta->sta_id], ERR_PTR(-EINVAL));
2187	return 0;
2188}
2189
2190void iwl_mvm_dealloc_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta)
2191{
2192	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta->sta_id], NULL);
2193	memset(sta, 0, sizeof(struct iwl_mvm_int_sta));
2194	sta->sta_id = IWL_MVM_INVALID_STA;
2195}
2196
2197static void iwl_mvm_enable_aux_snif_queue(struct iwl_mvm *mvm, u16 queue,
2198					  u8 sta_id, u8 fifo)
2199{
2200	unsigned int wdg_timeout =
2201		mvm->trans->trans_cfg->base_params->wd_timeout;
2202	struct iwl_trans_txq_scd_cfg cfg = {
2203		.fifo = fifo,
2204		.sta_id = sta_id,
2205		.tid = IWL_MAX_TID_COUNT,
2206		.aggregate = false,
2207		.frame_limit = IWL_FRAME_LIMIT,
2208	};
2209
2210	WARN_ON(iwl_mvm_has_new_tx_api(mvm));
2211
2212	iwl_mvm_enable_txq(mvm, NULL, queue, 0, &cfg, wdg_timeout);
2213}
2214
2215static int iwl_mvm_enable_aux_snif_queue_tvqm(struct iwl_mvm *mvm, u8 sta_id)
2216{
2217	unsigned int wdg_timeout =
2218		mvm->trans->trans_cfg->base_params->wd_timeout;
2219
2220	WARN_ON(!iwl_mvm_has_new_tx_api(mvm));
2221
2222	return iwl_mvm_tvqm_enable_txq(mvm, NULL, sta_id, IWL_MAX_TID_COUNT,
2223				       wdg_timeout);
2224}
2225
2226static int iwl_mvm_add_int_sta_with_queue(struct iwl_mvm *mvm, int macidx,
2227					  int maccolor, u8 *addr,
2228					  struct iwl_mvm_int_sta *sta,
2229					  u16 *queue, int fifo)
2230{
2231	int ret;
2232
2233	/* Map queue to fifo - needs to happen before adding station */
2234	if (!iwl_mvm_has_new_tx_api(mvm))
2235		iwl_mvm_enable_aux_snif_queue(mvm, *queue, sta->sta_id, fifo);
2236
2237	ret = iwl_mvm_add_int_sta_common(mvm, sta, addr, macidx, maccolor);
2238	if (ret) {
2239		if (!iwl_mvm_has_new_tx_api(mvm))
2240			iwl_mvm_disable_txq(mvm, NULL, sta->sta_id, queue,
2241					    IWL_MAX_TID_COUNT);
2242		return ret;
2243	}
2244
2245	/*
2246	 * For 22000 firmware and on we cannot add queue to a station unknown
2247	 * to firmware so enable queue here - after the station was added
2248	 */
2249	if (iwl_mvm_has_new_tx_api(mvm)) {
2250		int txq;
2251
2252		txq = iwl_mvm_enable_aux_snif_queue_tvqm(mvm, sta->sta_id);
2253		if (txq < 0) {
2254			iwl_mvm_rm_sta_common(mvm, sta->sta_id);
2255			return txq;
2256		}
2257
2258		*queue = txq;
2259	}
2260
2261	return 0;
2262}
2263
2264int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm, u32 lmac_id)
2265{
2266	int ret;
2267	u32 qmask = mvm->aux_queue == IWL_MVM_INVALID_QUEUE ? 0 :
2268		BIT(mvm->aux_queue);
2269
2270	lockdep_assert_held(&mvm->mutex);
2271
2272	/* Allocate aux station and assign to it the aux queue */
2273	ret = iwl_mvm_allocate_int_sta(mvm, &mvm->aux_sta, qmask,
2274				       NL80211_IFTYPE_UNSPECIFIED,
2275				       IWL_STA_AUX_ACTIVITY);
2276	if (ret)
2277		return ret;
2278
2279	/*
2280	 * In CDB NICs we need to specify which lmac to use for aux activity
2281	 * using the mac_id argument place to send lmac_id to the function
2282	 */
2283	ret = iwl_mvm_add_int_sta_with_queue(mvm, lmac_id, 0, NULL,
2284					     &mvm->aux_sta, &mvm->aux_queue,
2285					     IWL_MVM_TX_FIFO_MCAST);
2286	if (ret) {
2287		iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);
2288		return ret;
2289	}
2290
2291	return 0;
2292}
2293
2294int iwl_mvm_add_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2295{
2296	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2297
2298	lockdep_assert_held(&mvm->mutex);
2299
2300	return iwl_mvm_add_int_sta_with_queue(mvm, mvmvif->id, mvmvif->color,
2301					      NULL, &mvm->snif_sta,
2302					      &mvm->snif_queue,
2303					      IWL_MVM_TX_FIFO_BE);
2304}
2305
2306int iwl_mvm_rm_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2307{
2308	int ret;
2309
2310	lockdep_assert_held(&mvm->mutex);
2311
2312	if (WARN_ON_ONCE(mvm->snif_sta.sta_id == IWL_MVM_INVALID_STA))
2313		return -EINVAL;
2314
2315	iwl_mvm_disable_txq(mvm, NULL, mvm->snif_sta.sta_id,
2316			    &mvm->snif_queue, IWL_MAX_TID_COUNT);
2317	ret = iwl_mvm_rm_sta_common(mvm, mvm->snif_sta.sta_id);
2318	if (ret)
2319		IWL_WARN(mvm, "Failed sending remove station\n");
2320
2321	return ret;
2322}
2323
2324int iwl_mvm_rm_aux_sta(struct iwl_mvm *mvm)
2325{
2326	int ret;
2327
2328	lockdep_assert_held(&mvm->mutex);
2329
2330	if (WARN_ON_ONCE(mvm->aux_sta.sta_id == IWL_MVM_INVALID_STA))
2331		return -EINVAL;
2332
2333	iwl_mvm_disable_txq(mvm, NULL, mvm->aux_sta.sta_id,
2334			    &mvm->aux_queue, IWL_MAX_TID_COUNT);
2335	ret = iwl_mvm_rm_sta_common(mvm, mvm->aux_sta.sta_id);
2336	if (ret)
2337		IWL_WARN(mvm, "Failed sending remove station\n");
2338	iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);
2339
2340	return ret;
2341}
2342
2343void iwl_mvm_dealloc_snif_sta(struct iwl_mvm *mvm)
2344{
2345	iwl_mvm_dealloc_int_sta(mvm, &mvm->snif_sta);
2346}
2347
2348/*
2349 * Send the add station command for the vif's broadcast station.
2350 * Assumes that the station was already allocated.
2351 *
2352 * @mvm: the mvm component
2353 * @vif: the interface to which the broadcast station is added
2354 * @bsta: the broadcast station to add.
2355 */
2356int iwl_mvm_send_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2357{
2358	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2359	struct iwl_mvm_int_sta *bsta = &mvmvif->deflink.bcast_sta;
2360	static const u8 _baddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
2361	const u8 *baddr = _baddr;
2362	int queue;
2363	int ret;
2364	unsigned int wdg_timeout =
2365		iwl_mvm_get_wd_timeout(mvm, vif, false, false);
2366	struct iwl_trans_txq_scd_cfg cfg = {
2367		.fifo = IWL_MVM_TX_FIFO_VO,
2368		.sta_id = mvmvif->deflink.bcast_sta.sta_id,
2369		.tid = IWL_MAX_TID_COUNT,
2370		.aggregate = false,
2371		.frame_limit = IWL_FRAME_LIMIT,
2372	};
2373
2374	lockdep_assert_held(&mvm->mutex);
2375
2376	if (!iwl_mvm_has_new_tx_api(mvm)) {
2377		if (vif->type == NL80211_IFTYPE_AP ||
2378		    vif->type == NL80211_IFTYPE_ADHOC) {
2379			queue = mvm->probe_queue;
2380		} else if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
2381			queue = mvm->p2p_dev_queue;
2382		} else {
2383			WARN(1, "Missing required TXQ for adding bcast STA\n");
2384			return -EINVAL;
2385		}
2386
2387		bsta->tfd_queue_msk |= BIT(queue);
2388
2389		iwl_mvm_enable_txq(mvm, NULL, queue, 0, &cfg, wdg_timeout);
2390	}
2391
2392	if (vif->type == NL80211_IFTYPE_ADHOC)
2393		baddr = vif->bss_conf.bssid;
2394
2395	if (WARN_ON_ONCE(bsta->sta_id == IWL_MVM_INVALID_STA))
2396		return -ENOSPC;
2397
2398	ret = iwl_mvm_add_int_sta_common(mvm, bsta, baddr,
2399					 mvmvif->id, mvmvif->color);
2400	if (ret)
2401		return ret;
2402
2403	/*
2404	 * For 22000 firmware and on we cannot add queue to a station unknown
2405	 * to firmware so enable queue here - after the station was added
2406	 */
2407	if (iwl_mvm_has_new_tx_api(mvm)) {
2408		queue = iwl_mvm_tvqm_enable_txq(mvm, NULL, bsta->sta_id,
2409						IWL_MAX_TID_COUNT,
2410						wdg_timeout);
2411		if (queue < 0) {
2412			iwl_mvm_rm_sta_common(mvm, bsta->sta_id);
2413			return queue;
2414		}
2415
2416		if (vif->type == NL80211_IFTYPE_AP ||
2417		    vif->type == NL80211_IFTYPE_ADHOC) {
2418			/* for queue management */
2419			mvm->probe_queue = queue;
2420			/* for use in TX */
2421			mvmvif->deflink.mgmt_queue = queue;
2422		} else if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
2423			mvm->p2p_dev_queue = queue;
2424		}
2425	} else if (vif->type == NL80211_IFTYPE_AP ||
2426		   vif->type == NL80211_IFTYPE_ADHOC) {
2427		/* set it for use in TX */
2428		mvmvif->deflink.mgmt_queue = mvm->probe_queue;
2429	}
2430
2431	return 0;
2432}
2433
2434void iwl_mvm_free_bcast_sta_queues(struct iwl_mvm *mvm,
2435				   struct ieee80211_vif *vif)
2436{
2437	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2438	u16 *queueptr, queue;
2439
2440	lockdep_assert_held(&mvm->mutex);
2441
2442	iwl_mvm_flush_sta(mvm, mvmvif->deflink.bcast_sta.sta_id,
2443			  mvmvif->deflink.bcast_sta.tfd_queue_msk);
2444
2445	switch (vif->type) {
2446	case NL80211_IFTYPE_AP:
2447	case NL80211_IFTYPE_ADHOC:
2448		queueptr = &mvm->probe_queue;
2449		break;
2450	case NL80211_IFTYPE_P2P_DEVICE:
2451		queueptr = &mvm->p2p_dev_queue;
2452		break;
2453	default:
2454		WARN(1, "Can't free bcast queue on vif type %d\n",
2455		     vif->type);
2456		return;
2457	}
2458
2459	queue = *queueptr;
2460	iwl_mvm_disable_txq(mvm, NULL, mvmvif->deflink.bcast_sta.sta_id,
2461			    queueptr, IWL_MAX_TID_COUNT);
2462
2463	if (vif->type == NL80211_IFTYPE_AP || vif->type == NL80211_IFTYPE_ADHOC)
2464		mvmvif->deflink.mgmt_queue = mvm->probe_queue;
2465
2466	if (iwl_mvm_has_new_tx_api(mvm))
2467		return;
2468
2469	WARN_ON(!(mvmvif->deflink.bcast_sta.tfd_queue_msk & BIT(queue)));
2470	mvmvif->deflink.bcast_sta.tfd_queue_msk &= ~BIT(queue);
2471}
2472
2473/* Send the FW a request to remove the station from it's internal data
2474 * structures, but DO NOT remove the entry from the local data structures. */
2475int iwl_mvm_send_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2476{
2477	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2478	int ret;
2479
2480	lockdep_assert_held(&mvm->mutex);
2481
2482	iwl_mvm_free_bcast_sta_queues(mvm, vif);
2483
2484	ret = iwl_mvm_rm_sta_common(mvm, mvmvif->deflink.bcast_sta.sta_id);
2485	if (ret)
2486		IWL_WARN(mvm, "Failed sending remove station\n");
2487	return ret;
2488}
2489
2490int iwl_mvm_alloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2491{
2492	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2493
2494	lockdep_assert_held(&mvm->mutex);
2495
2496	return iwl_mvm_allocate_int_sta(mvm, &mvmvif->deflink.bcast_sta, 0,
2497					ieee80211_vif_type_p2p(vif),
2498					IWL_STA_GENERAL_PURPOSE);
2499}
2500
2501/* Allocate a new station entry for the broadcast station to the given vif,
2502 * and send it to the FW.
2503 * Note that each P2P mac should have its own broadcast station.
2504 *
2505 * @mvm: the mvm component
2506 * @vif: the interface to which the broadcast station is added
2507 * @bsta: the broadcast station to add. */
2508int iwl_mvm_add_p2p_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2509{
2510	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2511	struct iwl_mvm_int_sta *bsta = &mvmvif->deflink.bcast_sta;
2512	int ret;
2513
2514	lockdep_assert_held(&mvm->mutex);
2515
2516	ret = iwl_mvm_alloc_bcast_sta(mvm, vif);
2517	if (ret)
2518		return ret;
2519
2520	ret = iwl_mvm_send_add_bcast_sta(mvm, vif);
2521
2522	if (ret)
2523		iwl_mvm_dealloc_int_sta(mvm, bsta);
2524
2525	return ret;
2526}
2527
2528void iwl_mvm_dealloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2529{
2530	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2531
2532	iwl_mvm_dealloc_int_sta(mvm, &mvmvif->deflink.bcast_sta);
2533}
2534
2535/*
2536 * Send the FW a request to remove the station from it's internal data
2537 * structures, and in addition remove it from the local data structure.
2538 */
2539int iwl_mvm_rm_p2p_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2540{
2541	int ret;
2542
2543	lockdep_assert_held(&mvm->mutex);
2544
2545	ret = iwl_mvm_send_rm_bcast_sta(mvm, vif);
2546
2547	iwl_mvm_dealloc_bcast_sta(mvm, vif);
2548
2549	return ret;
2550}
2551
2552/*
2553 * Allocate a new station entry for the multicast station to the given vif,
2554 * and send it to the FW.
2555 * Note that each AP/GO mac should have its own multicast station.
2556 *
2557 * @mvm: the mvm component
2558 * @vif: the interface to which the multicast station is added
2559 */
2560int iwl_mvm_add_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2561{
2562	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2563	struct iwl_mvm_int_sta *msta = &mvmvif->deflink.mcast_sta;
2564	static const u8 _maddr[] = {0x03, 0x00, 0x00, 0x00, 0x00, 0x00};
2565	const u8 *maddr = _maddr;
2566	struct iwl_trans_txq_scd_cfg cfg = {
2567		.fifo = vif->type == NL80211_IFTYPE_AP ?
2568			IWL_MVM_TX_FIFO_MCAST : IWL_MVM_TX_FIFO_BE,
2569		.sta_id = msta->sta_id,
2570		.tid = 0,
2571		.aggregate = false,
2572		.frame_limit = IWL_FRAME_LIMIT,
2573	};
2574	unsigned int timeout = iwl_mvm_get_wd_timeout(mvm, vif, false, false);
2575	int ret;
2576
2577	lockdep_assert_held(&mvm->mutex);
2578
2579	if (WARN_ON(vif->type != NL80211_IFTYPE_AP &&
2580		    vif->type != NL80211_IFTYPE_ADHOC))
2581		return -EOPNOTSUPP;
2582
2583	/*
2584	 * In IBSS, ieee80211_check_queues() sets the cab_queue to be
2585	 * invalid, so make sure we use the queue we want.
2586	 * Note that this is done here as we want to avoid making DQA
2587	 * changes in mac80211 layer.
2588	 */
2589	if (vif->type == NL80211_IFTYPE_ADHOC)
2590		mvmvif->deflink.cab_queue = IWL_MVM_DQA_GCAST_QUEUE;
2591
2592	/*
2593	 * While in previous FWs we had to exclude cab queue from TFD queue
2594	 * mask, now it is needed as any other queue.
2595	 */
2596	if (!iwl_mvm_has_new_tx_api(mvm) &&
2597	    fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE)) {
2598		iwl_mvm_enable_txq(mvm, NULL, mvmvif->deflink.cab_queue, 0,
2599				   &cfg,
2600				   timeout);
2601		msta->tfd_queue_msk |= BIT(mvmvif->deflink.cab_queue);
2602	}
2603	ret = iwl_mvm_add_int_sta_common(mvm, msta, maddr,
2604					 mvmvif->id, mvmvif->color);
2605	if (ret)
2606		goto err;
2607
2608	/*
2609	 * Enable cab queue after the ADD_STA command is sent.
2610	 * This is needed for 22000 firmware which won't accept SCD_QUEUE_CFG
2611	 * command with unknown station id, and for FW that doesn't support
2612	 * station API since the cab queue is not included in the
2613	 * tfd_queue_mask.
2614	 */
2615	if (iwl_mvm_has_new_tx_api(mvm)) {
2616		int queue = iwl_mvm_tvqm_enable_txq(mvm, NULL, msta->sta_id,
2617						    0, timeout);
2618		if (queue < 0) {
2619			ret = queue;
2620			goto err;
2621		}
2622		mvmvif->deflink.cab_queue = queue;
2623	} else if (!fw_has_api(&mvm->fw->ucode_capa,
2624			       IWL_UCODE_TLV_API_STA_TYPE))
2625		iwl_mvm_enable_txq(mvm, NULL, mvmvif->deflink.cab_queue, 0,
2626				   &cfg,
2627				   timeout);
2628
2629	return 0;
2630err:
2631	iwl_mvm_dealloc_int_sta(mvm, msta);
2632	return ret;
2633}
2634
2635static int __iwl_mvm_remove_sta_key(struct iwl_mvm *mvm, u8 sta_id,
2636				    struct ieee80211_key_conf *keyconf,
2637				    bool mcast)
2638{
2639	union {
2640		struct iwl_mvm_add_sta_key_cmd_v1 cmd_v1;
2641		struct iwl_mvm_add_sta_key_cmd cmd;
2642	} u = {};
2643	bool new_api = fw_has_api(&mvm->fw->ucode_capa,
2644				  IWL_UCODE_TLV_API_TKIP_MIC_KEYS);
2645	__le16 key_flags;
2646	int ret, size;
2647	u32 status;
2648
2649	/* This is a valid situation for GTK removal */
2650	if (sta_id == IWL_MVM_INVALID_STA)
2651		return 0;
2652
2653	key_flags = cpu_to_le16((keyconf->keyidx << STA_KEY_FLG_KEYID_POS) &
2654				 STA_KEY_FLG_KEYID_MSK);
2655	key_flags |= cpu_to_le16(STA_KEY_FLG_NO_ENC | STA_KEY_FLG_WEP_KEY_MAP);
2656	key_flags |= cpu_to_le16(STA_KEY_NOT_VALID);
2657
2658	if (mcast)
2659		key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
2660
2661	/*
2662	 * The fields assigned here are in the same location at the start
2663	 * of the command, so we can do this union trick.
2664	 */
2665	u.cmd.common.key_flags = key_flags;
2666	u.cmd.common.key_offset = keyconf->hw_key_idx;
2667	u.cmd.common.sta_id = sta_id;
2668
2669	size = new_api ? sizeof(u.cmd) : sizeof(u.cmd_v1);
2670
2671	status = ADD_STA_SUCCESS;
2672	ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA_KEY, size, &u.cmd,
2673					  &status);
2674
2675	switch (status) {
2676	case ADD_STA_SUCCESS:
2677		IWL_DEBUG_WEP(mvm, "MODIFY_STA: remove sta key passed\n");
2678		break;
2679	default:
2680		ret = -EIO;
2681		IWL_ERR(mvm, "MODIFY_STA: remove sta key failed\n");
2682		break;
2683	}
2684
2685	return ret;
2686}
2687
2688/*
2689 * Send the FW a request to remove the station from it's internal data
2690 * structures, and in addition remove it from the local data structure.
2691 */
2692int iwl_mvm_rm_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
2693{
2694	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
2695	int ret;
2696
2697	lockdep_assert_held(&mvm->mutex);
2698
2699	iwl_mvm_flush_sta(mvm, mvmvif->deflink.mcast_sta.sta_id,
2700			  mvmvif->deflink.mcast_sta.tfd_queue_msk);
2701
2702	iwl_mvm_disable_txq(mvm, NULL, mvmvif->deflink.mcast_sta.sta_id,
2703			    &mvmvif->deflink.cab_queue, 0);
2704
2705	ret = iwl_mvm_rm_sta_common(mvm, mvmvif->deflink.mcast_sta.sta_id);
2706	if (ret)
2707		IWL_WARN(mvm, "Failed sending remove station\n");
2708
2709	return ret;
2710}
2711
2712static void iwl_mvm_sync_rxq_del_ba(struct iwl_mvm *mvm, u8 baid)
2713{
2714	struct iwl_mvm_delba_data notif = {
2715		.baid = baid,
2716	};
2717
2718	iwl_mvm_sync_rx_queues_internal(mvm, IWL_MVM_RXQ_NOTIF_DEL_BA, true,
2719					&notif, sizeof(notif));
2720};
2721
2722static void iwl_mvm_free_reorder(struct iwl_mvm *mvm,
2723				 struct iwl_mvm_baid_data *data)
2724{
2725	int i;
2726
2727	iwl_mvm_sync_rxq_del_ba(mvm, data->baid);
2728
2729	for (i = 0; i < mvm->trans->num_rx_queues; i++) {
2730		int j;
2731		struct iwl_mvm_reorder_buffer *reorder_buf =
2732			&data->reorder_buf[i];
2733		struct iwl_mvm_reorder_buf_entry *entries =
2734			&data->entries[i * data->entries_per_queue];
2735
2736		spin_lock_bh(&reorder_buf->lock);
2737		if (likely(!reorder_buf->num_stored)) {
2738			spin_unlock_bh(&reorder_buf->lock);
2739			continue;
2740		}
2741
2742		/*
2743		 * This shouldn't happen in regular DELBA since the internal
2744		 * delBA notification should trigger a release of all frames in
2745		 * the reorder buffer.
2746		 */
2747		WARN_ON(1);
2748
2749		for (j = 0; j < reorder_buf->buf_size; j++)
2750			__skb_queue_purge(&entries[j].frames);
2751
2752		spin_unlock_bh(&reorder_buf->lock);
2753	}
2754}
2755
2756static void iwl_mvm_init_reorder_buffer(struct iwl_mvm *mvm,
2757					struct iwl_mvm_baid_data *data,
2758					u16 ssn, u16 buf_size)
2759{
2760	int i;
2761
2762	for (i = 0; i < mvm->trans->num_rx_queues; i++) {
2763		struct iwl_mvm_reorder_buffer *reorder_buf =
2764			&data->reorder_buf[i];
2765		struct iwl_mvm_reorder_buf_entry *entries =
2766			&data->entries[i * data->entries_per_queue];
2767		int j;
2768
2769		reorder_buf->num_stored = 0;
2770		reorder_buf->head_sn = ssn;
2771		reorder_buf->buf_size = buf_size;
2772		spin_lock_init(&reorder_buf->lock);
2773		reorder_buf->mvm = mvm;
2774		reorder_buf->queue = i;
2775		reorder_buf->valid = false;
2776		for (j = 0; j < reorder_buf->buf_size; j++)
2777			__skb_queue_head_init(&entries[j].frames);
2778	}
2779}
2780
2781static int iwl_mvm_fw_baid_op_sta(struct iwl_mvm *mvm,
2782				  struct ieee80211_sta *sta,
2783				  bool start, int tid, u16 ssn,
2784				  u16 buf_size)
2785{
2786	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
2787	struct iwl_mvm_add_sta_cmd cmd = {
2788		.mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color),
2789		.sta_id = mvm_sta->deflink.sta_id,
2790		.add_modify = STA_MODE_MODIFY,
2791	};
2792	u32 status;
2793	int ret;
2794
2795	if (start) {
2796		cmd.add_immediate_ba_tid = tid;
2797		cmd.add_immediate_ba_ssn = cpu_to_le16(ssn);
2798		cmd.rx_ba_window = cpu_to_le16(buf_size);
2799		cmd.modify_mask = STA_MODIFY_ADD_BA_TID;
2800	} else {
2801		cmd.remove_immediate_ba_tid = tid;
2802		cmd.modify_mask = STA_MODIFY_REMOVE_BA_TID;
2803	}
2804
2805	status = ADD_STA_SUCCESS;
2806	ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
2807					  iwl_mvm_add_sta_cmd_size(mvm),
2808					  &cmd, &status);
2809	if (ret)
2810		return ret;
2811
2812	switch (status & IWL_ADD_STA_STATUS_MASK) {
2813	case ADD_STA_SUCCESS:
2814		IWL_DEBUG_HT(mvm, "RX BA Session %sed in fw\n",
2815			     start ? "start" : "stopp");
2816		if (WARN_ON(start && iwl_mvm_has_new_rx_api(mvm) &&
2817			    !(status & IWL_ADD_STA_BAID_VALID_MASK)))
2818			return -EINVAL;
2819		return u32_get_bits(status, IWL_ADD_STA_BAID_MASK);
2820	case ADD_STA_IMMEDIATE_BA_FAILURE:
2821		IWL_WARN(mvm, "RX BA Session refused by fw\n");
2822		return -ENOSPC;
2823	default:
2824		IWL_ERR(mvm, "RX BA Session failed %sing, status 0x%x\n",
2825			start ? "start" : "stopp", status);
2826		return -EIO;
2827	}
2828}
2829
2830static int iwl_mvm_fw_baid_op_cmd(struct iwl_mvm *mvm,
2831				  struct ieee80211_sta *sta,
2832				  bool start, int tid, u16 ssn,
2833				  u16 buf_size, int baid)
2834{
2835	struct iwl_rx_baid_cfg_cmd cmd = {
2836		.action = start ? cpu_to_le32(IWL_RX_BAID_ACTION_ADD) :
2837				  cpu_to_le32(IWL_RX_BAID_ACTION_REMOVE),
2838	};
2839	u32 cmd_id = WIDE_ID(DATA_PATH_GROUP, RX_BAID_ALLOCATION_CONFIG_CMD);
2840	int ret;
2841
2842	BUILD_BUG_ON(sizeof(struct iwl_rx_baid_cfg_resp) != sizeof(baid));
2843
2844	if (start) {
2845		cmd.alloc.sta_id_mask =
2846			cpu_to_le32(iwl_mvm_sta_fw_id_mask(mvm, sta, -1));
2847		cmd.alloc.tid = tid;
2848		cmd.alloc.ssn = cpu_to_le16(ssn);
2849		cmd.alloc.win_size = cpu_to_le16(buf_size);
2850		baid = -EIO;
2851	} else if (iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 1) == 1) {
2852		cmd.remove_v1.baid = cpu_to_le32(baid);
2853		BUILD_BUG_ON(sizeof(cmd.remove_v1) > sizeof(cmd.remove));
2854	} else {
2855		cmd.remove.sta_id_mask =
2856			cpu_to_le32(iwl_mvm_sta_fw_id_mask(mvm, sta, -1));
2857		cmd.remove.tid = cpu_to_le32(tid);
2858	}
2859
2860	ret = iwl_mvm_send_cmd_pdu_status(mvm, cmd_id, sizeof(cmd),
2861					  &cmd, &baid);
2862	if (ret)
2863		return ret;
2864
2865	if (!start) {
2866		/* ignore firmware baid on remove */
2867		baid = 0;
2868	}
2869
2870	IWL_DEBUG_HT(mvm, "RX BA Session %sed in fw\n",
2871		     start ? "start" : "stopp");
2872
2873	if (baid < 0 || baid >= ARRAY_SIZE(mvm->baid_map))
2874		return -EINVAL;
2875
2876	return baid;
2877}
2878
2879static int iwl_mvm_fw_baid_op(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
2880			      bool start, int tid, u16 ssn, u16 buf_size,
2881			      int baid)
2882{
2883	if (fw_has_capa(&mvm->fw->ucode_capa,
2884			IWL_UCODE_TLV_CAPA_BAID_ML_SUPPORT))
2885		return iwl_mvm_fw_baid_op_cmd(mvm, sta, start,
2886					      tid, ssn, buf_size, baid);
2887
2888	return iwl_mvm_fw_baid_op_sta(mvm, sta, start,
2889				      tid, ssn, buf_size);
2890}
2891
2892int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
2893		       int tid, u16 ssn, bool start, u16 buf_size, u16 timeout)
2894{
2895	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
2896	struct iwl_mvm_baid_data *baid_data = NULL;
2897	int ret, baid;
2898	u32 max_ba_id_sessions = iwl_mvm_has_new_tx_api(mvm) ? IWL_MAX_BAID :
2899							       IWL_MAX_BAID_OLD;
2900
2901	lockdep_assert_held(&mvm->mutex);
2902
2903	if (start && mvm->rx_ba_sessions >= max_ba_id_sessions) {
2904		IWL_WARN(mvm, "Not enough RX BA SESSIONS\n");
2905		return -ENOSPC;
2906	}
2907
2908	if (iwl_mvm_has_new_rx_api(mvm) && start) {
2909		u32 reorder_buf_size = buf_size * sizeof(baid_data->entries[0]);
2910
2911		/* sparse doesn't like the __align() so don't check */
2912#ifndef __CHECKER__
2913		/*
2914		 * The division below will be OK if either the cache line size
2915		 * can be divided by the entry size (ALIGN will round up) or if
2916		 * if the entry size can be divided by the cache line size, in
2917		 * which case the ALIGN() will do nothing.
2918		 */
2919		BUILD_BUG_ON(SMP_CACHE_BYTES % sizeof(baid_data->entries[0]) &&
2920			     sizeof(baid_data->entries[0]) % SMP_CACHE_BYTES);
2921#endif
2922
2923		/*
2924		 * Upward align the reorder buffer size to fill an entire cache
2925		 * line for each queue, to avoid sharing cache lines between
2926		 * different queues.
2927		 */
2928		reorder_buf_size = ALIGN(reorder_buf_size, SMP_CACHE_BYTES);
2929
2930		/*
2931		 * Allocate here so if allocation fails we can bail out early
2932		 * before starting the BA session in the firmware
2933		 */
2934		baid_data = kzalloc(sizeof(*baid_data) +
2935				    mvm->trans->num_rx_queues *
2936				    reorder_buf_size,
2937				    GFP_KERNEL);
2938		if (!baid_data)
2939			return -ENOMEM;
2940
2941		/*
2942		 * This division is why we need the above BUILD_BUG_ON(),
2943		 * if that doesn't hold then this will not be right.
2944		 */
2945		baid_data->entries_per_queue =
2946			reorder_buf_size / sizeof(baid_data->entries[0]);
2947	}
2948
2949	if (iwl_mvm_has_new_rx_api(mvm) && !start) {
2950		baid = mvm_sta->tid_to_baid[tid];
2951	} else {
2952		/* we don't really need it in this case */
2953		baid = -1;
2954	}
2955
2956	/* Don't send command to remove (start=0) BAID during restart */
2957	if (start || !test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
2958		baid = iwl_mvm_fw_baid_op(mvm, sta, start, tid, ssn, buf_size,
2959					  baid);
2960
2961	if (baid < 0) {
2962		ret = baid;
2963		goto out_free;
2964	}
2965
2966	if (start) {
2967		mvm->rx_ba_sessions++;
2968
2969		if (!iwl_mvm_has_new_rx_api(mvm))
2970			return 0;
2971
2972		baid_data->baid = baid;
2973		baid_data->timeout = timeout;
2974		baid_data->last_rx = jiffies;
2975		baid_data->rcu_ptr = &mvm->baid_map[baid];
2976		timer_setup(&baid_data->session_timer,
2977			    iwl_mvm_rx_agg_session_expired, 0);
2978		baid_data->mvm = mvm;
2979		baid_data->tid = tid;
2980		baid_data->sta_mask = iwl_mvm_sta_fw_id_mask(mvm, sta, -1);
2981
2982		mvm_sta->tid_to_baid[tid] = baid;
2983		if (timeout)
2984			mod_timer(&baid_data->session_timer,
2985				  TU_TO_EXP_TIME(timeout * 2));
2986
2987		iwl_mvm_init_reorder_buffer(mvm, baid_data, ssn, buf_size);
2988		/*
2989		 * protect the BA data with RCU to cover a case where our
2990		 * internal RX sync mechanism will timeout (not that it's
2991		 * supposed to happen) and we will free the session data while
2992		 * RX is being processed in parallel
2993		 */
2994		IWL_DEBUG_HT(mvm, "Sta %d(%d) is assigned to BAID %d\n",
2995			     mvm_sta->deflink.sta_id, tid, baid);
2996		WARN_ON(rcu_access_pointer(mvm->baid_map[baid]));
2997		rcu_assign_pointer(mvm->baid_map[baid], baid_data);
2998	} else  {
2999		baid = mvm_sta->tid_to_baid[tid];
3000
3001		if (mvm->rx_ba_sessions > 0)
3002			/* check that restart flow didn't zero the counter */
3003			mvm->rx_ba_sessions--;
3004		if (!iwl_mvm_has_new_rx_api(mvm))
3005			return 0;
3006
3007		if (WARN_ON(baid == IWL_RX_REORDER_DATA_INVALID_BAID))
3008			return -EINVAL;
3009
3010		baid_data = rcu_access_pointer(mvm->baid_map[baid]);
3011		if (WARN_ON(!baid_data))
3012			return -EINVAL;
3013
3014		/* synchronize all rx queues so we can safely delete */
3015		iwl_mvm_free_reorder(mvm, baid_data);
3016		timer_shutdown_sync(&baid_data->session_timer);
3017		RCU_INIT_POINTER(mvm->baid_map[baid], NULL);
3018		kfree_rcu(baid_data, rcu_head);
3019		IWL_DEBUG_HT(mvm, "BAID %d is free\n", baid);
3020
3021		/*
3022		 * After we've deleted it, do another queue sync
3023		 * so if an IWL_MVM_RXQ_NSSN_SYNC was concurrently
3024		 * running it won't find a new session in the old
3025		 * BAID. It can find the NULL pointer for the BAID,
3026		 * but we must not have it find a different session.
3027		 */
3028		iwl_mvm_sync_rx_queues_internal(mvm, IWL_MVM_RXQ_EMPTY,
3029						true, NULL, 0);
3030	}
3031	return 0;
3032
3033out_free:
3034	kfree(baid_data);
3035	return ret;
3036}
3037
3038int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
3039		       int tid, u8 queue, bool start)
3040{
3041	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
3042	struct iwl_mvm_add_sta_cmd cmd = {};
3043	int ret;
3044	u32 status;
3045
3046	lockdep_assert_held(&mvm->mutex);
3047
3048	if (start) {
3049		mvm_sta->tfd_queue_msk |= BIT(queue);
3050		mvm_sta->tid_disable_agg &= ~BIT(tid);
3051	} else {
3052		/* In DQA-mode the queue isn't removed on agg termination */
3053		mvm_sta->tid_disable_agg |= BIT(tid);
3054	}
3055
3056	cmd.mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color);
3057	cmd.sta_id = mvm_sta->deflink.sta_id;
3058	cmd.add_modify = STA_MODE_MODIFY;
3059	if (!iwl_mvm_has_new_tx_api(mvm))
3060		cmd.modify_mask = STA_MODIFY_QUEUES;
3061	cmd.modify_mask |= STA_MODIFY_TID_DISABLE_TX;
3062	cmd.tfd_queue_msk = cpu_to_le32(mvm_sta->tfd_queue_msk);
3063	cmd.tid_disable_tx = cpu_to_le16(mvm_sta->tid_disable_agg);
3064
3065	status = ADD_STA_SUCCESS;
3066	ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA,
3067					  iwl_mvm_add_sta_cmd_size(mvm),
3068					  &cmd, &status);
3069	if (ret)
3070		return ret;
3071
3072	switch (status & IWL_ADD_STA_STATUS_MASK) {
3073	case ADD_STA_SUCCESS:
3074		break;
3075	default:
3076		ret = -EIO;
3077		IWL_ERR(mvm, "TX BA Session failed %sing, status 0x%x\n",
3078			start ? "start" : "stopp", status);
3079		break;
3080	}
3081
3082	return ret;
3083}
3084
3085const u8 tid_to_mac80211_ac[] = {
3086	IEEE80211_AC_BE,
3087	IEEE80211_AC_BK,
3088	IEEE80211_AC_BK,
3089	IEEE80211_AC_BE,
3090	IEEE80211_AC_VI,
3091	IEEE80211_AC_VI,
3092	IEEE80211_AC_VO,
3093	IEEE80211_AC_VO,
3094	IEEE80211_AC_VO, /* We treat MGMT as TID 8, which is set as AC_VO */
3095};
3096
3097static const u8 tid_to_ucode_ac[] = {
3098	AC_BE,
3099	AC_BK,
3100	AC_BK,
3101	AC_BE,
3102	AC_VI,
3103	AC_VI,
3104	AC_VO,
3105	AC_VO,
3106};
3107
3108int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
3109			     struct ieee80211_sta *sta, u16 tid, u16 *ssn)
3110{
3111	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
3112	struct iwl_mvm_tid_data *tid_data;
3113	u16 normalized_ssn;
3114	u16 txq_id;
3115	int ret;
3116
3117	if (WARN_ON_ONCE(tid >= IWL_MAX_TID_COUNT))
3118		return -EINVAL;
3119
3120	if (mvmsta->tid_data[tid].state != IWL_AGG_QUEUED &&
3121	    mvmsta->tid_data[tid].state != IWL_AGG_OFF) {
3122		IWL_ERR(mvm,
3123			"Start AGG when state is not IWL_AGG_QUEUED or IWL_AGG_OFF %d!\n",
3124			mvmsta->tid_data[tid].state);
3125		return -ENXIO;
3126	}
3127
3128	lockdep_assert_held(&mvm->mutex);
3129
3130	if (mvmsta->tid_data[tid].txq_id == IWL_MVM_INVALID_QUEUE &&
3131	    iwl_mvm_has_new_tx_api(mvm)) {
3132		u8 ac = tid_to_mac80211_ac[tid];
3133
3134		ret = iwl_mvm_sta_alloc_queue_tvqm(mvm, sta, ac, tid);
3135		if (ret)
3136			return ret;
3137	}
3138
3139	spin_lock_bh(&mvmsta->lock);
3140
3141	/*
3142	 * Note the possible cases:
3143	 *  1. An enabled TXQ - TXQ needs to become agg'ed
3144	 *  2. The TXQ hasn't yet been enabled, so find a free one and mark
3145	 *	it as reserved
3146	 */
3147	txq_id = mvmsta->tid_data[tid].txq_id;
3148	if (txq_id == IWL_MVM_INVALID_QUEUE) {
3149		ret = iwl_mvm_find_free_queue(mvm, mvmsta->deflink.sta_id,
3150					      IWL_MVM_DQA_MIN_DATA_QUEUE,
3151					      IWL_MVM_DQA_MAX_DATA_QUEUE);
3152		if (ret < 0) {
3153			IWL_ERR(mvm, "Failed to allocate agg queue\n");
3154			goto out;
3155		}
3156
3157		txq_id = ret;
3158
3159		/* TXQ hasn't yet been enabled, so mark it only as reserved */
3160		mvm->queue_info[txq_id].status = IWL_MVM_QUEUE_RESERVED;
3161	} else if (WARN_ON(txq_id >= IWL_MAX_HW_QUEUES)) {
3162		ret = -ENXIO;
3163		IWL_ERR(mvm, "tid_id %d out of range (0, %d)!\n",
3164			tid, IWL_MAX_HW_QUEUES - 1);
3165		goto out;
3166
3167	} else if (unlikely(mvm->queue_info[txq_id].status ==
3168			    IWL_MVM_QUEUE_SHARED)) {
3169		ret = -ENXIO;
3170		IWL_DEBUG_TX_QUEUES(mvm,
3171				    "Can't start tid %d agg on shared queue!\n",
3172				    tid);
3173		goto out;
3174	}
3175
3176	IWL_DEBUG_TX_QUEUES(mvm,
3177			    "AGG for tid %d will be on queue #%d\n",
3178			    tid, txq_id);
3179
3180	tid_data = &mvmsta->tid_data[tid];
3181	tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
3182	tid_data->txq_id = txq_id;
3183	*ssn = tid_data->ssn;
3184
3185	IWL_DEBUG_TX_QUEUES(mvm,
3186			    "Start AGG: sta %d tid %d queue %d - ssn = %d, next_recl = %d\n",
3187			    mvmsta->deflink.sta_id, tid, txq_id,
3188			    tid_data->ssn,
3189			    tid_data->next_reclaimed);
3190
3191	/*
3192	 * In 22000 HW, the next_reclaimed index is only 8 bit, so we'll need
3193	 * to align the wrap around of ssn so we compare relevant values.
3194	 */
3195	normalized_ssn = tid_data->ssn;
3196	if (mvm->trans->trans_cfg->gen2)
3197		normalized_ssn &= 0xff;
3198
3199	if (normalized_ssn == tid_data->next_reclaimed) {
3200		tid_data->state = IWL_AGG_STARTING;
3201		ret = IEEE80211_AMPDU_TX_START_IMMEDIATE;
3202	} else {
3203		tid_data->state = IWL_EMPTYING_HW_QUEUE_ADDBA;
3204		ret = IEEE80211_AMPDU_TX_START_DELAY_ADDBA;
3205	}
3206
3207out:
3208	spin_unlock_bh(&mvmsta->lock);
3209
3210	return ret;
3211}
3212
3213int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
3214			    struct ieee80211_sta *sta, u16 tid, u16 buf_size,
3215			    bool amsdu)
3216{
3217	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
3218	struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
3219	unsigned int wdg_timeout =
3220		iwl_mvm_get_wd_timeout(mvm, vif, sta->tdls, false);
3221	int queue, ret;
3222	bool alloc_queue = true;
3223	enum iwl_mvm_queue_status queue_status;
3224	u16 ssn;
3225
3226	struct iwl_trans_txq_scd_cfg cfg = {
3227		.sta_id = mvmsta->deflink.sta_id,
3228		.tid = tid,
3229		.frame_limit = buf_size,
3230		.aggregate = true,
3231	};
3232
3233	/*
3234	 * When FW supports TLC_OFFLOAD, it also implements Tx aggregation
3235	 * manager, so this function should never be called in this case.
3236	 */
3237	if (WARN_ON_ONCE(iwl_mvm_has_tlc_offload(mvm)))
3238		return -EINVAL;
3239
3240	BUILD_BUG_ON((sizeof(mvmsta->agg_tids) * BITS_PER_BYTE)
3241		     != IWL_MAX_TID_COUNT);
3242
3243	spin_lock_bh(&mvmsta->lock);
3244	ssn = tid_data->ssn;
3245	queue = tid_data->txq_id;
3246	tid_data->state = IWL_AGG_ON;
3247	mvmsta->agg_tids |= BIT(tid);
3248	tid_data->ssn = 0xffff;
3249	tid_data->amsdu_in_ampdu_allowed = amsdu;
3250	spin_unlock_bh(&mvmsta->lock);
3251
3252	if (iwl_mvm_has_new_tx_api(mvm)) {
3253		/*
3254		 * If there is no queue for this tid, iwl_mvm_sta_tx_agg_start()
3255		 * would have failed, so if we are here there is no need to
3256		 * allocate a queue.
3257		 * However, if aggregation size is different than the default
3258		 * size, the scheduler should be reconfigured.
3259		 * We cannot do this with the new TX API, so return unsupported
3260		 * for now, until it will be offloaded to firmware..
3261		 * Note that if SCD default value changes - this condition
3262		 * should be updated as well.
3263		 */
3264		if (buf_size < IWL_FRAME_LIMIT)
3265			return -EOPNOTSUPP;
3266
3267		ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
3268		if (ret)
3269			return -EIO;
3270		goto out;
3271	}
3272
3273	cfg.fifo = iwl_mvm_ac_to_tx_fifo[tid_to_mac80211_ac[tid]];
3274
3275	queue_status = mvm->queue_info[queue].status;
3276
3277	/* Maybe there is no need to even alloc a queue... */
3278	if (mvm->queue_info[queue].status == IWL_MVM_QUEUE_READY)
3279		alloc_queue = false;
3280
3281	/*
3282	 * Only reconfig the SCD for the queue if the window size has
3283	 * changed from current (become smaller)
3284	 */
3285	if (!alloc_queue && buf_size < IWL_FRAME_LIMIT) {
3286		/*
3287		 * If reconfiguring an existing queue, it first must be
3288		 * drained
3289		 */
3290		ret = iwl_trans_wait_tx_queues_empty(mvm->trans,
3291						     BIT(queue));
3292		if (ret) {
3293			IWL_ERR(mvm,
3294				"Error draining queue before reconfig\n");
3295			return ret;
3296		}
3297
3298		ret = iwl_mvm_reconfig_scd(mvm, queue, cfg.fifo,
3299					   mvmsta->deflink.sta_id, tid,
3300					   buf_size, ssn);
3301		if (ret) {
3302			IWL_ERR(mvm,
3303				"Error reconfiguring TXQ #%d\n", queue);
3304			return ret;
3305		}
3306	}
3307
3308	if (alloc_queue)
3309		iwl_mvm_enable_txq(mvm, sta, queue, ssn,
3310				   &cfg, wdg_timeout);
3311
3312	/* Send ADD_STA command to enable aggs only if the queue isn't shared */
3313	if (queue_status != IWL_MVM_QUEUE_SHARED) {
3314		ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
3315		if (ret)
3316			return -EIO;
3317	}
3318
3319	/* No need to mark as reserved */
3320	mvm->queue_info[queue].status = IWL_MVM_QUEUE_READY;
3321
3322out:
3323	/*
3324	 * Even though in theory the peer could have different
3325	 * aggregation reorder buffer sizes for different sessions,
3326	 * our ucode doesn't allow for that and has a global limit
3327	 * for each station. Therefore, use the minimum of all the
3328	 * aggregation sessions and our default value.
3329	 */
3330	mvmsta->deflink.lq_sta.rs_drv.pers.max_agg_bufsize =
3331		min(mvmsta->deflink.lq_sta.rs_drv.pers.max_agg_bufsize,
3332		    buf_size);
3333	mvmsta->deflink.lq_sta.rs_drv.lq.agg_frame_cnt_limit =
3334		mvmsta->deflink.lq_sta.rs_drv.pers.max_agg_bufsize;
3335
3336	IWL_DEBUG_HT(mvm, "Tx aggregation enabled on ra = %pM tid = %d\n",
3337		     sta->addr, tid);
3338
3339	return iwl_mvm_send_lq_cmd(mvm, &mvmsta->deflink.lq_sta.rs_drv.lq);
3340}
3341
3342static void iwl_mvm_unreserve_agg_queue(struct iwl_mvm *mvm,
3343					struct iwl_mvm_sta *mvmsta,
3344					struct iwl_mvm_tid_data *tid_data)
3345{
3346	u16 txq_id = tid_data->txq_id;
3347
3348	lockdep_assert_held(&mvm->mutex);
3349
3350	if (iwl_mvm_has_new_tx_api(mvm))
3351		return;
3352
3353	/*
3354	 * The TXQ is marked as reserved only if no traffic came through yet
3355	 * This means no traffic has been sent on this TID (agg'd or not), so
3356	 * we no longer have use for the queue. Since it hasn't even been
3357	 * allocated through iwl_mvm_enable_txq, so we can just mark it back as
3358	 * free.
3359	 */
3360	if (mvm->queue_info[txq_id].status == IWL_MVM_QUEUE_RESERVED) {
3361		mvm->queue_info[txq_id].status = IWL_MVM_QUEUE_FREE;
3362		tid_data->txq_id = IWL_MVM_INVALID_QUEUE;
3363	}
3364}
3365
3366int iwl_mvm_sta_tx_agg_stop(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
3367			    struct ieee80211_sta *sta, u16 tid)
3368{
3369	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
3370	struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
3371	u16 txq_id;
3372	int err;
3373
3374	/*
3375	 * If mac80211 is cleaning its state, then say that we finished since
3376	 * our state has been cleared anyway.
3377	 */
3378	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
3379		ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
3380		return 0;
3381	}
3382
3383	spin_lock_bh(&mvmsta->lock);
3384
3385	txq_id = tid_data->txq_id;
3386
3387	IWL_DEBUG_TX_QUEUES(mvm, "Stop AGG: sta %d tid %d q %d state %d\n",
3388			    mvmsta->deflink.sta_id, tid, txq_id,
3389			    tid_data->state);
3390
3391	mvmsta->agg_tids &= ~BIT(tid);
3392
3393	iwl_mvm_unreserve_agg_queue(mvm, mvmsta, tid_data);
3394
3395	switch (tid_data->state) {
3396	case IWL_AGG_ON:
3397		tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
3398
3399		IWL_DEBUG_TX_QUEUES(mvm,
3400				    "ssn = %d, next_recl = %d\n",
3401				    tid_data->ssn, tid_data->next_reclaimed);
3402
3403		tid_data->ssn = 0xffff;
3404		tid_data->state = IWL_AGG_OFF;
3405		spin_unlock_bh(&mvmsta->lock);
3406
3407		ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
3408
3409		iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
3410		return 0;
3411	case IWL_AGG_STARTING:
3412	case IWL_EMPTYING_HW_QUEUE_ADDBA:
3413		/*
3414		 * The agg session has been stopped before it was set up. This
3415		 * can happen when the AddBA timer times out for example.
3416		 */
3417
3418		/* No barriers since we are under mutex */
3419		lockdep_assert_held(&mvm->mutex);
3420
3421		ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
3422		tid_data->state = IWL_AGG_OFF;
3423		err = 0;
3424		break;
3425	default:
3426		IWL_ERR(mvm,
3427			"Stopping AGG while state not ON or starting for %d on %d (%d)\n",
3428			mvmsta->deflink.sta_id, tid, tid_data->state);
3429		IWL_ERR(mvm,
3430			"\ttid_data->txq_id = %d\n", tid_data->txq_id);
3431		err = -EINVAL;
3432	}
3433
3434	spin_unlock_bh(&mvmsta->lock);
3435
3436	return err;
3437}
3438
3439int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
3440			    struct ieee80211_sta *sta, u16 tid)
3441{
3442	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
3443	struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
3444	u16 txq_id;
3445	enum iwl_mvm_agg_state old_state;
3446
3447	/*
3448	 * First set the agg state to OFF to avoid calling
3449	 * ieee80211_stop_tx_ba_cb in iwl_mvm_check_ratid_empty.
3450	 */
3451	spin_lock_bh(&mvmsta->lock);
3452	txq_id = tid_data->txq_id;
3453	IWL_DEBUG_TX_QUEUES(mvm, "Flush AGG: sta %d tid %d q %d state %d\n",
3454			    mvmsta->deflink.sta_id, tid, txq_id,
3455			    tid_data->state);
3456	old_state = tid_data->state;
3457	tid_data->state = IWL_AGG_OFF;
3458	mvmsta->agg_tids &= ~BIT(tid);
3459	spin_unlock_bh(&mvmsta->lock);
3460
3461	iwl_mvm_unreserve_agg_queue(mvm, mvmsta, tid_data);
3462
3463	if (old_state >= IWL_AGG_ON) {
3464		iwl_mvm_drain_sta(mvm, mvmsta, true);
3465
3466		if (iwl_mvm_has_new_tx_api(mvm)) {
3467			if (iwl_mvm_flush_sta_tids(mvm, mvmsta->deflink.sta_id,
3468						   BIT(tid)))
3469				IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
3470			iwl_trans_wait_txq_empty(mvm->trans, txq_id);
3471		} else {
3472			if (iwl_mvm_flush_tx_path(mvm, BIT(txq_id)))
3473				IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
3474			iwl_trans_wait_tx_queues_empty(mvm->trans, BIT(txq_id));
3475		}
3476
3477		iwl_mvm_drain_sta(mvm, mvmsta, false);
3478
3479		iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
3480	}
3481
3482	return 0;
3483}
3484
3485static int iwl_mvm_set_fw_key_idx(struct iwl_mvm *mvm)
3486{
3487	int i, max = -1, max_offs = -1;
3488
3489	lockdep_assert_held(&mvm->mutex);
3490
3491	/* Pick the unused key offset with the highest 'deleted'
3492	 * counter. Every time a key is deleted, all the counters
3493	 * are incremented and the one that was just deleted is
3494	 * reset to zero. Thus, the highest counter is the one
3495	 * that was deleted longest ago. Pick that one.
3496	 */
3497	for (i = 0; i < STA_KEY_MAX_NUM; i++) {
3498		if (test_bit(i, mvm->fw_key_table))
3499			continue;
3500		if (mvm->fw_key_deleted[i] > max) {
3501			max = mvm->fw_key_deleted[i];
3502			max_offs = i;
3503		}
3504	}
3505
3506	if (max_offs < 0)
3507		return STA_KEY_IDX_INVALID;
3508
3509	return max_offs;
3510}
3511
3512static struct iwl_mvm_sta *iwl_mvm_get_key_sta(struct iwl_mvm *mvm,
3513					       struct ieee80211_vif *vif,
3514					       struct ieee80211_sta *sta)
3515{
3516	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
3517
3518	if (sta)
3519		return iwl_mvm_sta_from_mac80211(sta);
3520
3521	/*
3522	 * The device expects GTKs for station interfaces to be
3523	 * installed as GTKs for the AP station. If we have no
3524	 * station ID, then use AP's station ID.
3525	 */
3526	if (vif->type == NL80211_IFTYPE_STATION &&
3527	    mvmvif->deflink.ap_sta_id != IWL_MVM_INVALID_STA) {
3528		u8 sta_id = mvmvif->deflink.ap_sta_id;
3529
3530		sta = rcu_dereference_check(mvm->fw_id_to_mac_id[sta_id],
3531					    lockdep_is_held(&mvm->mutex));
3532
3533		/*
3534		 * It is possible that the 'sta' parameter is NULL,
3535		 * for example when a GTK is removed - the sta_id will then
3536		 * be the AP ID, and no station was passed by mac80211.
3537		 */
3538		if (IS_ERR_OR_NULL(sta))
3539			return NULL;
3540
3541		return iwl_mvm_sta_from_mac80211(sta);
3542	}
3543
3544	return NULL;
3545}
3546
3547static int iwl_mvm_pn_cmp(const u8 *pn1, const u8 *pn2, int len)
3548{
3549	int i;
3550
3551	for (i = len - 1; i >= 0; i--) {
3552		if (pn1[i] > pn2[i])
3553			return 1;
3554		if (pn1[i] < pn2[i])
3555			return -1;
3556	}
3557
3558	return 0;
3559}
3560
3561static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
3562				u32 sta_id,
3563				struct ieee80211_key_conf *key, bool mcast,
3564				u32 tkip_iv32, u16 *tkip_p1k, u32 cmd_flags,
3565				u8 key_offset, bool mfp)
3566{
3567	union {
3568		struct iwl_mvm_add_sta_key_cmd_v1 cmd_v1;
3569		struct iwl_mvm_add_sta_key_cmd cmd;
3570	} u = {};
3571	__le16 key_flags;
3572	int ret;
3573	u32 status;
3574	u16 keyidx;
3575	u64 pn = 0;
3576	int i, size;
3577	bool new_api = fw_has_api(&mvm->fw->ucode_capa,
3578				  IWL_UCODE_TLV_API_TKIP_MIC_KEYS);
3579	int api_ver = iwl_fw_lookup_cmd_ver(mvm->fw, ADD_STA_KEY,
3580					    new_api ? 2 : 1);
3581
3582	if (sta_id == IWL_MVM_INVALID_STA)
3583		return -EINVAL;
3584
3585	keyidx = (key->keyidx << STA_KEY_FLG_KEYID_POS) &
3586		 STA_KEY_FLG_KEYID_MSK;
3587	key_flags = cpu_to_le16(keyidx);
3588	key_flags |= cpu_to_le16(STA_KEY_FLG_WEP_KEY_MAP);
3589
3590	switch (key->cipher) {
3591	case WLAN_CIPHER_SUITE_TKIP:
3592		key_flags |= cpu_to_le16(STA_KEY_FLG_TKIP);
3593		if (api_ver >= 2) {
3594			memcpy((void *)&u.cmd.tx_mic_key,
3595			       &key->key[NL80211_TKIP_DATA_OFFSET_TX_MIC_KEY],
3596			       IWL_MIC_KEY_SIZE);
3597
3598			memcpy((void *)&u.cmd.rx_mic_key,
3599			       &key->key[NL80211_TKIP_DATA_OFFSET_RX_MIC_KEY],
3600			       IWL_MIC_KEY_SIZE);
3601			pn = atomic64_read(&key->tx_pn);
3602
3603		} else {
3604			u.cmd_v1.tkip_rx_tsc_byte2 = tkip_iv32;
3605			for (i = 0; i < 5; i++)
3606				u.cmd_v1.tkip_rx_ttak[i] =
3607					cpu_to_le16(tkip_p1k[i]);
3608		}
3609		memcpy(u.cmd.common.key, key->key, key->keylen);
3610		break;
3611	case WLAN_CIPHER_SUITE_CCMP:
3612		key_flags |= cpu_to_le16(STA_KEY_FLG_CCM);
3613		memcpy(u.cmd.common.key, key->key, key->keylen);
3614		if (api_ver >= 2)
3615			pn = atomic64_read(&key->tx_pn);
3616		break;
3617	case WLAN_CIPHER_SUITE_WEP104:
3618		key_flags |= cpu_to_le16(STA_KEY_FLG_WEP_13BYTES);
3619		fallthrough;
3620	case WLAN_CIPHER_SUITE_WEP40:
3621		key_flags |= cpu_to_le16(STA_KEY_FLG_WEP);
3622		memcpy(u.cmd.common.key + 3, key->key, key->keylen);
3623		break;
3624	case WLAN_CIPHER_SUITE_GCMP_256:
3625		key_flags |= cpu_to_le16(STA_KEY_FLG_KEY_32BYTES);
3626		fallthrough;
3627	case WLAN_CIPHER_SUITE_GCMP:
3628		key_flags |= cpu_to_le16(STA_KEY_FLG_GCMP);
3629		memcpy(u.cmd.common.key, key->key, key->keylen);
3630		if (api_ver >= 2)
3631			pn = atomic64_read(&key->tx_pn);
3632		break;
3633	default:
3634		key_flags |= cpu_to_le16(STA_KEY_FLG_EXT);
3635		memcpy(u.cmd.common.key, key->key, key->keylen);
3636	}
3637
3638	if (mcast)
3639		key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
3640	if (mfp)
3641		key_flags |= cpu_to_le16(STA_KEY_MFP);
3642
3643	u.cmd.common.key_offset = key_offset;
3644	u.cmd.common.key_flags = key_flags;
3645	u.cmd.common.sta_id = sta_id;
3646
3647	if (key->cipher == WLAN_CIPHER_SUITE_TKIP)
3648		i = 0;
3649	else
3650		i = -1;
3651
3652	for (; i < IEEE80211_NUM_TIDS; i++) {
3653		struct ieee80211_key_seq seq = {};
3654		u8 _rx_pn[IEEE80211_MAX_PN_LEN] = {}, *rx_pn = _rx_pn;
3655		int rx_pn_len = 8;
3656		/* there's a hole at 2/3 in FW format depending on version */
3657		int hole = api_ver >= 3 ? 0 : 2;
3658
3659		ieee80211_get_key_rx_seq(key, i, &seq);
3660
3661		if (key->cipher == WLAN_CIPHER_SUITE_TKIP) {
3662			rx_pn[0] = seq.tkip.iv16;
3663			rx_pn[1] = seq.tkip.iv16 >> 8;
3664			rx_pn[2 + hole] = seq.tkip.iv32;
3665			rx_pn[3 + hole] = seq.tkip.iv32 >> 8;
3666			rx_pn[4 + hole] = seq.tkip.iv32 >> 16;
3667			rx_pn[5 + hole] = seq.tkip.iv32 >> 24;
3668		} else if (key_flags & cpu_to_le16(STA_KEY_FLG_EXT)) {
3669			rx_pn = seq.hw.seq;
3670			rx_pn_len = seq.hw.seq_len;
3671		} else {
3672			rx_pn[0] = seq.ccmp.pn[0];
3673			rx_pn[1] = seq.ccmp.pn[1];
3674			rx_pn[2 + hole] = seq.ccmp.pn[2];
3675			rx_pn[3 + hole] = seq.ccmp.pn[3];
3676			rx_pn[4 + hole] = seq.ccmp.pn[4];
3677			rx_pn[5 + hole] = seq.ccmp.pn[5];
3678		}
3679
3680		if (iwl_mvm_pn_cmp(rx_pn, (u8 *)&u.cmd.common.rx_secur_seq_cnt,
3681				   rx_pn_len) > 0)
3682			memcpy(&u.cmd.common.rx_secur_seq_cnt, rx_pn,
3683			       rx_pn_len);
3684	}
3685
3686	if (api_ver >= 2) {
3687		u.cmd.transmit_seq_cnt = cpu_to_le64(pn);
3688		size = sizeof(u.cmd);
3689	} else {
3690		size = sizeof(u.cmd_v1);
3691	}
3692
3693	status = ADD_STA_SUCCESS;
3694	if (cmd_flags & CMD_ASYNC)
3695		ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA_KEY, CMD_ASYNC, size,
3696					   &u.cmd);
3697	else
3698		ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA_KEY, size,
3699						  &u.cmd, &status);
3700
3701	switch (status) {
3702	case ADD_STA_SUCCESS:
3703		IWL_DEBUG_WEP(mvm, "MODIFY_STA: set dynamic key passed\n");
3704		break;
3705	default:
3706		ret = -EIO;
3707		IWL_ERR(mvm, "MODIFY_STA: set dynamic key failed\n");
3708		break;
3709	}
3710
3711	return ret;
3712}
3713
3714static int iwl_mvm_send_sta_igtk(struct iwl_mvm *mvm,
3715				 struct ieee80211_key_conf *keyconf,
3716				 u8 sta_id, bool remove_key)
3717{
3718	struct iwl_mvm_mgmt_mcast_key_cmd igtk_cmd = {};
3719
3720	/* verify the key details match the required command's expectations */
3721	if (WARN_ON((keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE) ||
3722		    (keyconf->keyidx != 4 && keyconf->keyidx != 5 &&
3723		     keyconf->keyidx != 6 && keyconf->keyidx != 7) ||
3724		    (keyconf->cipher != WLAN_CIPHER_SUITE_AES_CMAC &&
3725		     keyconf->cipher != WLAN_CIPHER_SUITE_BIP_GMAC_128 &&
3726		     keyconf->cipher != WLAN_CIPHER_SUITE_BIP_GMAC_256)))
3727		return -EINVAL;
3728
3729	if (WARN_ON(!iwl_mvm_has_new_rx_api(mvm) &&
3730		    keyconf->cipher != WLAN_CIPHER_SUITE_AES_CMAC))
3731		return -EINVAL;
3732
3733	igtk_cmd.key_id = cpu_to_le32(keyconf->keyidx);
3734	igtk_cmd.sta_id = cpu_to_le32(sta_id);
3735
3736	if (remove_key) {
3737		/* This is a valid situation for IGTK */
3738		if (sta_id == IWL_MVM_INVALID_STA)
3739			return 0;
3740
3741		igtk_cmd.ctrl_flags |= cpu_to_le32(STA_KEY_NOT_VALID);
3742	} else {
3743		struct ieee80211_key_seq seq;
3744		const u8 *pn;
3745
3746		switch (keyconf->cipher) {
3747		case WLAN_CIPHER_SUITE_AES_CMAC:
3748			igtk_cmd.ctrl_flags |= cpu_to_le32(STA_KEY_FLG_CCM);
3749			break;
3750		case WLAN_CIPHER_SUITE_BIP_GMAC_128:
3751		case WLAN_CIPHER_SUITE_BIP_GMAC_256:
3752			igtk_cmd.ctrl_flags |= cpu_to_le32(STA_KEY_FLG_GCMP);
3753			break;
3754		default:
3755			return -EINVAL;
3756		}
3757
3758		memcpy(igtk_cmd.igtk, keyconf->key, keyconf->keylen);
3759		if (keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_256)
3760			igtk_cmd.ctrl_flags |=
3761				cpu_to_le32(STA_KEY_FLG_KEY_32BYTES);
3762		ieee80211_get_key_rx_seq(keyconf, 0, &seq);
3763		pn = seq.aes_cmac.pn;
3764		igtk_cmd.receive_seq_cnt = cpu_to_le64(((u64) pn[5] << 0) |
3765						       ((u64) pn[4] << 8) |
3766						       ((u64) pn[3] << 16) |
3767						       ((u64) pn[2] << 24) |
3768						       ((u64) pn[1] << 32) |
3769						       ((u64) pn[0] << 40));
3770	}
3771
3772	IWL_DEBUG_INFO(mvm, "%s %sIGTK (%d) for sta %u\n",
3773		       remove_key ? "removing" : "installing",
3774		       keyconf->keyidx >= 6 ? "B" : "",
3775		       keyconf->keyidx, igtk_cmd.sta_id);
3776
3777	if (!iwl_mvm_has_new_rx_api(mvm)) {
3778		struct iwl_mvm_mgmt_mcast_key_cmd_v1 igtk_cmd_v1 = {
3779			.ctrl_flags = igtk_cmd.ctrl_flags,
3780			.key_id = igtk_cmd.key_id,
3781			.sta_id = igtk_cmd.sta_id,
3782			.receive_seq_cnt = igtk_cmd.receive_seq_cnt
3783		};
3784
3785		memcpy(igtk_cmd_v1.igtk, igtk_cmd.igtk,
3786		       ARRAY_SIZE(igtk_cmd_v1.igtk));
3787		return iwl_mvm_send_cmd_pdu(mvm, MGMT_MCAST_KEY, 0,
3788					    sizeof(igtk_cmd_v1), &igtk_cmd_v1);
3789	}
3790	return iwl_mvm_send_cmd_pdu(mvm, MGMT_MCAST_KEY, 0,
3791				    sizeof(igtk_cmd), &igtk_cmd);
3792}
3793
3794
3795static inline u8 *iwl_mvm_get_mac_addr(struct iwl_mvm *mvm,
3796				       struct ieee80211_vif *vif,
3797				       struct ieee80211_sta *sta)
3798{
3799	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
3800
3801	if (sta)
3802		return sta->addr;
3803
3804	if (vif->type == NL80211_IFTYPE_STATION &&
3805	    mvmvif->deflink.ap_sta_id != IWL_MVM_INVALID_STA) {
3806		u8 sta_id = mvmvif->deflink.ap_sta_id;
3807		sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
3808						lockdep_is_held(&mvm->mutex));
3809		if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta)))
3810			return NULL;
3811
3812		return sta->addr;
3813	}
3814
3815
3816	return NULL;
3817}
3818
3819static int __iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
3820				 struct ieee80211_vif *vif,
3821				 struct ieee80211_sta *sta,
3822				 struct ieee80211_key_conf *keyconf,
3823				 u8 key_offset,
3824				 bool mcast)
3825{
3826	const u8 *addr;
3827	struct ieee80211_key_seq seq;
3828	u16 p1k[5];
3829	u32 sta_id;
3830	bool mfp = false;
3831
3832	if (sta) {
3833		struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
3834
3835		sta_id = mvm_sta->deflink.sta_id;
3836		mfp = sta->mfp;
3837	} else if (vif->type == NL80211_IFTYPE_AP &&
3838		   !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE)) {
3839		struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
3840
3841		sta_id = mvmvif->deflink.mcast_sta.sta_id;
3842	} else {
3843		IWL_ERR(mvm, "Failed to find station id\n");
3844		return -EINVAL;
3845	}
3846
3847	if (keyconf->cipher == WLAN_CIPHER_SUITE_TKIP) {
3848		addr = iwl_mvm_get_mac_addr(mvm, vif, sta);
3849		if (!addr) {
3850			IWL_ERR(mvm, "Failed to find mac address\n");
3851			return -EINVAL;
3852		}
3853
3854		/* get phase 1 key from mac80211 */
3855		ieee80211_get_key_rx_seq(keyconf, 0, &seq);
3856		ieee80211_get_tkip_rx_p1k(keyconf, addr, seq.tkip.iv32, p1k);
3857
3858		return iwl_mvm_send_sta_key(mvm, sta_id, keyconf, mcast,
3859					    seq.tkip.iv32, p1k, 0, key_offset,
3860					    mfp);
3861	}
3862
3863	return iwl_mvm_send_sta_key(mvm, sta_id, keyconf, mcast,
3864				    0, NULL, 0, key_offset, mfp);
3865}
3866
3867int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
3868			struct ieee80211_vif *vif,
3869			struct ieee80211_sta *sta,
3870			struct ieee80211_key_conf *keyconf,
3871			u8 key_offset)
3872{
3873	bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
3874	struct iwl_mvm_sta *mvm_sta;
3875	u8 sta_id = IWL_MVM_INVALID_STA;
3876	int ret;
3877	static const u8 __maybe_unused zero_addr[ETH_ALEN] = {0};
3878
3879	lockdep_assert_held(&mvm->mutex);
3880
3881	if (vif->type != NL80211_IFTYPE_AP ||
3882	    keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
3883		/* Get the station id from the mvm local station table */
3884		mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
3885		if (!mvm_sta) {
3886			IWL_ERR(mvm, "Failed to find station\n");
3887			return -EINVAL;
3888		}
3889		sta_id = mvm_sta->deflink.sta_id;
3890
3891		/*
3892		 * It is possible that the 'sta' parameter is NULL, and thus
3893		 * there is a need to retrieve the sta from the local station
3894		 * table.
3895		 */
3896		if (!sta) {
3897			sta = rcu_dereference_protected(
3898				mvm->fw_id_to_mac_id[sta_id],
3899				lockdep_is_held(&mvm->mutex));
3900			if (IS_ERR_OR_NULL(sta)) {
3901				IWL_ERR(mvm, "Invalid station id\n");
3902				return -EINVAL;
3903			}
3904		}
3905
3906		if (WARN_ON_ONCE(iwl_mvm_sta_from_mac80211(sta)->vif != vif))
3907			return -EINVAL;
3908	} else {
3909		struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
3910
3911		sta_id = mvmvif->deflink.mcast_sta.sta_id;
3912	}
3913
3914	if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC ||
3915	    keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_128 ||
3916	    keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_256) {
3917		ret = iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, false);
3918		goto end;
3919	}
3920
3921	/* If the key_offset is not pre-assigned, we need to find a
3922	 * new offset to use.  In normal cases, the offset is not
3923	 * pre-assigned, but during HW_RESTART we want to reuse the
3924	 * same indices, so we pass them when this function is called.
3925	 *
3926	 * In D3 entry, we need to hardcoded the indices (because the
3927	 * firmware hardcodes the PTK offset to 0).  In this case, we
3928	 * need to make sure we don't overwrite the hw_key_idx in the
3929	 * keyconf structure, because otherwise we cannot configure
3930	 * the original ones back when resuming.
3931	 */
3932	if (key_offset == STA_KEY_IDX_INVALID) {
3933		key_offset  = iwl_mvm_set_fw_key_idx(mvm);
3934		if (key_offset == STA_KEY_IDX_INVALID)
3935			return -ENOSPC;
3936		keyconf->hw_key_idx = key_offset;
3937	}
3938
3939	ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf, key_offset, mcast);
3940	if (ret)
3941		goto end;
3942
3943	/*
3944	 * For WEP, the same key is used for multicast and unicast. Upload it
3945	 * again, using the same key offset, and now pointing the other one
3946	 * to the same key slot (offset).
3947	 * If this fails, remove the original as well.
3948	 */
3949	if ((keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
3950	     keyconf->cipher == WLAN_CIPHER_SUITE_WEP104) &&
3951	    sta) {
3952		ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf,
3953					    key_offset, !mcast);
3954		if (ret) {
3955			__iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
3956			goto end;
3957		}
3958	}
3959
3960	__set_bit(key_offset, mvm->fw_key_table);
3961
3962end:
3963	IWL_DEBUG_WEP(mvm, "key: cipher=%x len=%d idx=%d sta=%pM ret=%d\n",
3964		      keyconf->cipher, keyconf->keylen, keyconf->keyidx,
3965		      sta ? sta->addr : zero_addr, ret);
3966	return ret;
3967}
3968
3969int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
3970			   struct ieee80211_vif *vif,
3971			   struct ieee80211_sta *sta,
3972			   struct ieee80211_key_conf *keyconf)
3973{
3974	bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
3975	struct iwl_mvm_sta *mvm_sta;
3976	u8 sta_id = IWL_MVM_INVALID_STA;
3977	int ret, i;
3978
3979	lockdep_assert_held(&mvm->mutex);
3980
3981	/* Get the station from the mvm local station table */
3982	mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
3983	if (mvm_sta)
3984		sta_id = mvm_sta->deflink.sta_id;
3985	else if (!sta && vif->type == NL80211_IFTYPE_AP && mcast)
3986		sta_id = iwl_mvm_vif_from_mac80211(vif)->deflink.mcast_sta.sta_id;
3987
3988
3989	IWL_DEBUG_WEP(mvm, "mvm remove dynamic key: idx=%d sta=%d\n",
3990		      keyconf->keyidx, sta_id);
3991
3992	if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC ||
3993	    keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_128 ||
3994	    keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_256)
3995		return iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, true);
3996
3997	if (!__test_and_clear_bit(keyconf->hw_key_idx, mvm->fw_key_table)) {
3998		IWL_ERR(mvm, "offset %d not used in fw key table.\n",
3999			keyconf->hw_key_idx);
4000		return -ENOENT;
4001	}
4002
4003	/* track which key was deleted last */
4004	for (i = 0; i < STA_KEY_MAX_NUM; i++) {
4005		if (mvm->fw_key_deleted[i] < U8_MAX)
4006			mvm->fw_key_deleted[i]++;
4007	}
4008	mvm->fw_key_deleted[keyconf->hw_key_idx] = 0;
4009
4010	if (sta && !mvm_sta) {
4011		IWL_DEBUG_WEP(mvm, "station non-existent, early return.\n");
4012		return 0;
4013	}
4014
4015	ret = __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
4016	if (ret)
4017		return ret;
4018
4019	/* delete WEP key twice to get rid of (now useless) offset */
4020	if (keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
4021	    keyconf->cipher == WLAN_CIPHER_SUITE_WEP104)
4022		ret = __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, !mcast);
4023
4024	return ret;
4025}
4026
4027void iwl_mvm_update_tkip_key(struct iwl_mvm *mvm,
4028			     struct ieee80211_vif *vif,
4029			     struct ieee80211_key_conf *keyconf,
4030			     struct ieee80211_sta *sta, u32 iv32,
4031			     u16 *phase1key)
4032{
4033	struct iwl_mvm_sta *mvm_sta;
4034	bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
4035	bool mfp = sta ? sta->mfp : false;
4036
4037	rcu_read_lock();
4038
4039	mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
4040	if (WARN_ON_ONCE(!mvm_sta))
4041		goto unlock;
4042	iwl_mvm_send_sta_key(mvm, mvm_sta->deflink.sta_id, keyconf, mcast,
4043			     iv32, phase1key, CMD_ASYNC, keyconf->hw_key_idx,
4044			     mfp);
4045
4046 unlock:
4047	rcu_read_unlock();
4048}
4049
4050void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
4051				struct ieee80211_sta *sta)
4052{
4053	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
4054	struct iwl_mvm_add_sta_cmd cmd = {
4055		.add_modify = STA_MODE_MODIFY,
4056		.sta_id = mvmsta->deflink.sta_id,
4057		.station_flags_msk = cpu_to_le32(STA_FLG_PS),
4058		.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
4059	};
4060	int ret;
4061
4062	ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
4063				   iwl_mvm_add_sta_cmd_size(mvm), &cmd);
4064	if (ret)
4065		IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
4066}
4067
4068void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
4069				       struct ieee80211_sta *sta,
4070				       enum ieee80211_frame_release_type reason,
4071				       u16 cnt, u16 tids, bool more_data,
4072				       bool single_sta_queue)
4073{
4074	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
4075	struct iwl_mvm_add_sta_cmd cmd = {
4076		.add_modify = STA_MODE_MODIFY,
4077		.sta_id = mvmsta->deflink.sta_id,
4078		.modify_mask = STA_MODIFY_SLEEPING_STA_TX_COUNT,
4079		.sleep_tx_count = cpu_to_le16(cnt),
4080		.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
4081	};
4082	int tid, ret;
4083	unsigned long _tids = tids;
4084
4085	/* convert TIDs to ACs - we don't support TSPEC so that's OK
4086	 * Note that this field is reserved and unused by firmware not
4087	 * supporting GO uAPSD, so it's safe to always do this.
4088	 */
4089	for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT)
4090		cmd.awake_acs |= BIT(tid_to_ucode_ac[tid]);
4091
4092	/* If we're releasing frames from aggregation or dqa queues then check
4093	 * if all the queues that we're releasing frames from, combined, have:
4094	 *  - more frames than the service period, in which case more_data
4095	 *    needs to be set
4096	 *  - fewer than 'cnt' frames, in which case we need to adjust the
4097	 *    firmware command (but do that unconditionally)
4098	 */
4099	if (single_sta_queue) {
4100		int remaining = cnt;
4101		int sleep_tx_count;
4102
4103		spin_lock_bh(&mvmsta->lock);
4104		for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT) {
4105			struct iwl_mvm_tid_data *tid_data;
4106			u16 n_queued;
4107
4108			tid_data = &mvmsta->tid_data[tid];
4109
4110			n_queued = iwl_mvm_tid_queued(mvm, tid_data);
4111			if (n_queued > remaining) {
4112				more_data = true;
4113				remaining = 0;
4114				break;
4115			}
4116			remaining -= n_queued;
4117		}
4118		sleep_tx_count = cnt - remaining;
4119		if (reason == IEEE80211_FRAME_RELEASE_UAPSD)
4120			mvmsta->sleep_tx_count = sleep_tx_count;
4121		spin_unlock_bh(&mvmsta->lock);
4122
4123		cmd.sleep_tx_count = cpu_to_le16(sleep_tx_count);
4124		if (WARN_ON(cnt - remaining == 0)) {
4125			ieee80211_sta_eosp(sta);
4126			return;
4127		}
4128	}
4129
4130	/* Note: this is ignored by firmware not supporting GO uAPSD */
4131	if (more_data)
4132		cmd.sleep_state_flags |= STA_SLEEP_STATE_MOREDATA;
4133
4134	if (reason == IEEE80211_FRAME_RELEASE_PSPOLL) {
4135		mvmsta->next_status_eosp = true;
4136		cmd.sleep_state_flags |= STA_SLEEP_STATE_PS_POLL;
4137	} else {
4138		cmd.sleep_state_flags |= STA_SLEEP_STATE_UAPSD;
4139	}
4140
4141	/* block the Tx queues until the FW updated the sleep Tx count */
4142	ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA,
4143				   CMD_ASYNC | CMD_BLOCK_TXQS,
4144				   iwl_mvm_add_sta_cmd_size(mvm), &cmd);
4145	if (ret)
4146		IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
4147}
4148
4149void iwl_mvm_rx_eosp_notif(struct iwl_mvm *mvm,
4150			   struct iwl_rx_cmd_buffer *rxb)
4151{
4152	struct iwl_rx_packet *pkt = rxb_addr(rxb);
4153	struct iwl_mvm_eosp_notification *notif = (void *)pkt->data;
4154	struct ieee80211_sta *sta;
4155	u32 sta_id = le32_to_cpu(notif->sta_id);
4156
4157	if (WARN_ON_ONCE(sta_id >= mvm->fw->ucode_capa.num_stations))
4158		return;
4159
4160	rcu_read_lock();
4161	sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
4162	if (!IS_ERR_OR_NULL(sta))
4163		ieee80211_sta_eosp(sta);
4164	rcu_read_unlock();
4165}
4166
4167void iwl_mvm_sta_modify_disable_tx(struct iwl_mvm *mvm,
4168				   struct iwl_mvm_sta *mvmsta,
4169				   bool disable)
4170{
4171	struct iwl_mvm_add_sta_cmd cmd = {
4172		.add_modify = STA_MODE_MODIFY,
4173		.sta_id = mvmsta->deflink.sta_id,
4174		.station_flags = disable ? cpu_to_le32(STA_FLG_DISABLE_TX) : 0,
4175		.station_flags_msk = cpu_to_le32(STA_FLG_DISABLE_TX),
4176		.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
4177	};
4178	int ret;
4179
4180	if (mvm->mld_api_is_used) {
4181		if (!iwl_mvm_has_no_host_disable_tx(mvm))
4182			iwl_mvm_mld_sta_modify_disable_tx(mvm, mvmsta, disable);
4183		return;
4184	}
4185
4186	ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
4187				   iwl_mvm_add_sta_cmd_size(mvm), &cmd);
4188	if (ret)
4189		IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
4190}
4191
4192void iwl_mvm_sta_modify_disable_tx_ap(struct iwl_mvm *mvm,
4193				      struct ieee80211_sta *sta,
4194				      bool disable)
4195{
4196	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
4197
4198	if (mvm->mld_api_is_used) {
4199		if (!iwl_mvm_has_no_host_disable_tx(mvm))
4200			iwl_mvm_mld_sta_modify_disable_tx_ap(mvm, sta, disable);
4201		return;
4202	}
4203
4204	spin_lock_bh(&mvm_sta->lock);
4205
4206	if (mvm_sta->disable_tx == disable) {
4207		spin_unlock_bh(&mvm_sta->lock);
4208		return;
4209	}
4210
4211	mvm_sta->disable_tx = disable;
4212
4213	/*
4214	 * If sta PS state is handled by mac80211, tell it to start/stop
4215	 * queuing tx for this station.
4216	 */
4217	if (!ieee80211_hw_check(mvm->hw, AP_LINK_PS))
4218		ieee80211_sta_block_awake(mvm->hw, sta, disable);
4219
4220	iwl_mvm_sta_modify_disable_tx(mvm, mvm_sta, disable);
4221
4222	spin_unlock_bh(&mvm_sta->lock);
4223}
4224
4225static void iwl_mvm_int_sta_modify_disable_tx(struct iwl_mvm *mvm,
4226					      struct iwl_mvm_vif *mvmvif,
4227					      struct iwl_mvm_int_sta *sta,
4228					      bool disable)
4229{
4230	u32 id = FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color);
4231	struct iwl_mvm_add_sta_cmd cmd = {
4232		.add_modify = STA_MODE_MODIFY,
4233		.sta_id = sta->sta_id,
4234		.station_flags = disable ? cpu_to_le32(STA_FLG_DISABLE_TX) : 0,
4235		.station_flags_msk = cpu_to_le32(STA_FLG_DISABLE_TX),
4236		.mac_id_n_color = cpu_to_le32(id),
4237	};
4238	int ret;
4239
4240	ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
4241				   iwl_mvm_add_sta_cmd_size(mvm), &cmd);
4242	if (ret)
4243		IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
4244}
4245
4246void iwl_mvm_modify_all_sta_disable_tx(struct iwl_mvm *mvm,
4247				       struct iwl_mvm_vif *mvmvif,
4248				       bool disable)
4249{
4250	struct ieee80211_sta *sta;
4251	struct iwl_mvm_sta *mvm_sta;
4252	int i;
4253
4254	if (mvm->mld_api_is_used) {
4255		if (!iwl_mvm_has_no_host_disable_tx(mvm))
4256			iwl_mvm_mld_modify_all_sta_disable_tx(mvm, mvmvif,
4257							      disable);
4258		return;
4259	}
4260
4261	rcu_read_lock();
4262
4263	/* Block/unblock all the stations of the given mvmvif */
4264	for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++) {
4265		sta = rcu_dereference(mvm->fw_id_to_mac_id[i]);
4266		if (IS_ERR_OR_NULL(sta))
4267			continue;
4268
4269		mvm_sta = iwl_mvm_sta_from_mac80211(sta);
4270		if (mvm_sta->mac_id_n_color !=
4271		    FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color))
4272			continue;
4273
4274		iwl_mvm_sta_modify_disable_tx_ap(mvm, sta, disable);
4275	}
4276
4277	rcu_read_unlock();
4278
4279	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STA_TYPE))
4280		return;
4281
4282	/* Need to block/unblock also multicast station */
4283	if (mvmvif->deflink.mcast_sta.sta_id != IWL_MVM_INVALID_STA)
4284		iwl_mvm_int_sta_modify_disable_tx(mvm, mvmvif,
4285						  &mvmvif->deflink.mcast_sta,
4286						  disable);
4287
4288	/*
4289	 * Only unblock the broadcast station (FW blocks it for immediate
4290	 * quiet, not the driver)
4291	 */
4292	if (!disable && mvmvif->deflink.bcast_sta.sta_id != IWL_MVM_INVALID_STA)
4293		iwl_mvm_int_sta_modify_disable_tx(mvm, mvmvif,
4294						  &mvmvif->deflink.bcast_sta,
4295						  disable);
4296}
4297
4298void iwl_mvm_csa_client_absent(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
4299{
4300	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
4301	struct iwl_mvm_sta *mvmsta;
4302
4303	rcu_read_lock();
4304
4305	mvmsta = iwl_mvm_sta_from_staid_rcu(mvm, mvmvif->deflink.ap_sta_id);
4306
4307	if (mvmsta)
4308		iwl_mvm_sta_modify_disable_tx(mvm, mvmsta, true);
4309
4310	rcu_read_unlock();
4311}
4312
4313u16 iwl_mvm_tid_queued(struct iwl_mvm *mvm, struct iwl_mvm_tid_data *tid_data)
4314{
4315	u16 sn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
4316
4317	/*
4318	 * In 22000 HW, the next_reclaimed index is only 8 bit, so we'll need
4319	 * to align the wrap around of ssn so we compare relevant values.
4320	 */
4321	if (mvm->trans->trans_cfg->gen2)
4322		sn &= 0xff;
4323
4324	return ieee80211_sn_sub(sn, tid_data->next_reclaimed);
4325}
4326
4327int iwl_mvm_add_pasn_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
4328			 struct iwl_mvm_int_sta *sta, u8 *addr, u32 cipher,
4329			 u8 *key, u32 key_len)
4330{
4331	int ret;
4332	u16 queue;
4333	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
4334	struct ieee80211_key_conf *keyconf;
4335	unsigned int wdg_timeout =
4336		iwl_mvm_get_wd_timeout(mvm, vif, false, false);
4337	bool mld = iwl_mvm_has_mld_api(mvm->fw);
4338	u32 type = mld ? STATION_TYPE_PEER : IWL_STA_LINK;
4339
4340	ret = iwl_mvm_allocate_int_sta(mvm, sta, 0,
4341				       NL80211_IFTYPE_UNSPECIFIED, type);
4342	if (ret)
4343		return ret;
4344
4345	if (mld)
4346		ret = iwl_mvm_mld_add_int_sta_with_queue(mvm, sta, addr,
4347							 mvmvif->deflink.fw_link_id,
4348							 &queue,
4349							 IWL_MAX_TID_COUNT,
4350							 &wdg_timeout);
4351	else
4352		ret = iwl_mvm_add_int_sta_with_queue(mvm, mvmvif->id,
4353						     mvmvif->color, addr, sta,
4354						     &queue,
4355						     IWL_MVM_TX_FIFO_BE);
4356	if (ret)
4357		goto out;
4358
4359	keyconf = kzalloc(sizeof(*keyconf) + key_len, GFP_KERNEL);
4360	if (!keyconf) {
4361		ret = -ENOBUFS;
4362		goto out;
4363	}
4364
4365	keyconf->cipher = cipher;
4366	memcpy(keyconf->key, key, key_len);
4367	keyconf->keylen = key_len;
4368	keyconf->flags = IEEE80211_KEY_FLAG_PAIRWISE;
4369
4370	if (mld) {
4371		/* The MFP flag is set according to the station mfp field. Since
4372		 * we don't have a station, set it manually.
4373		 */
4374		u32 key_flags =
4375			iwl_mvm_get_sec_flags(mvm, vif, NULL, keyconf) |
4376			IWL_SEC_KEY_FLAG_MFP;
4377		u32 sta_mask = BIT(sta->sta_id);
4378
4379		ret = iwl_mvm_mld_send_key(mvm, sta_mask, key_flags, keyconf);
4380	} else {
4381		ret = iwl_mvm_send_sta_key(mvm, sta->sta_id, keyconf, false,
4382					   0, NULL, 0, 0, true);
4383	}
4384
4385	kfree(keyconf);
4386	return 0;
4387out:
4388	iwl_mvm_dealloc_int_sta(mvm, sta);
4389	return ret;
4390}
4391
4392void iwl_mvm_cancel_channel_switch(struct iwl_mvm *mvm,
4393				   struct ieee80211_vif *vif,
4394				   u32 id)
4395{
4396	struct iwl_cancel_channel_switch_cmd cancel_channel_switch_cmd = {
4397		.id = cpu_to_le32(id),
4398	};
4399	int ret;
4400
4401	ret = iwl_mvm_send_cmd_pdu(mvm,
4402				   WIDE_ID(MAC_CONF_GROUP, CANCEL_CHANNEL_SWITCH_CMD),
4403				   CMD_ASYNC,
4404				   sizeof(cancel_channel_switch_cmd),
4405				   &cancel_channel_switch_cmd);
4406	if (ret)
4407		IWL_ERR(mvm, "Failed to cancel the channel switch\n");
4408}