3 files changed, 93 insertions, 7 deletions

diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 18b91601770a..a74d6738b30a 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -538,6 +538,10 @@ enum {
         IEEE80211_ADDBA_MSG     = 4,
 };
 
+enum queue_stop_reason {
+        IEEE80211_QUEUE_STOP_REASON_DRIVER,
+};
+
 /* maximum number of hardware queues we support. */
 #define QD_MAX_QUEUES (IEEE80211_MAX_AMPDU_QUEUES + IEEE80211_MAX_QUEUES)
 
@@ -554,7 +558,8 @@ struct ieee80211_local {
         const struct ieee80211_ops *ops;
 
         unsigned long queue_pool[BITS_TO_LONGS(QD_MAX_QUEUES)];
-
+        unsigned long queue_stop_reasons[IEEE80211_MAX_QUEUES];
+        spinlock_t queue_stop_reason_lock;
         struct net_device *mdev; /* wmaster# - "master" 802.11 device */
         int open_count;
         int monitors, cooked_mntrs;
@@ -972,6 +977,11 @@ int ieee80211_set_freq(struct ieee80211_sub_if_data *sdata, int freq);
 u64 ieee80211_mandatory_rates(struct ieee80211_local *local,
                               enum ieee80211_band band);
 
+void ieee80211_wake_queues_by_reason(struct ieee80211_hw *hw,
+                                     enum queue_stop_reason reason);
+void ieee80211_stop_queues_by_reason(struct ieee80211_hw *hw,
+                                     enum queue_stop_reason reason);
+
 #ifdef CONFIG_MAC80211_NOINLINE
 #define debug_noinline noinline
 #else
diff --git a/net/mac80211/main.c b/net/mac80211/main.c
index ab6b73647b3e..21335382f530 100644
--- a/net/mac80211/main.c
+++ b/net/mac80211/main.c
@@ -725,6 +725,8 @@ struct ieee80211_hw *ieee80211_alloc_hw(size_t priv_data_len,
 
         spin_lock_init(&local->key_lock);
 
+        spin_lock_init(&local->queue_stop_reason_lock);
+
         INIT_DELAYED_WORK(&local->scan_work, ieee80211_scan_work);
 
         sta_info_init(local);
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index 71a8391c54f6..fb89e1d0aa03 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -330,10 +330,20 @@ __le16 ieee80211_ctstoself_duration(struct ieee80211_hw *hw,
 }
 EXPORT_SYMBOL(ieee80211_ctstoself_duration);
 
-void ieee80211_wake_queue(struct ieee80211_hw *hw, int queue)
+static void __ieee80211_wake_queue(struct ieee80211_hw *hw, int queue,
+                                   enum queue_stop_reason reason)
 {
         struct ieee80211_local *local = hw_to_local(hw);
 
+        /* we don't need to track ampdu queues */
+        if (queue < ieee80211_num_regular_queues(hw)) {
+                __clear_bit(reason, &local->queue_stop_reasons[queue]);
+
+                if (local->queue_stop_reasons[queue] != 0)
+                        /* someone still has this queue stopped */
+                        return;
+        }
+
         if (test_bit(queue, local->queues_pending)) {
                 set_bit(queue, local->queues_pending_run);
                 tasklet_schedule(&local->tx_pending_tasklet);
@@ -341,22 +351,74 @@ void ieee80211_wake_queue(struct ieee80211_hw *hw, int queue)
                 netif_wake_subqueue(local->mdev, queue);
         }
 }
+
+void ieee80211_wake_queue_by_reason(struct ieee80211_hw *hw, int queue,
+                                    enum queue_stop_reason reason)
+{
+        struct ieee80211_local *local = hw_to_local(hw);
+        unsigned long flags;
+
+        spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
+        __ieee80211_wake_queue(hw, queue, reason);
+        spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
+}
+
+void ieee80211_wake_queue(struct ieee80211_hw *hw, int queue)
+{
+        ieee80211_wake_queue_by_reason(hw, queue,
+                                       IEEE80211_QUEUE_STOP_REASON_DRIVER);
+}
 EXPORT_SYMBOL(ieee80211_wake_queue);
 
-void ieee80211_stop_queue(struct ieee80211_hw *hw, int queue)
+static void __ieee80211_stop_queue(struct ieee80211_hw *hw, int queue,
+                                   enum queue_stop_reason reason)
 {
         struct ieee80211_local *local = hw_to_local(hw);
 
+        /* we don't need to track ampdu queues */
+        if (queue < ieee80211_num_regular_queues(hw))
+                __set_bit(reason, &local->queue_stop_reasons[queue]);
+
         netif_stop_subqueue(local->mdev, queue);
 }
+
+void ieee80211_stop_queue_by_reason(struct ieee80211_hw *hw, int queue,
+                                    enum queue_stop_reason reason)
+{
+        struct ieee80211_local *local = hw_to_local(hw);
+        unsigned long flags;
+
+        spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
+        __ieee80211_stop_queue(hw, queue, reason);
+        spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
+}
+
+void ieee80211_stop_queue(struct ieee80211_hw *hw, int queue)
+{
+        ieee80211_stop_queue_by_reason(hw, queue,
+                                       IEEE80211_QUEUE_STOP_REASON_DRIVER);
+}
 EXPORT_SYMBOL(ieee80211_stop_queue);
 
-void ieee80211_stop_queues(struct ieee80211_hw *hw)
+void ieee80211_stop_queues_by_reason(struct ieee80211_hw *hw,
+                                    enum queue_stop_reason reason)
 {
+        struct ieee80211_local *local = hw_to_local(hw);
+        unsigned long flags;
         int i;
 
+        spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
+
         for (i = 0; i < ieee80211_num_queues(hw); i++)
-                ieee80211_stop_queue(hw, i);
+                __ieee80211_stop_queue(hw, i, reason);
+
+        spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
+}
+
+void ieee80211_stop_queues(struct ieee80211_hw *hw)
+{
+        ieee80211_stop_queues_by_reason(hw,
+                                        IEEE80211_QUEUE_STOP_REASON_DRIVER);
 }
 EXPORT_SYMBOL(ieee80211_stop_queues);
 
@@ -367,12 +429,24 @@ int ieee80211_queue_stopped(struct ieee80211_hw *hw, int queue)
 }
 EXPORT_SYMBOL(ieee80211_queue_stopped);
 
-void ieee80211_wake_queues(struct ieee80211_hw *hw)
+void ieee80211_wake_queues_by_reason(struct ieee80211_hw *hw,
+                                     enum queue_stop_reason reason)
 {
+        struct ieee80211_local *local = hw_to_local(hw);
+        unsigned long flags;
         int i;
 
+        spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
+
         for (i = 0; i < hw->queues + hw->ampdu_queues; i++)
-                ieee80211_wake_queue(hw, i);
+                __ieee80211_wake_queue(hw, i, reason);
+
+        spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
+}
+
+void ieee80211_wake_queues(struct ieee80211_hw *hw)
+{
+        ieee80211_wake_queues_by_reason(hw, IEEE80211_QUEUE_STOP_REASON_DRIVER);
 }
 EXPORT_SYMBOL(ieee80211_wake_queues);