Contributors: 10
Author Tokens Token Proportion Commits Commit Proportion
Johannes Berg 2141 45.52% 9 24.32%
Ilan Peer 1380 29.34% 7 18.92%
Miri Korenblit 744 15.82% 6 16.22%
Avraham Stern 328 6.97% 4 10.81%
Emmanuel Grumbach 26 0.55% 5 13.51%
Anjaneyulu 25 0.53% 1 2.70%
Don Fry 24 0.51% 1 2.70%
Benjamin Berg 22 0.47% 1 2.70%
Daniel Gabay 10 0.21% 2 5.41%
Nathan Errera 3 0.06% 1 2.70%
Total 4703 37 


// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
/*
 * Copyright (C) 2025 Intel Corporation
 */

#include "mld.h"
#include "iface.h"
#include "link.h"
#include "mlo.h"
#include "tlc.h"
#include "fw/api/mac-cfg.h"
#include "fw/api/mac.h"
#include "fw/api/rs.h"

#define IWL_NAN_DISOVERY_BEACON_INTERNVAL_TU 512
#define IWL_NAN_RSSI_CLOSE 55
#define IWL_NAN_RSSI_MIDDLE 70

bool iwl_mld_nan_supported(struct iwl_mld *mld)
{
	const struct iwl_fw *fw = mld->fw;

	if (fw_has_capa(&fw->ucode_capa, IWL_UCODE_TLV_CAPA_NAN_SYNC_SUPPORT) &&
	    iwl_fw_lookup_cmd_ver(fw, WIDE_ID(MAC_CONF_GROUP, NAN_SCHEDULE_CMD), 0) >= 1 &&
	    iwl_fw_lookup_cmd_ver(fw, WIDE_ID(MAC_CONF_GROUP, NAN_PEER_CMD), 0) >= 1 &&
	    iwl_fw_lookup_cmd_ver(fw, WIDE_ID(MAC_CONF_GROUP, STA_CONFIG_CMD), 0) >= 3 &&
	    iwl_fw_lookup_cmd_ver(fw, WIDE_ID(MAC_CONF_GROUP, MAC_CONFIG_CMD), 0) >= 4 &&
	    iwl_fw_lookup_cmd_ver(fw, WIDE_ID(DATA_PATH_GROUP, TLC_MNG_CONFIG_CMD), 0) >= 6 &&
		mld->nvm_data->nan_phy_capa.ht.ht_supported)
		return true;
	return false;
}

static int iwl_mld_nan_send_config_cmd(struct iwl_mld *mld,
				       struct iwl_nan_config_cmd *cmd,
				       u8 *beacon_data, size_t beacon_data_len)
{
	struct iwl_host_cmd hcmd = {
		.id = WIDE_ID(MAC_CONF_GROUP, NAN_CFG_CMD),
	};

	hcmd.len[0] = sizeof(*cmd);
	hcmd.data[0] = cmd;

	if (beacon_data_len) {
		hcmd.len[1] = beacon_data_len;
		hcmd.data[1] = beacon_data;
		hcmd.dataflags[1] = IWL_HCMD_DFL_DUP;
	}

	return iwl_mld_send_cmd(mld, &hcmd);
}

bool iwl_mld_nan_use_nan_stations(struct iwl_mld *mld)
{
	/*
	 * If the FW supports version 1 of the NAN config command, it means that
	 * it needs to receive the station ID of the auxiliary station in the
	 * NAN configuration command. Otherwise, use the NAN dedicated station
	 * types.
	 */
	return iwl_fw_lookup_cmd_ver(mld->fw,
				     WIDE_ID(MAC_CONF_GROUP,
					     NAN_CFG_CMD), 1) != 1;
}

static const struct iwl_mld_int_sta *
iwl_mld_nan_get_mgmt_sta(struct iwl_mld *mld, struct ieee80211_vif *vif)
{
	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
	const struct iwl_mld_int_sta *sta;

	if (iwl_mld_nan_use_nan_stations(mld))
		sta = &mld_vif->nan.mgmt_sta;
	else
		sta = &mld_vif->aux_sta;

	if (WARN_ON(sta->sta_id == IWL_INVALID_STA))
		return NULL;

	return sta;
}

int iwl_mld_nan_get_mgmt_queue(struct iwl_mld *mld, struct ieee80211_vif *vif)
{
	const struct iwl_mld_int_sta *sta = iwl_mld_nan_get_mgmt_sta(mld, vif);

	if (!sta)
		return IWL_MLD_INVALID_QUEUE;

	return sta->queue_id;
}

static void iwl_mld_nan_flush(struct iwl_mld *mld, struct ieee80211_vif *vif)
{
	const struct iwl_mld_int_sta *sta = iwl_mld_nan_get_mgmt_sta(mld, vif);

	if (!sta)
		return;

	if (WARN_ON(sta->queue_id == IWL_MLD_INVALID_QUEUE))
		return;

	IWL_DEBUG_INFO(mld, "NAN: flush queues for sta=%u\n",
		       sta->sta_id);

	iwl_mld_flush_link_sta_txqs(mld, sta->sta_id);
}

