drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.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
   7 static int iwl_mvm_mld_mac_add_interface(struct ieee80211_hw *hw,
   8                                          struct ieee80211_vif *vif)
   9 {
  10         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
  11         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
  12         int ret;
  13         int i;
  14
  15         mutex_lock(&mvm->mutex);
  16
  17         mvmvif->mvm = mvm;
  18
  19         /* Not much to do here. The stack will not allow interface
  20          * types or combinations that we didn't advertise, so we
  21          * don't really have to check the types.
  22          */
  23
  24         /* make sure that beacon statistics don't go backwards with FW reset */
  25         if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
  26                 for_each_mvm_vif_valid_link(mvmvif, i)
  27                         mvmvif->link[i]->beacon_stats.accu_num_beacons +=
  28                                 mvmvif->link[i]->beacon_stats.num_beacons;
  29
  30         /* Allocate resources for the MAC context, and add it to the fw  */
  31         ret = iwl_mvm_mac_ctxt_init(mvm, vif);
  32         if (ret)
  33                 goto out_unlock;
  34
  35         rcu_assign_pointer(mvm->vif_id_to_mac[mvmvif->id], vif);
  36
  37         mvmvif->features |= hw->netdev_features;
  38
  39         /* reset deflink MLO parameters */
  40         mvmvif->deflink.fw_link_id = IWL_MVM_FW_LINK_ID_INVALID;
  41         mvmvif->deflink.active = 0;
  42         /* the first link always points to the default one */
  43         mvmvif->link[0] = &mvmvif->deflink;
  44
  45         ret = iwl_mvm_mld_mac_ctxt_add(mvm, vif);
  46         if (ret)
  47                 goto out_unlock;
  48
  49         /* beacon filtering */
  50         ret = iwl_mvm_disable_beacon_filter(mvm, vif, 0);
  51         if (ret)
  52                 goto out_remove_mac;
  53
  54         if (!mvm->bf_allowed_vif &&
  55             vif->type == NL80211_IFTYPE_STATION && !vif->p2p) {
  56                 mvm->bf_allowed_vif = mvmvif;
  57                 vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER |
  58                                      IEEE80211_VIF_SUPPORTS_CQM_RSSI;
  59         }
  60
  61         ret = iwl_mvm_add_link(mvm, vif, &vif->bss_conf);
  62         if (ret)
  63                 goto out_free_bf;
  64
  65         /* Save a pointer to p2p device vif, so it can later be used to
  66          * update the p2p device MAC when a GO is started/stopped
  67          */
  68         if (vif->type == NL80211_IFTYPE_P2P_DEVICE)
  69                 mvm->p2p_device_vif = vif;
  70
  71         ret = iwl_mvm_power_update_mac(mvm);
  72         if (ret)
  73                 goto out_free_bf;
  74
  75         iwl_mvm_tcm_add_vif(mvm, vif);
  76         INIT_DELAYED_WORK(&mvmvif->csa_work,
  77                           iwl_mvm_channel_switch_disconnect_wk);
  78
  79         if (vif->type == NL80211_IFTYPE_MONITOR) {
  80                 mvm->monitor_on = true;
  81                 ieee80211_hw_set(mvm->hw, RX_INCLUDES_FCS);
  82         }
  83
  84         iwl_mvm_vif_dbgfs_add_link(mvm, vif);
  85
  86         if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status) &&
  87             vif->type == NL80211_IFTYPE_STATION && !vif->p2p &&
  88             !mvm->csme_vif && mvm->mei_registered) {
  89                 iwl_mei_set_nic_info(vif->addr, mvm->nvm_data->hw_addr);
  90                 iwl_mei_set_netdev(ieee80211_vif_to_wdev(vif)->netdev);
  91                 mvm->csme_vif = vif;
  92         }
  93
  94         goto out_unlock;
  95
  96  out_free_bf:
  97         if (mvm->bf_allowed_vif == mvmvif) {
  98                 mvm->bf_allowed_vif = NULL;
  99                 vif->driver_flags &= ~(IEEE80211_VIF_BEACON_FILTER |
 100                                        IEEE80211_VIF_SUPPORTS_CQM_RSSI);
 101         }
 102  out_remove_mac:
 103         mvmvif->link[0] = NULL;
 104         iwl_mvm_mld_mac_ctxt_remove(mvm, vif);
 105  out_unlock:
 106         mutex_unlock(&mvm->mutex);
 107
 108         return ret;
 109 }
 110
 111 static void iwl_mvm_mld_mac_remove_interface(struct ieee80211_hw *hw,
 112                                              struct ieee80211_vif *vif)
 113 {
 114         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 115         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 116         struct iwl_probe_resp_data *probe_data;
 117
 118         iwl_mvm_prepare_mac_removal(mvm, vif);
 119
 120         if (!(vif->type == NL80211_IFTYPE_AP ||
 121               vif->type == NL80211_IFTYPE_ADHOC))
 122                 iwl_mvm_tcm_rm_vif(mvm, vif);
 123
 124         mutex_lock(&mvm->mutex);
 125
 126         if (vif == mvm->csme_vif) {
 127                 iwl_mei_set_netdev(NULL);
 128                 mvm->csme_vif = NULL;
 129         }
 130
 131         if (mvm->bf_allowed_vif == mvmvif) {
 132                 mvm->bf_allowed_vif = NULL;
 133                 vif->driver_flags &= ~(IEEE80211_VIF_BEACON_FILTER |
 134                                        IEEE80211_VIF_SUPPORTS_CQM_RSSI);
 135         }
 136
 137         if (vif->bss_conf.ftm_responder)
 138                 memset(&mvm->ftm_resp_stats, 0, sizeof(mvm->ftm_resp_stats));
 139
 140         iwl_mvm_vif_dbgfs_rm_link(mvm, vif);
 141
 142         /* For AP/GO interface, the tear down of the resources allocated to the
 143          * interface is be handled as part of the stop_ap flow.
 144          */
 145         if (vif->type == NL80211_IFTYPE_AP ||
 146             vif->type == NL80211_IFTYPE_ADHOC) {
 147 #ifdef CONFIG_NL80211_TESTMODE
 148                 if (vif == mvm->noa_vif) {
 149                         mvm->noa_vif = NULL;
 150                         mvm->noa_duration = 0;
 151                 }
 152 #endif
 153         }
 154
 155         iwl_mvm_power_update_mac(mvm);
 156
 157         /* Before the interface removal, mac80211 would cancel the ROC, and the
 158          * ROC worker would be scheduled if needed. The worker would be flushed
 159          * in iwl_mvm_prepare_mac_removal() and thus at this point the link is
 160          * not active. So need only to remove the link.
 161          */
 162         if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
 163                 if (mvmvif->deflink.phy_ctxt) {
 164                         iwl_mvm_phy_ctxt_unref(mvm, mvmvif->deflink.phy_ctxt);
 165                         mvmvif->deflink.phy_ctxt = NULL;
 166                 }
 167                 mvm->p2p_device_vif = NULL;
 168                 iwl_mvm_remove_link(mvm, vif, &vif->bss_conf);
 169         } else {
 170                 iwl_mvm_disable_link(mvm, vif, &vif->bss_conf);
 171         }
 172
 173         iwl_mvm_mld_mac_ctxt_remove(mvm, vif);
 174
 175         RCU_INIT_POINTER(mvm->vif_id_to_mac[mvmvif->id], NULL);
 176
 177         probe_data = rcu_dereference_protected(mvmvif->deflink.probe_resp_data,
 178                                                lockdep_is_held(&mvm->mutex));
 179         RCU_INIT_POINTER(mvmvif->deflink.probe_resp_data, NULL);
 180         if (probe_data)
 181                 kfree_rcu(probe_data, rcu_head);
 182
 183         if (vif->type == NL80211_IFTYPE_MONITOR) {
 184                 mvm->monitor_on = false;
 185                 __clear_bit(IEEE80211_HW_RX_INCLUDES_FCS, mvm->hw->flags);
 186         }
 187
 188         mutex_unlock(&mvm->mutex);
 189 }
 190
 191 static unsigned int iwl_mvm_mld_count_active_links(struct ieee80211_vif *vif)
 192 {
 193         unsigned int n_active = 0;
 194         int i;
 195
 196         for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++) {
 197                 struct ieee80211_bss_conf *link_conf;
 198
 199                 link_conf = link_conf_dereference_protected(vif, i);
 200                 if (link_conf &&
 201                     rcu_access_pointer(link_conf->chanctx_conf))
 202                         n_active++;
 203         }
 204
 205         return n_active;
 206 }
 207
 208 static int iwl_mvm_esr_mode_active(struct iwl_mvm *mvm,
 209                                    struct ieee80211_vif *vif)
 210 {
 211         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 212         int link_id, ret = 0;
 213
 214         mvmvif->esr_active = true;
 215
 216         /* Indicate to mac80211 that EML is enabled */
 217         vif->driver_flags |= IEEE80211_VIF_EML_ACTIVE;
 218
 219         iwl_mvm_update_smps_on_active_links(mvm, vif, IWL_MVM_SMPS_REQ_FW,
 220                                             IEEE80211_SMPS_OFF);
 221
 222         for_each_mvm_vif_valid_link(mvmvif, link_id) {
 223                 struct iwl_mvm_vif_link_info *link = mvmvif->link[link_id];
 224
 225                 if (!link->phy_ctxt)
 226                         continue;
 227
 228                 ret = iwl_mvm_phy_send_rlc(mvm, link->phy_ctxt, 2, 2);
 229                 if (ret)
 230                         break;
 231
 232                 link->phy_ctxt->rlc_disabled = true;
 233         }
 234
 235         return ret;
 236 }
 237
 238 static int
 239 __iwl_mvm_mld_assign_vif_chanctx(struct iwl_mvm *mvm,
 240                                  struct ieee80211_vif *vif,
 241                                  struct ieee80211_bss_conf *link_conf,
 242                                  struct ieee80211_chanctx_conf *ctx,
 243                                  bool switching_chanctx)
 244 {
 245         u16 *phy_ctxt_id = (u16 *)ctx->drv_priv;
 246         struct iwl_mvm_phy_ctxt *phy_ctxt = &mvm->phy_ctxts[*phy_ctxt_id];
 247         unsigned int n_active = iwl_mvm_mld_count_active_links(vif);
 248         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 249         unsigned int link_id = link_conf->link_id;
 250         int ret;
 251
 252         /* if the assigned one was not counted yet, count it now */
 253         if (!rcu_access_pointer(link_conf->chanctx_conf))
 254                 n_active++;
 255
 256         if (n_active > iwl_mvm_max_active_links(mvm, vif))
 257                 return -EOPNOTSUPP;
 258
 259         if (WARN_ON_ONCE(!mvmvif->link[link_id]))
 260                 return -EINVAL;
 261
 262         /* mac parameters such as HE support can change at this stage
 263          * For sta, need first to configure correct state from drv_sta_state
 264          * and only after that update mac config.
 265          */
 266         if (vif->type == NL80211_IFTYPE_AP) {
 267                 ret = iwl_mvm_mld_mac_ctxt_changed(mvm, vif, false);
 268                 if (ret) {
 269                         IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
 270                         return -EINVAL;
 271                 }
 272         }
 273
 274         mvmvif->link[link_id]->phy_ctxt = phy_ctxt;
 275
 276         if (iwl_mvm_is_esr_supported(mvm->fwrt.trans) && n_active > 1) {
 277                 mvmvif->link[link_id]->listen_lmac = true;
 278                 ret = iwl_mvm_esr_mode_active(mvm, vif);
 279                 if (ret) {
 280                         IWL_ERR(mvm, "failed to activate ESR mode (%d)\n", ret);
 281                         goto out;
 282                 }
 283         }
 284
 285         if (switching_chanctx) {
 286                 /* reactivate if we turned this off during channel switch */
 287                 if (vif->type == NL80211_IFTYPE_AP)
 288                         mvmvif->ap_ibss_active = true;
 289         }
 290
 291         /* send it first with phy context ID */
 292         ret = iwl_mvm_link_changed(mvm, vif, link_conf, 0, false);
 293         if (ret)
 294                 goto out;
 295
 296         /* Initialize rate control for the AP station, since we might be
 297          * doing a link switch here - we cannot initialize it before since
 298          * this needs the phy context assigned (and in FW?), and we cannot
 299          * do it later because it needs to be initialized as soon as we're
 300          * able to TX on the link, i.e. when active.
 301          *
 302          * Firmware restart isn't quite correct yet for MLO, but we don't
 303          * need to do it in that case anyway since it will happen from the
 304          * normal station state callback.
 305          */
 306         if (mvmvif->ap_sta &&
 307             !test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
 308                 struct ieee80211_link_sta *link_sta;
 309
 310                 rcu_read_lock();
 311                 link_sta = rcu_dereference(mvmvif->ap_sta->link[link_id]);
 312
 313                 if (!WARN_ON_ONCE(!link_sta))
 314                         iwl_mvm_rs_rate_init(mvm, vif, mvmvif->ap_sta,
 315                                              link_conf, link_sta,
 316                                              phy_ctxt->channel->band);
 317                 rcu_read_unlock();
 318         }
 319
 320         /* then activate */
 321         ret = iwl_mvm_link_changed(mvm, vif, link_conf,
 322                                    LINK_CONTEXT_MODIFY_ACTIVE |
 323                                    LINK_CONTEXT_MODIFY_RATES_INFO,
 324                                    true);
 325         if (ret)
 326                 goto out;
 327
 328         /*
 329          * Power state must be updated before quotas,
 330          * otherwise fw will complain.
 331          */
 332         iwl_mvm_power_update_mac(mvm);
 333
 334         if (vif->type == NL80211_IFTYPE_MONITOR) {
 335                 ret = iwl_mvm_mld_add_snif_sta(mvm, vif, link_conf);
 336                 if (ret)
 337                         goto deactivate;
 338         }
 339
 340         return 0;
 341
 342 deactivate:
 343         iwl_mvm_link_changed(mvm, vif, link_conf, LINK_CONTEXT_MODIFY_ACTIVE,
 344                              false);
 345 out:
 346         mvmvif->link[link_id]->phy_ctxt = NULL;
 347         iwl_mvm_power_update_mac(mvm);
 348         return ret;
 349 }
 350
 351 static int iwl_mvm_mld_assign_vif_chanctx(struct ieee80211_hw *hw,
 352                                           struct ieee80211_vif *vif,
 353                                           struct ieee80211_bss_conf *link_conf,
 354                                           struct ieee80211_chanctx_conf *ctx)
 355 {
 356         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 357         int ret;
 358
 359         mutex_lock(&mvm->mutex);
 360         ret = __iwl_mvm_mld_assign_vif_chanctx(mvm, vif, link_conf, ctx, false);
 361         mutex_unlock(&mvm->mutex);
 362
 363         return ret;
 364 }
 365
 366 static int iwl_mvm_esr_mode_inactive(struct iwl_mvm *mvm,
 367                                      struct ieee80211_vif *vif)
 368 {
 369         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 370         struct ieee80211_bss_conf *link_conf;
 371         int link_id, ret = 0;
 372
 373         mvmvif->esr_active = false;
 374
 375         vif->driver_flags &= ~IEEE80211_VIF_EML_ACTIVE;
 376
 377         iwl_mvm_update_smps_on_active_links(mvm, vif, IWL_MVM_SMPS_REQ_FW,
 378                                             IEEE80211_SMPS_AUTOMATIC);
 379
 380         for_each_vif_active_link(vif, link_conf, link_id) {
 381                 struct ieee80211_chanctx_conf *chanctx_conf;
 382                 struct iwl_mvm_phy_ctxt *phy_ctxt;
 383                 u8 static_chains, dynamic_chains;
 384
 385                 mvmvif->link[link_id]->listen_lmac = false;
 386
 387                 rcu_read_lock();
 388
 389                 chanctx_conf = rcu_dereference(link_conf->chanctx_conf);
 390                 phy_ctxt = mvmvif->link[link_id]->phy_ctxt;
 391
 392                 if (!chanctx_conf || !phy_ctxt) {
 393                         rcu_read_unlock();
 394                         continue;
 395                 }
 396
 397                 phy_ctxt->rlc_disabled = false;
 398                 static_chains = chanctx_conf->rx_chains_static;
 399                 dynamic_chains = chanctx_conf->rx_chains_dynamic;
 400
 401                 rcu_read_unlock();
 402
 403                 ret = iwl_mvm_phy_send_rlc(mvm, phy_ctxt, static_chains,
 404                                            dynamic_chains);
 405                 if (ret)
 406                         break;
 407         }
 408
 409         return ret;
 410 }
 411
 412 static void
 413 __iwl_mvm_mld_unassign_vif_chanctx(struct iwl_mvm *mvm,
 414                                    struct ieee80211_vif *vif,
 415                                    struct ieee80211_bss_conf *link_conf,
 416                                    struct ieee80211_chanctx_conf *ctx,
 417                                    bool switching_chanctx)
 418
 419 {
 420         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 421         unsigned int n_active = iwl_mvm_mld_count_active_links(vif);
 422         unsigned int link_id = link_conf->link_id;
 423
 424         /* shouldn't happen, but verify link_id is valid before accessing */
 425         if (WARN_ON_ONCE(!mvmvif->link[link_id]))
 426                 return;
 427
 428         if (vif->type == NL80211_IFTYPE_AP && switching_chanctx) {
 429                 mvmvif->csa_countdown = false;
 430
 431                 /* Set CS bit on all the stations */
 432                 iwl_mvm_modify_all_sta_disable_tx(mvm, mvmvif, true);
 433
 434                 /* Save blocked iface, the timeout is set on the next beacon */
 435                 rcu_assign_pointer(mvm->csa_tx_blocked_vif, vif);
 436
 437                 mvmvif->ap_ibss_active = false;
 438         }
 439
 440         if (iwl_mvm_is_esr_supported(mvm->fwrt.trans) && n_active > 1) {
 441                 int ret = iwl_mvm_esr_mode_inactive(mvm, vif);
 442
 443                 if (ret)
 444                         IWL_ERR(mvm, "failed to deactivate ESR mode (%d)\n",
 445                                 ret);
 446         }
 447
 448         if (vif->type == NL80211_IFTYPE_MONITOR)
 449                 iwl_mvm_mld_rm_snif_sta(mvm, vif);
 450
 451         iwl_mvm_link_changed(mvm, vif, link_conf,
 452                              LINK_CONTEXT_MODIFY_ACTIVE, false);
 453
 454         if (switching_chanctx)
 455                 return;
 456         mvmvif->link[link_id]->phy_ctxt = NULL;
 457         iwl_mvm_power_update_mac(mvm);
 458 }
 459
 460 static void iwl_mvm_mld_unassign_vif_chanctx(struct ieee80211_hw *hw,
 461                                              struct ieee80211_vif *vif,
 462                                              struct ieee80211_bss_conf *link_conf,
 463                                              struct ieee80211_chanctx_conf *ctx)
 464 {
 465         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 466         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 467
 468         mutex_lock(&mvm->mutex);
 469         __iwl_mvm_mld_unassign_vif_chanctx(mvm, vif, link_conf, ctx, false);
 470         /* in the non-MLD case, remove/re-add the link to clean up FW state */
 471         if (!ieee80211_vif_is_mld(vif) && !mvmvif->ap_sta &&
 472             !WARN_ON_ONCE(vif->cfg.assoc)) {
 473                 iwl_mvm_remove_link(mvm, vif, link_conf);
 474                 iwl_mvm_add_link(mvm, vif, link_conf);
 475         }
 476         mutex_unlock(&mvm->mutex);
 477 }
 478
 479 static int iwl_mvm_mld_start_ap_ibss(struct ieee80211_hw *hw,
 480                                      struct ieee80211_vif *vif,
 481                                      struct ieee80211_bss_conf *link_conf)
 482 {
 483         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 484         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 485         int ret;
 486
 487         mutex_lock(&mvm->mutex);
 488         /* Send the beacon template */
 489         ret = iwl_mvm_mac_ctxt_beacon_changed(mvm, vif, link_conf);
 490         if (ret)
 491                 goto out_unlock;
 492
 493         /* the link should be already activated when assigning chan context */
 494         ret = iwl_mvm_link_changed(mvm, vif, link_conf,
 495                                    LINK_CONTEXT_MODIFY_ALL &
 496                                    ~LINK_CONTEXT_MODIFY_ACTIVE,
 497                                    true);
 498         if (ret)
 499                 goto out_unlock;
 500
 501         ret = iwl_mvm_mld_add_mcast_sta(mvm, vif, link_conf);
 502         if (ret)
 503                 goto out_unlock;
 504
 505         /* Send the bcast station. At this stage the TBTT and DTIM time
 506          * events are added and applied to the scheduler
 507          */
 508         ret = iwl_mvm_mld_add_bcast_sta(mvm, vif, link_conf);
 509         if (ret)
 510                 goto out_rm_mcast;
 511
 512         if (iwl_mvm_start_ap_ibss_common(hw, vif, &ret))
 513                 goto out_failed;
 514
 515         /* Need to update the P2P Device MAC (only GO, IBSS is single vif) */
 516         if (vif->p2p && mvm->p2p_device_vif)
 517                 iwl_mvm_mld_mac_ctxt_changed(mvm, mvm->p2p_device_vif, false);
 518
 519         iwl_mvm_bt_coex_vif_change(mvm);
 520
 521         /* we don't support TDLS during DCM */
 522         if (iwl_mvm_phy_ctx_count(mvm) > 1)
 523                 iwl_mvm_teardown_tdls_peers(mvm);
 524
 525         iwl_mvm_ftm_restart_responder(mvm, vif, link_conf);
 526
 527         goto out_unlock;
 528
 529 out_failed:
 530         iwl_mvm_power_update_mac(mvm);
 531         mvmvif->ap_ibss_active = false;
 532         iwl_mvm_mld_rm_bcast_sta(mvm, vif, link_conf);
 533 out_rm_mcast:
 534         iwl_mvm_mld_rm_mcast_sta(mvm, vif, link_conf);
 535 out_unlock:
 536         mutex_unlock(&mvm->mutex);
 537         return ret;
 538 }
 539
 540 static int iwl_mvm_mld_start_ap(struct ieee80211_hw *hw,
 541                                 struct ieee80211_vif *vif,
 542                                 struct ieee80211_bss_conf *link_conf)
 543 {
 544         return iwl_mvm_mld_start_ap_ibss(hw, vif, link_conf);
 545 }
 546
 547 static int iwl_mvm_mld_start_ibss(struct ieee80211_hw *hw,
 548                                   struct ieee80211_vif *vif)
 549 {
 550         return iwl_mvm_mld_start_ap_ibss(hw, vif, &vif->bss_conf);
 551 }
 552
 553 static void iwl_mvm_mld_stop_ap_ibss(struct ieee80211_hw *hw,
 554                                      struct ieee80211_vif *vif,
 555                                      struct ieee80211_bss_conf *link_conf)
 556 {
 557         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 558
 559         mutex_lock(&mvm->mutex);
 560
 561         iwl_mvm_stop_ap_ibss_common(mvm, vif);
 562
 563         /* Need to update the P2P Device MAC (only GO, IBSS is single vif) */
 564         if (vif->p2p && mvm->p2p_device_vif)
 565                 iwl_mvm_mld_mac_ctxt_changed(mvm, mvm->p2p_device_vif, false);
 566
 567         iwl_mvm_ftm_responder_clear(mvm, vif);
 568
 569         iwl_mvm_mld_rm_bcast_sta(mvm, vif, link_conf);
 570         iwl_mvm_mld_rm_mcast_sta(mvm, vif, link_conf);
 571
 572         iwl_mvm_power_update_mac(mvm);
 573         mutex_unlock(&mvm->mutex);
 574 }
 575
 576 static void iwl_mvm_mld_stop_ap(struct ieee80211_hw *hw,
 577                                 struct ieee80211_vif *vif,
 578                                 struct ieee80211_bss_conf *link_conf)
 579 {
 580         iwl_mvm_mld_stop_ap_ibss(hw, vif, link_conf);
 581 }
 582
 583 static void iwl_mvm_mld_stop_ibss(struct ieee80211_hw *hw,
 584                                   struct ieee80211_vif *vif)
 585 {
 586         iwl_mvm_mld_stop_ap_ibss(hw, vif, &vif->bss_conf);
 587 }
 588
 589 static int iwl_mvm_mld_mac_sta_state(struct ieee80211_hw *hw,
 590                                      struct ieee80211_vif *vif,
 591                                      struct ieee80211_sta *sta,
 592                                      enum ieee80211_sta_state old_state,
 593                                      enum ieee80211_sta_state new_state)
 594 {
 595         static const struct iwl_mvm_sta_state_ops callbacks = {
 596                 .add_sta = iwl_mvm_mld_add_sta,
 597                 .update_sta = iwl_mvm_mld_update_sta,
 598                 .rm_sta = iwl_mvm_mld_rm_sta,
 599                 .mac_ctxt_changed = iwl_mvm_mld_mac_ctxt_changed,
 600         };
 601
 602         return iwl_mvm_mac_sta_state_common(hw, vif, sta, old_state, new_state,
 603                                             &callbacks);
 604 }
 605
 606 struct iwl_mvm_link_sel_data {
 607         u8 link_id;
 608         enum nl80211_band band;
 609         bool active;
 610 };
 611
 612 static bool iwl_mvm_mld_valid_link_pair(struct iwl_mvm_link_sel_data *a,
 613                                         struct iwl_mvm_link_sel_data *b)
 614 {
 615         return a->band != b->band;
 616 }
 617
 618 void iwl_mvm_mld_select_links(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 619                               bool valid_links_changed)
 620 {
 621         struct iwl_mvm_link_sel_data data[IEEE80211_MLD_MAX_NUM_LINKS];
 622         unsigned long usable_links = ieee80211_vif_usable_links(vif);
 623         u32 max_active_links = iwl_mvm_max_active_links(mvm, vif);
 624         u16 new_active_links;
 625         u8 link_id, n_data = 0, i, j;
 626
 627         if (!IWL_MVM_AUTO_EML_ENABLE)
 628                 return;
 629
 630         if (!ieee80211_vif_is_mld(vif) || usable_links == 1)
 631                 return;
 632
 633         /* The logic below is a simple version that doesn't suit more than 2
 634          * links
 635          */
 636         WARN_ON_ONCE(max_active_links > 2);
 637
 638         /* if only a single active link is supported, assume that the one
 639          * selected by higher layer for connection establishment is the best.
 640          */
 641         if (max_active_links == 1 && !valid_links_changed)
 642                 return;
 643
 644         /* If we are already using the maximal number of active links, don't do
 645          * any change. This can later be optimized to pick a 'better' link pair.
 646          */
 647         if (hweight16(vif->active_links) == max_active_links)
 648                 return;
 649
 650         rcu_read_lock();
 651
 652         for_each_set_bit(link_id, &usable_links, IEEE80211_MLD_MAX_NUM_LINKS) {
 653                 struct ieee80211_bss_conf *link_conf =
 654                         rcu_dereference(vif->link_conf[link_id]);
 655
 656                 if (WARN_ON_ONCE(!link_conf))
 657                         continue;
 658
 659                 data[n_data].link_id = link_id;
 660                 data[n_data].band = link_conf->chandef.chan->band;
 661                 data[n_data].active = vif->active_links & BIT(link_id);
 662                 n_data++;
 663         }
 664
 665         rcu_read_unlock();
 666
 667         /* this is expected to be the current active link */
 668         if (n_data == 1)
 669                 return;
 670
 671         new_active_links = 0;
 672
 673         /* Assume that after association only a single link is active, thus,
 674          * select only the 2nd link
 675          */
 676         if (!valid_links_changed) {
 677                 for (i = 0; i < n_data; i++) {
 678                         if (data[i].active)
 679                                 break;
 680                 }
 681
 682                 if (WARN_ON_ONCE(i == n_data))
 683                         return;
 684
 685                 for (j = 0; j < n_data; j++) {
 686                         if (i == j)
 687                                 continue;
 688
 689                         if (iwl_mvm_mld_valid_link_pair(&data[i], &data[j]))
 690                                 break;
 691                 }
 692
 693                 if (j != n_data)
 694                         new_active_links = BIT(data[i].link_id) |
 695                                 BIT(data[j].link_id);
 696         } else {
 697                 /* Try to find a valid link pair for EMLSR operation. If a pair
 698                  * is not found continue using the current active link.
 699                  */
 700                 for (i = 0; i < n_data; i++) {
 701                         for (j = 0; j < n_data; j++) {
 702                                 if (i == j)
 703                                         continue;
 704
 705                                 if (iwl_mvm_mld_valid_link_pair(&data[i],
 706                                                                 &data[j]))
 707                                         break;
 708                         }
 709
 710                         /* found a valid pair for EMLSR, use it */
 711                         if (j != n_data) {
 712                                 new_active_links = BIT(data[i].link_id) |
 713                                         BIT(data[j].link_id);
 714                                 break;
 715                         }
 716                 }
 717         }
 718
 719         if (!new_active_links)
 720                 return;
 721
 722         if (vif->active_links != new_active_links)
 723                 ieee80211_set_active_links_async(vif, new_active_links);
 724 }
 725
 726 static void
 727 iwl_mvm_mld_link_info_changed_station(struct iwl_mvm *mvm,
 728                                       struct ieee80211_vif *vif,
 729                                       struct ieee80211_bss_conf *link_conf,
 730                                       u64 changes)
 731 {
 732         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 733         bool has_he, has_eht;
 734         u32 link_changes = 0;
 735         int ret;
 736
 737         if (WARN_ON_ONCE(!mvmvif->link[link_conf->link_id]))
 738                 return;
 739
 740         has_he = link_conf->he_support && !iwlwifi_mod_params.disable_11ax;
 741         has_eht = link_conf->eht_support && !iwlwifi_mod_params.disable_11be;
 742
 743         /* Update EDCA params */
 744         if (changes & BSS_CHANGED_QOS && vif->cfg.assoc && link_conf->qos)
 745                 link_changes |= LINK_CONTEXT_MODIFY_QOS_PARAMS;
 746
 747         if (changes & BSS_CHANGED_ERP_SLOT)
 748                 link_changes |= LINK_CONTEXT_MODIFY_RATES_INFO;
 749
 750         if (vif->cfg.assoc && (has_he || has_eht)) {
 751                 IWL_DEBUG_MAC80211(mvm, "Associated in HE mode\n");
 752                 link_changes |= LINK_CONTEXT_MODIFY_HE_PARAMS;
 753         }
 754
 755         /* Update EHT Puncturing info */
 756         if (changes & BSS_CHANGED_EHT_PUNCTURING && vif->cfg.assoc)
 757                 link_changes |= LINK_CONTEXT_MODIFY_EHT_PARAMS;
 758
 759         if (link_changes) {
 760                 ret = iwl_mvm_link_changed(mvm, vif, link_conf, link_changes,
 761                                            true);
 762                 if (ret)
 763                         IWL_ERR(mvm, "failed to update link\n");
 764         }
 765
 766         ret = iwl_mvm_mld_mac_ctxt_changed(mvm, vif, false);
 767         if (ret)
 768                 IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
 769
 770         if (changes & BSS_CHANGED_MLD_VALID_LINKS)
 771                 iwl_mvm_mld_select_links(mvm, vif, true);
 772
 773         memcpy(mvmvif->link[link_conf->link_id]->bssid, link_conf->bssid,
 774                ETH_ALEN);
 775
 776         iwl_mvm_bss_info_changed_station_common(mvm, vif, link_conf, changes);
 777 }
 778
 779 static bool iwl_mvm_mld_vif_have_valid_ap_sta(struct iwl_mvm_vif *mvmvif)
 780 {
 781         int i;
 782
 783         for_each_mvm_vif_valid_link(mvmvif, i) {
 784                 if (mvmvif->link[i]->ap_sta_id != IWL_MVM_INVALID_STA)
 785                         return true;
 786         }
 787
 788         return false;
 789 }
 790
 791 static void iwl_mvm_mld_vif_delete_all_stas(struct iwl_mvm *mvm,
 792                                             struct ieee80211_vif *vif)
 793 {
 794         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 795         int i, ret;
 796
 797         if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
 798                 return;
 799
 800         for_each_mvm_vif_valid_link(mvmvif, i) {
 801                 struct iwl_mvm_vif_link_info *link = mvmvif->link[i];
 802
 803                 if (!link)
 804                         continue;
 805
 806                 iwl_mvm_sec_key_remove_ap(mvm, vif, link, i);
 807                 ret = iwl_mvm_mld_rm_sta_id(mvm, link->ap_sta_id);
 808                 if (ret)
 809                         IWL_ERR(mvm, "failed to remove AP station\n");
 810
 811                 link->ap_sta_id = IWL_MVM_INVALID_STA;
 812         }
 813 }
 814
 815 static void iwl_mvm_mld_vif_cfg_changed_station(struct iwl_mvm *mvm,
 816                                                 struct ieee80211_vif *vif,
 817                                                 u64 changes)
 818 {
 819         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 820         struct ieee80211_bss_conf *link_conf;
 821         bool protect = false;
 822         unsigned int i;
 823         int ret;
 824
 825         /* This might get called without active links during the
 826          * chanctx switch, but we don't care about it anyway.
 827          */
 828         if (changes == BSS_CHANGED_IDLE)
 829                 return;
 830
 831         ret = iwl_mvm_mld_mac_ctxt_changed(mvm, vif, false);
 832         if (ret)
 833                 IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
 834
 835         mvmvif->associated = vif->cfg.assoc;
 836
 837         if (changes & BSS_CHANGED_ASSOC) {
 838                 if (vif->cfg.assoc) {
 839                         /* clear statistics to get clean beacon counter */
 840                         iwl_mvm_request_statistics(mvm, true);
 841                         iwl_mvm_sf_update(mvm, vif, false);
 842                         iwl_mvm_power_vif_assoc(mvm, vif);
 843
 844                         for_each_mvm_vif_valid_link(mvmvif, i) {
 845                                 memset(&mvmvif->link[i]->beacon_stats, 0,
 846                                        sizeof(mvmvif->link[i]->beacon_stats));
 847
 848                                 if (vif->p2p) {
 849                                         iwl_mvm_update_smps(mvm, vif,
 850                                                             IWL_MVM_SMPS_REQ_PROT,
 851                                                             IEEE80211_SMPS_DYNAMIC, i);
 852                                 }
 853
 854                                 rcu_read_lock();
 855                                 link_conf = rcu_dereference(vif->link_conf[i]);
 856                                 if (link_conf && !link_conf->dtim_period)
 857                                         protect = true;
 858                                 rcu_read_unlock();
 859                         }
 860
 861                         if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status) &&
 862                             protect) {
 863                                 /* We are in assoc so only one link is active-
 864                                  * The association link
 865                                  */
 866                                 unsigned int link_id =
 867                                         ffs(vif->active_links) - 1;
 868
 869                                 /* If we're not restarting and still haven't
 870                                  * heard a beacon (dtim period unknown) then
 871                                  * make sure we still have enough minimum time
 872                                  * remaining in the time event, since the auth
 873                                  * might actually have taken quite a while
 874                                  * (especially for SAE) and so the remaining
 875                                  * time could be small without us having heard
 876                                  * a beacon yet.
 877                                  */
 878                                 iwl_mvm_protect_assoc(mvm, vif, 0, link_id);
 879                         }
 880
 881                         iwl_mvm_sf_update(mvm, vif, false);
 882
 883                         /* FIXME: need to decide about misbehaving AP handling */
 884                         iwl_mvm_power_vif_assoc(mvm, vif);
 885                 } else if (iwl_mvm_mld_vif_have_valid_ap_sta(mvmvif)) {
 886                         iwl_mvm_mei_host_disassociated(mvm);
 887
 888                         /* If update fails - SF might be running in associated
 889                          * mode while disassociated - which is forbidden.
 890                          */
 891                         ret = iwl_mvm_sf_update(mvm, vif, false);
 892                         WARN_ONCE(ret &&
 893                                   !test_bit(IWL_MVM_STATUS_HW_RESTART_REQUESTED,
 894                                             &mvm->status),
 895                                   "Failed to update SF upon disassociation\n");
 896
 897                         /* If we get an assert during the connection (after the
 898                          * station has been added, but before the vif is set
 899                          * to associated), mac80211 will re-add the station and
 900                          * then configure the vif. Since the vif is not
 901                          * associated, we would remove the station here and
 902                          * this would fail the recovery.
 903                          */
 904                         iwl_mvm_mld_vif_delete_all_stas(mvm, vif);
 905                 }
 906
 907                 iwl_mvm_bss_info_changed_station_assoc(mvm, vif, changes);
 908         }
 909
 910         if (changes & BSS_CHANGED_PS) {
 911                 ret = iwl_mvm_power_update_mac(mvm);
 912                 if (ret)
 913                         IWL_ERR(mvm, "failed to update power mode\n");
 914         }
 915 }
 916
 917 static void
 918 iwl_mvm_mld_link_info_changed_ap_ibss(struct iwl_mvm *mvm,
 919                                       struct ieee80211_vif *vif,
 920                                       struct ieee80211_bss_conf *link_conf,
 921                                       u64 changes)
 922 {
 923         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 924         u32 link_changes = LINK_CONTEXT_MODIFY_PROTECT_FLAGS |
 925                            LINK_CONTEXT_MODIFY_QOS_PARAMS;
 926
 927         /* Changes will be applied when the AP/IBSS is started */
 928         if (!mvmvif->ap_ibss_active)
 929                 return;
 930
 931         if (link_conf->he_support)
 932                 link_changes |= LINK_CONTEXT_MODIFY_HE_PARAMS;
 933
 934         if (changes & BSS_CHANGED_ERP_SLOT)
 935                 link_changes |= LINK_CONTEXT_MODIFY_RATES_INFO;
 936
 937         if (changes & (BSS_CHANGED_ERP_CTS_PROT | BSS_CHANGED_ERP_SLOT |
 938                        BSS_CHANGED_HT |
 939                        BSS_CHANGED_BANDWIDTH | BSS_CHANGED_QOS |
 940                        BSS_CHANGED_HE_BSS_COLOR) &&
 941                        iwl_mvm_link_changed(mvm, vif, link_conf,
 942                                             link_changes, true))
 943                 IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
 944
 945         /* Need to send a new beacon template to the FW */
 946         if (changes & BSS_CHANGED_BEACON &&
 947             iwl_mvm_mac_ctxt_beacon_changed(mvm, vif, link_conf))
 948                 IWL_WARN(mvm, "Failed updating beacon data\n");
 949
 950         /* FIXME: need to decide if we need FTM responder per link */
 951         if (changes & BSS_CHANGED_FTM_RESPONDER) {
 952                 int ret = iwl_mvm_ftm_start_responder(mvm, vif, link_conf);
 953
 954                 if (ret)
 955                         IWL_WARN(mvm, "Failed to enable FTM responder (%d)\n",
 956                                  ret);
 957         }
 958 }
 959
 960 static void iwl_mvm_mld_link_info_changed(struct ieee80211_hw *hw,
 961                                           struct ieee80211_vif *vif,
 962                                           struct ieee80211_bss_conf *link_conf,
 963                                           u64 changes)
 964 {
 965         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 966
 967         mutex_lock(&mvm->mutex);
 968
 969         switch (vif->type) {
 970         case NL80211_IFTYPE_STATION:
 971                 iwl_mvm_mld_link_info_changed_station(mvm, vif, link_conf,
 972                                                       changes);
 973                 break;
 974         case NL80211_IFTYPE_AP:
 975         case NL80211_IFTYPE_ADHOC:
 976                 iwl_mvm_mld_link_info_changed_ap_ibss(mvm, vif, link_conf,
 977                                                       changes);
 978                 break;
 979         case NL80211_IFTYPE_MONITOR:
 980                 if (changes & BSS_CHANGED_MU_GROUPS)
 981                         iwl_mvm_update_mu_groups(mvm, vif);
 982                 break;
 983         default:
 984                 /* shouldn't happen */
 985                 WARN_ON_ONCE(1);
 986         }
 987
 988         if (changes & BSS_CHANGED_TXPOWER) {
 989                 IWL_DEBUG_CALIB(mvm, "Changing TX Power to %d dBm\n",
 990                                 link_conf->txpower);
 991                 iwl_mvm_set_tx_power(mvm, vif, link_conf->txpower);
 992         }
 993
 994         mutex_unlock(&mvm->mutex);
 995 }
 996
 997 static void iwl_mvm_mld_vif_cfg_changed(struct ieee80211_hw *hw,
 998                                         struct ieee80211_vif *vif,
 999                                         u64 changes)
