aboutsummaryrefslogtreecommitdiff
path: root/sys/contrib/dev/iwlwifi/mld/tx.c
diff options
context:
space:
mode:
Diffstat (limited to 'sys/contrib/dev/iwlwifi/mld/tx.c')
-rw-r--r--sys/contrib/dev/iwlwifi/mld/tx.c1394
1 files changed, 1394 insertions, 0 deletions
diff --git a/sys/contrib/dev/iwlwifi/mld/tx.c b/sys/contrib/dev/iwlwifi/mld/tx.c
new file mode 100644
index 000000000000..3b4b575aadaa
--- /dev/null
+++ b/sys/contrib/dev/iwlwifi/mld/tx.c
@@ -0,0 +1,1394 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * Copyright (C) 2024 - 2025 Intel Corporation
+ */
+#include <net/ip.h>
+
+#include "tx.h"
+#include "sta.h"
+#include "hcmd.h"
+#include "iwl-utils.h"
+#include "iface.h"
+
+#include "fw/dbg.h"
+
+#include "fw/api/tx.h"
+#include "fw/api/rs.h"
+#include "fw/api/txq.h"
+#include "fw/api/datapath.h"
+#include "fw/api/time-event.h"
+
+#define MAX_ANT_NUM 2
+
+/* Toggles between TX antennas. Receives the bitmask of valid TX antennas and
+ * the *index* used for the last TX, and returns the next valid *index* to use.
+ * In order to set it in the tx_cmd, must do BIT(idx).
+ */
+static u8 iwl_mld_next_ant(u8 valid, u8 last_idx)
+{
+ u8 index = last_idx;
+
+ for (int i = 0; i < MAX_ANT_NUM; i++) {
+ index = (index + 1) % MAX_ANT_NUM;
+ if (valid & BIT(index))
+ return index;
+ }
+
+ WARN_ONCE(1, "Failed to toggle between antennas 0x%x", valid);
+
+ return last_idx;
+}
+
+void iwl_mld_toggle_tx_ant(struct iwl_mld *mld, u8 *ant)
+{
+ *ant = iwl_mld_next_ant(iwl_mld_get_valid_tx_ant(mld), *ant);
+}
+
+static int
+iwl_mld_get_queue_size(struct iwl_mld *mld, struct ieee80211_txq *txq)
+{
+ struct ieee80211_sta *sta = txq->sta;
+ struct ieee80211_link_sta *link_sta;
+ unsigned int link_id;
+ int max_size = IWL_DEFAULT_QUEUE_SIZE;
+
+ lockdep_assert_wiphy(mld->wiphy);
+
+ for_each_sta_active_link(txq->vif, sta, link_sta, link_id) {
+ if (link_sta->eht_cap.has_eht) {
+ max_size = IWL_DEFAULT_QUEUE_SIZE_EHT;
+ break;
+ }
+
+ if (link_sta->he_cap.has_he)
+ max_size = IWL_DEFAULT_QUEUE_SIZE_HE;
+ }
+
+ return max_size;
+}
+
+static int iwl_mld_allocate_txq(struct iwl_mld *mld, struct ieee80211_txq *txq)
+{
+ u8 tid = txq->tid == IEEE80211_NUM_TIDS ? IWL_MGMT_TID : txq->tid;
+ u32 fw_sta_mask = iwl_mld_fw_sta_id_mask(mld, txq->sta);
+ /* We can't know when the station is asleep or awake, so we
+ * must disable the queue hang detection.
+ */
+ unsigned int watchdog_timeout = txq->vif->type == NL80211_IFTYPE_AP ?
+ IWL_WATCHDOG_DISABLED :
+ mld->trans->mac_cfg->base->wd_timeout;
+ int queue, size;
+
+ lockdep_assert_wiphy(mld->wiphy);
+
+ if (tid == IWL_MGMT_TID)
+ size = max_t(u32, IWL_MGMT_QUEUE_SIZE,
+ mld->trans->mac_cfg->base->min_txq_size);
+ else
+ size = iwl_mld_get_queue_size(mld, txq);
+
+ queue = iwl_trans_txq_alloc(mld->trans, 0, fw_sta_mask, tid, size,
+ watchdog_timeout);
+
+ if (queue >= 0)
+ IWL_DEBUG_TX_QUEUES(mld,
+ "Enabling TXQ #%d for sta mask 0x%x tid %d\n",
+ queue, fw_sta_mask, tid);
+ return queue;
+}
+
+static int iwl_mld_add_txq(struct iwl_mld *mld, struct ieee80211_txq *txq)
+{
+ struct iwl_mld_txq *mld_txq = iwl_mld_txq_from_mac80211(txq);
+ int id;
+
+ lockdep_assert_wiphy(mld->wiphy);
+
+ /* This will alse send the SCD_QUEUE_CONFIG_CMD */
+ id = iwl_mld_allocate_txq(mld, txq);
+ if (id < 0)
+ return id;
+
+ mld_txq->fw_id = id;
+ mld_txq->status.allocated = true;
+
+ rcu_assign_pointer(mld->fw_id_to_txq[id], txq);
+
+ return 0;
+}
+
+void iwl_mld_add_txq_list(struct iwl_mld *mld)
+{
+ lockdep_assert_wiphy(mld->wiphy);
+
+ while (!list_empty(&mld->txqs_to_add)) {
+ struct ieee80211_txq *txq;
+ struct iwl_mld_txq *mld_txq =
+ list_first_entry(&mld->txqs_to_add, struct iwl_mld_txq,
+ list);
+ int failed;
+
+ txq = container_of((void *)mld_txq, struct ieee80211_txq,
+ drv_priv);
+
+ failed = iwl_mld_add_txq(mld, txq);
+
+ local_bh_disable();
+ spin_lock(&mld->add_txqs_lock);
+ list_del_init(&mld_txq->list);
+ spin_unlock(&mld->add_txqs_lock);
+ /* If the queue allocation failed, we can't transmit. Leave the
+ * frames on the txq, maybe the attempt to allocate the queue
+ * will succeed.
+ */
+ if (!failed)
+ iwl_mld_tx_from_txq(mld, txq);
+ local_bh_enable();
+ }
+}
+
+void iwl_mld_add_txqs_wk(struct wiphy *wiphy, struct wiphy_work *wk)
+{
+ struct iwl_mld *mld = container_of(wk, struct iwl_mld,
+ add_txqs_wk);
+
+ /* will reschedule to run after restart */
+ if (mld->fw_status.in_hw_restart)
+ return;
+
+ iwl_mld_add_txq_list(mld);
+}
+
+void
+iwl_mld_free_txq(struct iwl_mld *mld, u32 fw_sta_mask, u32 tid, u32 queue_id)
+{
+ struct iwl_scd_queue_cfg_cmd remove_cmd = {
+ .operation = cpu_to_le32(IWL_SCD_QUEUE_REMOVE),
+ .u.remove.tid = cpu_to_le32(tid),
+ .u.remove.sta_mask = cpu_to_le32(fw_sta_mask),
+ };
+
+ iwl_mld_send_cmd_pdu(mld,
+ WIDE_ID(DATA_PATH_GROUP, SCD_QUEUE_CONFIG_CMD),
+ &remove_cmd);
+
+ iwl_trans_txq_free(mld->trans, queue_id);
+}
+
+void iwl_mld_remove_txq(struct iwl_mld *mld, struct ieee80211_txq *txq)
+{
+ struct iwl_mld_txq *mld_txq = iwl_mld_txq_from_mac80211(txq);
+ u32 sta_msk, tid;
+
+ lockdep_assert_wiphy(mld->wiphy);
+
+ spin_lock_bh(&mld->add_txqs_lock);
+ if (!list_empty(&mld_txq->list))
+ list_del_init(&mld_txq->list);
+ spin_unlock_bh(&mld->add_txqs_lock);
+
+ if (!mld_txq->status.allocated ||
+ WARN_ON(mld_txq->fw_id >= ARRAY_SIZE(mld->fw_id_to_txq)))
+ return;
+
+ sta_msk = iwl_mld_fw_sta_id_mask(mld, txq->sta);
+
+ tid = txq->tid == IEEE80211_NUM_TIDS ? IWL_MGMT_TID :
+ txq->tid;
+
+ iwl_mld_free_txq(mld, sta_msk, tid, mld_txq->fw_id);
+
+ RCU_INIT_POINTER(mld->fw_id_to_txq[mld_txq->fw_id], NULL);
+ mld_txq->status.allocated = false;
+}
+
+#define OPT_HDR(type, skb, off) \
+ (type *)(skb_network_header(skb) + (off))
+
+static __le32
+iwl_mld_get_offload_assist(struct sk_buff *skb, bool amsdu)
+{
+ struct ieee80211_hdr *hdr = (void *)skb->data;
+ u16 mh_len = ieee80211_hdrlen(hdr->frame_control);
+ u16 offload_assist = 0;
+#if IS_ENABLED(CONFIG_INET)
+ u8 protocol = 0;
+
+ /* Do not compute checksum if already computed */
+ if (skb->ip_summed != CHECKSUM_PARTIAL)
+ goto out;
+
+ /* We do not expect to be requested to csum stuff we do not support */
+
+ /* TBD: do we also need to check
+ * !(mvm->hw->netdev_features & IWL_TX_CSUM_NETIF_FLAGS) now that all
+ * the devices we support has this flags?
+ */
+ if (WARN_ONCE(skb->protocol != htons(ETH_P_IP) &&
+ skb->protocol != htons(ETH_P_IPV6),
+ "No support for requested checksum\n")) {
+ skb_checksum_help(skb);
+ goto out;
+ }
+
+ if (skb->protocol == htons(ETH_P_IP)) {
+ protocol = ip_hdr(skb)->protocol;
+ } else {
+#if IS_ENABLED(CONFIG_IPV6)
+ struct ipv6hdr *ipv6h =
+ (struct ipv6hdr *)skb_network_header(skb);
+ unsigned int off = sizeof(*ipv6h);
+
+ protocol = ipv6h->nexthdr;
+ while (protocol != NEXTHDR_NONE && ipv6_ext_hdr(protocol)) {
+ struct ipv6_opt_hdr *hp;
+
+ /* only supported extension headers */
+ if (protocol != NEXTHDR_ROUTING &&
+ protocol != NEXTHDR_HOP &&
+ protocol != NEXTHDR_DEST) {
+ skb_checksum_help(skb);
+ goto out;
+ }
+
+ hp = OPT_HDR(struct ipv6_opt_hdr, skb, off);
+ protocol = hp->nexthdr;
+ off += ipv6_optlen(hp);
+ }
+ /* if we get here - protocol now should be TCP/UDP */
+#endif
+ }
+
+ if (protocol != IPPROTO_TCP && protocol != IPPROTO_UDP) {
+ WARN_ON_ONCE(1);
+ skb_checksum_help(skb);
+ goto out;
+ }
+
+ /* enable L4 csum */
+ offload_assist |= BIT(TX_CMD_OFFLD_L4_EN);
+
+ /* Set offset to IP header (snap).
+ * We don't support tunneling so no need to take care of inner header.
+ * Size is in words.
+ */
+ offload_assist |= (4 << TX_CMD_OFFLD_IP_HDR);
+
+ /* Do IPv4 csum for AMSDU only (no IP csum for Ipv6) */
+ if (skb->protocol == htons(ETH_P_IP) && amsdu) {
+ ip_hdr(skb)->check = 0;
+ offload_assist |= BIT(TX_CMD_OFFLD_L3_EN);
+ }
+
+ /* reset UDP/TCP header csum */
+ if (protocol == IPPROTO_TCP)
+ tcp_hdr(skb)->check = 0;
+ else
+ udp_hdr(skb)->check = 0;
+
+out:
+#endif
+ mh_len /= 2;
+ offload_assist |= mh_len << TX_CMD_OFFLD_MH_SIZE;
+
+ if (amsdu)
+ offload_assist |= BIT(TX_CMD_OFFLD_AMSDU);
+ else if (ieee80211_hdrlen(hdr->frame_control) % 4)
+ /* padding is inserted later in transport */
+ offload_assist |= BIT(TX_CMD_OFFLD_PAD);
+
+ return cpu_to_le32(offload_assist);
+}
+
+static void 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)
+{
+ u32 link_id = u32_get_bits(info->control.flags,
+ IEEE80211_TX_CTRL_MLO_LINK);
+
+ *basic_rates = vif->bss_conf.basic_rates;
+ *band = info->band;
+
+ if (link_id == IEEE80211_LINK_UNSPECIFIED &&
+ ieee80211_vif_is_mld(vif)) {
+ /* shouldn't do this when >1 link is active */
+ WARN_ON(hweight16(vif->active_links) != 1);
+ link_id = __ffs(vif->active_links);
+ }
+
+ if (link_id < IEEE80211_LINK_UNSPECIFIED) {
+ struct ieee80211_bss_conf *link_conf;
+
+ rcu_read_lock();
+ link_conf = rcu_dereference(vif->link_conf[link_id]);
+ if (link_conf) {
+ *basic_rates = link_conf->basic_rates;
+ if (link_conf->chanreq.oper.chan)
+ *band = link_conf->chanreq.oper.chan->band;
+ }
+ rcu_read_unlock();
+ }
+}
+
+u8 iwl_mld_get_lowest_rate(struct iwl_mld *mld,
+ struct ieee80211_tx_info *info,
+ struct ieee80211_vif *vif)
+{
+ struct ieee80211_supported_band *sband;
+ u16 lowest_cck = IWL_RATE_COUNT, lowest_ofdm = IWL_RATE_COUNT;
+ unsigned long basic_rates;
+ u8 band, rate;
+ u32 i;
+
+ iwl_mld_get_basic_rates_and_band(mld, vif, info, &basic_rates, &band);
+
+ sband = mld->hw->wiphy->bands[band];
+ for_each_set_bit(i, &basic_rates, BITS_PER_LONG) {
+ u16 hw = sband->bitrates[i].hw_value;
+
+ if (hw >= IWL_FIRST_OFDM_RATE) {
+ if (lowest_ofdm > hw)
+ lowest_ofdm = hw;
+ } else if (lowest_cck > hw) {
+ lowest_cck = hw;
+ }
+ }
+
+ if (band == NL80211_BAND_2GHZ && !vif->p2p &&
+ vif->type != NL80211_IFTYPE_P2P_DEVICE &&
+ !(info->flags & IEEE80211_TX_CTL_NO_CCK_RATE)) {
+ if (lowest_cck != IWL_RATE_COUNT)
+ rate = lowest_cck;
+ else if (lowest_ofdm != IWL_RATE_COUNT)
+ rate = lowest_ofdm;
+ else
+ rate = IWL_FIRST_CCK_RATE;
+ } else if (lowest_ofdm != IWL_RATE_COUNT) {
+ rate = lowest_ofdm;
+ } else {
+ rate = IWL_FIRST_OFDM_RATE;
+ }
+
+ return rate;
+}
+
+static u32 iwl_mld_mac80211_rate_idx_to_fw(struct iwl_mld *mld,
+ struct ieee80211_tx_info *info,
+ int rate_idx)
+{
+ u32 rate_flags = 0;
+ u8 rate_plcp;
+
+ /* if the rate isn't a well known legacy rate, take the lowest one */
+ if (rate_idx < 0 || rate_idx >= IWL_RATE_COUNT_LEGACY)
+ rate_idx = iwl_mld_get_lowest_rate(mld, info,
+ info->control.vif);
+
+ WARN_ON_ONCE(rate_idx < 0);
+
+ /* Set CCK or OFDM flag */
+ if (rate_idx <= IWL_LAST_CCK_RATE)
+ rate_flags |= RATE_MCS_MOD_TYPE_CCK;
+ else
+ rate_flags |= RATE_MCS_MOD_TYPE_LEGACY_OFDM;
+
+ /* Legacy rates are indexed:
+ * 0 - 3 for CCK and 0 - 7 for OFDM
+ */
+ rate_plcp = (rate_idx >= IWL_FIRST_OFDM_RATE ?
+ rate_idx - IWL_FIRST_OFDM_RATE : rate_idx);
+
+ return (u32)rate_plcp | rate_flags;
+}
+
+static u32 iwl_mld_get_tx_ant(struct iwl_mld *mld,
+ struct ieee80211_tx_info *info,
+ struct ieee80211_sta *sta, __le16 fc)
+{
+ if (sta && ieee80211_is_data(fc)) {
+ struct iwl_mld_sta *mld_sta = iwl_mld_sta_from_mac80211(sta);
+
+ return BIT(mld_sta->data_tx_ant) << RATE_MCS_ANT_POS;
+ }
+
+ return BIT(mld->mgmt_tx_ant) << RATE_MCS_ANT_POS;
+}
+
+static u32 iwl_mld_get_inject_tx_rate(struct iwl_mld *mld,
+ struct ieee80211_tx_info *info,
+ struct ieee80211_sta *sta,
+ __le16 fc)
+{
+ struct ieee80211_tx_rate *rate = &info->control.rates[0];
+ u32 result;
+
+ if (rate->flags & IEEE80211_TX_RC_VHT_MCS) {
+ u8 mcs = ieee80211_rate_get_vht_mcs(rate);
+ u8 nss = ieee80211_rate_get_vht_nss(rate);
+
+ result = RATE_MCS_MOD_TYPE_VHT;
+ result |= u32_encode_bits(mcs, RATE_MCS_CODE_MSK);
+ result |= u32_encode_bits(nss, RATE_MCS_NSS_MSK);
+
+ if (rate->flags & IEEE80211_TX_RC_SHORT_GI)
+ result |= RATE_MCS_SGI_MSK;
+
+ if (rate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
+ result |= RATE_MCS_CHAN_WIDTH_40;
+ else if (rate->flags & IEEE80211_TX_RC_80_MHZ_WIDTH)
+ result |= RATE_MCS_CHAN_WIDTH_80;
+ else if (rate->flags & IEEE80211_TX_RC_160_MHZ_WIDTH)
+ result |= RATE_MCS_CHAN_WIDTH_160;
+ } else if (rate->flags & IEEE80211_TX_RC_MCS) {
+ /* only MCS 0-15 are supported */
+ u8 mcs = rate->idx & 7;
+ u8 nss = rate->idx > 7;
+
+ result = RATE_MCS_MOD_TYPE_HT;
+ result |= u32_encode_bits(mcs, RATE_MCS_CODE_MSK);
+ result |= u32_encode_bits(nss, RATE_MCS_NSS_MSK);
+
+ if (rate->flags & IEEE80211_TX_RC_SHORT_GI)
+ result |= RATE_MCS_SGI_MSK;
+ if (rate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
+ result |= RATE_MCS_CHAN_WIDTH_40;
+ if (info->flags & IEEE80211_TX_CTL_LDPC)
+ result |= RATE_MCS_LDPC_MSK;
+ if (u32_get_bits(info->flags, IEEE80211_TX_CTL_STBC))
+ result |= RATE_MCS_STBC_MSK;
+ } else {
+ result = iwl_mld_mac80211_rate_idx_to_fw(mld, info, rate->idx);
+ }
+
+ if (info->control.antennas)
+ result |= u32_encode_bits(info->control.antennas,
+ RATE_MCS_ANT_AB_MSK);
+ else
+ result |= iwl_mld_get_tx_ant(mld, info, sta, fc);
+
+ return result;
+}
+
+static __le32 iwl_mld_get_tx_rate_n_flags(struct iwl_mld *mld,
+ struct ieee80211_tx_info *info,
+ struct ieee80211_sta *sta, __le16 fc)
+{
+ u32 rate;
+
+ if (unlikely(info->control.flags & IEEE80211_TX_CTRL_RATE_INJECT))
+ rate = iwl_mld_get_inject_tx_rate(mld, info, sta, fc);
+ else
+ rate = iwl_mld_mac80211_rate_idx_to_fw(mld, info, -1) |
+ iwl_mld_get_tx_ant(mld, info, sta, fc);
+
+ return iwl_v3_rate_to_v2_v3(rate, mld->fw_rates_ver_3);
+}
+
+static void
+iwl_mld_fill_tx_cmd_hdr(struct iwl_tx_cmd *tx_cmd,
+ struct sk_buff *skb, bool amsdu)
+{
+ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+ struct ieee80211_hdr *hdr = (void *)skb->data;
+ struct ieee80211_vif *vif;
+
+ /* Copy MAC header from skb into command buffer */
+ memcpy(tx_cmd->hdr, hdr, ieee80211_hdrlen(hdr->frame_control));
+
+ if (!amsdu || !skb_is_gso(skb))
+ return;
+
+ /* As described in IEEE sta 802.11-2020, table 9-30 (Address
+ * field contents), A-MSDU address 3 should contain the BSSID
+ * address.
+ *
+ * In TSO, the skb header address 3 contains the original address 3 to
+ * correctly create all the A-MSDU subframes headers from it.
+ * Override now the address 3 in the command header with the BSSID.
+ *
+ * Note: we fill in the MLD address, but the firmware will do the
+ * necessary translation to link address after encryption.
+ */
+ vif = info->control.vif;
+ switch (vif->type) {
+ case NL80211_IFTYPE_STATION:
+ ether_addr_copy(tx_cmd->hdr->addr3, vif->cfg.ap_addr);
+ break;
+ case NL80211_IFTYPE_AP:
+ ether_addr_copy(tx_cmd->hdr->addr3, vif->addr);
+ break;
+ default:
+ break;
+ }
+}
+
+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)
+{
+ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+ struct ieee80211_hdr *hdr = (void *)skb->data;
+ struct iwl_mld_sta *mld_sta = sta ? iwl_mld_sta_from_mac80211(sta) :
+ NULL;
+ struct iwl_tx_cmd *tx_cmd;
+ bool amsdu = ieee80211_is_data_qos(hdr->frame_control) &&
+ (*ieee80211_get_qos_ctl(hdr) &
+ IEEE80211_QOS_CTL_A_MSDU_PRESENT);
+ __le32 rate_n_flags = 0;
+ u16 flags = 0;
+
+ dev_tx_cmd->hdr.cmd = TX_CMD;
+
+ if (!info->control.hw_key)
+ flags |= IWL_TX_FLAGS_ENCRYPT_DIS;
+
+ /* For data and mgmt packets rate info comes from the fw.
+ * Only set rate/antenna for injected frames with fixed rate, or
+ * when no sta is given.
+ */
+ if (unlikely(!sta ||
+ info->control.flags & IEEE80211_TX_CTRL_RATE_INJECT)) {
+ flags |= IWL_TX_FLAGS_CMD_RATE;
+ rate_n_flags = iwl_mld_get_tx_rate_n_flags(mld, info, sta,
+ hdr->frame_control);
+ } else if (!ieee80211_is_data(hdr->frame_control) ||
+ (mld_sta &&
+ mld_sta->sta_state < IEEE80211_STA_AUTHORIZED)) {
+ /* These are important frames */
+ flags |= IWL_TX_FLAGS_HIGH_PRI;
+ }
+
+ tx_cmd = (void *)dev_tx_cmd->payload;
+
+ iwl_mld_fill_tx_cmd_hdr(tx_cmd, skb, amsdu);
+
+ tx_cmd->offload_assist = iwl_mld_get_offload_assist(skb, amsdu);
+
+ /* Total # bytes to be transmitted */
+ tx_cmd->len = cpu_to_le16((u16)skb->len);
+
+ tx_cmd->flags = cpu_to_le16(flags);
+
+ tx_cmd->rate_n_flags = rate_n_flags;
+}
+
+/* Caller of this need to check that info->control.vif is not NULL */
+static struct iwl_mld_link *
+iwl_mld_get_link_from_tx_info(struct ieee80211_tx_info *info)
+{
+ struct iwl_mld_vif *mld_vif =
+ iwl_mld_vif_from_mac80211(info->control.vif);
+ u32 link_id = u32_get_bits(info->control.flags,
+ IEEE80211_TX_CTRL_MLO_LINK);
+
+ if (link_id == IEEE80211_LINK_UNSPECIFIED) {
+ if (info->control.vif->active_links)
+ link_id = ffs(info->control.vif->active_links) - 1;
+ else
+ link_id = 0;
+ }
+
+ return rcu_dereference(mld_vif->link[link_id]);
+}
+
+static int
+iwl_mld_get_tx_queue_id(struct iwl_mld *mld, struct ieee80211_txq *txq,
+ struct sk_buff *skb)
+{
+ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+ struct ieee80211_hdr *hdr = (void *)skb->data;
+ __le16 fc = hdr->frame_control;
+ struct iwl_mld_vif *mld_vif;
+ struct iwl_mld_link *link;
+
+ if (txq && txq->sta)
+ return iwl_mld_txq_from_mac80211(txq)->fw_id;
+
+ if (!info->control.vif)
+ return IWL_MLD_INVALID_QUEUE;
+
+ switch (info->control.vif->type) {
+ case NL80211_IFTYPE_AP:
+ case NL80211_IFTYPE_ADHOC:
+ link = iwl_mld_get_link_from_tx_info(info);
+
+ if (WARN_ON(!link))
+ break;
+
+ /* ucast disassociate/deauth frames without a station might
+ * happen, especially with reason 7 ("Class 3 frame received
+ * from nonassociated STA").
+ */
+ if (ieee80211_is_mgmt(fc) &&
+ (!ieee80211_is_bufferable_mmpdu(skb) ||
+ ieee80211_is_deauth(fc) || ieee80211_is_disassoc(fc)))
+ return link->bcast_sta.queue_id;
+
+ if (is_multicast_ether_addr(hdr->addr1) &&
+ !ieee80211_has_order(fc))
+ return link->mcast_sta.queue_id;
+
+ WARN_ONCE(info->control.vif->type != NL80211_IFTYPE_ADHOC,
+ "Couldn't find a TXQ. fc=0x%02x", le16_to_cpu(fc));
+ return link->bcast_sta.queue_id;
+ case NL80211_IFTYPE_P2P_DEVICE:
+ mld_vif = iwl_mld_vif_from_mac80211(info->control.vif);
+
+ if (mld_vif->roc_activity != ROC_ACTIVITY_P2P_DISC &&
+ mld_vif->roc_activity != ROC_ACTIVITY_P2P_NEG) {
+ IWL_DEBUG_DROP(mld,
+ "Drop tx outside ROC with activity %d\n",
+ mld_vif->roc_activity);
+ return IWL_MLD_INVALID_DROP_TX;
+ }
+
+ WARN_ON(!ieee80211_is_mgmt(fc));
+
+ return mld_vif->aux_sta.queue_id;
+ case NL80211_IFTYPE_MONITOR:
+ mld_vif = iwl_mld_vif_from_mac80211(info->control.vif);
+ return mld_vif->deflink.mon_sta.queue_id;
+ case NL80211_IFTYPE_STATION:
+ mld_vif = iwl_mld_vif_from_mac80211(info->control.vif);
+
+ if (!(info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)) {
+ IWL_DEBUG_DROP(mld, "Drop tx not off-channel\n");
+ return IWL_MLD_INVALID_DROP_TX;
+ }
+
+ if (mld_vif->roc_activity != ROC_ACTIVITY_HOTSPOT) {
+ IWL_DEBUG_DROP(mld, "Drop tx outside ROC\n");
+ return IWL_MLD_INVALID_DROP_TX;
+ }
+
+ WARN_ON(!ieee80211_is_mgmt(fc));
+ return mld_vif->aux_sta.queue_id;
+ default:
+ WARN_ONCE(1, "Unsupported vif type\n");
+ break;
+ }
+
+ return IWL_MLD_INVALID_QUEUE;
+}
+
+static void iwl_mld_probe_resp_set_noa(struct iwl_mld *mld,
+ struct sk_buff *skb)
+{
+ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+ struct iwl_mld_link *mld_link =
+ &iwl_mld_vif_from_mac80211(info->control.vif)->deflink;
+ struct iwl_probe_resp_data *resp_data;
+ u8 *pos;
+
+ if (!info->control.vif->p2p)
+ return;
+
+ rcu_read_lock();
+
+ resp_data = rcu_dereference(mld_link->probe_resp_data);
+ if (!resp_data)
+ goto out;
+
+ if (!resp_data->notif.noa_active)
+ goto out;
+
+ if (skb_tailroom(skb) < resp_data->noa_len) {
+ if (pskb_expand_head(skb, 0, resp_data->noa_len, GFP_ATOMIC)) {
+ IWL_ERR(mld,
+ "Failed to reallocate probe resp\n");
+ goto out;
+ }
+ }
+
+ pos = skb_put(skb, resp_data->noa_len);
+
+ *pos++ = WLAN_EID_VENDOR_SPECIFIC;
+ /* Set length of IE body (not including ID and length itself) */
+ *pos++ = resp_data->noa_len - 2;
+ *pos++ = (WLAN_OUI_WFA >> 16) & 0xff;
+ *pos++ = (WLAN_OUI_WFA >> 8) & 0xff;
+ *pos++ = WLAN_OUI_WFA & 0xff;
+ *pos++ = WLAN_OUI_TYPE_WFA_P2P;
+
+ memcpy(pos, &resp_data->notif.noa_attr,
+ resp_data->noa_len - sizeof(struct ieee80211_vendor_ie));
+
+out:
+ rcu_read_unlock();
+}
+
+/* This function must be called with BHs disabled */
+static int iwl_mld_tx_mpdu(struct iwl_mld *mld, struct sk_buff *skb,
+ struct ieee80211_txq *txq)
+{
+ struct ieee80211_hdr *hdr = (void *)skb->data;
+ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+ struct ieee80211_sta *sta = txq ? txq->sta : NULL;
+ struct iwl_device_tx_cmd *dev_tx_cmd;
+ int queue = iwl_mld_get_tx_queue_id(mld, txq, skb);
+ u8 tid = IWL_MAX_TID_COUNT;
+
+ if (WARN_ONCE(queue == IWL_MLD_INVALID_QUEUE, "Invalid TX Queue id") ||
+ queue == IWL_MLD_INVALID_DROP_TX)
+ return -1;
+
+ if (unlikely(ieee80211_is_any_nullfunc(hdr->frame_control)))
+ return -1;
+
+ dev_tx_cmd = iwl_trans_alloc_tx_cmd(mld->trans);
+ if (unlikely(!dev_tx_cmd))
+ return -1;
+
+ if (unlikely(ieee80211_is_probe_resp(hdr->frame_control))) {
+ if (IWL_MLD_NON_TRANSMITTING_AP)
+ return -1;
+
+ iwl_mld_probe_resp_set_noa(mld, skb);
+ }
+
+ iwl_mld_fill_tx_cmd(mld, skb, dev_tx_cmd, sta);
+
+ if (ieee80211_is_data(hdr->frame_control)) {
+ if (ieee80211_is_data_qos(hdr->frame_control))
+ tid = ieee80211_get_tid(hdr);
+ else
+ tid = IWL_TID_NON_QOS;
+ }
+
+ IWL_DEBUG_TX(mld, "TX TID:%d from Q:%d len %d\n",
+ tid, queue, skb->len);
+
+ /* From now on, we cannot access info->control */
+ memset(&info->status, 0, sizeof(info->status));
+ memset(info->driver_data, 0, sizeof(info->driver_data));
+
+ info->driver_data[1] = dev_tx_cmd;
+
+ if (iwl_trans_tx(mld->trans, skb, dev_tx_cmd, queue))
+ goto err;
+
+ /* Update low-latency counter when a packet is queued instead
+ * of after TX, it makes sense for early low-latency detection
+ */
+ if (sta)
+ iwl_mld_low_latency_update_counters(mld, hdr, sta, 0);
+
+ return 0;
+
+err:
+ iwl_trans_free_tx_cmd(mld->trans, dev_tx_cmd);
+ IWL_DEBUG_TX(mld, "TX from Q:%d dropped\n", queue);
+ return -1;
+}
+
+#ifdef CONFIG_INET
+
+/* This function handles the segmentation of a large TSO packet into multiple
+ * MPDUs, ensuring that the resulting segments conform to AMSDU limits and
+ * constraints.
+ */
+static int iwl_mld_tx_tso_segment(struct iwl_mld *mld, struct sk_buff *skb,
+ struct ieee80211_sta *sta,
+ struct sk_buff_head *mpdus_skbs)
+{
+ struct ieee80211_hdr *hdr = (void *)skb->data;
+ netdev_features_t netdev_flags = NETIF_F_CSUM_MASK | NETIF_F_SG;
+ unsigned int mss = skb_shinfo(skb)->gso_size;
+ unsigned int num_subframes, tcp_payload_len, subf_len;
+ u16 snap_ip_tcp, pad, max_tid_amsdu_len;
+ u8 tid;
+
+ snap_ip_tcp = 8 + skb_network_header_len(skb) + tcp_hdrlen(skb);
+
+ if (!ieee80211_is_data_qos(hdr->frame_control) ||
+ !sta->cur->max_rc_amsdu_len)
+ return iwl_tx_tso_segment(skb, 1, netdev_flags, mpdus_skbs);
+
+ /* Do not build AMSDU for IPv6 with extension headers.
+ * Ask stack to segment and checksum the generated MPDUs for us.
+ */
+ if (skb->protocol == htons(ETH_P_IPV6) &&
+ ((struct ipv6hdr *)skb_network_header(skb))->nexthdr !=
+ IPPROTO_TCP) {
+ netdev_flags &= ~NETIF_F_CSUM_MASK;
+ return iwl_tx_tso_segment(skb, 1, netdev_flags, mpdus_skbs);
+ }
+
+ tid = ieee80211_get_tid(hdr);
+ if (WARN_ON_ONCE(tid >= IWL_MAX_TID_COUNT))
+ return -EINVAL;
+
+ max_tid_amsdu_len = sta->cur->max_tid_amsdu_len[tid];
+ if (!max_tid_amsdu_len)
+ return iwl_tx_tso_segment(skb, 1, netdev_flags, mpdus_skbs);
+
+ /* Sub frame header + SNAP + IP header + TCP header + MSS */
+ subf_len = sizeof(struct ethhdr) + snap_ip_tcp + mss;
+ pad = (4 - subf_len) & 0x3;
+
+ /* If we have N subframes in the A-MSDU, then the A-MSDU's size is
+ * N * subf_len + (N - 1) * pad.
+ */
+ num_subframes = (max_tid_amsdu_len + pad) / (subf_len + pad);
+
+ if (sta->max_amsdu_subframes &&
+ num_subframes > sta->max_amsdu_subframes)
+ num_subframes = sta->max_amsdu_subframes;
+
+ tcp_payload_len = skb_tail_pointer(skb) - skb_transport_header(skb) -
+ tcp_hdrlen(skb) + skb->data_len;
+
+ /* Make sure we have enough TBs for the A-MSDU:
+ * 2 for each subframe
+ * 1 more for each fragment
+ * 1 more for the potential data in the header
+ */
+ if ((num_subframes * 2 + skb_shinfo(skb)->nr_frags + 1) >
+ mld->trans->info.max_skb_frags)
+ num_subframes = 1;
+
+ if (num_subframes > 1)
+ *ieee80211_get_qos_ctl(hdr) |= IEEE80211_QOS_CTL_A_MSDU_PRESENT;
+
+ /* This skb fits in one single A-MSDU */
+ if (tcp_payload_len <= num_subframes * mss) {
+ __skb_queue_tail(mpdus_skbs, skb);
+ return 0;
+ }
+
+ /* Trick the segmentation function to make it create SKBs that can fit
+ * into one A-MSDU.
+ */
+ return iwl_tx_tso_segment(skb, num_subframes, netdev_flags, mpdus_skbs);
+}
+
+/* Manages TSO (TCP Segmentation Offload) packet transmission by segmenting
+ * large packets when necessary and transmitting each segment as MPDU.
+ */
+static int iwl_mld_tx_tso(struct iwl_mld *mld, struct sk_buff *skb,
+ struct ieee80211_txq *txq)
+{
+ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+ struct sk_buff *orig_skb = skb;
+ struct sk_buff_head mpdus_skbs;
+ unsigned int payload_len;
+ int ret;
+
+ if (WARN_ON(!txq || !txq->sta))
+ return -1;
+
+ payload_len = skb_tail_pointer(skb) - skb_transport_header(skb) -
+ tcp_hdrlen(skb) + skb->data_len;
+
+ if (payload_len <= skb_shinfo(skb)->gso_size)
+ return iwl_mld_tx_mpdu(mld, skb, txq);
+
+ if (!info->control.vif)
+ return -1;
+
+ __skb_queue_head_init(&mpdus_skbs);
+
+ ret = iwl_mld_tx_tso_segment(mld, skb, txq->sta, &mpdus_skbs);
+ if (ret)
+ return ret;
+
+ WARN_ON(skb_queue_empty(&mpdus_skbs));
+
+ while (!skb_queue_empty(&mpdus_skbs)) {
+ skb = __skb_dequeue(&mpdus_skbs);
+
+ ret = iwl_mld_tx_mpdu(mld, skb, txq);
+ if (!ret)
+ continue;
+
+ /* Free skbs created as part of TSO logic that have not yet
+ * been dequeued
+ */
+ __skb_queue_purge(&mpdus_skbs);
+
+ /* skb here is not necessarily same as skb that entered
+ * this method, so free it explicitly.
+ */
+ if (skb == orig_skb)
+ ieee80211_free_txskb(mld->hw, skb);
+ else
+ kfree_skb(skb);
+
+ /* there was error, but we consumed skb one way or
+ * another, so return 0
+ */
+ return 0;
+ }
+
+ return 0;
+}
+#else
+static int iwl_mld_tx_tso(struct iwl_mld *mld, struct sk_buff *skb,
+ struct ieee80211_txq *txq)
+{
+ /* Impossible to get TSO without CONFIG_INET */
+ WARN_ON(1);
+
+ return -1;
+}
+#endif /* CONFIG_INET */
+
+void iwl_mld_tx_skb(struct iwl_mld *mld, struct sk_buff *skb,
+ struct ieee80211_txq *txq)
+{
+ if (skb_is_gso(skb)) {
+ if (!iwl_mld_tx_tso(mld, skb, txq))
+ return;
+ goto err;
+ }
+
+ if (likely(!iwl_mld_tx_mpdu(mld, skb, txq)))
+ return;
+
+err:
+ ieee80211_free_txskb(mld->hw, skb);
+}
+
+void iwl_mld_tx_from_txq(struct iwl_mld *mld, struct ieee80211_txq *txq)
+{
+ struct iwl_mld_txq *mld_txq = iwl_mld_txq_from_mac80211(txq);
+ struct sk_buff *skb = NULL;
+ u8 zero_addr[ETH_ALEN] = {};
+
+ /*
+ * No need for threads to be pending here, they can leave the first
+ * taker all the work.
+ *
+ * mld_txq->tx_request logic:
+ *
+ * If 0, no one is currently TXing, set to 1 to indicate current thread
+ * will now start TX and other threads should quit.
+ *
+ * If 1, another thread is currently TXing, set to 2 to indicate to
+ * that thread that there was another request. Since that request may
+ * have raced with the check whether the queue is empty, the TXing
+ * thread should check the queue's status one more time before leaving.
+ * This check is done in order to not leave any TX hanging in the queue
+ * until the next TX invocation (which may not even happen).
+ *
+ * If 2, another thread is currently TXing, and it will already double
+ * check the queue, so do nothing.
+ */
+ if (atomic_fetch_add_unless(&mld_txq->tx_request, 1, 2))
+ return;
+
+ rcu_read_lock();
+ do {
+ while (likely(!mld_txq->status.stop_full) &&
+ (skb = ieee80211_tx_dequeue(mld->hw, txq)))
+ iwl_mld_tx_skb(mld, skb, txq);
+ } while (atomic_dec_return(&mld_txq->tx_request));
+
+ IWL_DEBUG_TX(mld, "TXQ of sta %pM tid %d is now empty\n",
+ txq->sta ? txq->sta->addr : zero_addr, txq->tid);
+
+ rcu_read_unlock();
+}
+
+static void iwl_mld_hwrate_to_tx_rate(struct iwl_mld *mld,
+ __le32 rate_n_flags_fw,
+ struct ieee80211_tx_info *info)
+{
+ enum nl80211_band band = info->band;
+ struct ieee80211_tx_rate *tx_rate = &info->status.rates[0];
+ u32 rate_n_flags = iwl_v3_rate_from_v2_v3(rate_n_flags_fw,
+ mld->fw_rates_ver_3);
+ u32 sgi = rate_n_flags & RATE_MCS_SGI_MSK;
+ u32 chan_width = rate_n_flags & RATE_MCS_CHAN_WIDTH_MSK;
+ u32 format = rate_n_flags & RATE_MCS_MOD_TYPE_MSK;
+
+ if (sgi)
+ tx_rate->flags |= IEEE80211_TX_RC_SHORT_GI;
+
+ switch (chan_width) {
+ case RATE_MCS_CHAN_WIDTH_20:
+ break;
+ case RATE_MCS_CHAN_WIDTH_40:
+ tx_rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+ break;
+ case RATE_MCS_CHAN_WIDTH_80:
+ tx_rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
+ break;
+ case RATE_MCS_CHAN_WIDTH_160:
+ tx_rate->flags |= IEEE80211_TX_RC_160_MHZ_WIDTH;
+ break;
+ default:
+ break;
+ }
+
+ switch (format) {
+ case RATE_MCS_MOD_TYPE_HT:
+ tx_rate->flags |= IEEE80211_TX_RC_MCS;
+ tx_rate->idx = RATE_HT_MCS_INDEX(rate_n_flags);
+ break;
+ case RATE_MCS_MOD_TYPE_VHT:
+ ieee80211_rate_set_vht(tx_rate,
+ rate_n_flags & RATE_MCS_CODE_MSK,
+ u32_get_bits(rate_n_flags,
+ RATE_MCS_NSS_MSK) + 1);
+ tx_rate->flags |= IEEE80211_TX_RC_VHT_MCS;
+ break;
+ case RATE_MCS_MOD_TYPE_HE:
+ case RATE_MCS_MOD_TYPE_EHT:
+ /* mac80211 cannot do this without ieee80211_tx_status_ext()
+ * but it only matters for radiotap
+ */
+ tx_rate->idx = 0;
+ break;
+ default:
+ tx_rate->idx =
+ iwl_mld_legacy_hw_idx_to_mac80211_idx(rate_n_flags,
+ band);
+ break;
+ }
+}
+
+void iwl_mld_handle_tx_resp_notif(struct iwl_mld *mld,
+ struct iwl_rx_packet *pkt)
+{
+ struct iwl_tx_resp *tx_resp = (void *)pkt->data;
+ int txq_id = le16_to_cpu(tx_resp->tx_queue);
+ struct agg_tx_status *agg_status = &tx_resp->status;
+ u32 status = le16_to_cpu(agg_status->status);
+ u32 pkt_len = iwl_rx_packet_payload_len(pkt);
+ size_t notif_size = sizeof(*tx_resp) + sizeof(u32);
+ int sta_id = IWL_TX_RES_GET_RA(tx_resp->ra_tid);
+ int tid = IWL_TX_RES_GET_TID(tx_resp->ra_tid);
+ struct ieee80211_link_sta *link_sta;
+ struct iwl_mld_sta *mld_sta;
+ u16 ssn;
+ struct sk_buff_head skbs;
+ u8 skb_freed = 0;
+ bool mgmt = false;
+ bool tx_failure = (status & TX_STATUS_MSK) != TX_STATUS_SUCCESS;
+
+ if (IWL_FW_CHECK(mld, tx_resp->frame_count != 1,
+ "Invalid tx_resp notif frame_count (%d)\n",
+ tx_resp->frame_count))
+ return;
+
+ /* validate the size of the variable part of the notif */
+ if (IWL_FW_CHECK(mld, notif_size != pkt_len,
+ "Invalid tx_resp notif size (expected=%zu got=%u)\n",
+ notif_size, pkt_len))
+ return;
+
+ ssn = le32_to_cpup((__le32 *)agg_status +
+ tx_resp->frame_count) & 0xFFFF;
+
+ __skb_queue_head_init(&skbs);
+
+ /* we can free until ssn % q.n_bd not inclusive */
+ iwl_trans_reclaim(mld->trans, txq_id, ssn, &skbs, false);
+
+ while (!skb_queue_empty(&skbs)) {
+ struct sk_buff *skb = __skb_dequeue(&skbs);
+ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+ struct ieee80211_hdr *hdr = (void *)skb->data;
+
+ skb_freed++;
+
+ iwl_trans_free_tx_cmd(mld->trans, info->driver_data[1]);
+
+ memset(&info->status, 0, sizeof(info->status));
+
+ info->flags &= ~(IEEE80211_TX_STAT_ACK | IEEE80211_TX_STAT_TX_FILTERED);
+
+ /* inform mac80211 about what happened with the frame */
+ switch (status & TX_STATUS_MSK) {
+ case TX_STATUS_SUCCESS:
+ case TX_STATUS_DIRECT_DONE:
+ info->flags |= IEEE80211_TX_STAT_ACK;
+ break;
+ default:
+ break;
+ }
+
+ /* If we are freeing multiple frames, mark all the frames
+ * but the first one as acked, since they were acknowledged
+ * before
+ */
+ if (skb_freed > 1)
+ info->flags |= IEEE80211_TX_STAT_ACK;
+
+ if (tx_failure) {
+ enum iwl_fw_ini_time_point tp =
+ IWL_FW_INI_TIME_POINT_TX_FAILED;
+
+ if (ieee80211_is_action(hdr->frame_control))
+ tp = IWL_FW_INI_TIME_POINT_TX_WFD_ACTION_FRAME_FAILED;
+ else if (ieee80211_is_mgmt(hdr->frame_control))
+ mgmt = true;
+
+ iwl_dbg_tlv_time_point(&mld->fwrt, tp, NULL);
+ }
+
+ iwl_mld_hwrate_to_tx_rate(mld, tx_resp->initial_rate, info);
+
+ if (likely(!iwl_mld_time_sync_frame(mld, skb, hdr->addr1)))
+ ieee80211_tx_status_skb(mld->hw, skb);
+ }
+
+ IWL_DEBUG_TX_REPLY(mld,
+ "TXQ %d status 0x%08x ssn=%d initial_rate 0x%x retries %d\n",
+ txq_id, status, ssn, le32_to_cpu(tx_resp->initial_rate),
+ tx_resp->failure_frame);
+
+ if (tx_failure && mgmt)
+ iwl_mld_toggle_tx_ant(mld, &mld->mgmt_tx_ant);
+
+ if (IWL_FW_CHECK(mld, sta_id >= mld->fw->ucode_capa.num_stations,
+ "Got invalid sta_id (%d)\n", sta_id))
+ return;
+
+ rcu_read_lock();
+
+ link_sta = rcu_dereference(mld->fw_id_to_link_sta[sta_id]);
+ if (!link_sta) {
+ /* This can happen if the TX cmd was sent before pre_rcu_remove
+ * but the TX response was received after
+ */
+ IWL_DEBUG_TX_REPLY(mld,
+ "Got valid sta_id (%d) but sta is NULL\n",
+ sta_id);
+ goto out;
+ }
+
+ if (IS_ERR(link_sta))
+ goto out;
+
+ mld_sta = iwl_mld_sta_from_mac80211(link_sta->sta);
+
+ if (tx_failure && mld_sta->sta_state < IEEE80211_STA_AUTHORIZED)
+ iwl_mld_toggle_tx_ant(mld, &mld_sta->data_tx_ant);
+
+ if (tid < IWL_MAX_TID_COUNT)
+ iwl_mld_count_mpdu_tx(link_sta, 1);
+
+out:
+ rcu_read_unlock();
+}
+
+static void iwl_mld_tx_reclaim_txq(struct iwl_mld *mld, int txq, int index,
+ bool in_flush)
+{
+ struct sk_buff_head reclaimed_skbs;
+
+ __skb_queue_head_init(&reclaimed_skbs);
+
+ iwl_trans_reclaim(mld->trans, txq, index, &reclaimed_skbs, in_flush);
+
+ while (!skb_queue_empty(&reclaimed_skbs)) {
+ struct sk_buff *skb = __skb_dequeue(&reclaimed_skbs);
+ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+ iwl_trans_free_tx_cmd(mld->trans, info->driver_data[1]);
+
+ memset(&info->status, 0, sizeof(info->status));
+
+ /* Packet was transmitted successfully, failures come as single
+ * frames because before failing a frame the firmware transmits
+ * it without aggregation at least once.
+ */
+ if (!in_flush)
+ info->flags |= IEEE80211_TX_STAT_ACK;
+ else
+ info->flags &= ~IEEE80211_TX_STAT_ACK;
+
+ ieee80211_tx_status_skb(mld->hw, skb);
+ }
+}
+
+int iwl_mld_flush_link_sta_txqs(struct iwl_mld *mld, u32 fw_sta_id)
+{
+ struct iwl_tx_path_flush_cmd_rsp *rsp;
+ struct iwl_tx_path_flush_cmd flush_cmd = {
+ .sta_id = cpu_to_le32(fw_sta_id),
+ .tid_mask = cpu_to_le16(0xffff),
+ };
+ struct iwl_host_cmd cmd = {
+ .id = TXPATH_FLUSH,
+ .len = { sizeof(flush_cmd), },
+ .data = { &flush_cmd, },
+ .flags = CMD_WANT_SKB,
+ };
+ int ret, num_flushed_queues;
+ u32 resp_len;
+
+ IWL_DEBUG_TX_QUEUES(mld, "flush for sta id %d tid mask 0x%x\n",
+ fw_sta_id, 0xffff);
+
+ ret = iwl_mld_send_cmd(mld, &cmd);
+ if (ret) {
+ IWL_ERR(mld, "Failed to send flush command (%d)\n", ret);
+ return ret;
+ }
+
+ resp_len = iwl_rx_packet_payload_len(cmd.resp_pkt);
+ if (IWL_FW_CHECK(mld, resp_len != sizeof(*rsp),
+ "Invalid TXPATH_FLUSH response len: %d\n",
+ resp_len)) {
+ ret = -EIO;
+ goto free_rsp;
+ }
+
+ rsp = (void *)cmd.resp_pkt->data;
+
+ if (IWL_FW_CHECK(mld, le16_to_cpu(rsp->sta_id) != fw_sta_id,
+ "sta_id %d != rsp_sta_id %d\n", fw_sta_id,
+ le16_to_cpu(rsp->sta_id))) {
+ ret = -EIO;
+ goto free_rsp;
+ }
+
+ num_flushed_queues = le16_to_cpu(rsp->num_flushed_queues);
+ if (IWL_FW_CHECK(mld, num_flushed_queues > IWL_TX_FLUSH_QUEUE_RSP,
+ "num_flushed_queues %d\n", num_flushed_queues)) {
+ ret = -EIO;
+ goto free_rsp;
+ }
+
+ for (int i = 0; i < num_flushed_queues; i++) {
+ struct iwl_flush_queue_info *queue_info = &rsp->queues[i];
+ int read_after = le16_to_cpu(queue_info->read_after_flush);
+ int txq_id = le16_to_cpu(queue_info->queue_num);
+
+ if (IWL_FW_CHECK(mld,
+ txq_id >= ARRAY_SIZE(mld->fw_id_to_txq),
+ "Invalid txq id %d\n", txq_id))
+ continue;
+
+ IWL_DEBUG_TX_QUEUES(mld,
+ "tid %d txq_id %d read-before %d read-after %d\n",
+ le16_to_cpu(queue_info->tid), txq_id,
+ le16_to_cpu(queue_info->read_before_flush),
+ read_after);
+
+ iwl_mld_tx_reclaim_txq(mld, txq_id, read_after, true);
+ }
+
+free_rsp:
+ iwl_free_resp(&cmd);
+ return ret;
+}
+
+int iwl_mld_ensure_queue(struct iwl_mld *mld, struct ieee80211_txq *txq)
+{
+ struct iwl_mld_txq *mld_txq = iwl_mld_txq_from_mac80211(txq);
+ int ret;
+
+ lockdep_assert_wiphy(mld->wiphy);
+
+ if (likely(mld_txq->status.allocated))
+ return 0;
+
+ ret = iwl_mld_add_txq(mld, txq);
+
+ spin_lock_bh(&mld->add_txqs_lock);
+ if (!list_empty(&mld_txq->list))
+ list_del_init(&mld_txq->list);
+ spin_unlock_bh(&mld->add_txqs_lock);
+
+ return ret;
+}
+
+int iwl_mld_update_sta_txqs(struct iwl_mld *mld,
+ struct ieee80211_sta *sta,
+ u32 old_sta_mask, u32 new_sta_mask)
+{
+ struct iwl_scd_queue_cfg_cmd cmd = {
+ .operation = cpu_to_le32(IWL_SCD_QUEUE_MODIFY),
+ .u.modify.old_sta_mask = cpu_to_le32(old_sta_mask),
+ .u.modify.new_sta_mask = cpu_to_le32(new_sta_mask),
+ };
+
+ lockdep_assert_wiphy(mld->wiphy);
+
+ for (int tid = 0; tid <= IWL_MAX_TID_COUNT; tid++) {
+ struct ieee80211_txq *txq =
+ sta->txq[tid != IWL_MAX_TID_COUNT ?
+ tid : IEEE80211_NUM_TIDS];
+ struct iwl_mld_txq *mld_txq =
+ iwl_mld_txq_from_mac80211(txq);
+ int ret;
+
+ if (!mld_txq->status.allocated)
+ continue;
+
+ if (tid == IWL_MAX_TID_COUNT)
+ cmd.u.modify.tid = cpu_to_le32(IWL_MGMT_TID);
+ else
+ cmd.u.modify.tid = cpu_to_le32(tid);
+
+ ret = iwl_mld_send_cmd_pdu(mld,
+ WIDE_ID(DATA_PATH_GROUP,
+ SCD_QUEUE_CONFIG_CMD),
+ &cmd);
+ if (ret)
+ return ret;
+ }
+
+ return 0;
+}
+
+void iwl_mld_handle_compressed_ba_notif(struct iwl_mld *mld,
+ struct iwl_rx_packet *pkt)
+{
+ struct iwl_compressed_ba_notif *ba_res = (void *)pkt->data;
+ u32 pkt_len = iwl_rx_packet_payload_len(pkt);
+ u16 tfd_cnt = le16_to_cpu(ba_res->tfd_cnt);
+ u8 sta_id = ba_res->sta_id;
+ struct ieee80211_link_sta *link_sta;
+
+ if (!tfd_cnt)
+ return;
+
+ if (IWL_FW_CHECK(mld, struct_size(ba_res, tfd, tfd_cnt) > pkt_len,
+ "Short BA notif (tfd_cnt=%d, size:0x%x)\n",
+ tfd_cnt, pkt_len))
+ return;
+
+ IWL_DEBUG_TX_REPLY(mld,
+ "BA notif received from sta_id=%d, flags=0x%x, sent:%d, acked:%d\n",
+ sta_id, le32_to_cpu(ba_res->flags),
+ le16_to_cpu(ba_res->txed),
+ le16_to_cpu(ba_res->done));
+
+ for (int i = 0; i < tfd_cnt; i++) {
+ struct iwl_compressed_ba_tfd *ba_tfd = &ba_res->tfd[i];
+ int txq_id = le16_to_cpu(ba_tfd->q_num);
+ int index = le16_to_cpu(ba_tfd->tfd_index);
+
+ if (IWL_FW_CHECK(mld,
+ txq_id >= ARRAY_SIZE(mld->fw_id_to_txq),
+ "Invalid txq id %d\n", txq_id))
+ continue;
+
+ iwl_mld_tx_reclaim_txq(mld, txq_id, index, false);
+ }
+
+ if (IWL_FW_CHECK(mld, sta_id >= mld->fw->ucode_capa.num_stations,
+ "Got invalid sta_id (%d)\n", sta_id))
+ return;
+
+ rcu_read_lock();
+
+ link_sta = rcu_dereference(mld->fw_id_to_link_sta[sta_id]);
+ if (IWL_FW_CHECK(mld, IS_ERR_OR_NULL(link_sta),
+ "Got valid sta_id (%d) but link_sta is NULL\n",
+ sta_id))
+ goto out;
+
+ iwl_mld_count_mpdu_tx(link_sta, le16_to_cpu(ba_res->txed));
+out:
+ rcu_read_unlock();
+}
generated by cgit v1.2.3 (git 2.25.1) at 2025-11-04 05:57:32 +0000