static void iwl_mld_nan_remove_stations(struct iwl_mld *mld,
					struct ieee80211_vif *vif)
{
	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);

	iwl_mld_nan_flush(mld, vif);

	if (!iwl_mld_nan_use_nan_stations(mld)) {
		iwl_mld_remove_aux_sta(mld, vif);
		return;
	}

	iwl_mld_remove_nan_bcast_sta(mld, &mld_vif->nan.bcast_sta);
	iwl_mld_remove_nan_mgmt_sta(mld, &mld_vif->nan.mgmt_sta);
}

static int iwl_mld_nan_add_stations(struct iwl_mld *mld,
				    struct ieee80211_vif *vif)
{
	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
	int ret;

	if (!iwl_mld_nan_use_nan_stations(mld))
		return iwl_mld_add_aux_sta(mld, &mld_vif->aux_sta);

	ret = iwl_mld_add_nan_bcast_sta(mld, &mld_vif->nan.bcast_sta);
	if (ret)
		return ret;

	ret = iwl_mld_add_nan_mgmt_sta(mld, &mld_vif->nan.mgmt_sta);
	if (ret)
		iwl_mld_remove_nan_bcast_sta(mld, &mld_vif->nan.bcast_sta);

	return ret;
}

static int iwl_mld_nan_config(struct iwl_mld *mld,
			      struct ieee80211_vif *vif,
			      struct cfg80211_nan_conf *conf,
			      enum iwl_ctxt_action action)
{
	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
	struct iwl_nan_config_cmd cmd = {
		.action = cpu_to_le32(action),
	};
	u8 *data __free(kfree) = NULL;

	lockdep_assert_wiphy(mld->wiphy);

	ether_addr_copy(cmd.nmi_addr, vif->addr);
	cmd.master_pref = conf->master_pref;

	memcpy(cmd.cluster_id, conf->cluster_id + 4,
	       sizeof(cmd.cluster_id));

	cmd.scan_period = conf->scan_period < 255 ? conf->scan_period : 255;
	cmd.dwell_time =
		conf->scan_dwell_time < 255 ? conf->scan_dwell_time : 255;

	if (conf->discovery_beacon_interval)
		cmd.discovery_beacon_interval =
			cpu_to_le32(conf->discovery_beacon_interval);
	else
		cmd.discovery_beacon_interval =
			cpu_to_le32(IWL_NAN_DISOVERY_BEACON_INTERNVAL_TU);

	if (conf->enable_dw_notification)
		cmd.flags = IWL_NAN_FLAG_DW_END_NOTIF_ENABLED;

	/* 2 GHz band must be supported */
	cmd.band_config[IWL_NAN_BAND_2GHZ].rssi_close =
		abs(conf->band_cfgs[NL80211_BAND_2GHZ].rssi_close);
	cmd.band_config[IWL_NAN_BAND_2GHZ].rssi_middle =
		abs(conf->band_cfgs[NL80211_BAND_2GHZ].rssi_middle);
	cmd.band_config[IWL_NAN_BAND_2GHZ].dw_interval =
		conf->band_cfgs[NL80211_BAND_2GHZ].awake_dw_interval;

	/* 5 GHz band operation is optional. Configure its operation if
	 * supported. Note that conf->bands might be zero, so we need to check
	 * the channel pointer, not the band mask.
	 */
	if (conf->band_cfgs[NL80211_BAND_5GHZ].chan) {
		cmd.hb_channel =
			conf->band_cfgs[NL80211_BAND_5GHZ].chan->hw_value;

		cmd.band_config[IWL_NAN_BAND_5GHZ].rssi_close =
			abs(conf->band_cfgs[NL80211_BAND_5GHZ].rssi_close);
		cmd.band_config[IWL_NAN_BAND_5GHZ].rssi_middle =
			abs(conf->band_cfgs[NL80211_BAND_5GHZ].rssi_middle);
		cmd.band_config[IWL_NAN_BAND_5GHZ].dw_interval =
			conf->band_cfgs[NL80211_BAND_5GHZ].awake_dw_interval;
	}

	if (conf->extra_nan_attrs_len || conf->vendor_elems_len) {
		data = kmalloc(conf->extra_nan_attrs_len +
			       conf->vendor_elems_len, GFP_KERNEL);
		if (!data)
			return -ENOMEM;

		cmd.nan_attr_len = cpu_to_le32(conf->extra_nan_attrs_len);
		cmd.nan_vendor_elems_len = cpu_to_le32(conf->vendor_elems_len);

		if (conf->extra_nan_attrs_len)
			memcpy(data, conf->extra_nan_attrs,
			       conf->extra_nan_attrs_len);

		if (conf->vendor_elems_len)
			memcpy(data + conf->extra_nan_attrs_len,
			       conf->vendor_elems,
			       conf->vendor_elems_len);
	}

	/* FW needs to know about the station ID only with version 1 of the
	 * NAN configuration command
	 */
	if (!iwl_mld_nan_use_nan_stations(mld))
		cmd.sta_id = mld_vif->aux_sta.sta_id;

