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

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