mld-sta.c

   1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
   2/*
   3 * Copyright (C) 2022-2025 Intel Corporation
   4 */
   5#include "mvm.h"
   6#include "time-sync.h"
   7#include "sta.h"
   8
   9u32 iwl_mvm_sta_fw_id_mask(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
  10			   int filter_link_id)
  11{
  12	struct ieee80211_link_sta *link_sta;
  13	struct iwl_mvm_sta *mvmsta;
  14	struct ieee80211_vif *vif;
  15	unsigned int link_id;
  16	u32 result = 0;
  17
  18	if (!sta)
  19		return 0;
  20
  21	mvmsta = iwl_mvm_sta_from_mac80211(sta);
  22	vif = mvmsta->vif;
  23
  24	/* it's easy when the STA is not an MLD */
  25	if (!sta->valid_links)
  26		return BIT(mvmsta->deflink.sta_id);
  27
  28	/* but if it is an MLD, get the mask of all the FW STAs it has ... */
  29	for_each_sta_active_link(vif, sta, link_sta, link_id) {
  30		struct iwl_mvm_link_sta *mvm_link_sta;
  31
  32		/* unless we have a specific link in mind */
  33		if (filter_link_id >= 0 && link_id != filter_link_id)
  34			continue;
  35
  36		mvm_link_sta =
  37			rcu_dereference_check(mvmsta->link[link_id],
  38					      lockdep_is_held(&mvm->mutex));
  39		if (!mvm_link_sta)
  40			continue;
  41
  42		result |= BIT(mvm_link_sta->sta_id);
  43	}
  44
  45	return result;
  46}
  47
  48static int iwl_mvm_mld_send_sta_cmd(struct iwl_mvm *mvm,
  49				    struct iwl_sta_cfg_cmd *cmd)
  50{
  51	u32 cmd_id = WIDE_ID(MAC_CONF_GROUP, STA_CONFIG_CMD);
  52	int cmd_len = iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 0) > 1 ?
  53		      sizeof(*cmd) :
  54		      sizeof(struct iwl_sta_cfg_cmd_v1);
  55	int ret = iwl_mvm_send_cmd_pdu(mvm,
  56				       WIDE_ID(MAC_CONF_GROUP, STA_CONFIG_CMD),
  57				       0, cmd_len, cmd);
  58	if (ret)
  59		IWL_ERR(mvm, "STA_CONFIG_CMD send failed, ret=0x%x\n", ret);
  60	return ret;
  61}
  62
  63/*
  64 * Add an internal station to the FW table
  65 */
  66static int iwl_mvm_mld_add_int_sta_to_fw(struct iwl_mvm *mvm,
  67					 struct iwl_mvm_int_sta *sta,
  68					 const u8 *addr, int link_id)
  69{
  70	struct iwl_sta_cfg_cmd cmd;
  71
  72	lockdep_assert_held(&mvm->mutex);
  73
  74	memset(&cmd, 0, sizeof(cmd));
  75	cmd.sta_id = cpu_to_le32((u8)sta->sta_id);
  76
  77	cmd.link_id = cpu_to_le32(link_id);
  78
  79	cmd.station_type = cpu_to_le32(sta->type);
  80
  81	if (fw_has_capa(&mvm->fw->ucode_capa,
  82			IWL_UCODE_TLV_CAPA_STA_EXP_MFP_SUPPORT) &&
  83	    sta->type == STATION_TYPE_BCAST_MGMT)
  84		cmd.mfp = cpu_to_le32(1);
  85
  86	if (addr) {
  87		memcpy(cmd.peer_mld_address, addr, ETH_ALEN);
  88		memcpy(cmd.peer_link_address, addr, ETH_ALEN);
  89	}
  90
  91	return iwl_mvm_mld_send_sta_cmd(mvm, &cmd);
  92}
  93
  94/*
  95 * Remove a station from the FW table. Before sending the command to remove
  96 * the station validate that the station is indeed known to the driver (sanity
  97 * only).
  98 */
  99static int iwl_mvm_mld_rm_sta_from_fw(struct iwl_mvm *mvm, u32 sta_id)
 100{
 101	struct iwl_remove_sta_cmd rm_sta_cmd = {
 102		.sta_id = cpu_to_le32(sta_id),
 103	};
 104	int ret;
 105
 106	/* Note: internal stations are marked as error values */
 107	if (!rcu_access_pointer(mvm->fw_id_to_mac_id[sta_id])) {
 108		IWL_ERR(mvm, "Invalid station id %d\n", sta_id);
 109		return -EINVAL;
 110	}
 111
 112	ret = iwl_mvm_send_cmd_pdu(mvm, WIDE_ID(MAC_CONF_GROUP, STA_REMOVE_CMD),
 113				   0, sizeof(rm_sta_cmd), &rm_sta_cmd);
 114	if (ret) {
 115		IWL_ERR(mvm, "Failed to remove station. Id=%d\n", sta_id);
 116		return ret;
 117	}
 118
 119	return 0;
 120}
 121
 122static int iwl_mvm_add_aux_sta_to_fw(struct iwl_mvm *mvm,
 123				     struct iwl_mvm_int_sta *sta,
 124				     u32 lmac_id)
 125{
 126	int ret;
 127
 128	struct iwl_aux_sta_cmd cmd = {
 129		.sta_id = cpu_to_le32(sta->sta_id),
 130		.lmac_id = cpu_to_le32(lmac_id),
 131	};
 132
 133	ret = iwl_mvm_send_cmd_pdu(mvm, WIDE_ID(MAC_CONF_GROUP, AUX_STA_CMD),
 134				   0, sizeof(cmd), &cmd);
 135	if (ret)
 136		IWL_ERR(mvm, "Failed to send AUX_STA_CMD\n");
 137	return ret;
 138}
 139
 140/*
 141 * Adds an internal sta to the FW table with its queues
 142 */
 143int iwl_mvm_mld_add_int_sta_with_queue(struct iwl_mvm *mvm,
 144				       struct iwl_mvm_int_sta *sta,
 145				       const u8 *addr, int link_id,
 146				       u16 *queue, u8 tid,
 147				       unsigned int *_wdg_timeout)
 148{
 149	int ret, txq;
 150	unsigned int wdg_timeout = _wdg_timeout ? *_wdg_timeout :
 151		mvm->trans->mac_cfg->base->wd_timeout;
 152
 153	if (WARN_ON_ONCE(sta->sta_id == IWL_INVALID_STA))
 154		return -ENOSPC;
 155
 156	if (sta->type == STATION_TYPE_AUX)
 157		ret = iwl_mvm_add_aux_sta_to_fw(mvm, sta, link_id);
 158	else
 159		ret = iwl_mvm_mld_add_int_sta_to_fw(mvm, sta, addr, link_id);
 160	if (ret)
 161		return ret;
 162
 163	/*
 164	 * For 22000 firmware and on we cannot add queue to a station unknown
 165	 * to firmware so enable queue here - after the station was added
 166	 */
 167	txq = iwl_mvm_tvqm_enable_txq(mvm, NULL, sta->sta_id, tid,
 168				      wdg_timeout);
 169	if (txq < 0) {
 170		iwl_mvm_mld_rm_sta_from_fw(mvm, sta->sta_id);
 171		return txq;
 172	}
 173	*queue = txq;
 174
 175	return 0;
 176}
 177
 178/*
 179 * Adds a new int sta: allocate it in the driver, add it to the FW table,
 180 * and add its queues.
 181 */
 182static int iwl_mvm_mld_add_int_sta(struct iwl_mvm *mvm,
 183				   struct iwl_mvm_int_sta *int_sta, u16 *queue,
 184				   enum nl80211_iftype iftype,
 185				   enum iwl_fw_sta_type sta_type,
 186				   int link_id, const u8 *addr, u8 tid,
 187				   unsigned int *wdg_timeout)
 188{
 189	int ret;
 190
 191	lockdep_assert_held(&mvm->mutex);
 192
 193	/* qmask argument is not used in the new tx api, send a don't care */
 194	ret = iwl_mvm_allocate_int_sta(mvm, int_sta, 0, iftype,
 195				       sta_type);
 196	if (ret)
 197		return ret;
 198
 199	ret = iwl_mvm_mld_add_int_sta_with_queue(mvm, int_sta, addr, link_id,
 200						 queue, tid, wdg_timeout);
 201	if (ret) {
 202		iwl_mvm_dealloc_int_sta(mvm, int_sta);
 203		return ret;
 204	}
 205
 206	return 0;
 207}
 208
 209/* Allocate a new station entry for the broadcast station to the given vif,
 210 * and send it to the FW.
 211 * Note that each P2P mac should have its own broadcast station.
 212 */
 213int iwl_mvm_mld_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 214			      struct ieee80211_bss_conf *link_conf)
 215{
 216	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 217	struct iwl_mvm_vif_link_info *mvm_link =
 218		mvmvif->link[link_conf->link_id];
 219	struct iwl_mvm_int_sta *bsta = &mvm_link->bcast_sta;
 220	static const u8 _baddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 221	const u8 *baddr = _baddr;
 222	unsigned int wdg_timeout =
 223		iwl_mvm_get_wd_timeout(mvm, vif);
 224	u16 *queue;
 225
 226	lockdep_assert_held(&mvm->mutex);
 227
 228	if (vif->type == NL80211_IFTYPE_ADHOC)
 229		baddr = link_conf->bssid;
 230
 231	if (vif->type == NL80211_IFTYPE_AP ||
 232	    vif->type == NL80211_IFTYPE_ADHOC) {
 233		queue = &mvm_link->mgmt_queue;
 234	} else if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
 235		queue = &mvm->p2p_dev_queue;
 236	} else {
 237		WARN(1, "Missing required TXQ for adding bcast STA\n");
 238		return -EINVAL;
 239	}
 240
 241	return iwl_mvm_mld_add_int_sta(mvm, bsta, queue,
 242				       ieee80211_vif_type_p2p(vif),
 243				       STATION_TYPE_BCAST_MGMT,
 244				       mvm_link->fw_link_id, baddr,
 245				       IWL_MAX_TID_COUNT, &wdg_timeout);
 246}
 247
 248/* Allocate a new station entry for the multicast station to the given vif,
 249 * and send it to the FW.
 250 * Note that each AP/GO mac should have its own multicast station.
 251 */
 252int iwl_mvm_mld_add_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 253			      struct ieee80211_bss_conf *link_conf)
 254{
 255	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 256	struct iwl_mvm_vif_link_info *mvm_link =
 257		mvmvif->link[link_conf->link_id];
 258	struct iwl_mvm_int_sta *msta = &mvm_link->mcast_sta;
 259	static const u8 _maddr[] = {0x03, 0x00, 0x00, 0x00, 0x00, 0x00};
 260	const u8 *maddr = _maddr;
 261	unsigned int timeout = iwl_mvm_get_wd_timeout(mvm, vif);
 262
 263	lockdep_assert_held(&mvm->mutex);
 264
 265	if (WARN_ON(vif->type != NL80211_IFTYPE_AP &&
 266		    vif->type != NL80211_IFTYPE_ADHOC))
 267		return -EOPNOTSUPP;
 268
 269	/* In IBSS, ieee80211_check_queues() sets the cab_queue to be
 270	 * invalid, so make sure we use the queue we want.
 271	 * Note that this is done here as we want to avoid making DQA
 272	 * changes in mac80211 layer.
 273	 */
 274	if (vif->type == NL80211_IFTYPE_ADHOC)
 275		mvm_link->cab_queue = IWL_MVM_DQA_GCAST_QUEUE;
 276
 277	return iwl_mvm_mld_add_int_sta(mvm, msta, &mvm_link->cab_queue,
 278				       vif->type, STATION_TYPE_MCAST,
 279				       mvm_link->fw_link_id, maddr, 0,
 280				       &timeout);
 281}
 282
 283/* Allocate a new station entry for the sniffer station to the given vif,
 284 * and send it to the FW.
 285 */
 286int iwl_mvm_mld_add_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 287			     struct ieee80211_bss_conf *link_conf)
 288{
 289	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 290	struct iwl_mvm_vif_link_info *mvm_link =
 291		mvmvif->link[link_conf->link_id];
 292
 293	lockdep_assert_held(&mvm->mutex);
 294
 295	return iwl_mvm_mld_add_int_sta(mvm, &mvm->snif_sta, &mvm->snif_queue,
 296				       vif->type, STATION_TYPE_BCAST_MGMT,
 297				       mvm_link->fw_link_id, NULL,
 298				       IWL_MAX_TID_COUNT, NULL);
 299}
 300
 301int iwl_mvm_mld_add_aux_sta(struct iwl_mvm *mvm, u32 lmac_id)
 302{
 303	lockdep_assert_held(&mvm->mutex);
 304
 305	/* In CDB NICs we need to specify which lmac to use for aux activity;
 306	 * use the link_id argument place to send lmac_id to the function.
 307	 */
 308	return iwl_mvm_mld_add_int_sta(mvm, &mvm->aux_sta, &mvm->aux_queue,
 309				       NL80211_IFTYPE_UNSPECIFIED,
 310				       STATION_TYPE_AUX, lmac_id, NULL,
 311				       IWL_MAX_TID_COUNT, NULL);
 312}
 313
 314static int iwl_mvm_mld_disable_txq(struct iwl_mvm *mvm, u32 sta_mask,
 315				   u16 *queueptr, u8 tid)
 316{
 317	int queue = *queueptr;
 318	int ret = 0;
 319
 320	if (tid == IWL_MAX_TID_COUNT)
 321		tid = IWL_MGMT_TID;
 322
 323	if (mvm->sta_remove_requires_queue_remove) {
 324		u32 cmd_id = WIDE_ID(DATA_PATH_GROUP,
 325				     SCD_QUEUE_CONFIG_CMD);
 326		struct iwl_scd_queue_cfg_cmd remove_cmd = {
 327			.operation = cpu_to_le32(IWL_SCD_QUEUE_REMOVE),
 328			.u.remove.tid = cpu_to_le32(tid),
 329			.u.remove.sta_mask = cpu_to_le32(sta_mask),
 330		};
 331
 332		ret = iwl_mvm_send_cmd_pdu(mvm, cmd_id, 0,
 333					   sizeof(remove_cmd),
 334					   &remove_cmd);
 335	}
 336
 337	iwl_trans_txq_free(mvm->trans, queue);
 338	*queueptr = IWL_MVM_INVALID_QUEUE;
 339
 340	return ret;
 341}
 342
 343/* Removes a sta from the FW table, disable its queues, and dealloc it
 344 */
 345static int iwl_mvm_mld_rm_int_sta(struct iwl_mvm *mvm,
 346				  struct iwl_mvm_int_sta *int_sta,
 347				  bool flush, u8 tid, u16 *queuptr)
 348{
 349	int ret;
 350
 351	lockdep_assert_held(&mvm->mutex);
 352
 353	if (WARN_ON_ONCE(int_sta->sta_id == IWL_INVALID_STA))
 354		return -EINVAL;
 355
 356	if (flush)
 357		iwl_mvm_flush_sta(mvm, int_sta->sta_id, int_sta->tfd_queue_msk);
 358
 359	iwl_mvm_mld_disable_txq(mvm, BIT(int_sta->sta_id), queuptr, tid);
 360
 361	ret = iwl_mvm_mld_rm_sta_from_fw(mvm, int_sta->sta_id);
 362	if (ret)
 363		IWL_WARN(mvm, "Failed sending remove station\n");
 364
 365	iwl_mvm_dealloc_int_sta(mvm, int_sta);
 366
 367	return ret;
 368}
 369
 370int iwl_mvm_mld_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 371			     struct ieee80211_bss_conf *link_conf)
 372{
 373	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 374	struct iwl_mvm_vif_link_info *link = mvmvif->link[link_conf->link_id];
 375	u16 *queueptr;
 376
 377	lockdep_assert_held(&mvm->mutex);
 378
 379	if (WARN_ON(!link))
 380		return -EIO;
 381
 382	switch (vif->type) {
 383	case NL80211_IFTYPE_AP:
 384	case NL80211_IFTYPE_ADHOC:
 385		queueptr = &link->mgmt_queue;
 386		break;
 387	case NL80211_IFTYPE_P2P_DEVICE:
 388		queueptr = &mvm->p2p_dev_queue;
 389		break;
 390	default:
 391		WARN(1, "Can't free bcast queue on vif type %d\n",
 392		     vif->type);
 393		return -EINVAL;
 394	}
 395
 396	return iwl_mvm_mld_rm_int_sta(mvm, &link->bcast_sta,
 397				      true, IWL_MAX_TID_COUNT, queueptr);
 398}
 399
 400/* Send the FW a request to remove the station from it's internal data
 401 * structures, and in addition remove it from the local data structure.
 402 */
 403int iwl_mvm_mld_rm_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 404			     struct ieee80211_bss_conf *link_conf)
 405{
 406	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 407	struct iwl_mvm_vif_link_info *link = mvmvif->link[link_conf->link_id];
 408
 409	lockdep_assert_held(&mvm->mutex);
 410
 411	if (WARN_ON(!link))
 412		return -EIO;
 413
 414	return iwl_mvm_mld_rm_int_sta(mvm, &link->mcast_sta, true, 0,
 415				      &link->cab_queue);
 416}
 417
 418int iwl_mvm_mld_rm_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 419{
 420	lockdep_assert_held(&mvm->mutex);
 421
 422	return iwl_mvm_mld_rm_int_sta(mvm, &mvm->snif_sta, false,
 423				      IWL_MAX_TID_COUNT, &mvm->snif_queue);
 424}
 425
 426int iwl_mvm_mld_rm_aux_sta(struct iwl_mvm *mvm)
 427{
 428	lockdep_assert_held(&mvm->mutex);
 429
 430	return iwl_mvm_mld_rm_int_sta(mvm, &mvm->aux_sta, false,
 431				      IWL_MAX_TID_COUNT, &mvm->aux_queue);
 432}
 433
 434/* send a cfg sta command to add/update a sta in firmware */
 435static int iwl_mvm_mld_cfg_sta(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 436			       struct ieee80211_vif *vif,
 437			       struct ieee80211_link_sta *link_sta,
 438			       struct ieee80211_bss_conf *link_conf,
 439			       struct iwl_mvm_link_sta *mvm_link_sta)
 440{
 441	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 442	struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif);
 443	struct iwl_mvm_vif_link_info *link_info =
 444					mvm_vif->link[link_conf->link_id];
 445	struct iwl_sta_cfg_cmd cmd = {
 446		.sta_id = cpu_to_le32(mvm_link_sta->sta_id),
 447		.station_type = cpu_to_le32(mvm_sta->sta_type),
 448	};
 449	u32 agg_size = 0, mpdu_dens = 0;
 450
 451	/* when adding sta, link should exist in FW */
 452	if (WARN_ON(link_info->fw_link_id == IWL_MVM_FW_LINK_ID_INVALID))
 453		return -EINVAL;
 454
 455	cmd.link_id = cpu_to_le32(link_info->fw_link_id);
 456
 457	memcpy(&cmd.peer_mld_address, sta->addr, ETH_ALEN);
 458	memcpy(&cmd.peer_link_address, link_sta->addr, ETH_ALEN);
 459
 460	if (mvm_sta->sta_state >= IEEE80211_STA_ASSOC)
 461		cmd.assoc_id = cpu_to_le32(sta->aid);
 462
 463	if (fw_has_capa(&mvm->fw->ucode_capa,
 464			IWL_UCODE_TLV_CAPA_STA_EXP_MFP_SUPPORT) &&
 465	    (sta->mfp || mvm_sta->sta_state < IEEE80211_STA_AUTHORIZED))
 466		cmd.mfp = cpu_to_le32(1);
 467
 468	switch (link_sta->rx_nss) {
 469	case 1:
 470		cmd.mimo = cpu_to_le32(0);
 471		break;
 472	case 2 ... 8:
 473		cmd.mimo = cpu_to_le32(1);
 474		break;
 475	}
 476
 477	switch (link_sta->smps_mode) {
 478	case IEEE80211_SMPS_AUTOMATIC:
 479	case IEEE80211_SMPS_NUM_MODES:
 480		WARN_ON(1);
 481		break;
 482	case IEEE80211_SMPS_STATIC:
 483		/* override NSS */
 484		cmd.mimo = cpu_to_le32(0);
 485		break;
 486	case IEEE80211_SMPS_DYNAMIC:
 487		cmd.mimo_protection = cpu_to_le32(1);
 488		break;
 489	case IEEE80211_SMPS_OFF:
 490		/* nothing */
 491		break;
 492	}
 493
 494	mpdu_dens = iwl_mvm_get_sta_ampdu_dens(link_sta, link_conf, &agg_size);
 495	cmd.tx_ampdu_spacing = cpu_to_le32(mpdu_dens);
 496	cmd.tx_ampdu_max_size = cpu_to_le32(agg_size);
 497
 498	if (sta->wme) {
 499		cmd.sp_length =
 500			cpu_to_le32(sta->max_sp ? sta->max_sp * 2 : 128);
 501		cmd.uapsd_acs = cpu_to_le32(iwl_mvm_get_sta_uapsd_acs(sta));
 502	}
 503
 504	if (link_sta->he_cap.has_he) {
 505		cmd.trig_rnd_alloc =
 506			cpu_to_le32(link_conf->uora_exists ? 1 : 0);
 507
 508		/* PPE Thresholds */
 509		iwl_mvm_set_sta_pkt_ext(mvm, link_sta, &cmd.pkt_ext);
 510
 511		/* HTC flags */
 512		cmd.htc_flags = iwl_mvm_get_sta_htc_flags(sta, link_sta);
 513
 514		if (link_sta->he_cap.he_cap_elem.mac_cap_info[2] &
 515		    IEEE80211_HE_MAC_CAP2_ACK_EN)
 516			cmd.ack_enabled = cpu_to_le32(1);
 517	}
 518
 519	return iwl_mvm_mld_send_sta_cmd(mvm, &cmd);
 520}
 521
 522void iwl_mvm_mld_free_sta_link(struct iwl_mvm *mvm,
 523			       struct iwl_mvm_sta *mvm_sta,
 524			       struct iwl_mvm_link_sta *mvm_sta_link,
 525			       unsigned int link_id)
 526{
 527	lockdep_assert_wiphy(mvm->hw->wiphy);
 528	lockdep_assert_held(&mvm->mutex);
 529
 530	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta_link->sta_id], NULL);
 531	RCU_INIT_POINTER(mvm->fw_id_to_link_sta[mvm_sta_link->sta_id], NULL);
 532	RCU_INIT_POINTER(mvm_sta->link[link_id], NULL);
 533
 534	if (mvm_sta_link != &mvm_sta->deflink)
 535		kfree_rcu(mvm_sta_link, rcu_head);
 536}
 537
 538static void iwl_mvm_mld_sta_rm_all_sta_links(struct iwl_mvm *mvm,
 539					     struct iwl_mvm_sta *mvm_sta)
 540{
 541	unsigned int link_id;
 542
 543	for (link_id = 0; link_id < ARRAY_SIZE(mvm_sta->link); link_id++) {
 544		struct iwl_mvm_link_sta *link =
 545			rcu_dereference_protected(mvm_sta->link[link_id],
 546						  lockdep_is_held(&mvm->mutex));
 547
 548		if (!link)
 549			continue;
 550
 551		iwl_mvm_mld_free_sta_link(mvm, mvm_sta, link, link_id);
 552	}
 553}
 554
 555static int iwl_mvm_mld_alloc_sta_link(struct iwl_mvm *mvm,
 556				      struct ieee80211_vif *vif,
 557				      struct ieee80211_sta *sta,
 558				      unsigned int link_id)
 559{
 560	struct ieee80211_link_sta *link_sta =
 561		link_sta_dereference_protected(sta, link_id);
 562	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 563	struct iwl_mvm_link_sta *link;
 564	u32 sta_id = iwl_mvm_find_free_sta_id(mvm,
 565					  ieee80211_vif_type_p2p(vif));
 566
 567	lockdep_assert_wiphy(mvm->hw->wiphy);
 568	lockdep_assert_held(&mvm->mutex);
 569
 570	if (sta_id == IWL_INVALID_STA)
 571		return -ENOSPC;
 572
 573	if (rcu_access_pointer(sta->link[link_id]) == &sta->deflink) {
 574		link = &mvm_sta->deflink;
 575	} else {
 576		link = kzalloc(sizeof(*link), GFP_KERNEL);
 577		if (!link)
 578			return -ENOMEM;
 579	}
 580
 581	link->sta_id = sta_id;
 582	rcu_assign_pointer(mvm_sta->link[link_id], link);
 583	rcu_assign_pointer(mvm->fw_id_to_mac_id[link->sta_id], sta);
 584	rcu_assign_pointer(mvm->fw_id_to_link_sta[link->sta_id],
 585			   link_sta);
 586
 587	return 0;
 588}
 589
 590/* allocate all the links of a sta, called when the station is first added */
 591static int iwl_mvm_mld_alloc_sta_links(struct iwl_mvm *mvm,
 592				       struct ieee80211_vif *vif,
 593				       struct ieee80211_sta *sta)
 594{
 595	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 596	struct ieee80211_link_sta *link_sta;
 597	unsigned int link_id;
 598	int ret;
 599
 600	lockdep_assert_held(&mvm->mutex);
 601
 602	for_each_sta_active_link(vif, sta, link_sta, link_id) {
 603		if (WARN_ON(mvm_sta->link[link_id]))
 604			continue;
 605
 606		ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, link_id);
 607		if (ret)
 608			goto err;
 609	}
 610
 611	return 0;
 612
 613err:
 614	iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta);
 615	return ret;
 616}
 617
 618static void iwl_mvm_mld_set_ap_sta_id(struct ieee80211_sta *sta,
 619				      struct iwl_mvm_vif_link_info *vif_link,
 620				      struct iwl_mvm_link_sta *sta_link)
 621{
 622	if (!sta->tdls) {
 623		WARN_ON(vif_link->ap_sta_id != IWL_INVALID_STA);
 624		vif_link->ap_sta_id = sta_link->sta_id;
 625	} else {
 626		WARN_ON(vif_link->ap_sta_id == IWL_INVALID_STA);
 627	}
 628}
 629
 630static int iwl_mvm_alloc_sta_after_restart(struct iwl_mvm *mvm,
 631					   struct ieee80211_vif *vif,
 632					   struct ieee80211_sta *sta)
 633{
 634	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 635	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 636	struct ieee80211_link_sta *link_sta;
 637	unsigned int link_id;
 638	/* no active link found */
 639	int ret = -EINVAL;
 640	int sta_id;
 641
 642	lockdep_assert_wiphy(mvm->hw->wiphy);
 643	lockdep_assert_held(&mvm->mutex);
 644
 645	/* First add an empty station since allocating a queue requires
 646	 * a valid station. Since we need a link_id to allocate a station,
 647	 * pick up the first valid one.
 648	 */
 649	for_each_sta_active_link(vif, sta, link_sta, link_id) {
 650		struct iwl_mvm_vif_link_info *mvm_link;
 651		struct ieee80211_bss_conf *link_conf =
 652			link_conf_dereference_protected(vif, link_id);
 653		struct iwl_mvm_link_sta *mvm_link_sta =
 654			rcu_dereference_protected(mvm_sta->link[link_id],
 655						  lockdep_is_held(&mvm->mutex));
 656
 657		if (!link_conf)
 658			continue;
 659
 660		mvm_link = mvmvif->link[link_conf->link_id];
 661
 662		if (!mvm_link || !mvm_link_sta)
 663			continue;
 664
 665		sta_id = mvm_link_sta->sta_id;
 666		ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta,
 667					  link_conf, mvm_link_sta);
 668		if (ret)
 669			return ret;
 670
 671		rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);
 672		rcu_assign_pointer(mvm->fw_id_to_link_sta[sta_id], link_sta);
 673		ret = 0;
 674	}
 675
 676	iwl_mvm_realloc_queues_after_restart(mvm, sta);
 677
 678	return ret;
 679}
 680
 681int iwl_mvm_mld_add_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 682			struct ieee80211_sta *sta)
 683{
 684	struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif);
 685	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 686	unsigned long link_sta_added_to_fw = 0;
 687	struct ieee80211_link_sta *link_sta;
 688	int ret = 0;
 689	unsigned int link_id;
 690
 691	lockdep_assert_held(&mvm->mutex);
 692
 693	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
 694		ret = iwl_mvm_mld_alloc_sta_links(mvm, vif, sta);
 695		if (ret)
 696			return ret;
 697
 698		spin_lock_init(&mvm_sta->lock);
 699
 700		ret = iwl_mvm_sta_init(mvm, vif, sta, IWL_INVALID_STA,
 701				       STATION_TYPE_PEER);
 702	} else {
 703		ret = iwl_mvm_alloc_sta_after_restart(mvm, vif, sta);
 704	}
 705
 706	if (ret)
 707		goto err;
 708
 709	/* at this stage sta link pointers are already allocated */
 710	ret = iwl_mvm_mld_update_sta(mvm, vif, sta);
 711	if (ret)
 712		goto err;
 713
 714	for_each_sta_active_link(vif, sta, link_sta, link_id) {
 715		struct ieee80211_bss_conf *link_conf =
 716			link_conf_dereference_protected(vif, link_id);
 717		struct iwl_mvm_link_sta *mvm_link_sta =
 718			rcu_dereference_protected(mvm_sta->link[link_id],
 719						  lockdep_is_held(&mvm->mutex));
 720
 721		if (WARN_ON(!link_conf || !mvm_link_sta)) {
 722			ret = -EINVAL;
 723			goto err;
 724		}
 725
 726		ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
 727					  mvm_link_sta);
 728		if (ret)
 729			goto err;
 730
 731		link_sta_added_to_fw |= BIT(link_id);
 732
 733		if (vif->type == NL80211_IFTYPE_STATION)
 734			iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif->link[link_id],
 735						  mvm_link_sta);
 736	}
 737	return 0;
 738
 739err:
 740	/* remove all already allocated stations in FW */
 741	for_each_set_bit(link_id, &link_sta_added_to_fw,
 742			 IEEE80211_MLD_MAX_NUM_LINKS) {
 743		struct iwl_mvm_link_sta *mvm_link_sta =
 744			rcu_dereference_protected(mvm_sta->link[link_id],
 745						  lockdep_is_held(&mvm->mutex));
 746
 747		iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_link_sta->sta_id);
 748	}
 749
 750	/* free all sta resources in the driver */
 751	iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta);
 752	return ret;
 753}
 754
 755int iwl_mvm_mld_update_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 756			   struct ieee80211_sta *sta)
 757{
 758	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 759	struct ieee80211_link_sta *link_sta;
 760	unsigned int link_id;
 761	int ret = -EINVAL;
 762
 763	lockdep_assert_held(&mvm->mutex);
 764
 765	for_each_sta_active_link(vif, sta, link_sta, link_id) {
 766		struct ieee80211_bss_conf *link_conf =
 767			link_conf_dereference_protected(vif, link_id);
 768		struct iwl_mvm_link_sta *mvm_link_sta =
 769			rcu_dereference_protected(mvm_sta->link[link_id],
 770						  lockdep_is_held(&mvm->mutex));
 771
 772		if (WARN_ON(!link_conf || !mvm_link_sta))
 773			return -EINVAL;
 774
 775		ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
 776					  mvm_link_sta);
 777
 778		if (ret) {
 779			IWL_ERR(mvm, "Failed to update sta link %d\n", link_id);
 780			break;
 781		}
 782	}
 783
 784	return ret;
 785}
 786
 787static void iwl_mvm_mld_disable_sta_queues(struct iwl_mvm *mvm,
 788					   struct ieee80211_vif *vif,
 789					   struct ieee80211_sta *sta)
 790{
 791	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 792	u32 sta_mask = iwl_mvm_sta_fw_id_mask(mvm, sta, -1);
 793	int i;
 794
 795	lockdep_assert_held(&mvm->mutex);
 796
 797	for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
 798		if (mvm_sta->tid_data[i].txq_id == IWL_MVM_INVALID_QUEUE)
 799			continue;
 800
 801		iwl_mvm_mld_disable_txq(mvm, sta_mask,
 802					&mvm_sta->tid_data[i].txq_id, i);
 803		mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE;
 804	}
 805
 806	for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
 807		struct iwl_mvm_txq *mvmtxq =
 808			iwl_mvm_txq_from_mac80211(sta->txq[i]);
 809
 810		mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
 811	}
 812}
 813
 814int iwl_mvm_mld_rm_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 815		       struct ieee80211_sta *sta)
 816{
 817	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 818	struct ieee80211_link_sta *link_sta;
 819	unsigned int link_id;
 820	int ret;
 821
 822	lockdep_assert_held(&mvm->mutex);
 823
 824	/* flush its queues here since we are freeing mvm_sta */
 825	for_each_sta_active_link(vif, sta, link_sta, link_id) {
 826		struct iwl_mvm_link_sta *mvm_link_sta =
 827			rcu_dereference_protected(mvm_sta->link[link_id],
 828						  lockdep_is_held(&mvm->mutex));
 829
 830		if (WARN_ON(!mvm_link_sta))
 831			return -EINVAL;
 832
 833		ret = iwl_mvm_flush_sta_tids(mvm, mvm_link_sta->sta_id,
 834					     0xffff);
 835		if (ret)
 836			return ret;
 837	}
 838
 839	ret = iwl_mvm_wait_sta_queues_empty(mvm, mvm_sta);
 840	if (ret)
 841		return ret;
 842
 843	iwl_mvm_mld_disable_sta_queues(mvm, vif, sta);
 844
 845	for_each_sta_active_link(vif, sta, link_sta, link_id) {
 846		struct iwl_mvm_link_sta *mvm_link_sta =
 847			rcu_dereference_protected(mvm_sta->link[link_id],
 848						  lockdep_is_held(&mvm->mutex));
 849		iwl_mvm_sta_del(mvm, vif, sta, link_sta);
 850
 851		ret = iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_link_sta->sta_id);
 852
 853		iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_link_sta, link_id);
 854	}
 855
 856	return ret;
 857}
 858
 859int iwl_mvm_mld_rm_sta_id(struct iwl_mvm *mvm, u8 sta_id)
 860{
 861	int ret;
 862
 863	lockdep_assert_wiphy(mvm->hw->wiphy);
 864	lockdep_assert_held(&mvm->mutex);
 865
 866	if (WARN_ON(sta_id == IWL_INVALID_STA))
 867		return 0;
 868
 869	ret = iwl_mvm_mld_rm_sta_from_fw(mvm, sta_id);
 870
 871	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL);
 872	RCU_INIT_POINTER(mvm->fw_id_to_link_sta[sta_id], NULL);
 873	return ret;
 874}
 875
 876void iwl_mvm_mld_sta_modify_disable_tx(struct iwl_mvm *mvm,
 877				       struct iwl_mvm_sta *mvmsta,
 878				       bool disable)
 879{
 880	struct iwl_mvm_sta_disable_tx_cmd cmd;
 881	int ret;
 882
 883	cmd.sta_id = cpu_to_le32(mvmsta->deflink.sta_id);
 884	cmd.disable = cpu_to_le32(disable);
 885
 886	if (WARN_ON(iwl_mvm_has_no_host_disable_tx(mvm)))
 887		return;
 888
 889	ret = iwl_mvm_send_cmd_pdu(mvm,
 890				   WIDE_ID(MAC_CONF_GROUP, STA_DISABLE_TX_CMD),
 891				   CMD_ASYNC, sizeof(cmd), &cmd);
 892	if (ret)
 893		IWL_ERR(mvm,
 894			"Failed to send STA_DISABLE_TX_CMD command (%d)\n",
 895			ret);
 896}
 897
 898void iwl_mvm_mld_sta_modify_disable_tx_ap(struct iwl_mvm *mvm,
 899					  struct ieee80211_sta *sta,
 900					  bool disable)
 901{
 902	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 903
 904	spin_lock_bh(&mvm_sta->lock);
 905
 906	if (mvm_sta->disable_tx == disable) {
 907		spin_unlock_bh(&mvm_sta->lock);
 908		return;
 909	}
 910
 911	iwl_mvm_mld_sta_modify_disable_tx(mvm, mvm_sta, disable);
 912
 913	spin_unlock_bh(&mvm_sta->lock);
 914}
 915
 916void iwl_mvm_mld_modify_all_sta_disable_tx(struct iwl_mvm *mvm,
 917					   struct iwl_mvm_vif *mvmvif,
 918					   bool disable)
 919{
 920	struct ieee80211_sta *sta;
 921	struct iwl_mvm_sta *mvm_sta;
 922	int i;
 923
 924	rcu_read_lock();
 925
 926	/* Block/unblock all the stations of the given mvmvif */
 927	for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++) {
 928		sta = rcu_dereference(mvm->fw_id_to_mac_id[i]);
 929		if (IS_ERR_OR_NULL(sta))
 930			continue;
 931
 932		mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 933		if (mvm_sta->mac_id_n_color !=
 934		    FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color))
 935			continue;
 936
 937		iwl_mvm_mld_sta_modify_disable_tx(mvm, mvm_sta, disable);
 938	}
 939
 940	rcu_read_unlock();
 941}
 942
 943static int iwl_mvm_mld_update_sta_queues(struct iwl_mvm *mvm,
 944					 struct ieee80211_sta *sta,
 945					 u32 old_sta_mask,
 946					 u32 new_sta_mask)
 947{
 948	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 949	struct iwl_scd_queue_cfg_cmd cmd = {
 950		.operation = cpu_to_le32(IWL_SCD_QUEUE_MODIFY),
 951		.u.modify.old_sta_mask = cpu_to_le32(old_sta_mask),
 952		.u.modify.new_sta_mask = cpu_to_le32(new_sta_mask),
 953	};
 954	struct iwl_host_cmd hcmd = {
 955		.id = WIDE_ID(DATA_PATH_GROUP, SCD_QUEUE_CONFIG_CMD),
 956		.len[0] = sizeof(cmd),
 957		.data[0] = &cmd
 958	};
 959	int tid;
 960	int ret;
 961
 962	lockdep_assert_held(&mvm->mutex);
 963
 964	for (tid = 0; tid <= IWL_MAX_TID_COUNT; tid++) {
 965		struct iwl_mvm_tid_data *tid_data = &mvm_sta->tid_data[tid];
 966		int txq_id = tid_data->txq_id;
 967
 968		if (txq_id == IWL_MVM_INVALID_QUEUE)
 969			continue;
 970
 971		if (tid == IWL_MAX_TID_COUNT)
 972			cmd.u.modify.tid = cpu_to_le32(IWL_MGMT_TID);
 973		else
 974			cmd.u.modify.tid = cpu_to_le32(tid);
 975
 976		ret = iwl_mvm_send_cmd(mvm, &hcmd);
 977		if (ret)
 978			return ret;
 979	}
 980
 981	return 0;
 982}
 983
 984static int iwl_mvm_mld_update_sta_baids(struct iwl_mvm *mvm,
 985					u32 old_sta_mask,
 986					u32 new_sta_mask)
 987{
 988	struct iwl_rx_baid_cfg_cmd cmd = {
 989		.action = cpu_to_le32(IWL_RX_BAID_ACTION_MODIFY),
 990		.modify.old_sta_id_mask = cpu_to_le32(old_sta_mask),
 991		.modify.new_sta_id_mask = cpu_to_le32(new_sta_mask),
 992	};
 993	u32 cmd_id = WIDE_ID(DATA_PATH_GROUP, RX_BAID_ALLOCATION_CONFIG_CMD);
 994	int baid;
 995
 996	/* mac80211 will remove sessions later, but we ignore all that */
 997	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
 998		return 0;
 999
1000	BUILD_BUG_ON(sizeof(struct iwl_rx_baid_cfg_resp) != sizeof(baid));
1001
1002	for (baid = 0; baid < ARRAY_SIZE(mvm->baid_map); baid++) {
1003		struct iwl_mvm_baid_data *data;
1004		int ret;
1005
1006		data = rcu_dereference_protected(mvm->baid_map[baid],
1007						 lockdep_is_held(&mvm->mutex));
1008		if (!data)
1009			continue;
1010
1011		if (!(data->sta_mask & old_sta_mask))
1012			continue;
1013
1014		WARN_ONCE(data->sta_mask != old_sta_mask,
1015			  "BAID data for %d corrupted - expected 0x%x found 0x%x\n",
1016			  baid, old_sta_mask, data->sta_mask);
1017
1018		cmd.modify.tid = cpu_to_le32(data->tid);
1019
1020		ret = iwl_mvm_send_cmd_pdu(mvm, cmd_id, CMD_SEND_IN_RFKILL,
1021					   sizeof(cmd), &cmd);
1022		data->sta_mask = new_sta_mask;
1023		if (ret)
1024			return ret;
1025	}
1026
1027	return 0;
1028}
1029
1030static int iwl_mvm_mld_update_sta_resources(struct iwl_mvm *mvm,
1031					    struct ieee80211_vif *vif,
1032					    struct ieee80211_sta *sta,
1033					    u32 old_sta_mask,
1034					    u32 new_sta_mask)
1035{
1036	int ret;
1037
1038	ret = iwl_mvm_mld_update_sta_queues(mvm, sta,
1039					    old_sta_mask,
1040					    new_sta_mask);
1041	if (ret)
1042		return ret;
1043
1044	ret = iwl_mvm_mld_update_sta_keys(mvm, vif, sta,
1045					  old_sta_mask,
1046					  new_sta_mask);
1047	if (ret)
1048		return ret;
1049
1050	return iwl_mvm_mld_update_sta_baids(mvm, old_sta_mask, new_sta_mask);
1051}
1052
1053int iwl_mvm_mld_update_sta_links(struct iwl_mvm *mvm,
1054				 struct ieee80211_vif *vif,
1055				 struct ieee80211_sta *sta,
1056				 u16 old_links, u16 new_links)
1057{
1058	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1059	struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif);
1060	struct iwl_mvm_link_sta *mvm_sta_link;
1061	struct iwl_mvm_vif_link_info *mvm_vif_link;
1062	unsigned long links_to_add = ~old_links & new_links;
1063	unsigned long links_to_rem = old_links & ~new_links;
1064	unsigned long old_links_long = old_links;
1065	u32 current_sta_mask = 0, sta_mask_added = 0, sta_mask_to_rem = 0;
1066	unsigned long link_sta_added_to_fw = 0, link_sta_allocated = 0;
1067	unsigned int link_id;
1068	int ret;
1069
1070	lockdep_assert_wiphy(mvm->hw->wiphy);
1071	lockdep_assert_held(&mvm->mutex);
1072
1073	for_each_set_bit(link_id, &old_links_long,
1074			 IEEE80211_MLD_MAX_NUM_LINKS) {
1075		mvm_sta_link =
1076			rcu_dereference_protected(mvm_sta->link[link_id],
1077						  lockdep_is_held(&mvm->mutex));
1078
1079		if (WARN_ON(!mvm_sta_link)) {
1080			ret = -EINVAL;
1081			goto err;
1082		}
1083
1084		current_sta_mask |= BIT(mvm_sta_link->sta_id);
1085		if (links_to_rem & BIT(link_id))
1086			sta_mask_to_rem |= BIT(mvm_sta_link->sta_id);
1087	}
1088
1089	if (sta_mask_to_rem) {
1090		ret = iwl_mvm_mld_update_sta_resources(mvm, vif, sta,
1091						       current_sta_mask,
1092						       current_sta_mask &
1093							~sta_mask_to_rem);
1094		if (WARN_ON(ret))
1095			goto err;
1096
1097		current_sta_mask &= ~sta_mask_to_rem;
1098	}
1099
1100	for_each_set_bit(link_id, &links_to_rem, IEEE80211_MLD_MAX_NUM_LINKS) {
1101		mvm_sta_link =
1102			rcu_dereference_protected(mvm_sta->link[link_id],
1103						  lockdep_is_held(&mvm->mutex));
1104		mvm_vif_link = mvm_vif->link[link_id];
1105
1106		if (WARN_ON(!mvm_sta_link || !mvm_vif_link)) {
1107			ret = -EINVAL;
1108			goto err;
1109		}
1110
1111		ret = iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id);
1112		if (WARN_ON(ret))
1113			goto err;
1114
1115		if (vif->type == NL80211_IFTYPE_STATION)
1116			mvm_vif_link->ap_sta_id = IWL_INVALID_STA;
1117
1118		iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id);
1119	}
1120
1121	for_each_set_bit(link_id, &links_to_add, IEEE80211_MLD_MAX_NUM_LINKS) {
1122		struct ieee80211_bss_conf *link_conf =
1123			link_conf_dereference_protected(vif, link_id);
1124		struct ieee80211_link_sta *link_sta =
1125			link_sta_dereference_protected(sta, link_id);
1126		mvm_vif_link = mvm_vif->link[link_id];
1127
1128		if (WARN_ON(!mvm_vif_link || !link_conf || !link_sta)) {
1129			ret = -EINVAL;
1130			goto err;
1131		}
1132
1133		if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
1134			struct iwl_mvm_link_sta *mvm_link_sta =
1135				rcu_dereference_protected(mvm_sta->link[link_id],
1136							  lockdep_is_held(&mvm->mutex));
1137			u32 sta_id;
1138
1139			if (WARN_ON(!mvm_link_sta)) {
1140				ret = -EINVAL;
1141				goto err;
1142			}
1143
1144			sta_id = mvm_link_sta->sta_id;
1145
1146			rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);
1147			rcu_assign_pointer(mvm->fw_id_to_link_sta[sta_id],
1148					   link_sta);
1149		} else {
1150			if (WARN_ON(mvm_sta->link[link_id])) {
1151				ret = -EINVAL;
1152				goto err;
1153			}
1154			ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta,
1155							 link_id);
1156			if (WARN_ON(ret))
1157				goto err;
1158		}
1159
1160		link_sta->agg.max_rc_amsdu_len = 1;
1161		ieee80211_sta_recalc_aggregates(sta);
1162
1163		mvm_sta_link =
1164			rcu_dereference_protected(mvm_sta->link[link_id],
1165						  lockdep_is_held(&mvm->mutex));
1166
1167		if (WARN_ON(!mvm_sta_link)) {
1168			ret = -EINVAL;
1169			goto err;
1170		}
1171
1172		if (vif->type == NL80211_IFTYPE_STATION)
1173			iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif_link,
1174						  mvm_sta_link);
1175
1176		link_sta_allocated |= BIT(link_id);
1177
1178		sta_mask_added |= BIT(mvm_sta_link->sta_id);
1179
1180		ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
1181					  mvm_sta_link);
1182		if (WARN_ON(ret))
1183			goto err;
1184
1185		link_sta_added_to_fw |= BIT(link_id);
1186
1187		iwl_mvm_rs_add_sta_link(mvm, mvm_sta_link);
1188
1189		iwl_mvm_rs_rate_init(mvm, vif, sta, link_conf, link_sta,
1190				     link_conf->chanreq.oper.chan->band);
1191	}
1192
1193	if (sta_mask_added) {
1194		ret = iwl_mvm_mld_update_sta_resources(mvm, vif, sta,
1195						       current_sta_mask,
1196						       current_sta_mask |
1197							sta_mask_added);
1198		if (WARN_ON(ret))
1199			goto err;
1200	}
1201
1202	return 0;
1203
1204err:
1205	/* remove all already allocated stations in FW */
1206	for_each_set_bit(link_id, &link_sta_added_to_fw,
1207			 IEEE80211_MLD_MAX_NUM_LINKS) {
1208		mvm_sta_link =
1209			rcu_dereference_protected(mvm_sta->link[link_id],
1210						  lockdep_is_held(&mvm->mutex));
1211
1212		iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id);
1213	}
1214
1215	/* remove all already allocated station links in driver */
1216	for_each_set_bit(link_id, &link_sta_allocated,
1217			 IEEE80211_MLD_MAX_NUM_LINKS) {
1218		mvm_sta_link =
1219			rcu_dereference_protected(mvm_sta->link[link_id],
1220						  lockdep_is_held(&mvm->mutex));
1221
1222		iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id);
1223	}
1224
1225	return ret;
1226}