	return iwl_mld_nan_send_config_cmd(mld, &cmd, data,
					   conf->extra_nan_attrs_len +
					   conf->vendor_elems_len);
}

int iwl_mld_start_nan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
		      struct cfg80211_nan_conf *conf)
{
	struct iwl_mld *mld = IWL_MAC80211_GET_MLD(hw);
	int ret;

	IWL_DEBUG_MAC80211(mld, "NAN: start: bands=0x%x\n", conf->bands);

	ret = iwl_mld_update_emlsr_block(mld, true, IWL_MLD_EMLSR_BLOCKED_NAN);
	if (ret)
		return ret;

	ret = iwl_mld_nan_add_stations(mld, vif);
	if (ret)
		goto unblock_emlsr;

	ret = iwl_mld_nan_config(mld, vif, conf, FW_CTXT_ACTION_ADD);
	if (ret) {
		IWL_ERR(mld, "Failed to start NAN. ret=%d\n", ret);
		goto remove_stas;
	}

	return 0;

remove_stas:
	iwl_mld_nan_remove_stations(mld, vif);
unblock_emlsr:
	iwl_mld_update_emlsr_block(mld, false, IWL_MLD_EMLSR_BLOCKED_NAN);

	return ret;
}

int iwl_mld_nan_change_config(struct ieee80211_hw *hw,
			      struct ieee80211_vif *vif,
			      struct cfg80211_nan_conf *conf,
			      u32 changes)
{
	struct iwl_mld *mld = IWL_MAC80211_GET_MLD(hw);

	IWL_DEBUG_MAC80211(mld, "NAN: change: changes=0x%x, bands=0x%x\n",
			   changes, conf->bands);

	/* Note that we do not use 'changes' as the FW always expects the
	 * complete configuration, and mac80211 always provides the complete
	 * configuration.
	 */
	return iwl_mld_nan_config(mld, vif, conf, FW_CTXT_ACTION_MODIFY);
}

int iwl_mld_stop_nan(struct ieee80211_hw *hw,
		     struct ieee80211_vif *vif)
{
	struct iwl_mld *mld = IWL_MAC80211_GET_MLD(hw);
	struct iwl_nan_config_cmd cmd = {
		.action = cpu_to_le32(FW_CTXT_ACTION_REMOVE),
	};
	int ret;

	lockdep_assert_wiphy(mld->wiphy);

	ret = iwl_mld_send_cmd_pdu(mld,
				   WIDE_ID(MAC_CONF_GROUP, NAN_CFG_CMD),
				   &cmd);
	if (ret)
		IWL_ERR(mld, "NAN: Failed to stop NAN. ret=%d\n", ret);

	/* assume that higher layer guarantees that no additional frames are
	 * added before calling this callback
	 */
	iwl_mld_nan_remove_stations(mld, vif);

	/* cancel based on object type being NAN, as the NAN objects do
	 * not have a unique identifier associated with them
	 */
	iwl_mld_cancel_notifications_of_object(mld,
					       IWL_MLD_OBJECT_TYPE_NAN,
					       0);

	iwl_mld_update_emlsr_block(mld, false, IWL_MLD_EMLSR_BLOCKED_NAN);

	return 0;
}

void iwl_mld_handle_nan_cluster_notif(struct iwl_mld *mld,
				      struct iwl_rx_packet *pkt)
{
	struct iwl_nan_cluster_notif *notif = (void *)pkt->data;
	struct wireless_dev *wdev = mld->nan_device_vif ?
		ieee80211_vif_to_wdev(mld->nan_device_vif) : NULL;
	bool new_cluster = !!(notif->flags &
			      IWL_NAN_CLUSTER_NOTIF_FLAG_NEW_CLUSTER);
	u8 cluster_id[ETH_ALEN] = {
		0x50, 0x6f, 0x9a, 0x01,
		notif->cluster_id[0], notif->cluster_id[1]
	};

	IWL_DEBUG_INFO(mld,
		       "NAN: cluster event: cluster_id=%pM, flags=0x%x\n",
		       cluster_id, notif->flags);

	if (IWL_FW_CHECK(mld, !wdev, "NAN: cluster event without wdev\n"))
		return;

	if (IWL_FW_CHECK(mld, !ieee80211_vif_nan_started(mld->nan_device_vif),
			 "NAN: cluster event without NAN started\n"))
		return;

	cfg80211_nan_cluster_joined(wdev, cluster_id, new_cluster, GFP_KERNEL);
}

bool iwl_mld_cancel_nan_cluster_notif(struct iwl_mld *mld,
				      struct iwl_rx_packet *pkt,
				      u32 obj_id)
{
	return true;
}

bool iwl_mld_cancel_nan_dw_end_notif(struct iwl_mld *mld,
				     struct iwl_rx_packet *pkt,
				     u32 obj_id)
{
	return true;
}

bool iwl_mld_cancel_nan_ulw_attr_notif(struct iwl_mld *mld,
				       struct iwl_rx_packet *pkt,
				       u32 obj_id)
{
	return true;
}

void iwl_mld_handle_nan_ulw_attr_notif(struct iwl_mld *mld,
				       struct iwl_rx_packet *pkt)
{
	struct iwl_nan_ulw_attr_notif *notif = (void *)pkt->data;
	struct wireless_dev *wdev;

