1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
2 /*
3 * Copyright (C) 2024 - 2026 Intel Corporation
4 */
5 #include <net/ip.h>
6
7 #include "tx.h"
8 #include "sta.h"
9 #include "hcmd.h"
10 #include "iwl-utils.h"
11 #include "iface.h"
12
13 #include "fw/dbg.h"
14
15 #include "fw/api/tx.h"
16 #include "fw/api/rs.h"
17 #include "fw/api/txq.h"
18 #include "fw/api/datapath.h"
19 #include "fw/api/time-event.h"
20
21 #define MAX_ANT_NUM 2
22
23 /* Toggles between TX antennas. Receives the bitmask of valid TX antennas and
24 * the *index* used for the last TX, and returns the next valid *index* to use.
25 * In order to set it in the tx_cmd, must do BIT(idx).
26 */
iwl_mld_next_ant(u8 valid,u8 last_idx)27 static u8 iwl_mld_next_ant(u8 valid, u8 last_idx)
28 {
29 u8 index = last_idx;
30
31 for (int i = 0; i < MAX_ANT_NUM; i++) {
32 index = (index + 1) % MAX_ANT_NUM;
33 if (valid & BIT(index))
34 return index;
35 }
36
37 WARN_ONCE(1, "Failed to toggle between antennas 0x%x", valid);
38
39 return last_idx;
40 }
41
iwl_mld_toggle_tx_ant(struct iwl_mld * mld,u8 * ant)42 void iwl_mld_toggle_tx_ant(struct iwl_mld *mld, u8 *ant)
43 {
44 *ant = iwl_mld_next_ant(iwl_mld_get_valid_tx_ant(mld), *ant);
45 }
46
47 static int
iwl_mld_get_queue_size(struct iwl_mld * mld,struct ieee80211_txq * txq)48 iwl_mld_get_queue_size(struct iwl_mld *mld, struct ieee80211_txq *txq)
49 {
50 struct ieee80211_sta *sta = txq->sta;
51 struct ieee80211_link_sta *link_sta;
52 unsigned int link_id;
53 int max_size = IWL_DEFAULT_QUEUE_SIZE;
54
55 lockdep_assert_wiphy(mld->wiphy);
56
57 for_each_sta_active_link(txq->vif, sta, link_sta, link_id) {
58 if (link_sta->eht_cap.has_eht) {
59 max_size = IWL_DEFAULT_QUEUE_SIZE_EHT;
60 break;
61 }
62
63 if (link_sta->he_cap.has_he)
64 max_size = IWL_DEFAULT_QUEUE_SIZE_HE;
65 }
66
67 return max_size;
68 }
69
iwl_mld_allocate_txq(struct iwl_mld * mld,struct ieee80211_txq * txq)70 static int iwl_mld_allocate_txq(struct iwl_mld *mld, struct ieee80211_txq *txq)
71 {
72 u8 tid = txq->tid == IEEE80211_NUM_TIDS ? IWL_MGMT_TID : txq->tid;
73 u32 fw_sta_mask = iwl_mld_fw_sta_id_mask(mld, txq->sta);
74 unsigned int watchdog_timeout;
75 int queue, size;
76
77 switch (txq->vif->type) {
78 case NL80211_IFTYPE_AP: /* STA might go to PS */
79 case NL80211_IFTYPE_NAN_DATA: /* peer might ULW/break schedule */
80 watchdog_timeout = IWL_WATCHDOG_DISABLED;
81 break;
82 default:
83 watchdog_timeout = mld->trans->mac_cfg->base->wd_timeout;
84 break;
85 }
86
87 lockdep_assert_wiphy(mld->wiphy);
88
89 if (tid == IWL_MGMT_TID)
90 size = max_t(u32, IWL_MGMT_QUEUE_SIZE,
91 mld->trans->mac_cfg->base->min_txq_size);
92 else
93 size = iwl_mld_get_queue_size(mld, txq);
94
95 queue = iwl_trans_txq_alloc(mld->trans, 0, fw_sta_mask, tid, size,
96 watchdog_timeout);
97
98 if (queue >= 0)
99 IWL_DEBUG_TX_QUEUES(mld,
100 "Enabling TXQ #%d for sta mask 0x%x tid %d\n",
101 queue, fw_sta_mask, tid);
102 return queue;
103 }
104
iwl_mld_add_txq(struct iwl_mld * mld,struct ieee80211_txq * txq)105 static int iwl_mld_add_txq(struct iwl_mld *mld, struct ieee80211_txq *txq)
106 {
107 struct iwl_mld_txq *mld_txq = iwl_mld_txq_from_mac80211(txq);
108 int id;
109
110 lockdep_assert_wiphy(mld->wiphy);
111
112 /* This will alse send the SCD_QUEUE_CONFIG_CMD */
113 id = iwl_mld_allocate_txq(mld, txq);
114 if (id < 0)
115 return id;
116
117 mld_txq->fw_id = id;
118 mld_txq->status.allocated = true;
119
120 rcu_assign_pointer(mld->fw_id_to_txq[id], txq);
121
122 return 0;
123 }
124
iwl_mld_add_txq_list(struct iwl_mld * mld)125 void iwl_mld_add_txq_list(struct iwl_mld *mld)
126 {
127 lockdep_assert_wiphy(mld->wiphy);
128
129 while (!list_empty(&mld->txqs_to_add)) {
130 struct ieee80211_txq *txq;
131 struct iwl_mld_txq *mld_txq =
132 list_first_entry(&mld->txqs_to_add, struct iwl_mld_txq,
133 list);
134 int failed;
135
136 txq = container_of((void *)mld_txq, struct ieee80211_txq,
137 drv_priv);
138
139 failed = iwl_mld_add_txq(mld, txq);
140
141 local_bh_disable();
142 spin_lock(&mld->add_txqs_lock);
143 list_del_init(&mld_txq->list);
144 spin_unlock(&mld->add_txqs_lock);
145 /* If the queue allocation failed, we can't transmit. Leave the
146 * frames on the txq, maybe the attempt to allocate the queue
147 * will succeed.
148 */
149 if (!failed)
150 iwl_mld_tx_from_txq(mld, txq);
151 local_bh_enable();
152 }
153 }
154
iwl_mld_add_txqs_wk(struct wiphy * wiphy,struct wiphy_work * wk)155 void iwl_mld_add_txqs_wk(struct wiphy *wiphy, struct wiphy_work *wk)
156 {
157 struct iwl_mld *mld = container_of(wk, struct iwl_mld,
158 add_txqs_wk);
159
160 /* will reschedule to run after restart */
161 if (mld->fw_status.in_hw_restart)
162 return;
163
164 iwl_mld_add_txq_list(mld);
165 }
166
167 void
iwl_mld_free_txq(struct iwl_mld * mld,u32 fw_sta_mask,u32 tid,u32 queue_id)168 iwl_mld_free_txq(struct iwl_mld *mld, u32 fw_sta_mask, u32 tid, u32 queue_id)
169 {
170 struct iwl_scd_queue_cfg_cmd remove_cmd = {
171 .operation = cpu_to_le32(IWL_SCD_QUEUE_REMOVE),
172 .u.remove.tid = cpu_to_le32(tid),
173 .u.remove.sta_mask = cpu_to_le32(fw_sta_mask),
174 };
175
176 iwl_mld_send_cmd_pdu(mld,
177 WIDE_ID(DATA_PATH_GROUP, SCD_QUEUE_CONFIG_CMD),
178 &remove_cmd);
179
180 iwl_trans_txq_free(mld->trans, queue_id);
181 }
182
iwl_mld_remove_txq(struct iwl_mld * mld,struct ieee80211_txq * txq)183 void iwl_mld_remove_txq(struct iwl_mld *mld, struct ieee80211_txq *txq)
184 {
185 struct iwl_mld_txq *mld_txq = iwl_mld_txq_from_mac80211(txq);
186 u32 sta_msk, tid;
187
188 lockdep_assert_wiphy(mld->wiphy);
189
190 spin_lock_bh(&mld->add_txqs_lock);
191 if (!list_empty(&mld_txq->list))
192 list_del_init(&mld_txq->list);
193 spin_unlock_bh(&mld->add_txqs_lock);
194
195 if (!mld_txq->status.allocated ||
196 WARN_ON(mld_txq->fw_id >= ARRAY_SIZE(mld->fw_id_to_txq)))
197 return;
198
199 sta_msk = iwl_mld_fw_sta_id_mask(mld, txq->sta);
200
201 tid = txq->tid == IEEE80211_NUM_TIDS ? IWL_MGMT_TID :
202 txq->tid;
203
204 iwl_mld_free_txq(mld, sta_msk, tid, mld_txq->fw_id);
205
206 RCU_INIT_POINTER(mld->fw_id_to_txq[mld_txq->fw_id], NULL);
207 mld_txq->status.allocated = false;
208 }
209
210 #define OPT_HDR(type, skb, off) \
211 (type *)(skb_network_header(skb) + (off))
212
213 static __le32
iwl_mld_get_offload_assist(struct sk_buff * skb,bool amsdu)214 iwl_mld_get_offload_assist(struct sk_buff *skb, bool amsdu)
215 {
216 struct ieee80211_hdr *hdr = (void *)skb->data;
217 u16 mh_len = ieee80211_hdrlen(hdr->frame_control);
218 u16 offload_assist = 0;
219 #if IS_ENABLED(CONFIG_INET)
220 u8 protocol = 0;
221
222 /* Do not compute checksum if already computed */
223 if (skb->ip_summed != CHECKSUM_PARTIAL)
224 goto out;
225
226 /* We do not expect to be requested to csum stuff we do not support */
227
228 /* TBD: do we also need to check
229 * !(mvm->hw->netdev_features & IWL_TX_CSUM_NETIF_FLAGS) now that all
230 * the devices we support has this flags?
231 */
232 if (WARN_ONCE(skb->protocol != htons(ETH_P_IP) &&
233 skb->protocol != htons(ETH_P_IPV6),
234 "No support for requested checksum\n")) {
235 skb_checksum_help(skb);
236 goto out;
237 }
238
239 if (skb->protocol == htons(ETH_P_IP)) {
240 protocol = ip_hdr(skb)->protocol;
241 } else {
242 #if IS_ENABLED(CONFIG_IPV6)
243 struct ipv6hdr *ipv6h =
244 (struct ipv6hdr *)skb_network_header(skb);
245 unsigned int off = sizeof(*ipv6h);
246
247 protocol = ipv6h->nexthdr;
248 while (protocol != NEXTHDR_NONE && ipv6_ext_hdr(protocol)) {
249 struct ipv6_opt_hdr *hp;
250
251 /* only supported extension headers */
252 if (protocol != NEXTHDR_ROUTING &&
253 protocol != NEXTHDR_HOP &&
254 protocol != NEXTHDR_DEST) {
255 skb_checksum_help(skb);
256 goto out;
257 }
258
259 hp = OPT_HDR(struct ipv6_opt_hdr, skb, off);
260 protocol = hp->nexthdr;
261 off += ipv6_optlen(hp);
262 }
263 /* if we get here - protocol now should be TCP/UDP */
264 #endif
265 }
266
267 if (protocol != IPPROTO_TCP && protocol != IPPROTO_UDP) {
268 WARN_ON_ONCE(1);
269 skb_checksum_help(skb);
270 goto out;
271 }
272
273 /* enable L4 csum */
274 offload_assist |= BIT(TX_CMD_OFFLD_L4_EN);
275
276 /* Set offset to IP header (snap).
277 * We don't support tunneling so no need to take care of inner header.
278 * Size is in words.
279 */
280 offload_assist |= (4 << TX_CMD_OFFLD_IP_HDR);
281
282 /* Do IPv4 csum for AMSDU only (no IP csum for Ipv6) */
283 if (skb->protocol == htons(ETH_P_IP) && amsdu) {
284 ip_hdr(skb)->check = 0;
285 offload_assist |= BIT(TX_CMD_OFFLD_L3_EN);
286 }
287
288 /* reset UDP/TCP header csum */
289 if (protocol == IPPROTO_TCP)
290 tcp_hdr(skb)->check = 0;
291 else
292 udp_hdr(skb)->check = 0;
293
294 out:
295 #endif
296 mh_len /= 2;
297 offload_assist |= mh_len << TX_CMD_OFFLD_MH_SIZE;
298
299 if (amsdu)
300 offload_assist |= BIT(TX_CMD_OFFLD_AMSDU);
301 else if (ieee80211_hdrlen(hdr->frame_control) % 4)
302 /* padding is inserted later in transport */
303 offload_assist |= BIT(TX_CMD_OFFLD_PAD);
304
305 return cpu_to_le32(offload_assist);
306 }
307
iwl_mld_get_basic_rates_and_band(struct iwl_mld * mld,struct ieee80211_vif * vif,struct ieee80211_tx_info * info,unsigned long * basic_rates,u8 * band)308 static void iwl_mld_get_basic_rates_and_band(struct iwl_mld *mld,
309 struct ieee80211_vif *vif,
310 struct ieee80211_tx_info *info,
311 unsigned long *basic_rates,
312 u8 *band)
313 {
314 u32 link_id = u32_get_bits(info->control.flags,
315 IEEE80211_TX_CTRL_MLO_LINK);
316
317 *basic_rates = vif->bss_conf.basic_rates;
318 *band = info->band;
319
320 if (link_id == IEEE80211_LINK_UNSPECIFIED &&
321 ieee80211_vif_is_mld(vif)) {
322 /* shouldn't do this when >1 link is active */
323 WARN_ON(hweight16(vif->active_links) != 1);
324 link_id = __ffs(vif->active_links);
325 }
326
327 if (link_id < IEEE80211_LINK_UNSPECIFIED) {
328 struct ieee80211_bss_conf *link_conf;
329
330 rcu_read_lock();
331 link_conf = rcu_dereference(vif->link_conf[link_id]);
332 if (link_conf) {
333 *basic_rates = link_conf->basic_rates;
334 if (link_conf->chanreq.oper.chan)
335 *band = link_conf->chanreq.oper.chan->band;
336 }
337 rcu_read_unlock();
338 }
339 }
340
iwl_mld_get_lowest_rate(struct iwl_mld * mld,struct ieee80211_tx_info * info,struct ieee80211_vif * vif)341 u8 iwl_mld_get_lowest_rate(struct iwl_mld *mld,
342 struct ieee80211_tx_info *info,
343 struct ieee80211_vif *vif)
344 {
345 struct ieee80211_supported_band *sband;
346 u16 lowest_cck = IWL_RATE_COUNT, lowest_ofdm = IWL_RATE_COUNT;
347 unsigned long basic_rates;
348 u8 band, rate;
349 u32 i;
350
351 iwl_mld_get_basic_rates_and_band(mld, vif, info, &basic_rates, &band);
352
353 if (band >= NUM_NL80211_BANDS) {
354 WARN_ON(vif->type != NL80211_IFTYPE_NAN &&
355 vif->type != NL80211_IFTYPE_NAN_DATA);
356 return IWL_FIRST_OFDM_RATE;
357 }
358
359 sband = mld->hw->wiphy->bands[band];
360 for_each_set_bit(i, &basic_rates, BITS_PER_LONG) {
361 u16 hw = sband->bitrates[i].hw_value;
362
363 if (hw >= IWL_FIRST_OFDM_RATE) {
364 if (lowest_ofdm > hw)
365 lowest_ofdm = hw;
366 } else if (lowest_cck > hw) {
367 lowest_cck = hw;
368 }
369 }
370
371 if (band == NL80211_BAND_2GHZ && !vif->p2p &&
372 vif->type != NL80211_IFTYPE_P2P_DEVICE &&
373 !(info->flags & IEEE80211_TX_CTL_NO_CCK_RATE)) {
374 if (lowest_cck != IWL_RATE_COUNT)
375 rate = lowest_cck;
376 else if (lowest_ofdm != IWL_RATE_COUNT)
377 rate = lowest_ofdm;
378 else
379 rate = IWL_FIRST_CCK_RATE;
380 } else if (lowest_ofdm != IWL_RATE_COUNT) {
381 rate = lowest_ofdm;
382 } else {
383 rate = IWL_FIRST_OFDM_RATE;
384 }
385
386 return rate;
387 }
388
iwl_mld_mac80211_rate_idx_to_fw(struct iwl_mld * mld,struct ieee80211_tx_info * info,int rate_idx)389 static u32 iwl_mld_mac80211_rate_idx_to_fw(struct iwl_mld *mld,
390 struct ieee80211_tx_info *info,
391 int rate_idx)
392 {
393 u32 rate_flags = 0;
394 u8 rate_plcp;
395
396 /* if the rate isn't a well known legacy rate, take the lowest one */
397 if (rate_idx < 0 || rate_idx >= IWL_RATE_COUNT_LEGACY)
398 rate_idx = iwl_mld_get_lowest_rate(mld, info,
399 info->control.vif);
400
401 WARN_ON_ONCE(rate_idx < 0);
402
403 /* Set CCK or OFDM flag */
404 if (rate_idx <= IWL_LAST_CCK_RATE)
405 rate_flags |= RATE_MCS_MOD_TYPE_CCK;
406 else
407 rate_flags |= RATE_MCS_MOD_TYPE_LEGACY_OFDM;
408
409 /* Legacy rates are indexed:
410 * 0 - 3 for CCK and 0 - 7 for OFDM
411 */
412 rate_plcp = (rate_idx >= IWL_FIRST_OFDM_RATE ?
413 rate_idx - IWL_FIRST_OFDM_RATE : rate_idx);
414
415 return (u32)rate_plcp | rate_flags;
416 }
417
iwl_mld_get_tx_ant(struct iwl_mld * mld,struct ieee80211_tx_info * info,struct ieee80211_sta * sta,__le16 fc)418 static u32 iwl_mld_get_tx_ant(struct iwl_mld *mld,
419 struct ieee80211_tx_info *info,
420 struct ieee80211_sta *sta, __le16 fc)
421 {
422 if (sta && ieee80211_is_data(fc)) {
423 struct iwl_mld_sta *mld_sta = iwl_mld_sta_from_mac80211(sta);
424
425 return BIT(mld_sta->data_tx_ant) << RATE_MCS_ANT_POS;
426 }
427
428 return BIT(mld->mgmt_tx_ant) << RATE_MCS_ANT_POS;
429 }
430
iwl_mld_get_inject_tx_rate(struct iwl_mld * mld,struct ieee80211_tx_info * info,struct ieee80211_sta * sta,__le16 fc)431 static u32 iwl_mld_get_inject_tx_rate(struct iwl_mld *mld,
432 struct ieee80211_tx_info *info,
433 struct ieee80211_sta *sta,
434 __le16 fc)
435 {
436 struct ieee80211_tx_rate *rate = &info->control.rates[0];
437 u32 result;
438
439 if (rate->flags & IEEE80211_TX_RC_VHT_MCS) {
440 u8 mcs = ieee80211_rate_get_vht_mcs(rate);
441 u8 nss = ieee80211_rate_get_vht_nss(rate);
442
443 result = RATE_MCS_MOD_TYPE_VHT;
444 result |= u32_encode_bits(mcs, RATE_MCS_CODE_MSK);
445 result |= u32_encode_bits(nss, RATE_MCS_NSS_MSK);
446
447 if (rate->flags & IEEE80211_TX_RC_SHORT_GI)
448 result |= RATE_MCS_SGI_MSK;
449
450 if (rate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
451 result |= RATE_MCS_CHAN_WIDTH_40;
452 else if (rate->flags & IEEE80211_TX_RC_80_MHZ_WIDTH)
453 result |= RATE_MCS_CHAN_WIDTH_80;
454 else if (rate->flags & IEEE80211_TX_RC_160_MHZ_WIDTH)
455 result |= RATE_MCS_CHAN_WIDTH_160;
456 } else if (rate->flags & IEEE80211_TX_RC_MCS) {
457 /* only MCS 0-15 are supported */
458 u8 mcs = rate->idx & 7;
459 u8 nss = rate->idx > 7;
460
461 result = RATE_MCS_MOD_TYPE_HT;
462 result |= u32_encode_bits(mcs, RATE_MCS_CODE_MSK);
463 result |= u32_encode_bits(nss, RATE_MCS_NSS_MSK);
464
465 if (rate->flags & IEEE80211_TX_RC_SHORT_GI)
466 result |= RATE_MCS_SGI_MSK;
467 if (rate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
468 result |= RATE_MCS_CHAN_WIDTH_40;
469 if (info->flags & IEEE80211_TX_CTL_LDPC)
470 result |= RATE_MCS_LDPC_MSK;
471 if (u32_get_bits(info->flags, IEEE80211_TX_CTL_STBC))
472 result |= RATE_MCS_STBC_MSK;
473 } else {
474 result = iwl_mld_mac80211_rate_idx_to_fw(mld, info, rate->idx);
475 }
476
477 if (info->control.antennas)
478 result |= u32_encode_bits(info->control.antennas,
479 RATE_MCS_ANT_AB_MSK);
480 else
481 result |= iwl_mld_get_tx_ant(mld, info, sta, fc);
482
483 return result;
484 }
485
iwl_mld_get_tx_rate_n_flags(struct iwl_mld * mld,struct ieee80211_tx_info * info,struct ieee80211_sta * sta,__le16 fc)486 static __le32 iwl_mld_get_tx_rate_n_flags(struct iwl_mld *mld,
487 struct ieee80211_tx_info *info,
488 struct ieee80211_sta *sta, __le16 fc)
489 {
490 u32 rate;
491
492 if (unlikely(info->control.flags & IEEE80211_TX_CTRL_RATE_INJECT))
493 rate = iwl_mld_get_inject_tx_rate(mld, info, sta, fc);
494 else
495 rate = iwl_mld_mac80211_rate_idx_to_fw(mld, info, -1) |
496 iwl_mld_get_tx_ant(mld, info, sta, fc);
497
498 return cpu_to_le32(rate);
499 }
500
501 static void
iwl_mld_fill_tx_cmd_hdr(struct iwl_tx_cmd * tx_cmd,struct sk_buff * skb,bool amsdu)502 iwl_mld_fill_tx_cmd_hdr(struct iwl_tx_cmd *tx_cmd,
503 struct sk_buff *skb, bool amsdu)
504 {
505 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
506 struct ieee80211_hdr *hdr = (void *)skb->data;
507 struct ieee80211_vif *vif;
508
509 /* Copy MAC header from skb into command buffer */
510 memcpy(tx_cmd->hdr, hdr, ieee80211_hdrlen(hdr->frame_control));
511
512 if (!amsdu || !skb_is_gso(skb))
513 return;
514
515 /* As described in IEEE sta 802.11-2020, table 9-30 (Address
516 * field contents), A-MSDU address 3 should contain the BSSID
517 * address.
518 *
519 * In TSO, the skb header address 3 contains the original address 3 to
520 * correctly create all the A-MSDU subframes headers from it.
521 * Override now the address 3 in the command header with the BSSID.
522 *
523 * Note: we fill in the MLD address, but the firmware will do the
524 * necessary translation to link address after encryption.
525 */
526 vif = info->control.vif;
527 switch (vif->type) {
528 case NL80211_IFTYPE_STATION:
529 ether_addr_copy(tx_cmd->hdr->addr3, vif->cfg.ap_addr);
530 break;
531 case NL80211_IFTYPE_AP:
532 ether_addr_copy(tx_cmd->hdr->addr3, vif->addr);
533 break;
534 default:
535 break;
536 }
537 }
538
539 static void
iwl_mld_fill_tx_cmd(struct iwl_mld * mld,struct sk_buff * skb,struct iwl_device_tx_cmd * dev_tx_cmd,struct ieee80211_sta * sta)540 iwl_mld_fill_tx_cmd(struct iwl_mld *mld, struct sk_buff *skb,
541 struct iwl_device_tx_cmd *dev_tx_cmd,
542 struct ieee80211_sta *sta)
543 {
544 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
545 struct ieee80211_hdr *hdr = (void *)skb->data;
546 struct iwl_mld_sta *mld_sta = sta ? iwl_mld_sta_from_mac80211(sta) :
547 NULL;
548 struct iwl_tx_cmd *tx_cmd;
549 bool amsdu = ieee80211_is_data_qos(hdr->frame_control) &&
550 (*ieee80211_get_qos_ctl(hdr) &
551 IEEE80211_QOS_CTL_A_MSDU_PRESENT);
552 __le32 rate_n_flags = 0;
553 u16 flags = 0;
554
555 dev_tx_cmd->hdr.cmd = TX_CMD;
556
557 if (!info->control.hw_key)
558 flags |= IWL_TX_FLAGS_ENCRYPT_DIS;
559
560 /* For data and mgmt packets rate info comes from the fw.
561 * Only set rate/antenna for:
562 * - injected frames with fixed rate,
563 * - when no sta is given.
564 * - frames that are sent to an NMI sta, which is only used for management.
565 */
566 if (unlikely(!sta ||
567 (mld_sta && mld_sta->vif->type == NL80211_IFTYPE_NAN) ||
568 info->control.flags & IEEE80211_TX_CTRL_RATE_INJECT)) {
569 flags |= IWL_TX_FLAGS_CMD_RATE;
570 rate_n_flags = iwl_mld_get_tx_rate_n_flags(mld, info, sta,
571 hdr->frame_control);
572 } else if (!ieee80211_is_data(hdr->frame_control) ||
573 (mld_sta &&
574 mld_sta->sta_state < IEEE80211_STA_AUTHORIZED)) {
575 /* These are important frames */
576 flags |= IWL_TX_FLAGS_HIGH_PRI;
577 }
578
579 tx_cmd = (void *)dev_tx_cmd->payload;
580
581 iwl_mld_fill_tx_cmd_hdr(tx_cmd, skb, amsdu);
582
583 tx_cmd->offload_assist = iwl_mld_get_offload_assist(skb, amsdu);
584
585 /* Total # bytes to be transmitted */
586 tx_cmd->len = cpu_to_le16((u16)skb->len);
587
588 tx_cmd->flags = cpu_to_le16(flags);
589
590 tx_cmd->rate_n_flags = rate_n_flags;
591 }
592
593 /* Caller of this need to check that info->control.vif is not NULL */
594 static struct iwl_mld_link *
iwl_mld_get_link_from_tx_info(struct ieee80211_tx_info * info)595 iwl_mld_get_link_from_tx_info(struct ieee80211_tx_info *info)
596 {
597 struct iwl_mld_vif *mld_vif =
598 iwl_mld_vif_from_mac80211(info->control.vif);
599 u32 link_id = u32_get_bits(info->control.flags,
600 IEEE80211_TX_CTRL_MLO_LINK);
601
602 if (link_id == IEEE80211_LINK_UNSPECIFIED) {
603 if (info->control.vif->active_links)
604 link_id = ffs(info->control.vif->active_links) - 1;
605 else
606 link_id = 0;
607 }
608
609 return rcu_dereference(mld_vif->link[link_id]);
610 }
611
612 static int
iwl_mld_get_tx_queue_id(struct iwl_mld * mld,struct ieee80211_txq * txq,struct sk_buff * skb)613 iwl_mld_get_tx_queue_id(struct iwl_mld *mld, struct ieee80211_txq *txq,
614 struct sk_buff *skb)
615 {
616 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
617 struct ieee80211_hdr *hdr = (void *)skb->data;
618 __le16 fc = hdr->frame_control;
619 struct iwl_mld_vif *mld_vif;
620 struct iwl_mld_link *link;
621
622 if (txq && txq->sta)
623 return iwl_mld_txq_from_mac80211(txq)->fw_id;
624
625 if (!info->control.vif)
626 return IWL_MLD_INVALID_QUEUE;
627
628 switch (info->control.vif->type) {
629 case NL80211_IFTYPE_AP:
630 case NL80211_IFTYPE_ADHOC:
631 link = iwl_mld_get_link_from_tx_info(info);
632
633 if (WARN_ON(!link))
634 break;
635
636 /* ucast disassociate/deauth frames without a station might
637 * happen, especially with reason 7 ("Class 3 frame received
638 * from nonassociated STA").
639 */
640 if (ieee80211_is_mgmt(fc) &&
641 (!ieee80211_is_bufferable_mmpdu(skb) ||
642 ieee80211_is_deauth(fc) || ieee80211_is_disassoc(fc)))
643 return link->bcast_sta.queue_id;
644
645 if (is_multicast_ether_addr(hdr->addr1) &&
646 !ieee80211_has_order(fc))
647 return link->mcast_sta.queue_id;
648
649 WARN_ONCE(info->control.vif->type != NL80211_IFTYPE_ADHOC,
650 "Couldn't find a TXQ. fc=0x%02x", le16_to_cpu(fc));
651 return link->bcast_sta.queue_id;
652 case NL80211_IFTYPE_P2P_DEVICE:
653 mld_vif = iwl_mld_vif_from_mac80211(info->control.vif);
654
655 if (mld_vif->roc_activity != ROC_ACTIVITY_P2P_DISC &&
656 mld_vif->roc_activity != ROC_ACTIVITY_P2P_NEG) {
657 IWL_DEBUG_DROP(mld,
658 "Drop tx outside ROC with activity %d\n",
659 mld_vif->roc_activity);
660 return IWL_MLD_INVALID_DROP_TX;
661 }
662
663 WARN_ON(!ieee80211_is_mgmt(fc));
664
665 return mld_vif->aux_sta.queue_id;
666 case NL80211_IFTYPE_MONITOR:
667 mld_vif = iwl_mld_vif_from_mac80211(info->control.vif);
668 return mld_vif->deflink.mon_sta.queue_id;
669 case NL80211_IFTYPE_STATION:
670 mld_vif = iwl_mld_vif_from_mac80211(info->control.vif);
671
672 if (!(info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)) {
673 IWL_DEBUG_DROP(mld, "Drop tx not off-channel\n");
674 return IWL_MLD_INVALID_DROP_TX;
675 }
676
677 if (mld_vif->roc_activity != ROC_ACTIVITY_HOTSPOT) {
678 IWL_DEBUG_DROP(mld, "Drop tx outside ROC\n");
679 return IWL_MLD_INVALID_DROP_TX;
680 }
681
682 WARN_ON(!ieee80211_is_mgmt(fc));
683 return mld_vif->aux_sta.queue_id;
684 case NL80211_IFTYPE_NAN:
685 WARN_ON(!ieee80211_is_mgmt(fc));
686 return iwl_mld_nan_get_mgmt_queue(mld, info->control.vif);
687 case NL80211_IFTYPE_NAN_DATA:
688 WARN_ON(!ieee80211_is_data(fc));
689
690 if (!iwl_mld_nan_use_nan_stations(mld))
691 break;
692
693 mld_vif = iwl_mld_vif_from_mac80211(info->control.vif);
694 return mld_vif->nan.mcast_data_sta.queue_id;
695 default:
696 WARN_ONCE(1, "Unsupported vif type\n");
697 break;
698 }
699
700 return IWL_MLD_INVALID_QUEUE;
701 }
702
iwl_mld_probe_resp_set_noa(struct iwl_mld * mld,struct sk_buff * skb)703 static void iwl_mld_probe_resp_set_noa(struct iwl_mld *mld,
704 struct sk_buff *skb)
705 {
706 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
707 struct iwl_mld_link *mld_link =
708 &iwl_mld_vif_from_mac80211(info->control.vif)->deflink;
709 struct iwl_probe_resp_data *resp_data;
710 u8 *pos;
711
712 if (!info->control.vif->p2p)
713 return;
714
715 rcu_read_lock();
716
717 resp_data = rcu_dereference(mld_link->probe_resp_data);
718 if (!resp_data)
719 goto out;
720
721 if (!resp_data->notif.noa_active)
722 goto out;
723
724 if (skb_tailroom(skb) < resp_data->noa_len) {
725 if (pskb_expand_head(skb, 0, resp_data->noa_len, GFP_ATOMIC)) {
726 IWL_ERR(mld,
727 "Failed to reallocate probe resp\n");
728 goto out;
729 }
730 }
731
732 pos = skb_put(skb, resp_data->noa_len);
733
734 *pos++ = WLAN_EID_VENDOR_SPECIFIC;
735 /* Set length of IE body (not including ID and length itself) */
736 *pos++ = resp_data->noa_len - 2;
737 *pos++ = (WLAN_OUI_WFA >> 16) & 0xff;
738 *pos++ = (WLAN_OUI_WFA >> 8) & 0xff;
739 *pos++ = WLAN_OUI_WFA & 0xff;
740 *pos++ = WLAN_OUI_TYPE_WFA_P2P;
741
742 memcpy(pos, &resp_data->notif.noa_attr,
743 resp_data->noa_len - sizeof(struct ieee80211_vendor_ie));
744
745 out:
746 rcu_read_unlock();
747 }
748
749 /* This function must be called with BHs disabled */
iwl_mld_tx_mpdu(struct iwl_mld * mld,struct sk_buff * skb,struct ieee80211_txq * txq)750 static int iwl_mld_tx_mpdu(struct iwl_mld *mld, struct sk_buff *skb,
751 struct ieee80211_txq *txq)
752 {
753 struct ieee80211_hdr *hdr = (void *)skb->data;
754 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
755 struct ieee80211_sta *sta = txq ? txq->sta : NULL;
756 struct iwl_device_tx_cmd *dev_tx_cmd;
757 int queue = iwl_mld_get_tx_queue_id(mld, txq, skb);
758 u8 tid = IWL_MAX_TID_COUNT;
759
760 if (WARN_ONCE(queue == IWL_MLD_INVALID_QUEUE, "Invalid TX Queue id") ||
761 queue == IWL_MLD_INVALID_DROP_TX)
762 return -1;
763
764 if (unlikely(ieee80211_is_any_nullfunc(hdr->frame_control)))
765 return -1;
766
767 dev_tx_cmd = iwl_trans_alloc_tx_cmd(mld->trans);
768 if (unlikely(!dev_tx_cmd))
769 return -1;
770
771 if (unlikely(ieee80211_is_probe_resp(hdr->frame_control))) {
772 if (IWL_MLD_NON_TRANSMITTING_AP)
773 return -1;
774
775 iwl_mld_probe_resp_set_noa(mld, skb);
776 }
777
778 iwl_mld_fill_tx_cmd(mld, skb, dev_tx_cmd, sta);
779
780 if (ieee80211_is_data(hdr->frame_control)) {
781 if (ieee80211_is_data_qos(hdr->frame_control))
782 tid = ieee80211_get_tid(hdr);
783 else
784 tid = IWL_TID_NON_QOS;
785 }
786
787 IWL_DEBUG_TX(mld, "TX TID:%d from Q:%d len %d\n",
788 tid, queue, skb->len);
789
790 /* From now on, we cannot access info->control */
791 memset(&info->status, 0, sizeof(info->status));
792 memset(info->driver_data, 0, sizeof(info->driver_data));
793
794 info->driver_data[1] = dev_tx_cmd;
795
796 if (iwl_trans_tx(mld->trans, skb, dev_tx_cmd, queue))
797 goto err;
798
799 /* Update low-latency counter when a packet is queued instead
800 * of after TX, it makes sense for early low-latency detection
801 */
802 if (sta)
803 iwl_mld_low_latency_update_counters(mld, hdr, sta, 0);
804
805 return 0;
806
807 err:
808 iwl_trans_free_tx_cmd(mld->trans, dev_tx_cmd);
809 IWL_DEBUG_TX(mld, "TX from Q:%d dropped\n", queue);
810 return -1;
811 }
812
813 #ifdef CONFIG_INET
814
815 /* This function handles the segmentation of a large TSO packet into multiple
816 * MPDUs, ensuring that the resulting segments conform to AMSDU limits and
817 * constraints.
818 */
iwl_mld_tx_tso_segment(struct iwl_mld * mld,struct sk_buff * skb,struct ieee80211_sta * sta,struct sk_buff_head * mpdus_skbs)819 static int iwl_mld_tx_tso_segment(struct iwl_mld *mld, struct sk_buff *skb,
820 struct ieee80211_sta *sta,
821 struct sk_buff_head *mpdus_skbs)
822 {
823 struct ieee80211_hdr *hdr = (void *)skb->data;
824 netdev_features_t netdev_flags = NETIF_F_CSUM_MASK | NETIF_F_SG;
825 unsigned int mss = skb_shinfo(skb)->gso_size;
826 unsigned int num_subframes, tcp_payload_len, subf_len;
827 u16 snap_ip_tcp, pad, max_tid_amsdu_len;
828 u8 tid;
829
830 snap_ip_tcp = 8 + skb_network_header_len(skb) + tcp_hdrlen(skb);
831
832 if (!ieee80211_is_data_qos(hdr->frame_control) ||
833 !sta->cur->max_rc_amsdu_len)
834 return iwl_tx_tso_segment(skb, 1, netdev_flags, mpdus_skbs);
835
836 /* Do not build AMSDU for IPv6 with extension headers.
837 * Ask stack to segment and checksum the generated MPDUs for us.
838 */
839 if (skb->protocol == htons(ETH_P_IPV6) &&
840 ((struct ipv6hdr *)skb_network_header(skb))->nexthdr !=
841 IPPROTO_TCP) {
842 netdev_flags &= ~NETIF_F_CSUM_MASK;
843 return iwl_tx_tso_segment(skb, 1, netdev_flags, mpdus_skbs);
844 }
845
846 tid = ieee80211_get_tid(hdr);
847 if (WARN_ON_ONCE(tid >= IWL_MAX_TID_COUNT))
848 return -EINVAL;
849
850 max_tid_amsdu_len = sta->cur->max_tid_amsdu_len[tid];
851 if (!max_tid_amsdu_len || max_tid_amsdu_len == 1)
852 return iwl_tx_tso_segment(skb, 1, netdev_flags, mpdus_skbs);
853
854 /* Sub frame header + SNAP + IP header + TCP header + MSS */
855 subf_len = sizeof(struct ethhdr) + snap_ip_tcp + mss;
856 pad = (4 - subf_len) & 0x3;
857
858 /* If we have N subframes in the A-MSDU, then the A-MSDU's size is
859 * N * subf_len + (N - 1) * pad.
860 */
861 num_subframes = (max_tid_amsdu_len + pad) / (subf_len + pad);
862
863 if (WARN_ON_ONCE(!num_subframes))
864 return iwl_tx_tso_segment(skb, 1, netdev_flags, mpdus_skbs);
865
866 if (sta->max_amsdu_subframes &&
867 num_subframes > sta->max_amsdu_subframes)
868 num_subframes = sta->max_amsdu_subframes;
869
870 tcp_payload_len = skb_tail_pointer(skb) - skb_transport_header(skb) -
871 tcp_hdrlen(skb) + skb->data_len;
872
873 /* Make sure we have enough TBs for the A-MSDU:
874 * 2 for each subframe
875 * 1 more for each fragment
876 * 1 more for the potential data in the header
877 */
878 if ((num_subframes * 2 + skb_shinfo(skb)->nr_frags + 1) >
879 mld->trans->info.max_skb_frags)
880 num_subframes = 1;
881
882 if (num_subframes > 1)
883 *ieee80211_get_qos_ctl(hdr) |= IEEE80211_QOS_CTL_A_MSDU_PRESENT;
884
885 /* This skb fits in one single A-MSDU */
886 if (tcp_payload_len <= num_subframes * mss) {
887 __skb_queue_tail(mpdus_skbs, skb);
888 return 0;
889 }
890
891 /* Trick the segmentation function to make it create SKBs that can fit
892 * into one A-MSDU.
893 */
894 return iwl_tx_tso_segment(skb, num_subframes, netdev_flags, mpdus_skbs);
895 }
896
897 /* Manages TSO (TCP Segmentation Offload) packet transmission by segmenting
898 * large packets when necessary and transmitting each segment as MPDU.
899 */
iwl_mld_tx_tso(struct iwl_mld * mld,struct sk_buff * skb,struct ieee80211_txq * txq)900 static int iwl_mld_tx_tso(struct iwl_mld *mld, struct sk_buff *skb,
901 struct ieee80211_txq *txq)
902 {
903 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
904 struct sk_buff *orig_skb = skb;
905 struct sk_buff_head mpdus_skbs;
906 unsigned int payload_len;
907 int ret;
908
909 if (WARN_ON(!txq || !txq->sta))
910 return -1;
911
912 payload_len = skb_tail_pointer(skb) - skb_transport_header(skb) -
913 tcp_hdrlen(skb) + skb->data_len;
914
915 if (payload_len <= skb_shinfo(skb)->gso_size)
916 return iwl_mld_tx_mpdu(mld, skb, txq);
917
918 if (!info->control.vif)
919 return -1;
920
921 __skb_queue_head_init(&mpdus_skbs);
922
923 ret = iwl_mld_tx_tso_segment(mld, skb, txq->sta, &mpdus_skbs);
924 if (ret)
925 return ret;
926
927 WARN_ON(skb_queue_empty(&mpdus_skbs));
928
929 while (!skb_queue_empty(&mpdus_skbs)) {
930 skb = __skb_dequeue(&mpdus_skbs);
931
932 ret = iwl_mld_tx_mpdu(mld, skb, txq);
933 if (!ret)
934 continue;
935
936 /* Free skbs created as part of TSO logic that have not yet
937 * been dequeued
938 */
939 __skb_queue_purge(&mpdus_skbs);
940
941 /* skb here is not necessarily same as skb that entered
942 * this method, so free it explicitly.
943 */
944 if (skb == orig_skb)
945 ieee80211_free_txskb(mld->hw, skb);
946 else
947 kfree_skb(skb);
948
949 /* there was error, but we consumed skb one way or
950 * another, so return 0
951 */
952 return 0;
953 }
954
955 return 0;
956 }
957 #else
iwl_mld_tx_tso(struct iwl_mld * mld,struct sk_buff * skb,struct ieee80211_txq * txq)958 static int iwl_mld_tx_tso(struct iwl_mld *mld, struct sk_buff *skb,
959 struct ieee80211_txq *txq)
960 {
961 /* Impossible to get TSO without CONFIG_INET */
962 WARN_ON(1);
963
964 return -1;
965 }
966 #endif /* CONFIG_INET */
967
iwl_mld_tx_skb(struct iwl_mld * mld,struct sk_buff * skb,struct ieee80211_txq * txq)968 void iwl_mld_tx_skb(struct iwl_mld *mld, struct sk_buff *skb,
969 struct ieee80211_txq *txq)
970 {
971 if (skb_is_gso(skb)) {
972 if (!iwl_mld_tx_tso(mld, skb, txq))
973 return;
974 goto err;
975 }
976
977 if (likely(!iwl_mld_tx_mpdu(mld, skb, txq)))
978 return;
979
980 err:
981 ieee80211_free_txskb(mld->hw, skb);
982 }
983
iwl_mld_tx_from_txq(struct iwl_mld * mld,struct ieee80211_txq * txq)984 void iwl_mld_tx_from_txq(struct iwl_mld *mld, struct ieee80211_txq *txq)
985 {
986 struct iwl_mld_txq *mld_txq = iwl_mld_txq_from_mac80211(txq);
987 struct sk_buff *skb = NULL;
988 u8 zero_addr[ETH_ALEN] = {};
989
990 /*
991 * Don't transmit during firmware restart. The firmware is dead,
992 * so iwl_trans_tx() would return -EIO for each frame. Avoid the
993 * overhead of dequeuing from mac80211 only to immediately free
994 * the skbs, and the potential memory pressure from rapid skb
995 * allocation churn during high-throughput restart scenarios.
996 */
997 if (unlikely(mld->fw_status.in_hw_restart))
998 return;
999
1000 /*
1001 * No need for threads to be pending here, they can leave the first
1002 * taker all the work.
1003 *
1004 * mld_txq->tx_request logic:
1005 *
1006 * If 0, no one is currently TXing, set to 1 to indicate current thread
1007 * will now start TX and other threads should quit.
1008 *
1009 * If 1, another thread is currently TXing, set to 2 to indicate to
1010 * that thread that there was another request. Since that request may
1011 * have raced with the check whether the queue is empty, the TXing
1012 * thread should check the queue's status one more time before leaving.
1013 * This check is done in order to not leave any TX hanging in the queue
1014 * until the next TX invocation (which may not even happen).
1015 *
1016 * If 2, another thread is currently TXing, and it will already double
1017 * check the queue, so do nothing.
1018 */
1019 if (atomic_fetch_add_unless(&mld_txq->tx_request, 1, 2))
1020 return;
1021
1022 rcu_read_lock();
1023 do {
1024 while (likely(!mld_txq->status.stop_full) &&
1025 (skb = ieee80211_tx_dequeue(mld->hw, txq)))
1026 iwl_mld_tx_skb(mld, skb, txq);
1027 } while (atomic_dec_return(&mld_txq->tx_request));
1028
1029 IWL_DEBUG_TX(mld, "TXQ of sta %pM tid %d is now empty\n",
1030 txq->sta ? txq->sta->addr : zero_addr, txq->tid);
1031
1032 rcu_read_unlock();
1033 }
1034
iwl_mld_hwrate_to_tx_rate(u32 rate_n_flags,struct ieee80211_tx_info * info)1035 static void iwl_mld_hwrate_to_tx_rate(u32 rate_n_flags,
1036 struct ieee80211_tx_info *info)
1037 {
1038 enum nl80211_band band = info->band;
1039 struct ieee80211_tx_rate *tx_rate = &info->status.rates[0];
1040 u32 sgi = rate_n_flags & RATE_MCS_SGI_MSK;
1041 u32 chan_width = rate_n_flags & RATE_MCS_CHAN_WIDTH_MSK;
1042 u32 format = rate_n_flags & RATE_MCS_MOD_TYPE_MSK;
1043
1044 if (sgi)
1045 tx_rate->flags |= IEEE80211_TX_RC_SHORT_GI;
1046
1047 switch (chan_width) {
1048 case RATE_MCS_CHAN_WIDTH_20:
1049 break;
1050 case RATE_MCS_CHAN_WIDTH_40:
1051 tx_rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
1052 break;
1053 case RATE_MCS_CHAN_WIDTH_80:
1054 tx_rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
1055 break;
1056 case RATE_MCS_CHAN_WIDTH_160:
1057 tx_rate->flags |= IEEE80211_TX_RC_160_MHZ_WIDTH;
1058 break;
1059 default:
1060 break;
1061 }
1062
1063 switch (format) {
1064 case RATE_MCS_MOD_TYPE_HT:
1065 tx_rate->flags |= IEEE80211_TX_RC_MCS;
1066 tx_rate->idx = RATE_HT_MCS_INDEX(rate_n_flags);
1067 break;
1068 case RATE_MCS_MOD_TYPE_VHT:
1069 ieee80211_rate_set_vht(tx_rate,
1070 rate_n_flags & RATE_MCS_CODE_MSK,
1071 u32_get_bits(rate_n_flags,
1072 RATE_MCS_NSS_MSK) + 1);
1073 tx_rate->flags |= IEEE80211_TX_RC_VHT_MCS;
1074 break;
1075 case RATE_MCS_MOD_TYPE_HE:
1076 case RATE_MCS_MOD_TYPE_EHT:
1077 /* mac80211 cannot do this without ieee80211_tx_status_ext()
1078 * but it only matters for radiotap
1079 */
1080 tx_rate->idx = 0;
1081 break;
1082 default:
1083 tx_rate->idx =
1084 iwl_mld_legacy_hw_idx_to_mac80211_idx(rate_n_flags,
1085 band);
1086 break;
1087 }
1088 }
1089
iwl_mld_handle_tx_resp_notif(struct iwl_mld * mld,struct iwl_rx_packet * pkt)1090 void iwl_mld_handle_tx_resp_notif(struct iwl_mld *mld,
1091 struct iwl_rx_packet *pkt)
1092 {
1093 struct iwl_tx_resp *tx_resp = (void *)pkt->data;
1094 int txq_id = le16_to_cpu(tx_resp->tx_queue);
1095 struct agg_tx_status *agg_status = &tx_resp->status;
1096 u32 status = le16_to_cpu(agg_status->status);
1097 u32 pkt_len = iwl_rx_packet_payload_len(pkt);
1098 size_t notif_size = sizeof(*tx_resp) + sizeof(u32);
1099 int sta_id = IWL_TX_RES_GET_RA(tx_resp->ra_tid);
1100 int tid = IWL_TX_RES_GET_TID(tx_resp->ra_tid);
1101 struct ieee80211_link_sta *link_sta;
1102 struct iwl_mld_sta *mld_sta;
1103 u16 ssn;
1104 struct sk_buff_head skbs;
1105 u8 skb_freed = 0;
1106 bool mgmt = false;
1107 bool tx_failure = (status & TX_STATUS_MSK) != TX_STATUS_SUCCESS;
1108
1109 if (IWL_FW_CHECK(mld, tx_resp->frame_count != 1,
1110 "Invalid tx_resp notif frame_count (%d)\n",
1111 tx_resp->frame_count))
1112 return;
1113
1114 /* validate the size of the variable part of the notif */
1115 if (IWL_FW_CHECK(mld, notif_size != pkt_len,
1116 "Invalid tx_resp notif size (expected=%zu got=%u)\n",
1117 notif_size, pkt_len))
1118 return;
1119
1120 ssn = le32_to_cpup((__le32 *)agg_status +
1121 tx_resp->frame_count) & 0xFFFF;
1122
1123 if (IWL_FW_CHECK(mld, txq_id >= ARRAY_SIZE(mld->fw_id_to_txq),
1124 "Invalid txq id %d\n", txq_id))
1125 return;
1126
1127 __skb_queue_head_init(&skbs);
1128
1129 /* we can free until ssn % q.n_bd not inclusive */
1130 iwl_trans_reclaim(mld->trans, txq_id, ssn, &skbs, false);
1131
1132 while (!skb_queue_empty(&skbs)) {
1133 struct sk_buff *skb = __skb_dequeue(&skbs);
1134 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1135 struct ieee80211_hdr *hdr = (void *)skb->data;
1136
1137 skb_freed++;
1138
1139 iwl_trans_free_tx_cmd(mld->trans, info->driver_data[1]);
1140
1141 memset(&info->status, 0, sizeof(info->status));
1142
1143 info->flags &= ~(IEEE80211_TX_STAT_ACK | IEEE80211_TX_STAT_TX_FILTERED);
1144
1145 /* inform mac80211 about what happened with the frame */
1146 switch (status & TX_STATUS_MSK) {
1147 case TX_STATUS_SUCCESS:
1148 case TX_STATUS_DIRECT_DONE:
1149 info->flags |= IEEE80211_TX_STAT_ACK;
1150 break;
1151 default:
1152 break;
1153 }
1154
1155 /* If we are freeing multiple frames, mark all the frames
1156 * but the first one as acked, since they were acknowledged
1157 * before
1158 */
1159 if (skb_freed > 1)
1160 info->flags |= IEEE80211_TX_STAT_ACK;
1161
1162 if (tx_failure) {
1163 enum iwl_fw_ini_time_point tp =
1164 IWL_FW_INI_TIME_POINT_TX_FAILED;
1165
1166 if (ieee80211_is_action(hdr->frame_control))
1167 tp = IWL_FW_INI_TIME_POINT_TX_WFD_ACTION_FRAME_FAILED;
1168 else if (ieee80211_is_mgmt(hdr->frame_control))
1169 mgmt = true;
1170
1171 iwl_dbg_tlv_time_point(&mld->fwrt, tp, NULL);
1172 }
1173
1174 iwl_mld_hwrate_to_tx_rate(le32_to_cpu(tx_resp->initial_rate),
1175 info);
1176
1177 if (likely(!iwl_mld_time_sync_frame(mld, skb, hdr->addr1)))
1178 ieee80211_tx_status_skb(mld->hw, skb);
1179 }
1180
1181 IWL_DEBUG_TX_REPLY(mld,
1182 "TXQ %d status 0x%08x ssn=%d initial_rate 0x%x retries %d\n",
1183 txq_id, status, ssn, le32_to_cpu(tx_resp->initial_rate),
1184 tx_resp->failure_frame);
1185
1186 if (tx_failure && mgmt)
1187 iwl_mld_toggle_tx_ant(mld, &mld->mgmt_tx_ant);
1188
1189 if (IWL_FW_CHECK(mld, sta_id >= mld->fw->ucode_capa.num_stations,
1190 "Got invalid sta_id (%d)\n", sta_id))
1191 return;
1192
1193 rcu_read_lock();
1194
1195 link_sta = rcu_dereference(mld->fw_id_to_link_sta[sta_id]);
1196 if (!link_sta) {
1197 /* This can happen if the TX cmd was sent before pre_rcu_remove
1198 * but the TX response was received after
1199 */
1200 IWL_DEBUG_TX_REPLY(mld,
1201 "Got valid sta_id (%d) but sta is NULL\n",
1202 sta_id);
1203 goto out;
1204 }
1205
1206 if (IS_ERR(link_sta))
1207 goto out;
1208
1209 mld_sta = iwl_mld_sta_from_mac80211(link_sta->sta);
1210
1211 if (tx_failure && mld_sta->sta_state < IEEE80211_STA_AUTHORIZED)
1212 iwl_mld_toggle_tx_ant(mld, &mld_sta->data_tx_ant);
1213
1214 if (tid < IWL_MAX_TID_COUNT)
1215 iwl_mld_count_mpdu_tx(link_sta, 1);
1216
1217 out:
1218 rcu_read_unlock();
1219 }
1220
iwl_mld_tx_reclaim_txq(struct iwl_mld * mld,int txq,int index,bool in_flush)1221 static void iwl_mld_tx_reclaim_txq(struct iwl_mld *mld, int txq, int index,
1222 bool in_flush)
1223 {
1224 struct sk_buff_head reclaimed_skbs;
1225
1226 __skb_queue_head_init(&reclaimed_skbs);
1227
1228 iwl_trans_reclaim(mld->trans, txq, index, &reclaimed_skbs, in_flush);
1229
1230 while (!skb_queue_empty(&reclaimed_skbs)) {
1231 struct sk_buff *skb = __skb_dequeue(&reclaimed_skbs);
1232 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1233
1234 iwl_trans_free_tx_cmd(mld->trans, info->driver_data[1]);
1235
1236 memset(&info->status, 0, sizeof(info->status));
1237
1238 /* Packet was transmitted successfully, failures come as single
1239 * frames because before failing a frame the firmware transmits
1240 * it without aggregation at least once.
1241 */
1242 if (!in_flush)
1243 info->flags |= IEEE80211_TX_STAT_ACK;
1244 else
1245 info->flags &= ~IEEE80211_TX_STAT_ACK;
1246
1247 ieee80211_tx_status_skb(mld->hw, skb);
1248 }
1249 }
1250
iwl_mld_flush_link_sta_txqs(struct iwl_mld * mld,u32 fw_sta_id)1251 int iwl_mld_flush_link_sta_txqs(struct iwl_mld *mld, u32 fw_sta_id)
1252 {
1253 struct iwl_tx_path_flush_cmd_rsp *rsp;
1254 struct iwl_tx_path_flush_cmd flush_cmd = {
1255 .sta_id = cpu_to_le32(fw_sta_id),
1256 .tid_mask = cpu_to_le16(0xffff),
1257 };
1258 struct iwl_host_cmd cmd = {
1259 .id = TXPATH_FLUSH,
1260 .len = { sizeof(flush_cmd), },
1261 .data = { &flush_cmd, },
1262 .flags = CMD_WANT_SKB,
1263 };
1264 int ret, num_flushed_queues;
1265 u32 resp_len;
1266
1267 IWL_DEBUG_TX_QUEUES(mld, "flush for sta id %d tid mask 0x%x\n",
1268 fw_sta_id, 0xffff);
1269
1270 ret = iwl_mld_send_cmd(mld, &cmd);
1271 if (ret) {
1272 IWL_ERR(mld, "Failed to send flush command (%d)\n", ret);
1273 return ret;
1274 }
1275
1276 resp_len = iwl_rx_packet_payload_len(cmd.resp_pkt);
1277 if (IWL_FW_CHECK(mld, resp_len != sizeof(*rsp),
1278 "Invalid TXPATH_FLUSH response len: %d\n",
1279 resp_len)) {
1280 ret = -EIO;
1281 goto free_rsp;
1282 }
1283
1284 rsp = (void *)cmd.resp_pkt->data;
1285
1286 if (IWL_FW_CHECK(mld, le16_to_cpu(rsp->sta_id) != fw_sta_id,
1287 "sta_id %d != rsp_sta_id %d\n", fw_sta_id,
1288 le16_to_cpu(rsp->sta_id))) {
1289 ret = -EIO;
1290 goto free_rsp;
1291 }
1292
1293 num_flushed_queues = le16_to_cpu(rsp->num_flushed_queues);
1294 if (IWL_FW_CHECK(mld, num_flushed_queues > IWL_TX_FLUSH_QUEUE_RSP,
1295 "num_flushed_queues %d\n", num_flushed_queues)) {
1296 ret = -EIO;
1297 goto free_rsp;
1298 }
1299
1300 for (int i = 0; i < num_flushed_queues; i++) {
1301 struct iwl_flush_queue_info *queue_info = &rsp->queues[i];
1302 int read_after = le16_to_cpu(queue_info->read_after_flush);
1303 int txq_id = le16_to_cpu(queue_info->queue_num);
1304
1305 if (IWL_FW_CHECK(mld,
1306 txq_id >= ARRAY_SIZE(mld->fw_id_to_txq),
1307 "Invalid txq id %d\n", txq_id))
1308 continue;
1309
1310 IWL_DEBUG_TX_QUEUES(mld,
1311 "tid %d txq_id %d read-before %d read-after %d\n",
1312 le16_to_cpu(queue_info->tid), txq_id,
1313 le16_to_cpu(queue_info->read_before_flush),
1314 read_after);
1315
1316 iwl_mld_tx_reclaim_txq(mld, txq_id, read_after, true);
1317 }
1318
1319 free_rsp:
1320 iwl_free_resp(&cmd);
1321 return ret;
1322 }
1323
iwl_mld_ensure_queue(struct iwl_mld * mld,struct ieee80211_txq * txq)1324 int iwl_mld_ensure_queue(struct iwl_mld *mld, struct ieee80211_txq *txq)
1325 {
1326 struct iwl_mld_txq *mld_txq = iwl_mld_txq_from_mac80211(txq);
1327 int ret;
1328
1329 lockdep_assert_wiphy(mld->wiphy);
1330
1331 if (likely(mld_txq->status.allocated))
1332 return 0;
1333
1334 ret = iwl_mld_add_txq(mld, txq);
1335
1336 spin_lock_bh(&mld->add_txqs_lock);
1337 if (!list_empty(&mld_txq->list))
1338 list_del_init(&mld_txq->list);
1339 spin_unlock_bh(&mld->add_txqs_lock);
1340
1341 return ret;
1342 }
1343
iwl_mld_update_sta_txqs(struct iwl_mld * mld,struct ieee80211_sta * sta,u32 old_sta_mask,u32 new_sta_mask)1344 int iwl_mld_update_sta_txqs(struct iwl_mld *mld,
1345 struct ieee80211_sta *sta,
1346 u32 old_sta_mask, u32 new_sta_mask)
1347 {
1348 struct iwl_scd_queue_cfg_cmd cmd = {
1349 .operation = cpu_to_le32(IWL_SCD_QUEUE_MODIFY),
1350 .u.modify.old_sta_mask = cpu_to_le32(old_sta_mask),
1351 .u.modify.new_sta_mask = cpu_to_le32(new_sta_mask),
1352 };
1353
1354 lockdep_assert_wiphy(mld->wiphy);
1355
1356 for (int tid = 0; tid <= IWL_MAX_TID_COUNT; tid++) {
1357 struct ieee80211_txq *txq =
1358 sta->txq[tid != IWL_MAX_TID_COUNT ?
1359 tid : IEEE80211_NUM_TIDS];
1360 struct iwl_mld_txq *mld_txq =
1361 iwl_mld_txq_from_mac80211(txq);
1362 int ret;
1363
1364 if (!mld_txq->status.allocated)
1365 continue;
1366
1367 if (tid == IWL_MAX_TID_COUNT)
1368 cmd.u.modify.tid = cpu_to_le32(IWL_MGMT_TID);
1369 else
1370 cmd.u.modify.tid = cpu_to_le32(tid);
1371
1372 ret = iwl_mld_send_cmd_pdu(mld,
1373 WIDE_ID(DATA_PATH_GROUP,
1374 SCD_QUEUE_CONFIG_CMD),
1375 &cmd);
1376 if (ret)
1377 return ret;
1378 }
1379
1380 return 0;
1381 }
1382
iwl_mld_handle_compressed_ba_notif(struct iwl_mld * mld,struct iwl_rx_packet * pkt)1383 void iwl_mld_handle_compressed_ba_notif(struct iwl_mld *mld,
1384 struct iwl_rx_packet *pkt)
1385 {
1386 struct iwl_compressed_ba_notif *ba_res = (void *)pkt->data;
1387 u32 pkt_len = iwl_rx_packet_payload_len(pkt);
1388 u16 tfd_cnt = le16_to_cpu(ba_res->tfd_cnt);
1389 u8 sta_id = ba_res->sta_id;
1390 struct ieee80211_link_sta *link_sta;
1391
1392 if (!tfd_cnt)
1393 return;
1394
1395 if (IWL_FW_CHECK(mld, struct_size(ba_res, tfd, tfd_cnt) > pkt_len,
1396 "Short BA notif (tfd_cnt=%d, size:0x%x)\n",
1397 tfd_cnt, pkt_len))
1398 return;
1399
1400 IWL_DEBUG_TX_REPLY(mld,
1401 "BA notif received from sta_id=%d, flags=0x%x, sent:%d, acked:%d\n",
1402 sta_id, le32_to_cpu(ba_res->flags),
1403 le16_to_cpu(ba_res->txed),
1404 le16_to_cpu(ba_res->done));
1405
1406 for (int i = 0; i < tfd_cnt; i++) {
1407 struct iwl_compressed_ba_tfd *ba_tfd = &ba_res->tfd[i];
1408 int txq_id = le16_to_cpu(ba_tfd->q_num);
1409 int index = le16_to_cpu(ba_tfd->tfd_index);
1410
1411 if (IWL_FW_CHECK(mld,
1412 txq_id >= ARRAY_SIZE(mld->fw_id_to_txq),
1413 "Invalid txq id %d\n", txq_id))
1414 continue;
1415
1416 iwl_mld_tx_reclaim_txq(mld, txq_id, index, false);
1417 }
1418
1419 if (IWL_FW_CHECK(mld, sta_id >= mld->fw->ucode_capa.num_stations,
1420 "Got invalid sta_id (%d)\n", sta_id))
1421 return;
1422
1423 rcu_read_lock();
1424
1425 link_sta = rcu_dereference(mld->fw_id_to_link_sta[sta_id]);
1426 if (IWL_FW_CHECK(mld, IS_ERR_OR_NULL(link_sta),
1427 "Got valid sta_id (%d) but link_sta is NULL\n",
1428 sta_id))
1429 goto out;
1430
1431 iwl_mld_count_mpdu_tx(link_sta, le16_to_cpu(ba_res->txed));
1432 out:
1433 rcu_read_unlock();
1434 }
1435