1 files changed, 707 insertions, 0 deletions

diff --git a/sys/contrib/dev/iwlwifi/mld/iface.c b/sys/contrib/dev/iwlwifi/mld/iface.c
new file mode 100644
index 000000000000..38993d65c052
--- /dev/null
+++ b/sys/contrib/dev/iwlwifi/mld/iface.c
@@ -0,0 +1,707 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * Copyright (C) 2024-2025 Intel Corporation
+ */
+#include <net/cfg80211.h>
+
+#include "iface.h"
+#include "hcmd.h"
+#include "key.h"
+#include "mlo.h"
+#include "mac80211.h"
+
+#include "fw/api/context.h"
+#include "fw/api/mac.h"
+#include "fw/api/time-event.h"
+#include "fw/api/datapath.h"
+
+/* Cleanup function for struct iwl_mld_vif, will be called in restart */
+void iwl_mld_cleanup_vif(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	struct iwl_mld *mld = mld_vif->mld;
+	struct iwl_mld_link *link;
+
+	mld_vif->emlsr.blocked_reasons &= ~IWL_MLD_EMLSR_BLOCKED_ROC;
+
+	if (mld_vif->aux_sta.sta_id != IWL_INVALID_STA)
+		iwl_mld_free_internal_sta(mld, &mld_vif->aux_sta);
+
+	/* EMLSR is turned back on during recovery */
+	vif->driver_flags &= ~IEEE80211_VIF_EML_ACTIVE;
+
+	if (mld_vif->roc_activity != ROC_NUM_ACTIVITIES)
+		ieee80211_remain_on_channel_expired(mld->hw);
+
+	mld_vif->roc_activity = ROC_NUM_ACTIVITIES;
+
+	for_each_mld_vif_valid_link(mld_vif, link) {
+		iwl_mld_cleanup_link(mld_vif->mld, link);
+
+		/* Correctly allocated primary link in non-MLO mode */
+		if (!ieee80211_vif_is_mld(vif) &&
+		    link_id == 0 && link == &mld_vif->deflink)
+			continue;
+
+		if (vif->active_links & BIT(link_id))
+			continue;
+
+		/* Should not happen as link removal should always succeed */
+		WARN_ON(1);
+		if (link != &mld_vif->deflink)
+			kfree_rcu(link, rcu_head);
+		RCU_INIT_POINTER(mld_vif->link[link_id], NULL);
+	}
+
+	ieee80211_iter_keys(mld->hw, vif, iwl_mld_cleanup_keys_iter, NULL);
+
+	wiphy_delayed_work_cancel(mld->wiphy, &mld_vif->mlo_scan_start_wk);
+
+	CLEANUP_STRUCT(mld_vif);
+}
+
+static int iwl_mld_send_mac_cmd(struct iwl_mld *mld,
+				struct iwl_mac_config_cmd *cmd)
+{
+	int ret;
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	ret = iwl_mld_send_cmd_pdu(mld,
+				   WIDE_ID(MAC_CONF_GROUP, MAC_CONFIG_CMD),
+				   cmd);
+	if (ret)
+		IWL_ERR(mld, "Failed to send MAC_CONFIG_CMD ret = %d\n", ret);
+
+	return ret;
+}
+
+int iwl_mld_mac80211_iftype_to_fw(const struct ieee80211_vif *vif)
+{
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		return vif->p2p ? FW_MAC_TYPE_P2P_STA : FW_MAC_TYPE_BSS_STA;
+	case NL80211_IFTYPE_AP:
+		return FW_MAC_TYPE_GO;
+	case NL80211_IFTYPE_MONITOR:
+		return FW_MAC_TYPE_LISTENER;
+	case NL80211_IFTYPE_P2P_DEVICE:
+		return FW_MAC_TYPE_P2P_DEVICE;
+	case NL80211_IFTYPE_ADHOC:
+		return FW_MAC_TYPE_IBSS;
+	default:
+		WARN_ON_ONCE(1);
+	}
+	return FW_MAC_TYPE_BSS_STA;
+}
+
+static bool iwl_mld_is_nic_ack_enabled(struct iwl_mld *mld,
+				       struct ieee80211_vif *vif)
+{
+	const struct ieee80211_supported_band *sband;
+	const struct ieee80211_sta_he_cap *own_he_cap;
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	/* This capability is the same for all bands,
+	 * so take it from one of them.
+	 */
+	sband = mld->hw->wiphy->bands[NL80211_BAND_2GHZ];
+	own_he_cap = ieee80211_get_he_iftype_cap_vif(sband, vif);
+
+	return own_he_cap && (own_he_cap->he_cap_elem.mac_cap_info[2] &
+			       IEEE80211_HE_MAC_CAP2_ACK_EN);
+}
+
+static void iwl_mld_set_he_support(struct iwl_mld *mld,
+				   struct ieee80211_vif *vif,
+				   struct iwl_mac_config_cmd *cmd,
+				   int cmd_ver)
+{
+	if (vif->type == NL80211_IFTYPE_AP) {
+		if (cmd_ver == 2)
+			cmd->wifi_gen_v2.he_ap_support = cpu_to_le16(1);
+		else
+			cmd->wifi_gen.he_ap_support = 1;
+	} else {
+		if (cmd_ver == 2)
+			cmd->wifi_gen_v2.he_support = cpu_to_le16(1);
+		else
+			cmd->wifi_gen.he_support = 1;
+	}
+}
+
+/* fill the common part for all interface types */
+static void iwl_mld_mac_cmd_fill_common(struct iwl_mld *mld,
+					struct ieee80211_vif *vif,
+					struct iwl_mac_config_cmd *cmd,
+					u32 action)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	struct ieee80211_bss_conf *link_conf;
+	unsigned int link_id;
+	int cmd_ver = iwl_fw_lookup_cmd_ver(mld->fw,
+					    WIDE_ID(MAC_CONF_GROUP,
+						    MAC_CONFIG_CMD), 0);
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	cmd->id_and_color = cpu_to_le32(mld_vif->fw_id);
+	cmd->action = cpu_to_le32(action);
+
+	cmd->mac_type =
+		cpu_to_le32(iwl_mld_mac80211_iftype_to_fw(vif));
+
+	memcpy(cmd->local_mld_addr, vif->addr, ETH_ALEN);
+
+	if (iwlwifi_mod_params.disable_11ax)
+		return;
+
+	cmd->nic_not_ack_enabled =
+		cpu_to_le32(!iwl_mld_is_nic_ack_enabled(mld, vif));
+
+	/* If we have MLO enabled, then the firmware needs to enable
+	 * address translation for the station(s) we add. That depends
+	 * on having EHT enabled in firmware, which in turn depends on
+	 * mac80211 in the code below.
+	 * However, mac80211 doesn't enable HE/EHT until it has parsed
+	 * the association response successfully, so just skip all that
+	 * and enable both when we have MLO.
+	 */
+	if (ieee80211_vif_is_mld(vif)) {
+		iwl_mld_set_he_support(mld, vif, cmd, cmd_ver);
+		if (cmd_ver == 2)
+			cmd->wifi_gen_v2.eht_support = cpu_to_le32(1);
+		else
+			cmd->wifi_gen.eht_support = 1;
+		return;
+	}
+
+	for_each_vif_active_link(vif, link_conf, link_id) {
+		if (!link_conf->he_support)
+			continue;
+
+		iwl_mld_set_he_support(mld, vif, cmd, cmd_ver);
+
+		/* EHT, if supported, was already set above */
+		break;
+	}
+}
+
+static void iwl_mld_fill_mac_cmd_sta(struct iwl_mld *mld,
+				     struct ieee80211_vif *vif, u32 action,
+				     struct iwl_mac_config_cmd *cmd)
+{
+	struct ieee80211_bss_conf *link;
+	u32 twt_policy = 0;
+	int link_id;
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	WARN_ON(vif->type != NL80211_IFTYPE_STATION);
+
+	/* We always want to hear MCAST frames, if we're not authorized yet,
+	 * we'll drop them.
+	 */
+	cmd->filter_flags |= cpu_to_le32(MAC_CFG_FILTER_ACCEPT_GRP);
+
+	/* Adding a MAC ctxt with is_assoc set is not allowed in fw
+	 * (and shouldn't happen)
+	 */
+	if (vif->cfg.assoc && action != FW_CTXT_ACTION_ADD) {
+		cmd->client.is_assoc = 1;
+
+		if (!iwl_mld_vif_from_mac80211(vif)->authorized)
+			cmd->client.data_policy |=
+				cpu_to_le16(COEX_HIGH_PRIORITY_ENABLE);
+	} else {
+		/* Allow beacons to pass through as long as we are not
+		 * associated
+		 */
+		cmd->filter_flags |= cpu_to_le32(MAC_CFG_FILTER_ACCEPT_BEACON);
+	}
+
+	cmd->client.assoc_id = cpu_to_le16(vif->cfg.aid);
+
+	if (ieee80211_vif_is_mld(vif)) {
+		u16 esr_transition_timeout =
+			u16_get_bits(vif->cfg.eml_cap,
+				     IEEE80211_EML_CAP_TRANSITION_TIMEOUT);
+
+		cmd->client.esr_transition_timeout =
+			min_t(u16, IEEE80211_EML_CAP_TRANSITION_TIMEOUT_128TU,
+			      esr_transition_timeout);
+		cmd->client.medium_sync_delay =
+			cpu_to_le16(vif->cfg.eml_med_sync_delay);
+	}
+
+	for_each_vif_active_link(vif, link, link_id) {
+		if (!link->he_support)
+			continue;
+
+		if (link->twt_requester)
+			twt_policy |= TWT_SUPPORTED;
+		if (link->twt_protected)
+			twt_policy |= PROTECTED_TWT_SUPPORTED;
+		if (link->twt_broadcast)
+			twt_policy |= BROADCAST_TWT_SUPPORTED;
+	}
+
+	if (!iwlwifi_mod_params.disable_11ax)
+		cmd->client.data_policy |= cpu_to_le16(twt_policy);
+
+	if (vif->probe_req_reg && vif->cfg.assoc && vif->p2p)
+		cmd->filter_flags |=
+			cpu_to_le32(MAC_CFG_FILTER_ACCEPT_PROBE_REQ);
+}
+
+static void iwl_mld_fill_mac_cmd_ap(struct iwl_mld *mld,
+				    struct ieee80211_vif *vif,
+				    struct iwl_mac_config_cmd *cmd)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	WARN_ON(vif->type != NL80211_IFTYPE_AP);
+
+	cmd->filter_flags |= cpu_to_le32(MAC_CFG_FILTER_ACCEPT_PROBE_REQ);
+
+	/* in AP mode, pass beacons from other APs (needed for ht protection).
+	 * When there're no any associated station, which means that we are not
+	 * TXing anyway, don't ask FW to pass beacons to prevent unnecessary
+	 * wake-ups.
+	 */
+	if (mld_vif->num_associated_stas)
+		cmd->filter_flags |= cpu_to_le32(MAC_CFG_FILTER_ACCEPT_BEACON);
+}
+
+static void iwl_mld_go_iterator(void *_data, u8 *mac, struct ieee80211_vif *vif)
+{
+	bool *go_active = _data;
+
+	if (ieee80211_vif_type_p2p(vif) == NL80211_IFTYPE_P2P_GO &&
+	    iwl_mld_vif_from_mac80211(vif)->ap_ibss_active)
+		*go_active = true;
+}
+
+static bool iwl_mld_p2p_dev_has_extended_disc(struct iwl_mld *mld)
+{
+	bool go_active = false;
+
+	/* This flag should be set to true when the P2P Device is
+	 * discoverable and there is at least a P2P GO. Setting
+	 * this flag will allow the P2P Device to be discoverable on other
+	 * channels in addition to its listen channel.
+	 * Note that this flag should not be set in other cases as it opens the
+	 * Rx filters on all MAC and increases the number of interrupts.
+	 */
+	ieee80211_iterate_active_interfaces(mld->hw,
+					    IEEE80211_IFACE_ITER_RESUME_ALL,
+					    iwl_mld_go_iterator, &go_active);
+
+	return go_active;
+}
+
+static void iwl_mld_fill_mac_cmd_p2p_dev(struct iwl_mld *mld,
+					 struct ieee80211_vif *vif,
+					 struct iwl_mac_config_cmd *cmd)
+{
+	bool ext_disc = iwl_mld_p2p_dev_has_extended_disc(mld);
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	/* Override the filter flags to accept all management frames. This is
+	 * needed to support both P2P device discovery using probe requests and
+	 * P2P service discovery using action frames
+	 */
+	cmd->filter_flags = cpu_to_le32(MAC_CFG_FILTER_ACCEPT_CONTROL_AND_MGMT);
+
+	if (ext_disc)
+		cmd->p2p_dev.is_disc_extended = cpu_to_le32(1);
+}
+
+static void iwl_mld_fill_mac_cmd_ibss(struct iwl_mld *mld,
+				      struct ieee80211_vif *vif,
+				      struct iwl_mac_config_cmd *cmd)
+{
+	lockdep_assert_wiphy(mld->wiphy);
+
+	WARN_ON(vif->type != NL80211_IFTYPE_ADHOC);
+
+	cmd->filter_flags |= cpu_to_le32(MAC_CFG_FILTER_ACCEPT_BEACON |
+					 MAC_CFG_FILTER_ACCEPT_PROBE_REQ |
+					 MAC_CFG_FILTER_ACCEPT_GRP);
+}
+
+static int
+iwl_mld_rm_mac_from_fw(struct iwl_mld *mld, struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	struct iwl_mac_config_cmd cmd = {
+		.action = cpu_to_le32(FW_CTXT_ACTION_REMOVE),
+		.id_and_color = cpu_to_le32(mld_vif->fw_id),
+	};
+
+	return iwl_mld_send_mac_cmd(mld, &cmd);
+}
+
+int iwl_mld_mac_fw_action(struct iwl_mld *mld, struct ieee80211_vif *vif,
+			  u32 action)
+{
+	struct iwl_mac_config_cmd cmd = {};
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	if (action == FW_CTXT_ACTION_REMOVE)
+		return iwl_mld_rm_mac_from_fw(mld, vif);
+
+	iwl_mld_mac_cmd_fill_common(mld, vif, &cmd, action);
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		iwl_mld_fill_mac_cmd_sta(mld, vif, action, &cmd);
+		break;
+	case NL80211_IFTYPE_AP:
+		iwl_mld_fill_mac_cmd_ap(mld, vif, &cmd);
+		break;
+	case NL80211_IFTYPE_MONITOR:
+		cmd.filter_flags =
+			cpu_to_le32(MAC_CFG_FILTER_PROMISC |
+				    MAC_CFG_FILTER_ACCEPT_CONTROL_AND_MGMT |
+				    MAC_CFG_FILTER_ACCEPT_BEACON |
+				    MAC_CFG_FILTER_ACCEPT_PROBE_REQ |
+				    MAC_CFG_FILTER_ACCEPT_GRP);
+		break;
+	case NL80211_IFTYPE_P2P_DEVICE:
+		iwl_mld_fill_mac_cmd_p2p_dev(mld, vif, &cmd);
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		iwl_mld_fill_mac_cmd_ibss(mld, vif, &cmd);
+		break;
+	default:
+		WARN(1, "not supported yet\n");
+		return -EOPNOTSUPP;
+	}
+
+	return iwl_mld_send_mac_cmd(mld, &cmd);
+}
+
+static void iwl_mld_mlo_scan_start_wk(struct wiphy *wiphy,
+				      struct wiphy_work *wk)
+{
+	struct iwl_mld_vif *mld_vif = container_of(wk, struct iwl_mld_vif,
+						   mlo_scan_start_wk.work);
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct iwl_mld *mld = IWL_MAC80211_GET_MLD(hw);
+
+	iwl_mld_int_mlo_scan(mld, iwl_mld_vif_to_mac80211(mld_vif));
+}
+
+IWL_MLD_ALLOC_FN(vif, vif)
+
+/* Constructor function for struct iwl_mld_vif */
+static int
+iwl_mld_init_vif(struct iwl_mld *mld, struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	int ret;
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	mld_vif->mld = mld;
+	mld_vif->roc_activity = ROC_NUM_ACTIVITIES;
+
+	ret = iwl_mld_allocate_vif_fw_id(mld, &mld_vif->fw_id, vif);
+	if (ret)
+		return ret;
+
+	if (!mld->fw_status.in_hw_restart) {
+		wiphy_work_init(&mld_vif->emlsr.unblock_tpt_wk,
+				iwl_mld_emlsr_unblock_tpt_wk);
+		wiphy_delayed_work_init(&mld_vif->emlsr.check_tpt_wk,
+					iwl_mld_emlsr_check_tpt);
+		wiphy_delayed_work_init(&mld_vif->emlsr.prevent_done_wk,
+					iwl_mld_emlsr_prevent_done_wk);
+		wiphy_delayed_work_init(&mld_vif->emlsr.tmp_non_bss_done_wk,
+					iwl_mld_emlsr_tmp_non_bss_done_wk);
+		wiphy_delayed_work_init(&mld_vif->mlo_scan_start_wk,
+					iwl_mld_mlo_scan_start_wk);
+	}
+	iwl_mld_init_internal_sta(&mld_vif->aux_sta);
+
+	return 0;
+}
+
+int iwl_mld_add_vif(struct iwl_mld *mld, struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	int ret;
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	ret = iwl_mld_init_vif(mld, vif);
+	if (ret)
+		return ret;
+
+	ret = iwl_mld_mac_fw_action(mld, vif, FW_CTXT_ACTION_ADD);
+	if (ret)
+		RCU_INIT_POINTER(mld->fw_id_to_vif[mld_vif->fw_id], NULL);
+
+	return ret;
+}
+
+int iwl_mld_rm_vif(struct iwl_mld *mld, struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	int ret;
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	ret = iwl_mld_mac_fw_action(mld, vif, FW_CTXT_ACTION_REMOVE);
+
+	if (WARN_ON(mld_vif->fw_id >= ARRAY_SIZE(mld->fw_id_to_vif)))
+		return -EINVAL;
+
+	RCU_INIT_POINTER(mld->fw_id_to_vif[mld_vif->fw_id], NULL);
+
+	iwl_mld_cancel_notifications_of_object(mld, IWL_MLD_OBJECT_TYPE_VIF,
+					       mld_vif->fw_id);
+
+	return ret;
+}
+
+void iwl_mld_set_vif_associated(struct iwl_mld *mld,
+				struct ieee80211_vif *vif)
+{
+	struct ieee80211_bss_conf *link;
+	unsigned int link_id;
+
+	for_each_vif_active_link(vif, link, link_id) {
+		if (iwl_mld_link_set_associated(mld, vif, link))
+			IWL_ERR(mld, "failed to update link %d\n", link_id);
+	}
+
+	iwl_mld_recalc_multicast_filter(mld);
+}
+
+static void iwl_mld_get_fw_id_bss_bitmap_iter(void *_data, u8 *mac,
+					      struct ieee80211_vif *vif)
+{
+	u8 *fw_id_bitmap = _data;
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+
+	if (ieee80211_vif_type_p2p(vif) != NL80211_IFTYPE_STATION)
+		return;
+
+	*fw_id_bitmap |= BIT(mld_vif->fw_id);
+}
+
+u8 iwl_mld_get_fw_bss_vifs_ids(struct iwl_mld *mld)
+{
+	u8 fw_id_bitmap = 0;
+
+	ieee80211_iterate_active_interfaces_mtx(mld->hw,
+						IEEE80211_IFACE_SKIP_SDATA_NOT_IN_DRIVER,
+						iwl_mld_get_fw_id_bss_bitmap_iter,
+						&fw_id_bitmap);
+
+	return fw_id_bitmap;
+}
+
+void iwl_mld_handle_probe_resp_data_notif(struct iwl_mld *mld,
+					  struct iwl_rx_packet *pkt)
+{
+	const struct iwl_probe_resp_data_notif *notif = (void *)pkt->data;
+	struct iwl_probe_resp_data *old_data, *new_data;
+	struct ieee80211_vif *vif;
+	struct iwl_mld_link *mld_link;
+
+	IWL_DEBUG_INFO(mld, "Probe response data notif: noa %d, csa %d\n",
+		       notif->noa_active, notif->csa_counter);
+
+	if (IWL_FW_CHECK(mld, le32_to_cpu(notif->mac_id) >=
+			 ARRAY_SIZE(mld->fw_id_to_vif),
+			 "mac id is invalid: %d\n",
+			 le32_to_cpu(notif->mac_id)))
+		return;
+
+	vif = wiphy_dereference(mld->wiphy,
+				mld->fw_id_to_vif[le32_to_cpu(notif->mac_id)]);
+
+	/* the firmware gives us the mac_id (and not the link_id), mac80211
+	 * gets a vif and not a link, bottom line, this flow is not MLD ready
+	 * yet.
+	 */
+	if (WARN_ON(!vif) || ieee80211_vif_is_mld(vif))
+		return;
+
+	if (notif->csa_counter != IWL_PROBE_RESP_DATA_NO_CSA &&
+	    notif->csa_counter >= 1)
+		ieee80211_beacon_set_cntdwn(vif, notif->csa_counter);
+
+	if (!vif->p2p)
+		return;
+
+	mld_link = &iwl_mld_vif_from_mac80211(vif)->deflink;
+
+	new_data = kzalloc(sizeof(*new_data), GFP_KERNEL);
+	if (!new_data)
+		return;
+
+	memcpy(&new_data->notif, notif, sizeof(new_data->notif));
+
+	/* noa_attr contains 1 reserved byte, need to substruct it */
+	new_data->noa_len = sizeof(struct ieee80211_vendor_ie) +
+			    sizeof(new_data->notif.noa_attr) - 1;
+
+	/*
+	 * If it's a one time NoA, only one descriptor is needed,
+	 * adjust the length according to len_low.
+	 */
+	if (new_data->notif.noa_attr.len_low ==
+	    sizeof(struct ieee80211_p2p_noa_desc) + 2)
+		new_data->noa_len -= sizeof(struct ieee80211_p2p_noa_desc);
+
+	old_data = wiphy_dereference(mld->wiphy, mld_link->probe_resp_data);
+	rcu_assign_pointer(mld_link->probe_resp_data, new_data);
+
+	if (old_data)
+		kfree_rcu(old_data, rcu_head);
+}
+
+void iwl_mld_handle_uapsd_misbehaving_ap_notif(struct iwl_mld *mld,
+					       struct iwl_rx_packet *pkt)
+{
+	struct iwl_uapsd_misbehaving_ap_notif *notif = (void *)pkt->data;
+	struct ieee80211_vif *vif;
+
+	if (IWL_FW_CHECK(mld, notif->mac_id >= ARRAY_SIZE(mld->fw_id_to_vif),
+			 "mac id is invalid: %d\n", notif->mac_id))
+		return;
+
+	vif = wiphy_dereference(mld->wiphy, mld->fw_id_to_vif[notif->mac_id]);
+
+	if (WARN_ON(!vif) || ieee80211_vif_is_mld(vif))
+		return;
+
+	IWL_WARN(mld, "uapsd misbehaving AP: %pM\n", vif->bss_conf.bssid);
+}
+
+void iwl_mld_handle_datapath_monitor_notif(struct iwl_mld *mld,
+					   struct iwl_rx_packet *pkt)
+{
+	struct iwl_datapath_monitor_notif *notif = (void *)pkt->data;
+	struct ieee80211_bss_conf *link;
+	struct ieee80211_supported_band *sband;
+	const struct ieee80211_sta_he_cap *he_cap;
+	struct ieee80211_vif *vif;
+	struct iwl_mld_vif *mld_vif;
+
+	if (notif->type != cpu_to_le32(IWL_DP_MON_NOTIF_TYPE_EXT_CCA))
+		return;
+
+	link = iwl_mld_fw_id_to_link_conf(mld, notif->link_id);
+	if (WARN_ON(!link))
+		return;
+
+	vif = link->vif;
+	if (WARN_ON(!vif) || vif->type != NL80211_IFTYPE_STATION ||
+	    !vif->cfg.assoc)
+		return;
+
+	if (!link->chanreq.oper.chan ||
+	    link->chanreq.oper.chan->band != NL80211_BAND_2GHZ ||
+	    link->chanreq.oper.width < NL80211_CHAN_WIDTH_40)
+		return;
+
+	mld_vif = iwl_mld_vif_from_mac80211(vif);
+
+	/* this shouldn't happen *again*, ignore it */
+	if (mld_vif->cca_40mhz_workaround != CCA_40_MHZ_WA_NONE)
+		return;
+
+	mld_vif->cca_40mhz_workaround = CCA_40_MHZ_WA_RECONNECT;
+
+	/*
+	 * This capability manipulation isn't really ideal, but it's the
+	 * easiest choice - otherwise we'd have to do some major changes
+	 * in mac80211 to support this, which isn't worth it. This does
+	 * mean that userspace may have outdated information, but that's
+	 * actually not an issue at all.
+	 */
+	sband = mld->wiphy->bands[NL80211_BAND_2GHZ];
+
+	WARN_ON(!sband->ht_cap.ht_supported);
+	WARN_ON(!(sband->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40));
+	sband->ht_cap.cap &= ~IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+
+	he_cap = ieee80211_get_he_iftype_cap_vif(sband, vif);
+
+	if (he_cap) {
+		/* we know that ours is writable */
+		struct ieee80211_sta_he_cap *he = (void *)(uintptr_t)he_cap;
+
+		WARN_ON(!he->has_he);
+		WARN_ON(!(he->he_cap_elem.phy_cap_info[0] &
+			  IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_40MHZ_IN_2G));
+		he->he_cap_elem.phy_cap_info[0] &=
+			~IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_40MHZ_IN_2G;
+	}
+
+	ieee80211_disconnect(vif, true);
+}
+
+void iwl_mld_reset_cca_40mhz_workaround(struct iwl_mld *mld,
+					struct ieee80211_vif *vif)
+{
+	struct ieee80211_supported_band *sband;
+	const struct ieee80211_sta_he_cap *he_cap;
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+
+	if (vif->type != NL80211_IFTYPE_STATION)
+		return;
+
+	if (mld_vif->cca_40mhz_workaround == CCA_40_MHZ_WA_NONE)
+		return;
+
+	/* Now we are just reconnecting with the new capabilities,
+	 * but remember to reset the capabilities when we disconnect for real
+	 */
+	if (mld_vif->cca_40mhz_workaround == CCA_40_MHZ_WA_RECONNECT) {
+		mld_vif->cca_40mhz_workaround = CCA_40_MHZ_WA_RESET;
+		return;
+	}
+
+	/* Now cca_40mhz_workaround == CCA_40_MHZ_WA_RESET */
+
+	sband = mld->wiphy->bands[NL80211_BAND_2GHZ];
+
+	sband->ht_cap.cap |= IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+
+	he_cap = ieee80211_get_he_iftype_cap_vif(sband, vif);
+
+	if (he_cap) {
+		/* we know that ours is writable */
+		struct ieee80211_sta_he_cap *he = (void *)(uintptr_t)he_cap;
+
+		he->he_cap_elem.phy_cap_info[0] |=
+			IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_40MHZ_IN_2G;
+	}
+
+	mld_vif->cca_40mhz_workaround = CCA_40_MHZ_WA_NONE;
+}
+
+struct ieee80211_vif *iwl_mld_get_bss_vif(struct iwl_mld *mld)
+{
+	unsigned long fw_id_bitmap = iwl_mld_get_fw_bss_vifs_ids(mld);
+	int fw_id;
+
+	if (hweight8(fw_id_bitmap) != 1)
+		return NULL;
+
+	fw_id = __ffs(fw_id_bitmap);
+
+	return wiphy_dereference(mld->wiphy,
+				 mld->fw_id_to_vif[fw_id]);
+}