tx.c

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