wifi: iwlwifi: mvm: implement mac80211 callback change_sta_links

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

Signed-off-by: Shaul Triebitz <shaul.triebitz@intel.com>
Signed-off-by: Gregory Greenman <gregory.greenman@intel.com>
Link: https://lore.kernel.org/r/20230329100039.156d1aae5de1.I32973141be1190222169879f8caf7038c1a8f769@changeid
Signed-off-by: Johannes Berg <johannes.berg@intel.com>

diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c b/drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c
index be9b2efa5ef56ebed4753306fbc58bc4adb2fe5c..e475d0daf512e8c6a2a40a1a3df3b607d6b256e9 100644 (file)
--- a/drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c
@@ -953,7 +953,14 @@ iwl_mvm_mld_change_sta_links(struct ieee80211_hw *hw,
                             struct ieee80211_sta *sta,
                             u16 old_links, u16 new_links)
 {
-       return 0;
+       struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+       int ret;
+
+       mutex_lock(&mvm->mutex);
+       ret = iwl_mvm_mld_update_sta_links(mvm, vif, sta, old_links, new_links);
+       mutex_unlock(&mvm->mutex);
+
+       return ret;
 }
 
 const struct ieee80211_ops iwl_mvm_mld_hw_ops = {

diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/mld-sta.c b/drivers/net/wireless/intel/iwlwifi/mvm/mld-sta.c
index c877c94dd1d6b5e28716d08b1d1aced95acb7f54..9be6d858411dffe350b946cdc7a6f40236deccd1 100644 (file)
--- a/drivers/net/wireless/intel/iwlwifi/mvm/mld-sta.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mld-sta.c
@@ -479,15 +479,53 @@ static void iwl_mvm_mld_sta_rm_all_sta_links(struct iwl_mvm *mvm,
        }
 }
 
+static void iwl_mvm_mld_free_sta_link(struct iwl_mvm *mvm,
+                                     struct iwl_mvm_sta *mvm_sta,
+                                     struct iwl_mvm_link_sta *mvm_sta_link,
+                                     unsigned int link_id)
+{
+       RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta_link->sta_id], NULL);
+       RCU_INIT_POINTER(mvm_sta->link[link_id], NULL);
+
+       if (mvm_sta_link != &mvm_sta->deflink)
+               kfree_rcu(mvm_sta_link, rcu_head);
+}
+
+static int iwl_mvm_mld_alloc_sta_link(struct iwl_mvm *mvm,
+                                     struct ieee80211_vif *vif,
+                                     struct ieee80211_sta *sta,
+                                     unsigned int link_id)
+{
+       struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
+       struct iwl_mvm_link_sta *link;
+       u32 sta_id = iwl_mvm_find_free_sta_id(mvm,
+                                         ieee80211_vif_type_p2p(vif));
+
+       if (sta_id == IWL_MVM_INVALID_STA)
+               return -ENOSPC;
+
+       if (rcu_access_pointer(sta->link[link_id]) == &sta->deflink) {
+               link = &mvm_sta->deflink;
+       } else {
+               link = kzalloc(sizeof(*link), GFP_KERNEL);
+               if (!link)
+                       return -ENOMEM;
+       }
+
+       link->sta_id = sta_id;
+       rcu_assign_pointer(mvm_sta->link[link_id], link);
+       rcu_assign_pointer(mvm->fw_id_to_mac_id[link->sta_id], sta);
+
+       return 0;
+}
+
 /* allocate all the links of a sta, called when the station is first added */
 static int iwl_mvm_mld_alloc_sta_links(struct iwl_mvm *mvm,
                                       struct ieee80211_vif *vif,
                                       struct ieee80211_sta *sta)
 {
        struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
-       struct iwl_mvm_link_sta *link;
        unsigned int link_id;
-       u32 sta_id;
        int ret;
 
        lockdep_assert_held(&mvm->mutex);
@@ -497,27 +535,9 @@ static int iwl_mvm_mld_alloc_sta_links(struct iwl_mvm *mvm,
                    mvm_sta->link[link_id])
                        continue;
 
-               sta_id = iwl_mvm_find_free_sta_id(mvm,
-                                                 ieee80211_vif_type_p2p(vif));
-
-               if (sta_id == IWL_MVM_INVALID_STA) {
-                       ret = -ENOSPC;
+               ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, link_id);
+               if (ret)
                        goto err;
-               }
-
-               if (rcu_access_pointer(sta->link[link_id]) == &sta->deflink) {
-                       link = &mvm_sta->deflink;
-               } else {
-                       link = kzalloc(sizeof(*link), GFP_KERNEL);
-                       if (!link) {
-                               ret = -ENOMEM;
-                               goto err;
-                       }
-               }
-
-               link->sta_id = sta_id;
-               rcu_assign_pointer(mvm_sta->link[link_id], link);
-               rcu_assign_pointer(mvm->fw_id_to_mac_id[link->sta_id], sta);
        }
 
        return 0;
