iwlwifi: mvm: allow private per-STA TFD queues

TDLS stations will have private queues, so consider them as well when
allocating a new one. Consolidate the HW-queue iterating code into
a single exported function, to be used by the TDLS code in the future.

Signed-off-by: Arik Nemtsov <arikx.nemtsov@intel.com>
Reviewed-by: Johannes Berg <johannes.berg@intel.com>
Signed-off-by: Emmanuel Grumbach <emmanuel.grumbach@intel.com>

2 files changed, 57 insertions, 10 deletions

diff --git a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
index b8ab4a108720..a1dc9b86c2d1 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
@@ -83,11 +83,15 @@ struct iwl_mvm_mac_iface_iterator_data {
         struct ieee80211_vif *vif;
         unsigned long available_mac_ids[BITS_TO_LONGS(NUM_MAC_INDEX_DRIVER)];
         unsigned long available_tsf_ids[BITS_TO_LONGS(NUM_TSF_IDS)];
-        u32 used_hw_queues;
         enum iwl_tsf_id preferred_tsf;
         bool found_vif;
 };
 
+struct iwl_mvm_hw_queues_iface_iterator_data {
+        struct ieee80211_vif *exclude_vif;
+        unsigned long used_hw_queues;
+};
+
 static void iwl_mvm_mac_tsf_id_iter(void *_data, u8 *mac,
                                     struct ieee80211_vif *vif)
 {
@@ -213,6 +217,54 @@ u32 iwl_mvm_mac_get_queues_mask(struct ieee80211_vif *vif)
         return qmask;
 }
 
+static void iwl_mvm_iface_hw_queues_iter(void *_data, u8 *mac,
+                                         struct ieee80211_vif *vif)
+{
+        struct iwl_mvm_hw_queues_iface_iterator_data *data = _data;
+
+        /* exclude the given vif */
+        if (vif == data->exclude_vif)
+                return;
+
+        data->used_hw_queues |= iwl_mvm_mac_get_queues_mask(vif);
+}
+
+static void iwl_mvm_mac_sta_hw_queues_iter(void *_data,
+                                           struct ieee80211_sta *sta)
+{
+        struct iwl_mvm_hw_queues_iface_iterator_data *data = _data;
+        struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
+
+        /* Mark the queues used by the sta */
+        data->used_hw_queues |= mvmsta->tfd_queue_msk;
+}
+
+unsigned long iwl_mvm_get_used_hw_queues(struct iwl_mvm *mvm,
+                                         struct ieee80211_vif *exclude_vif)
+{
+        struct iwl_mvm_hw_queues_iface_iterator_data data = {
+                .exclude_vif = exclude_vif,
+                .used_hw_queues =
+                        BIT(IWL_MVM_OFFCHANNEL_QUEUE) |
+                        BIT(mvm->aux_queue) |
+                        BIT(IWL_MVM_CMD_QUEUE),
+        };
+
+        lockdep_assert_held(&mvm->mutex);
+
+        /* mark all VIF used hw queues */
+        ieee80211_iterate_active_interfaces_atomic(
+                mvm->hw, IEEE80211_IFACE_ITER_RESUME_ALL,
+                iwl_mvm_iface_hw_queues_iter, &data);
+
+        /* don't assign the same hw queues as TDLS stations */
+        ieee80211_iterate_stations_atomic(mvm->hw,
+                                          iwl_mvm_mac_sta_hw_queues_iter,
+                                          &data);
+
+        return data.used_hw_queues;
+}
+
 static void iwl_mvm_mac_iface_iterator(void *_data, u8 *mac,
                                        struct ieee80211_vif *vif)
 {
@@ -225,9 +277,6 @@ static void iwl_mvm_mac_iface_iterator(void *_data, u8 *mac,
                 return;
         }
 
-        /* Mark the queues used by the vif */
-        data->used_hw_queues |= iwl_mvm_mac_get_queues_mask(vif);
-
         /* Mark MAC IDs as used by clearing the available bit, and
          * (below) mark TSFs as used if their existing use is not
          * compatible with the new interface type.
@@ -274,10 +323,6 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
                 .available_tsf_ids = { (1 << NUM_TSF_IDS) - 1 },
                 /* no preference yet */
                 .preferred_tsf = NUM_TSF_IDS,
-                .used_hw_queues =
-                        BIT(IWL_MVM_OFFCHANNEL_QUEUE) |
-                        BIT(mvm->aux_queue) |
-                        BIT(IWL_MVM_CMD_QUEUE),
                 .found_vif = false,
         };
         u32 ac;
@@ -316,6 +361,8 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
                 mvm->hw, IEEE80211_IFACE_ITER_RESUME_ALL,
                 iwl_mvm_mac_iface_iterator, &data);
 
+        used_hw_queues = iwl_mvm_get_used_hw_queues(mvm, vif);
+
         /*
          * In the case we're getting here during resume, it's similar to
          * firmware restart, and with RESUME_ALL the iterator will find
@@ -365,8 +412,6 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
                 return 0;
         }
 
-        used_hw_queues = data.used_hw_queues;
-
         /* Find available queues, and allocate them to the ACs */
         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
                 u8 queue = find_first_zero_bit(&used_hw_queues,
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index be0285d34fd4..363be83e283e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -937,6 +937,8 @@ int iwl_mvm_rx_missed_beacons_notif(struct iwl_mvm *mvm,
                                     struct iwl_device_cmd *cmd);
 void iwl_mvm_mac_ctxt_recalc_tsf_id(struct iwl_mvm *mvm,
                                     struct ieee80211_vif *vif);
+unsigned long iwl_mvm_get_used_hw_queues(struct iwl_mvm *mvm,
+                                         struct ieee80211_vif *exclude_vif);
 
 /* Bindings */
 int iwl_mvm_binding_add_vif(struct iwl_mvm *mvm, struct ieee80211_vif *vif);