	IWL_DEBUG_INFO(mld, "NAN: ULW attr update: len=%u\n", notif->attr_len);

	if (IWL_FW_CHECK(mld, !mld->nan_device_vif,
			 "NAN: ULW attr update without NAN vif\n"))
		return;

	if (IWL_FW_CHECK(mld, !ieee80211_vif_nan_started(mld->nan_device_vif),
			 "NAN: ULW attr update without NAN started\n"))
		return;

	if (IWL_FW_CHECK(mld,
			 notif->attr_len > IWL_NAN_MAX_ENDLESS_ULW_ATTR_LEN,
			 "NAN: ULW attr update invalid len %u\n",
			 notif->attr_len))
		return;

	wdev = ieee80211_vif_to_wdev(mld->nan_device_vif);
	cfg80211_nan_ulw_update(wdev, notif->attr, notif->attr_len, GFP_KERNEL);
}

void iwl_mld_handle_nan_dw_end_notif(struct iwl_mld *mld,
				     struct iwl_rx_packet *pkt)
{
	struct iwl_nan_dw_end_notif *notif = (void *)pkt->data;
	struct iwl_mld_vif *mld_vif = mld->nan_device_vif ?
		iwl_mld_vif_from_mac80211(mld->nan_device_vif) :
		NULL;
	struct wireless_dev *wdev;
	struct ieee80211_channel *chan;

	IWL_DEBUG_INFO(mld, "NAN: DW end: band=%u\n", notif->band);

	if (IWL_FW_CHECK(mld, !mld_vif, "NAN: DW end without mld_vif\n"))
		return;

	if (IWL_FW_CHECK(mld, !ieee80211_vif_nan_started(mld->nan_device_vif),
			 "NAN: DW end without NAN started\n"))
		return;

	iwl_mld_nan_flush(mld, mld->nan_device_vif);

	/* TODO: currently the notification specified the band on which the DW
	 * ended. Need to change that to the actual channel on which the next DW
	 * will be started.
	 */
	switch (notif->band) {
	case IWL_NAN_BAND_2GHZ:
		chan = ieee80211_get_channel(mld->wiphy, 2437);
		break;
	case IWL_NAN_BAND_5GHZ:
		/* TODO: use the actual channel */
		chan = ieee80211_get_channel(mld->wiphy, 5745);
		break;
	default:
		IWL_FW_CHECK(mld, false,
			     "NAN: Invalid band %u in DW end notif\n",
			     notif->band);
		return;
	}

	if (WARN_ON_ONCE(!chan))
		return;

	wdev = ieee80211_vif_to_wdev(mld->nan_device_vif);
	cfg80211_next_nan_dw_notif(wdev, chan, GFP_KERNEL);
}

static void iwl_mld_nan_fill_rates(struct iwl_link_config_cmd *cmd)
{
	u32 ofdm = 0;

	/* All OFDM rates - NAN uses OFDM only, no CCK */
	ofdm |= IWL_RATE_BIT_MSK(6) >> IWL_FIRST_OFDM_RATE;
	ofdm |= IWL_RATE_BIT_MSK(9) >> IWL_FIRST_OFDM_RATE;
	ofdm |= IWL_RATE_BIT_MSK(12) >> IWL_FIRST_OFDM_RATE;
	ofdm |= IWL_RATE_BIT_MSK(18) >> IWL_FIRST_OFDM_RATE;
	ofdm |= IWL_RATE_BIT_MSK(24) >> IWL_FIRST_OFDM_RATE;
	ofdm |= IWL_RATE_BIT_MSK(36) >> IWL_FIRST_OFDM_RATE;
	ofdm |= IWL_RATE_BIT_MSK(48) >> IWL_FIRST_OFDM_RATE;
	ofdm |= IWL_RATE_BIT_MSK(54) >> IWL_FIRST_OFDM_RATE;

	cmd->ofdm_rates = cpu_to_le32(ofdm);
	cmd->short_slot = cpu_to_le32(1);
}

