Commit 6e4198d3 authored by Shaul Triebitz's avatar Shaul Triebitz Committed by Johannes Berg
Browse files

wifi: iwlwifi: mvm: implement mac80211 callback change_sta_links 



Add/removed from iwl driver and firmware station links.
Update the station queues accordingly (which station links
are connected to the station queues).

Signed-off-by: default avatarShaul Triebitz <shaul.triebitz@intel.com>
Signed-off-by: default avatarGregory Greenman <gregory.greenman@intel.com>
Link: https://lore.kernel.org/r/20230329100039.156d1aae5de1.I32973141be1190222169879f8caf7038c1a8f769@changeid


Signed-off-by: default avatarJohannes Berg <johannes.berg@intel.com>
parent 29df2a64

drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c

+8 −1
Original line number Diff line number Diff line
@@ -953,7 +953,14 @@ iwl_mvm_mld_change_sta_links(struct ieee80211_hw *hw, 
			     struct ieee80211_sta *sta,

			     u16 old_links, u16 new_links)

{

	return 0;

	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);

	int ret;



	mutex_lock(&mvm->mutex);

	ret = iwl_mvm_mld_update_sta_links(mvm, vif, sta, old_links, new_links);

	mutex_unlock(&mvm->mutex);



	return ret;

}



const struct ieee80211_ops iwl_mvm_mld_hw_ops = {

drivers/net/wireless/intel/iwlwifi/mvm/mld-sta.c

+224 −22
Original line number Diff line number Diff line
@@ -479,15 +479,53 @@ static void iwl_mvm_mld_sta_rm_all_sta_links(struct iwl_mvm *mvm, 
	}

}



static void iwl_mvm_mld_free_sta_link(struct iwl_mvm *mvm,

				      struct iwl_mvm_sta *mvm_sta,

				      struct iwl_mvm_link_sta *mvm_sta_link,

				      unsigned int link_id)

{

	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta_link->sta_id], NULL);

	RCU_INIT_POINTER(mvm_sta->link[link_id], NULL);



	if (mvm_sta_link != &mvm_sta->deflink)

		kfree_rcu(mvm_sta_link, rcu_head);

}



static int iwl_mvm_mld_alloc_sta_link(struct iwl_mvm *mvm,

				      struct ieee80211_vif *vif,

				      struct ieee80211_sta *sta,

				      unsigned int link_id)

{

	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);

	struct iwl_mvm_link_sta *link;

	u32 sta_id = iwl_mvm_find_free_sta_id(mvm,

					  ieee80211_vif_type_p2p(vif));



	if (sta_id == IWL_MVM_INVALID_STA)

		return -ENOSPC;



	if (rcu_access_pointer(sta->link[link_id]) == &sta->deflink) {

		link = &mvm_sta->deflink;

	} else {

		link = kzalloc(sizeof(*link), GFP_KERNEL);

		if (!link)

			return -ENOMEM;

	}



	link->sta_id = sta_id;

	rcu_assign_pointer(mvm_sta->link[link_id], link);

	rcu_assign_pointer(mvm->fw_id_to_mac_id[link->sta_id], sta);



	return 0;

}



/* allocate all the links of a sta, called when the station is first added */

static int iwl_mvm_mld_alloc_sta_links(struct iwl_mvm *mvm,

				       struct ieee80211_vif *vif,

				       struct ieee80211_sta *sta)

{

	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);

	struct iwl_mvm_link_sta *link;

	unsigned int link_id;

	u32 sta_id;

	int ret;



	lockdep_assert_held(&mvm->mutex);
@@ -497,28 +535,10 @@ static int iwl_mvm_mld_alloc_sta_links(struct iwl_mvm *mvm, 
		    mvm_sta->link[link_id])

			continue;



		sta_id = iwl_mvm_find_free_sta_id(mvm,

						  ieee80211_vif_type_p2p(vif));



		if (sta_id == IWL_MVM_INVALID_STA) {

			ret = -ENOSPC;

			goto err;

		}



		if (rcu_access_pointer(sta->link[link_id]) == &sta->deflink) {

			link = &mvm_sta->deflink;

		} else {

			link = kzalloc(sizeof(*link), GFP_KERNEL);

			if (!link) {

				ret = -ENOMEM;

		ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, link_id);

		if (ret)

			goto err;

	}

		}



		link->sta_id = sta_id;

		rcu_assign_pointer(mvm_sta->link[link_id], link);

		rcu_assign_pointer(mvm->fw_id_to_mac_id[link->sta_id], sta);

	}



	return 0;
@@ -846,3 +866,185 @@ void iwl_mvm_mld_modify_all_sta_disable_tx(struct iwl_mvm *mvm, 


	rcu_read_unlock();

}



static int iwl_mvm_mld_update_sta_queue(struct iwl_mvm *mvm,

					struct iwl_mvm_sta *mvm_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),

	};

	struct iwl_host_cmd hcmd = {

		.id = WIDE_ID(DATA_PATH_GROUP, SCD_QUEUE_CONFIG_CMD),

		.len[0] = sizeof(cmd),

		.data[0] = &cmd

	};

	int tid;

	int ret;



	lockdep_assert_held(&mvm->mutex);



	for (tid = 0; tid <= IWL_MAX_TID_COUNT; tid++) {

		struct iwl_mvm_tid_data *tid_data = &mvm_sta->tid_data[tid];

		int txq_id = tid_data->txq_id;



		if (txq_id == IWL_MVM_INVALID_QUEUE)

			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_mvm_send_cmd(mvm, &hcmd);

		if (ret)

			return ret;

	}



	return 0;

}



