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