static void iwl_mld_nan_fill_qos(struct iwl_ac_qos *ac, __le32 *qos_flags)
{
	/* AC_BK: CWmin=15, CWmax=1023, AIFSN=7, TXOP=0 */
	ac[AC_BK].cw_min = cpu_to_le16(15);
	ac[AC_BK].cw_max = cpu_to_le16(1023);
	ac[AC_BK].aifsn = 7;
	ac[AC_BK].fifos_mask = BIT(IWL_BZ_EDCA_TX_FIFO_BK);
	ac[AC_BK].edca_txop = 0;

	/* AC_BE: CWmin=15, CWmax=1023, AIFSN=3, TXOP=0 */
	ac[AC_BE].cw_min = cpu_to_le16(15);
	ac[AC_BE].cw_max = cpu_to_le16(1023);
	ac[AC_BE].aifsn = 3;
	ac[AC_BE].fifos_mask = BIT(IWL_BZ_EDCA_TX_FIFO_BE);
	ac[AC_BE].edca_txop = 0;

	/* AC_VI: CWmin=7, CWmax=15, AIFSN=2, TXOP=3008us */
	ac[AC_VI].cw_min = cpu_to_le16(7);
	ac[AC_VI].cw_max = cpu_to_le16(15);
	ac[AC_VI].aifsn = 2;
	ac[AC_VI].fifos_mask = BIT(IWL_BZ_EDCA_TX_FIFO_VI);
	ac[AC_VI].edca_txop = cpu_to_le16(3008);

	/* AC_VO: CWmin=3, CWmax=7, AIFSN=2, TXOP=1504us */
	ac[AC_VO].cw_min = cpu_to_le16(3);
	ac[AC_VO].cw_max = cpu_to_le16(7);
	ac[AC_VO].aifsn = 2;
	ac[AC_VO].fifos_mask = BIT(IWL_BZ_EDCA_TX_FIFO_VO);
	ac[AC_VO].edca_txop = cpu_to_le16(1504);

	*qos_flags |= cpu_to_le32(MAC_QOS_FLG_UPDATE_EDCA);
}

static void
iwl_mld_nan_link_prep_cmd(struct iwl_mld *mld,
			  struct iwl_mld_nan_link *nan_link,
			  struct iwl_link_config_cmd *cmd,
			  u32 modify_flags)
{
	struct ieee80211_vif *vif = mld->nan_device_vif;
	struct iwl_mld_vif *mld_vif;

	if (WARN_ON_ONCE(!vif))
		return;

	mld_vif = iwl_mld_vif_from_mac80211(vif);

	memset(cmd, 0, sizeof(*cmd));

	if (!nan_link->chanctx) {
		cmd->phy_id = cpu_to_le32(FW_CTXT_ID_INVALID);
	} else {
		struct iwl_mld_phy *mld_phy;

		mld_phy = iwl_mld_phy_from_mac80211(nan_link->chanctx);
		cmd->phy_id = cpu_to_le32(mld_phy->fw_id);
	}

	if (modify_flags & LINK_CONTEXT_MODIFY_RATES_INFO)
		iwl_mld_nan_fill_rates(cmd);

	if (modify_flags & LINK_CONTEXT_MODIFY_QOS_PARAMS)
		iwl_mld_nan_fill_qos(cmd->ac, &cmd->qos_flags);

	cmd->link_id = cpu_to_le32(nan_link->fw_id);
	cmd->mac_id = cpu_to_le32(mld_vif->fw_id);
	cmd->active = cpu_to_le32(nan_link->active);

	ether_addr_copy(cmd->local_link_addr, vif->addr);

	cmd->modify_mask = cpu_to_le32(modify_flags);
}

static struct iwl_mld_nan_link *
iwl_mld_nan_link_add(struct iwl_mld *mld,
		     struct iwl_mld_vif *mld_vif,
		     struct ieee80211_chanctx_conf *chanctx)
{
	struct iwl_mld_nan_link *nan_link;
	struct iwl_link_config_cmd cmd;
	u8 fw_id;
	int ret;

	lockdep_assert_wiphy(mld->wiphy);

	ret = iwl_mld_allocate_link_fw_id(mld, &fw_id, ERR_PTR(-ENODEV));
	/*
	 * We should always have enough links. The schedule contains up to 3,
	 * and the BSS vif cannot do EMLSR - so can only have 1.
	 */
	if (WARN_ON(ret < 0))
		return NULL;

	nan_link = &mld_vif->nan.links[fw_id];

	if (WARN_ON_ONCE(nan_link->fw_id != FW_CTXT_ID_INVALID))
		goto err;

	nan_link->fw_id = fw_id;
	nan_link->chanctx = chanctx;

	iwl_mld_nan_link_prep_cmd(mld, nan_link, &cmd,
				  LINK_CONTEXT_MODIFY_RATES_INFO |
				  LINK_CONTEXT_MODIFY_QOS_PARAMS);

	ret = iwl_mld_send_link_cmd(mld, &cmd, FW_CTXT_ACTION_ADD);
	if (ret) {
		nan_link->fw_id = FW_CTXT_ID_INVALID;
		nan_link->chanctx = NULL;
		goto err;
	}

	return nan_link;
err:
	RCU_INIT_POINTER(mld->fw_id_to_bss_conf[fw_id], NULL);
	return NULL;
}

static int iwl_mld_nan_link_set_active(struct iwl_mld *mld,
				       struct ieee80211_vif *vif,
				       struct iwl_mld_nan_link *nan_link,
				       bool active)
{
	struct iwl_link_config_cmd cmd;
	struct ieee80211_sta *sta;
	int ret;

	if (nan_link->active == active)
		return 0;

	if (active) {
		for_each_station(sta, mld->hw) {
			struct iwl_mld_sta *mld_sta = iwl_mld_sta_from_mac80211(sta);

			if (mld_sta->sta_type == STATION_TYPE_NAN_PEER_NDI)
				iwl_mld_config_tlc(mld, mld_sta->vif, sta);
		}
	}

	nan_link->active = active;

	iwl_mld_nan_link_prep_cmd(mld, nan_link, &cmd,
				  LINK_CONTEXT_MODIFY_ACTIVE);

	ret = iwl_mld_send_link_cmd(mld, &cmd, FW_CTXT_ACTION_MODIFY);
	if (ret) {
		nan_link->active = !nan_link->active;
		return ret;
	}

	if (!active) {
		nan_link->chanctx = NULL;
		/* TODO: when FW is ready, Update phy in TLC to invalid after */
	}

	return 0;
}