int iwl_mvm_mld_update_sta_links(struct iwl_mvm *mvm,

				 struct ieee80211_vif *vif,

				 struct ieee80211_sta *sta,

				 u16 old_links, u16 new_links)

{

	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);

	struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif);

	struct iwl_mvm_link_sta *mvm_sta_link;

	struct iwl_mvm_vif_link_info *mvm_vif_link;

	unsigned long links_to_add = ~old_links & new_links;

	unsigned long links_to_rem = old_links & ~new_links;

	unsigned long old_links_long = old_links;

	u32 current_sta_mask = 0, sta_mask_added = 0, sta_mask_to_rem = 0;

	unsigned long link_sta_added_to_fw = 0, link_sta_allocated = 0;

	unsigned int link_id;

	int ret;



	lockdep_assert_held(&mvm->mutex);



	for_each_set_bit(link_id, &old_links_long,

			 IEEE80211_MLD_MAX_NUM_LINKS) {

		mvm_sta_link =

			rcu_dereference_protected(mvm_sta->link[link_id],

						  lockdep_is_held(&mvm->mutex));



		if (WARN_ON(!mvm_sta_link)) {

			ret = -EINVAL;

			goto err;

		}



		current_sta_mask |= BIT(mvm_sta_link->sta_id);

		if (links_to_rem & BIT(link_id))

			sta_mask_to_rem |= BIT(mvm_sta_link->sta_id);

	}



	if (sta_mask_to_rem) {

		ret = iwl_mvm_mld_update_sta_queue(mvm, mvm_sta,

						   current_sta_mask,

						   current_sta_mask & ~sta_mask_to_rem);

		if (WARN_ON(ret))

			goto err;



		current_sta_mask &= ~sta_mask_to_rem;

	}



	for_each_set_bit(link_id, &links_to_rem, IEEE80211_MLD_MAX_NUM_LINKS) {

		mvm_sta_link =

			rcu_dereference_protected(mvm_sta->link[link_id],

						  lockdep_is_held(&mvm->mutex));

		mvm_vif_link = mvm_vif->link[link_id];



		if (WARN_ON(!mvm_sta_link || !mvm_vif_link)) {

			ret = -EINVAL;

			goto err;

		}



		ret = iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id);

		if (WARN_ON(ret))

			goto err;



		if (vif->type == NL80211_IFTYPE_STATION)

			mvm_vif_link->ap_sta_id = IWL_MVM_INVALID_STA;



		iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id);

	}



	for_each_set_bit(link_id, &links_to_add, IEEE80211_MLD_MAX_NUM_LINKS) {

		struct ieee80211_bss_conf *link_conf =

			rcu_dereference_protected(vif->link_conf[link_id], 1);

		struct ieee80211_link_sta *link_sta =

			rcu_dereference_protected(sta->link[link_id], 1);

		mvm_vif_link = mvm_vif->link[link_id];



		if (WARN_ON(!mvm_vif_link || !link_conf || !link_sta ||

			    mvm_sta->link[link_id])) {

			ret = -EINVAL;

			goto err;

		}



		ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, link_id);

		if (WARN_ON(ret))

			goto err;



		mvm_sta_link =

			rcu_dereference_protected(mvm_sta->link[link_id],

						  lockdep_is_held(&mvm->mutex));



		if (WARN_ON(!mvm_sta_link)) {

			ret = -EINVAL;

			goto err;

		}



		if (vif->type == NL80211_IFTYPE_STATION)

			iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif_link,

						  mvm_sta_link);



		link_sta_allocated |= BIT(link_id);



		sta_mask_added |= BIT(mvm_sta_link->sta_id);



		ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,

					  mvm_sta_link);

		if (WARN_ON(ret))

			goto err;



		link_sta_added_to_fw |= BIT(link_id);

	}



	if (sta_mask_added) {

		ret = iwl_mvm_mld_update_sta_queue(mvm, mvm_sta,

						   current_sta_mask,

						   current_sta_mask | sta_mask_added);

		if (WARN_ON(ret))

			goto err;

	}



	return 0;



err:

	/* remove all already allocated stations in FW */

	for_each_set_bit(link_id, &link_sta_added_to_fw,

			 IEEE80211_MLD_MAX_NUM_LINKS) {

		mvm_sta_link =

			rcu_dereference_protected(mvm_sta->link[link_id],

						  lockdep_is_held(&mvm->mutex));



		iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id);

	}



	/* remove all already allocated station links in driver */

	for_each_set_bit(link_id, &link_sta_allocated,

			 IEEE80211_MLD_MAX_NUM_LINKS) {

		mvm_sta_link =

			rcu_dereference_protected(mvm_sta->link[link_id],

						  lockdep_is_held(&mvm->mutex));



		iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id);

	}



	return ret;

}

drivers/net/wireless/intel/iwlwifi/mvm/sta.h

+4 −0
Original line number Diff line number Diff line
@@ -641,6 +641,10 @@ int iwl_mvm_mld_rm_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 
		       struct ieee80211_sta *sta);

int iwl_mvm_mld_rm_sta_id(struct iwl_mvm *mvm, struct ieee80211_vif *vif,

			  u8 sta_id);

int iwl_mvm_mld_update_sta_links(struct iwl_mvm *mvm,

				 struct ieee80211_vif *vif,

				 struct ieee80211_sta *sta,

				 u16 old_links, u16 new_links);



/* Queues */

void iwl_mvm_mld_modify_all_sta_disable_tx(struct iwl_mvm *mvm,