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