static void iwl_mld_nan_link_remove(struct iwl_mld *mld,
				    struct iwl_mld_nan_link *nan_link,
				    u32 link_id)
{
	struct iwl_link_config_cmd cmd = {
		.link_id = cpu_to_le32(link_id),
		.phy_id = cpu_to_le32(FW_CTXT_ID_INVALID),
	};

	iwl_mld_send_link_cmd(mld, &cmd, FW_CTXT_ACTION_REMOVE);

	RCU_INIT_POINTER(mld->fw_id_to_bss_conf[link_id], NULL);
	nan_link->fw_id = FW_CTXT_ID_INVALID;
	nan_link->active = false;
	nan_link->chanctx = NULL;
}

static bool iwl_mld_nan_have_links(struct iwl_mld_vif *mld_vif)
{
	struct iwl_mld_nan_link *nan_link;

	for_each_mld_nan_valid_link(mld_vif, nan_link)
		return true;

	return false;
}

static struct iwl_mld_nan_link *
iwl_mld_nan_find_link(struct iwl_mld_vif *mld_vif,
		      struct ieee80211_chanctx_conf *chanctx)
{
	struct iwl_mld_nan_link *nan_link;

	for_each_mld_nan_valid_link(mld_vif, nan_link) {
		if (nan_link->chanctx == chanctx)
			return nan_link;
	}

	return NULL;
}

static void iwl_mld_nan_set_mcast_data_links(struct ieee80211_vif *vif)
{
	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);

	if (vif->type != NL80211_IFTYPE_NAN_DATA)
		return;

	/* Note that all errors are handled internally so nothing to do
	 * with the return value (used only to silence compilation warnings)
	 */
	iwl_mld_update_nan_mcast_data_sta(mld_vif->mld, vif->addr,
					  &mld_vif->nan.mcast_data_sta);
}