@@ -846,3 +866,185 @@ void iwl_mvm_mld_modify_all_sta_disable_tx(struct iwl_mvm *mvm,
 
        rcu_read_unlock();
 }
+
+static int iwl_mvm_mld_update_sta_queue(struct iwl_mvm *mvm,
+                                       struct iwl_mvm_sta *mvm_sta,
+                                       u32 old_sta_mask,
+                                       u32 new_sta_mask)
+{
+       struct iwl_scd_queue_cfg_cmd cmd = {
+               .operation = cpu_to_le32(IWL_SCD_QUEUE_MODIFY),
+               .u.modify.old_sta_mask = cpu_to_le32(old_sta_mask),
+               .u.modify.new_sta_mask = cpu_to_le32(new_sta_mask),
+       };
+       struct iwl_host_cmd hcmd = {
+               .id = WIDE_ID(DATA_PATH_GROUP, SCD_QUEUE_CONFIG_CMD),
+               .len[0] = sizeof(cmd),
+               .data[0] = &cmd
+       };
+       int tid;
+       int ret;
+
+       lockdep_assert_held(&mvm->mutex);
+
+       for (tid = 0; tid <= IWL_MAX_TID_COUNT; tid++) {
+               struct iwl_mvm_tid_data *tid_data = &mvm_sta->tid_data[tid];
+               int txq_id = tid_data->txq_id;
+
+               if (txq_id == IWL_MVM_INVALID_QUEUE)
+                       continue;
+
+               if (tid == IWL_MAX_TID_COUNT)
+                       cmd.u.modify.tid = cpu_to_le32(IWL_MGMT_TID);
+               else
+                       cmd.u.modify.tid = cpu_to_le32(tid);
+
+               ret = iwl_mvm_send_cmd(mvm, &hcmd);
+               if (ret)
+                       return ret;
+       }
+
+       return 0;
+}
+
+int iwl_mvm_mld_update_sta_links(struct iwl_mvm *mvm,
+                                struct ieee80211_vif *vif,
+                                struct ieee80211_sta *sta,
+                                u16 old_links, u16 new_links)
+{
+       struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
+       struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif);
+       struct iwl_mvm_link_sta *mvm_sta_link;
+       struct iwl_mvm_vif_link_info *mvm_vif_link;
+       unsigned long links_to_add = ~old_links & new_links;
+       unsigned long links_to_rem = old_links & ~new_links;
+       unsigned long old_links_long = old_links;
+       u32 current_sta_mask = 0, sta_mask_added = 0, sta_mask_to_rem = 0;
+       unsigned long link_sta_added_to_fw = 0, link_sta_allocated = 0;
+       unsigned int link_id;
+       int ret;
+
+       lockdep_assert_held(&mvm->mutex);
+
+       for_each_set_bit(link_id, &old_links_long,
+                        IEEE80211_MLD_MAX_NUM_LINKS) {
+               mvm_sta_link =
+                       rcu_dereference_protected(mvm_sta->link[link_id],
+                                                 lockdep_is_held(&mvm->mutex));
+
+               if (WARN_ON(!mvm_sta_link)) {
+                       ret = -EINVAL;
+                       goto err;
+               }
+
+               current_sta_mask |= BIT(mvm_sta_link->sta_id);
+               if (links_to_rem & BIT(link_id))
+                       sta_mask_to_rem |= BIT(mvm_sta_link->sta_id);
+       }
+
+       if (sta_mask_to_rem) {
+               ret = iwl_mvm_mld_update_sta_queue(mvm, mvm_sta,
+                                                  current_sta_mask,
+                                                  current_sta_mask & ~sta_mask_to_rem);
+               if (WARN_ON(ret))
+                       goto err;
+
+               current_sta_mask &= ~sta_mask_to_rem;
+       }
+
+       for_each_set_bit(link_id, &links_to_rem, IEEE80211_MLD_MAX_NUM_LINKS) {
+               mvm_sta_link =
+                       rcu_dereference_protected(mvm_sta->link[link_id],
+                                                 lockdep_is_held(&mvm->mutex));
+               mvm_vif_link = mvm_vif->link[link_id];
+
+               if (WARN_ON(!mvm_sta_link || !mvm_vif_link)) {
+                       ret = -EINVAL;
+                       goto err;
+               }
+
+               ret = iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id);
+               if (WARN_ON(ret))
+                       goto err;
+
+               if (vif->type == NL80211_IFTYPE_STATION)
+                       mvm_vif_link->ap_sta_id = IWL_MVM_INVALID_STA;
+
+               iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id);
+       }
+
+       for_each_set_bit(link_id, &links_to_add, IEEE80211_MLD_MAX_NUM_LINKS) {
+               struct ieee80211_bss_conf *link_conf =
+                       rcu_dereference_protected(vif->link_conf[link_id], 1);
+               struct ieee80211_link_sta *link_sta =
+                       rcu_dereference_protected(sta->link[link_id], 1);
+               mvm_vif_link = mvm_vif->link[link_id];
+
+               if (WARN_ON(!mvm_vif_link || !link_conf || !link_sta ||
+                           mvm_sta->link[link_id])) {
+                       ret = -EINVAL;
+                       goto err;
+               }
+
+               ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, link_id);
+               if (WARN_ON(ret))
+                       goto err;
+
+               mvm_sta_link =
+                       rcu_dereference_protected(mvm_sta->link[link_id],
+                                                 lockdep_is_held(&mvm->mutex));
+
+               if (WARN_ON(!mvm_sta_link)) {
+                       ret = -EINVAL;
+                       goto err;
+               }
+
+               if (vif->type == NL80211_IFTYPE_STATION)
+                       iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif_link,
+                                                 mvm_sta_link);
+
+               link_sta_allocated |= BIT(link_id);
+
+               sta_mask_added |= BIT(mvm_sta_link->sta_id);
+
+               ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
+                                         mvm_sta_link);
+               if (WARN_ON(ret))
+                       goto err;
+
+               link_sta_added_to_fw |= BIT(link_id);
+       }
+
+       if (sta_mask_added) {
+               ret = iwl_mvm_mld_update_sta_queue(mvm, mvm_sta,
+                                                  current_sta_mask,
+                                                  current_sta_mask | sta_mask_added);
+               if (WARN_ON(ret))
+                       goto err;
+       }
+
+       return 0;
+
+err:
+       /* remove all already allocated stations in FW */
+       for_each_set_bit(link_id, &link_sta_added_to_fw,
+                        IEEE80211_MLD_MAX_NUM_LINKS) {
+               mvm_sta_link =
+                       rcu_dereference_protected(mvm_sta->link[link_id],
+                                                 lockdep_is_held(&mvm->mutex));
+
+               iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id);
+       }
+
+       /* remove all already allocated station links in driver */
+       for_each_set_bit(link_id, &link_sta_allocated,
+                        IEEE80211_MLD_MAX_NUM_LINKS) {
+               mvm_sta_link =
+                       rcu_dereference_protected(mvm_sta->link[link_id],
+                                                 lockdep_is_held(&mvm->mutex));
+
+               iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id);
+       }
+
+       return ret;
+}

diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/sta.h b/drivers/net/wireless/intel/iwlwifi/mvm/sta.h
index 6d4db666368b77242f78d3c3991516e44a9f6581..2402b41bc0bb11ef72d65f3af050985e6419cb13 100644 (file)
--- a/drivers/net/wireless/intel/iwlwifi/mvm/sta.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/sta.h
@@ -641,6 +641,10 @@ int iwl_mvm_mld_rm_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
                       struct ieee80211_sta *sta);
 int iwl_mvm_mld_rm_sta_id(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
                          u8 sta_id);
+int iwl_mvm_mld_update_sta_links(struct iwl_mvm *mvm,
+                                struct ieee80211_vif *vif,
+                                struct ieee80211_sta *sta,
+                                u16 old_links, u16 new_links);
 
 /* Queues */
 void iwl_mvm_mld_modify_all_sta_disable_tx(struct iwl_mvm *mvm,