1000 {
1001         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1002
1003         mutex_lock(&mvm->mutex);
1004
1005         if (changes & BSS_CHANGED_IDLE && !vif->cfg.idle)
1006                 iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_SCHED, true);
1007
1008         if (vif->type == NL80211_IFTYPE_STATION)
1009                 iwl_mvm_mld_vif_cfg_changed_station(mvm, vif, changes);
1010
1011         mutex_unlock(&mvm->mutex);
1012 }
1013
1014 static int
1015 iwl_mvm_mld_switch_vif_chanctx(struct ieee80211_hw *hw,
1016                                struct ieee80211_vif_chanctx_switch *vifs,
1017                                int n_vifs,
1018                                enum ieee80211_chanctx_switch_mode mode)
1019 {
1020         static const struct iwl_mvm_switch_vif_chanctx_ops ops = {
1021                 .__assign_vif_chanctx = __iwl_mvm_mld_assign_vif_chanctx,
1022                 .__unassign_vif_chanctx = __iwl_mvm_mld_unassign_vif_chanctx,
1023         };
1024
1025         return iwl_mvm_switch_vif_chanctx_common(hw, vifs, n_vifs, mode, &ops);
1026 }
1027
1028 static void iwl_mvm_mld_config_iface_filter(struct ieee80211_hw *hw,
1029                                             struct ieee80211_vif *vif,
1030                                             unsigned int filter_flags,
1031                                             unsigned int changed_flags)
1032 {
1033         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1034
1035         /* We support only filter for probe requests */
1036         if (!(changed_flags & FIF_PROBE_REQ))
1037                 return;
1038
1039         /* Supported only for p2p client interfaces */
1040         if (vif->type != NL80211_IFTYPE_STATION || !vif->cfg.assoc ||
1041             !vif->p2p)
1042                 return;
1043
1044         mutex_lock(&mvm->mutex);
1045         iwl_mvm_mld_mac_ctxt_changed(mvm, vif, false);
1046         mutex_unlock(&mvm->mutex);
1047 }
1048
1049 static int
1050 iwl_mvm_mld_mac_conf_tx(struct ieee80211_hw *hw,
1051                         struct ieee80211_vif *vif,
1052                         unsigned int link_id, u16 ac,
1053                         const struct ieee80211_tx_queue_params *params)
1054 {
1055         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1056         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1057         struct iwl_mvm_vif_link_info *mvm_link = mvmvif->link[link_id];
1058
1059         if (!mvm_link)
1060                 return -EINVAL;
1061
1062         mvm_link->queue_params[ac] = *params;
1063
1064         /* No need to update right away, we'll get BSS_CHANGED_QOS
1065          * The exception is P2P_DEVICE interface which needs immediate update.
1066          */
1067         if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
1068                 int ret;
1069
1070                 mutex_lock(&mvm->mutex);
1071                 ret = iwl_mvm_link_changed(mvm, vif, &vif->bss_conf,
1072                                            LINK_CONTEXT_MODIFY_QOS_PARAMS,
1073                                            true);
1074                 mutex_unlock(&mvm->mutex);
1075                 return ret;
1076         }
1077         return 0;
1078 }
1079
1080 static int iwl_mvm_mld_roc_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
1081 {
1082         int ret;
1083
1084         lockdep_assert_held(&mvm->mutex);
1085
1086         /* The PHY context ID might have changed so need to set it */
1087         ret = iwl_mvm_link_changed(mvm, vif, &vif->bss_conf, 0, false);
1088         if (WARN(ret, "Failed to set PHY context ID\n"))
1089                 return ret;
1090
1091         ret = iwl_mvm_link_changed(mvm, vif, &vif->bss_conf,
1092                                    LINK_CONTEXT_MODIFY_ACTIVE |
1093                                    LINK_CONTEXT_MODIFY_RATES_INFO,
1094                                    true);
1095
1096         if (WARN(ret, "Failed linking P2P_DEVICE\n"))
1097                 return ret;
1098
1099         /* The station and queue allocation must be done only after the linking
1100          * is done, as otherwise the FW might incorrectly configure its state.
1101          */
1102         return iwl_mvm_mld_add_bcast_sta(mvm, vif, &vif->bss_conf);
1103 }
1104
1105 static int iwl_mvm_mld_roc(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
1106                            struct ieee80211_channel *channel, int duration,
1107                            enum ieee80211_roc_type type)
1108 {
1109         static const struct iwl_mvm_roc_ops ops = {
1110                 .add_aux_sta_for_hs20 = iwl_mvm_mld_add_aux_sta,
1111                 .link = iwl_mvm_mld_roc_link,
1112         };
1113
1114         return iwl_mvm_roc_common(hw, vif, channel, duration, type, &ops);
1115 }
1116
1117 static int
1118 iwl_mvm_mld_change_vif_links(struct ieee80211_hw *hw,
1119                              struct ieee80211_vif *vif,
1120                              u16 old_links, u16 new_links,
1121                              struct ieee80211_bss_conf *old[IEEE80211_MLD_MAX_NUM_LINKS])
1122 {
1123         struct iwl_mvm_vif_link_info *new_link[IEEE80211_MLD_MAX_NUM_LINKS] = {};
1124         unsigned int n_active = iwl_mvm_mld_count_active_links(vif);
1125         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1126         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1127         u16 removed = old_links & ~new_links;
1128         u16 added = new_links & ~old_links;
1129         int err, i;
1130
1131         if (hweight16(new_links) > 1 &&
1132             n_active > iwl_mvm_max_active_links(mvm, vif))
1133                 return -EOPNOTSUPP;
1134
1135         for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++) {
1136                 int r;
1137
1138                 if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
1139                         break;
1140
1141                 if (!(added & BIT(i)))
1142                         continue;
1143                 new_link[i] = kzalloc(sizeof(*new_link[i]), GFP_KERNEL);
1144                 if (!new_link[i]) {
1145                         err = -ENOMEM;
1146                         goto free;
1147                 }
1148
1149                 new_link[i]->bcast_sta.sta_id = IWL_MVM_INVALID_STA;
1150                 new_link[i]->mcast_sta.sta_id = IWL_MVM_INVALID_STA;
1151                 new_link[i]->ap_sta_id = IWL_MVM_INVALID_STA;
1152                 new_link[i]->fw_link_id = IWL_MVM_FW_LINK_ID_INVALID;
1153
1154                 for (r = 0; r < NUM_IWL_MVM_SMPS_REQ; r++)
1155                         new_link[i]->smps_requests[r] =
1156                                 IEEE80211_SMPS_AUTOMATIC;
1157         }
1158
1159         mutex_lock(&mvm->mutex);
1160
1161         if (old_links == 0) {
1162                 err = iwl_mvm_disable_link(mvm, vif, &vif->bss_conf);
1163                 if (err)
1164                         goto out_err;
1165                 mvmvif->link[0] = NULL;
1166         }
1167
1168         for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++) {
1169                 if (removed & BIT(i)) {
1170                         struct ieee80211_bss_conf *link_conf = old[i];
1171
1172                         err = iwl_mvm_disable_link(mvm, vif, link_conf);
1173                         if (err)
1174                                 goto out_err;
1175                         kfree(mvmvif->link[i]);
1176                         mvmvif->link[i] = NULL;
1177                 } else if (added & BIT(i)) {
1178                         struct ieee80211_bss_conf *link_conf;
1179
1180                         link_conf = link_conf_dereference_protected(vif, i);
1181                         if (WARN_ON(!link_conf))
1182                                 continue;
1183
1184                         if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART,
1185                                       &mvm->status))
1186                                 mvmvif->link[i] = new_link[i];
1187                         new_link[i] = NULL;
1188                         err = iwl_mvm_add_link(mvm, vif, link_conf);
1189                         if (err)
1190                                 goto out_err;
1191                 }
1192         }
1193
1194         err = 0;
1195         if (new_links == 0) {
1196                 mvmvif->link[0] = &mvmvif->deflink;
1197                 err = iwl_mvm_add_link(mvm, vif, &vif->bss_conf);
1198         }
1199
1200 out_err:
1201         /* we really don't have a good way to roll back here ... */
1202         mutex_unlock(&mvm->mutex);
1203
1204 free:
1205         for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++)
1206                 kfree(new_link[i]);
1207         return err;
1208 }
1209
1210 static int
1211 iwl_mvm_mld_change_sta_links(struct ieee80211_hw *hw,
1212                              struct ieee80211_vif *vif,
1213                              struct ieee80211_sta *sta,
1214                              u16 old_links, u16 new_links)
1215 {
1216         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1217         int ret;
1218
1219         mutex_lock(&mvm->mutex);
1220         ret = iwl_mvm_mld_update_sta_links(mvm, vif, sta, old_links, new_links);
1221         mutex_unlock(&mvm->mutex);
1222
1223         return ret;
1224 }
1225
1226 const struct ieee80211_ops iwl_mvm_mld_hw_ops = {
1227         .tx = iwl_mvm_mac_tx,
1228         .wake_tx_queue = iwl_mvm_mac_wake_tx_queue,
1229         .ampdu_action = iwl_mvm_mac_ampdu_action,
1230         .get_antenna = iwl_mvm_op_get_antenna,
1231         .set_antenna = iwl_mvm_op_set_antenna,
1232         .start = iwl_mvm_mac_start,
1233         .reconfig_complete = iwl_mvm_mac_reconfig_complete,
1234         .stop = iwl_mvm_mac_stop,
1235         .add_interface = iwl_mvm_mld_mac_add_interface,
1236         .remove_interface = iwl_mvm_mld_mac_remove_interface,
1237         .config = iwl_mvm_mac_config,
1238         .prepare_multicast = iwl_mvm_prepare_multicast,
1239         .configure_filter = iwl_mvm_configure_filter,
1240         .config_iface_filter = iwl_mvm_mld_config_iface_filter,
1241         .link_info_changed = iwl_mvm_mld_link_info_changed,
1242         .vif_cfg_changed = iwl_mvm_mld_vif_cfg_changed,
1243         .hw_scan = iwl_mvm_mac_hw_scan,
1244         .cancel_hw_scan = iwl_mvm_mac_cancel_hw_scan,
1245         .sta_pre_rcu_remove = iwl_mvm_sta_pre_rcu_remove,
1246         .sta_state = iwl_mvm_mld_mac_sta_state,
1247         .sta_notify = iwl_mvm_mac_sta_notify,
1248         .allow_buffered_frames = iwl_mvm_mac_allow_buffered_frames,
1249         .release_buffered_frames = iwl_mvm_mac_release_buffered_frames,
1250         .set_rts_threshold = iwl_mvm_mac_set_rts_threshold,
1251         .sta_rc_update = iwl_mvm_sta_rc_update,
1252         .conf_tx = iwl_mvm_mld_mac_conf_tx,
1253         .mgd_prepare_tx = iwl_mvm_mac_mgd_prepare_tx,
1254         .mgd_complete_tx = iwl_mvm_mac_mgd_complete_tx,
1255         .mgd_protect_tdls_discover = iwl_mvm_mac_mgd_protect_tdls_discover,
1256         .flush = iwl_mvm_mac_flush,
1257         .flush_sta = iwl_mvm_mac_flush_sta,
1258         .sched_scan_start = iwl_mvm_mac_sched_scan_start,
1259         .sched_scan_stop = iwl_mvm_mac_sched_scan_stop,
1260         .set_key = iwl_mvm_mac_set_key,
1261         .update_tkip_key = iwl_mvm_mac_update_tkip_key,
1262         .remain_on_channel = iwl_mvm_mld_roc,
1263         .cancel_remain_on_channel = iwl_mvm_cancel_roc,
1264         .add_chanctx = iwl_mvm_add_chanctx,
1265         .remove_chanctx = iwl_mvm_remove_chanctx,
1266         .change_chanctx = iwl_mvm_change_chanctx,
1267         .assign_vif_chanctx = iwl_mvm_mld_assign_vif_chanctx,
1268         .unassign_vif_chanctx = iwl_mvm_mld_unassign_vif_chanctx,
1269         .switch_vif_chanctx = iwl_mvm_mld_switch_vif_chanctx,
1270
1271         .start_ap = iwl_mvm_mld_start_ap,
1272         .stop_ap = iwl_mvm_mld_stop_ap,
1273         .join_ibss = iwl_mvm_mld_start_ibss,
1274         .leave_ibss = iwl_mvm_mld_stop_ibss,
1275
1276         .tx_last_beacon = iwl_mvm_tx_last_beacon,
1277
1278         .channel_switch = iwl_mvm_channel_switch,
1279         .pre_channel_switch = iwl_mvm_pre_channel_switch,
1280         .post_channel_switch = iwl_mvm_post_channel_switch,
1281         .abort_channel_switch = iwl_mvm_abort_channel_switch,
1282         .channel_switch_rx_beacon = iwl_mvm_channel_switch_rx_beacon,
1283
1284         .tdls_channel_switch = iwl_mvm_tdls_channel_switch,
1285         .tdls_cancel_channel_switch = iwl_mvm_tdls_cancel_channel_switch,
1286         .tdls_recv_channel_switch = iwl_mvm_tdls_recv_channel_switch,
1287
1288         .event_callback = iwl_mvm_mac_event_callback,
1289
1290         .sync_rx_queues = iwl_mvm_sync_rx_queues,
1291
1292         CFG80211_TESTMODE_CMD(iwl_mvm_mac_testmode_cmd)
1293
1294 #ifdef CONFIG_PM_SLEEP
1295         /* look at d3.c */
1296         .suspend = iwl_mvm_suspend,
1297         .resume = iwl_mvm_resume,
1298         .set_wakeup = iwl_mvm_set_wakeup,
1299         .set_rekey_data = iwl_mvm_set_rekey_data,
1300 #if IS_ENABLED(CONFIG_IPV6)
1301         .ipv6_addr_change = iwl_mvm_ipv6_addr_change,
1302 #endif
1303         .set_default_unicast_key = iwl_mvm_set_default_unicast_key,
1304 #endif
1305         .get_survey = iwl_mvm_mac_get_survey,
1306         .sta_statistics = iwl_mvm_mac_sta_statistics,
1307         .get_ftm_responder_stats = iwl_mvm_mac_get_ftm_responder_stats,
1308         .start_pmsr = iwl_mvm_start_pmsr,
1309         .abort_pmsr = iwl_mvm_abort_pmsr,
1310
1311 #ifdef CONFIG_IWLWIFI_DEBUGFS
1312         .vif_add_debugfs = iwl_mvm_vif_add_debugfs,
1313         .link_add_debugfs = iwl_mvm_link_add_debugfs,
1314         .link_sta_add_debugfs = iwl_mvm_link_sta_add_debugfs,
1315 #endif
1316         .set_hw_timestamp = iwl_mvm_set_hw_timestamp,
1317
1318         .change_vif_links = iwl_mvm_mld_change_vif_links,
1319         .change_sta_links = iwl_mvm_mld_change_sta_links,
1320 };