void iwl_mld_nan_vif_cfg_changed(struct iwl_mld *mld,
				 struct ieee80211_vif *vif,
				 u64 changes)
{
	struct iwl_nan_schedule_cmd cmd = {};
	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
	bool previously_empty_schedule = !iwl_mld_nan_have_links(mld_vif);
	struct ieee80211_nan_sched_cfg *sched_cfg = &vif->cfg.nan_sched;
	struct iwl_mld_nan_link *links[ARRAY_SIZE(sched_cfg->channels)] = {};
	struct ieee80211_nan_channel **slots = sched_cfg->schedule;
	bool link_used[ARRAY_SIZE(mld_vif->nan.links)] = {};
	struct iwl_mld_nan_link *nan_link;
	unsigned long remove_link_ids = 0;
	bool added_links = false;
	bool empty_schedule = true;
	int ret, i;
	u16 cmd_size;
	u32 cmd_id = WIDE_ID(MAC_CONF_GROUP, NAN_SCHEDULE_CMD);
	u8 version = iwl_fw_lookup_cmd_ver(mld->fw, cmd_id, 0);

	if (!(changes & BSS_CHANGED_NAN_LOCAL_SCHED))
		return;

	switch (version) {
	case 1:
		if (sched_cfg->deferred) {
			IWL_ERR(mld,
				"NAN: deferred schedule not supported by FW\n");
			return;
		}

		cmd_size = sizeof(struct iwl_nan_schedule_cmd_v1);
		break;
	case 2:
		cmd_size = sizeof(struct iwl_nan_schedule_cmd);

		if (sched_cfg->deferred)
			cmd.deferred = 1;

		if (sched_cfg->avail_blob_len &&
		    !WARN_ON(sched_cfg->avail_blob_len >
			     sizeof(cmd.avail_attr.attr))) {
			cmd.avail_attr.attr_len = sched_cfg->avail_blob_len;
			memcpy(cmd.avail_attr.attr, sched_cfg->avail_blob,
			       sched_cfg->avail_blob_len);
		}
		break;
	default:
		IWL_ERR(mld, "NAN: unsupported NAN schedule cmd version %d\n",
			version);
		return;
	}

	for (i = 0; i < ARRAY_SIZE(sched_cfg->channels); i++) {
		if (!sched_cfg->channels[i].chanreq.oper.chan)
			continue;
		empty_schedule = false;
		break;
	}

	/* add the MAC if needed (before adding links) */
	if (!empty_schedule && previously_empty_schedule) {
		WARN_ON(mld_vif->nan.mac_added);
		ret = iwl_mld_add_nan_vif(mld, vif);

		if (ret) {
			IWL_ERR(mld, "NAN: failed to add MAC (%d)\n", ret);
			return;
		}
	}

	if (!mld_vif->nan.mac_added) {
		/* nothing to do */
		return;
	}

	/* this currently just uses the same index */
	BUILD_BUG_ON(ARRAY_SIZE(sched_cfg->channels) !=
		     ARRAY_SIZE(cmd.channels));

	/*
	 * mac80211 removes unused channels before adding new ones, so it may
	 * update an empty schedule with an availability attribute because it
	 * is going to add channels later. Since the firmware does not expect
	 * an availability attribute without channels, ignore it in that case.
	 */
	if (empty_schedule)
		cmd.avail_attr.attr_len = 0;

	/* find links we can keep (same chanctx/PHY) */
	for (i = 0; i < ARRAY_SIZE(sched_cfg->channels); i++) {
		struct ieee80211_chanctx_conf *chanctx;
		struct iwl_mld_nan_link *link;

		chanctx = sched_cfg->channels[i].chanctx_conf;
		/* ULW */
		if (!chanctx)
			continue;

		link = iwl_mld_nan_find_link(mld_vif, chanctx);
		links[i] = link;
		if (link)
			link_used[link->fw_id] = true;
	}

	/* add/reassign links for new channels */
	for (i = 0; i < ARRAY_SIZE(sched_cfg->channels); i++) {
		struct ieee80211_chanctx_conf *chanctx;

		/* already have an existing active link */
		if (links[i])
			continue;

		chanctx = sched_cfg->channels[i].chanctx_conf;
		/* ULW or unused slot */
		if (!chanctx)
			continue;

		/*
		 * if this fails we still update the schedule, but
		 * without a valid link we'll always ULW it
		 */
		links[i] = iwl_mld_nan_link_add(mld, mld_vif, chanctx);

		/* we have a link, activate it */
		if (links[i]) {
			added_links = true;
			link_used[links[i]->fw_id] = true;
			iwl_mld_nan_link_set_active(mld, vif, links[i], true);
		}
	}

	/* fill the command */
	for (i = 0; i < ARRAY_SIZE(sched_cfg->channels); i++) {
		cmd.channels[i].link_id = FW_CTXT_ID_INVALID;

		if (!sched_cfg->channels[i].chanreq.oper.chan)
			continue;

		memcpy(cmd.channels[i].channel_entry,
		       sched_cfg->channels[i].channel_entry, 6);
		cmd.channels[i].link_id =
			links[i] ? links[i]->fw_id : FW_CTXT_ID_INVALID;
	}

	for (i = 0; i < CFG80211_NAN_SCHED_NUM_TIME_SLOTS; i++) {
		int chan_idx;

		if (!slots[i])
			continue;

		chan_idx = slots[i] - sched_cfg->channels;
		if (WARN_ON_ONCE(chan_idx < 0 ||
				 chan_idx >= ARRAY_SIZE(cmd.channels)))
			continue;

		cmd.channels[chan_idx].availability_map |= cpu_to_le32(BIT(i));
	}

	ret = iwl_mld_send_cmd_pdu(mld, cmd_id, &cmd, cmd_size);
	if (ret)
		IWL_ERR(mld, "NAN: failed to update schedule (%d)\n", ret);

	/* prepare stations for links we'll remove */
	for_each_mld_nan_valid_link(mld_vif, nan_link) {
		if (!link_used[nan_link->fw_id]) {
			iwl_mld_nan_link_set_active(mld, vif, nan_link, false);
			remove_link_ids |= BIT(nan_link->fw_id);
			/* mark unused for STA updates */
			nan_link->fw_id = FW_CTXT_ID_INVALID;
		}
	}

	if (added_links || remove_link_ids) {
		struct ieee80211_sta *sta;

		for_each_station(sta, mld->hw) {
			struct iwl_mld_sta *mld_sta = iwl_mld_sta_from_mac80211(sta);

			if (mld_sta->sta_type == STATION_TYPE_NAN_PEER_NMI ||
			    mld_sta->sta_type == STATION_TYPE_NAN_PEER_NDI)
				iwl_mld_add_modify_sta_cmd(mld, &sta->deflink);
		}

		/*
		 * Iterate over all the NAN Data interfaces and update the links
		 * for the internal multicast data station.
		 * In recovery - the station will be added later in
		 * drv_add_interface
		 */
		if (iwl_mld_nan_use_nan_stations(mld) && !mld->fw_status.in_hw_restart) {
			struct ieee80211_vif *iter;

			for_each_active_interface(iter, mld->hw)
				iwl_mld_nan_set_mcast_data_links(iter);
		}
	}

	/* delete unused links */
	for_each_set_bit(i, &remove_link_ids, ARRAY_SIZE(mld_vif->nan.links))
		iwl_mld_nan_link_remove(mld, &mld_vif->nan.links[i], i);

	/* remove MAC if needed */
	if (!previously_empty_schedule && empty_schedule) {
		/* must have been added */
		WARN_ON_ONCE(!mld_vif->nan.mac_added);

		/* mac80211 should reconfigure same state */
		if (!WARN_ON_ONCE(mld->fw_status.in_hw_restart &&
				  !iwl_mld_error_before_recovery(mld)))
			iwl_mld_rm_vif(mld, vif);
	}
}

bool iwl_mld_cancel_nan_sched_update_completed_notif(struct iwl_mld *mld,
						     struct iwl_rx_packet *pkt,
						     u32 obj_id)
{
	return true;
}

void iwl_mld_handle_nan_sched_update_completed_notif(struct iwl_mld *mld,
						     struct iwl_rx_packet *pkt)
{
	struct iwl_nan_sched_update_completed_notif *notif = (void *)pkt->data;
	struct ieee80211_vif *vif = mld->nan_device_vif;

	if (IWL_FW_CHECK(mld, !vif,
			 "NAN: schedule update completed without NAN vif\n"))
		return;

	if (IWL_FW_CHECK(mld, !ieee80211_vif_nan_started(vif),
			 "NAN: schedule update completed without NAN started\n"))
		return;

	/*
	 * Deferred schedule update should not fail in firmware since all
	 * channels and links were added.
	 */
	IWL_FW_CHECK(mld, notif->status != IWL_NAN_SCHED_UPDATE_SUCCESS,
		     "NAN: deferred schedule update failed\n");

	if (WARN_ON(!vif->cfg.nan_sched.deferred))
		return;

	ieee80211_nan_sched_update_done(vif);
}

int iwl_mld_mac802111_nan_peer_sched_changed(struct ieee80211_hw *hw,
					     struct ieee80211_sta *sta)
{
	struct iwl_mld_sta *mld_sta = iwl_mld_sta_from_mac80211(sta);
	struct ieee80211_nan_peer_sched *sched = sta->nan_sched;
	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(mld_sta->vif);
	struct iwl_mld *mld = IWL_MAC80211_GET_MLD(hw);
	struct iwl_mld_nan_link *nan_link;
	struct iwl_nan_peer_cmd cmd = {
		.nmi_sta_id = mld_sta->deflink.fw_id,
		.sequence_id = sched->seq_id,
		.committed_dw_info = cpu_to_le16(sched->committed_dw),
		.max_channel_switch_time = cpu_to_le16(sched->max_chan_switch),
		.initial_ulw_size = cpu_to_le32(sched->ulw_size),
		.per_phy[0 ... NUM_PHY_CTX - 1] = {
			/* unused by FW if availability_map == 0 */
			.map_id = CFG80211_NAN_INVALID_MAP_ID,
			.link_id = FW_CTXT_ID_INVALID,
		},
		/* .initial_ulw directly provided below by data[1]/len[1] */
	};
	struct iwl_host_cmd hcmd = {
		.id = WIDE_ID(MAC_CONF_GROUP, NAN_PEER_CMD),
		.data[0] = &cmd,
		.len[0] = sizeof(cmd),
		.data[1] = sched->init_ulw,
		.len[1] = sched->ulw_size,
		.dataflags[1] = IWL_HCMD_DFL_DUP,
	};
	struct ieee80211_sta *iter;

	/* Update TLC in case peer channels were added/removed/updated */
	for_each_station(iter, mld->hw) {
		struct iwl_mld_sta *tmp = iwl_mld_sta_from_mac80211(iter);

		if (tmp->sta_type == STATION_TYPE_NAN_PEER_NDI)
			iwl_mld_config_tlc(mld, tmp->vif, iter);
	}

	for (int i = 0; i < ARRAY_SIZE(sched->maps); i++) {
		if (sched->maps[i].map_id == CFG80211_NAN_INVALID_MAP_ID)
			continue;

		BUILD_BUG_ON(ARRAY_SIZE(sched->maps[i].slots) != 32);
		for (int slot = 0;
		     slot < ARRAY_SIZE(sched->maps[i].slots);
		     slot++) {
			struct ieee80211_chanctx_conf *ctx;
			struct ieee80211_nan_channel *chan;
			struct iwl_mld_phy *phy;

			chan = sched->maps[i].slots[slot];
			if (!chan)
				continue;

			ctx = chan->chanctx_conf;
			if (!ctx)
				continue;

			phy = iwl_mld_phy_from_mac80211(ctx);

			for_each_mld_nan_valid_link(mld_vif, nan_link) {
				if (nan_link->chanctx == ctx) {
					cmd.per_phy[phy->fw_id].link_id =
						nan_link->fw_id;
					break;
				}
			}

			if (WARN_ON(cmd.per_phy[phy->fw_id].link_id ==
				    FW_CTXT_ID_INVALID))
				continue;

			/*
			 * each channel can only appear in one map,
			 * upper layers enforce that
			 */
			if (WARN_ON(cmd.per_phy[phy->fw_id].map_id != CFG80211_NAN_INVALID_MAP_ID &&
				    cmd.per_phy[phy->fw_id].map_id != sched->maps[i].map_id))
				continue;

			cmd.per_phy[phy->fw_id].map_id = sched->maps[i].map_id;
			memcpy(cmd.per_phy[phy->fw_id].channel_entry,
			       chan->channel_entry,
			       sizeof(cmd.per_phy[phy->fw_id].channel_entry));
			cmd.per_phy[phy->fw_id].availability_map |=
				cpu_to_le32(BIT(slot));
		}
	}

	return iwl_mld_send_cmd(mld, &hcmd);
}