4 files changed, 29 insertions, 7 deletions

diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 59a1d38212fd..3598abf21844 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -1278,6 +1278,9 @@ void ieee80211_add_pending_skb(struct ieee80211_local *local,
                                struct sk_buff *skb);
 int ieee80211_add_pending_skbs(struct ieee80211_local *local,
                                struct sk_buff_head *skbs);
+int ieee80211_add_pending_skbs_fn(struct ieee80211_local *local,
+                                  struct sk_buff_head *skbs,
+                                  void (*fn)(void *data), void *data);
 
 void ieee80211_send_auth(struct ieee80211_sub_if_data *sdata,
                          u16 transaction, u16 auth_alg,
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index 902b03ee8f60..d2fcd22ab06d 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -1102,8 +1102,6 @@ static void ap_sta_ps_end(struct sta_info *sta)
 
         atomic_dec(&sdata->bss->num_sta_ps);
 
-        clear_sta_flags(sta, WLAN_STA_PS_STA);
-
 #ifdef CONFIG_MAC80211_VERBOSE_PS_DEBUG
         printk(KERN_DEBUG "%s: STA %pM aid %d exits power save mode\n",
                sdata->name, sta->sta.addr, sta->sta.aid);
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index 6d8f897d8763..eff58571fd7e 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -199,8 +199,11 @@ static void sta_unblock(struct work_struct *wk)
 
         if (!test_sta_flags(sta, WLAN_STA_PS_STA))
                 ieee80211_sta_ps_deliver_wakeup(sta);
-        else if (test_and_clear_sta_flags(sta, WLAN_STA_PSPOLL))
+        else if (test_and_clear_sta_flags(sta, WLAN_STA_PSPOLL)) {
+                clear_sta_flags(sta, WLAN_STA_PS_DRIVER);
                 ieee80211_sta_ps_deliver_poll_response(sta);
+        } else
+                clear_sta_flags(sta, WLAN_STA_PS_DRIVER);
 }
 
 static int sta_prepare_rate_control(struct ieee80211_local *local,
@@ -880,6 +883,13 @@ struct ieee80211_sta *ieee80211_find_sta(struct ieee80211_vif *vif,
 }
 EXPORT_SYMBOL(ieee80211_find_sta);
 
+static void clear_sta_ps_flags(void *_sta)
+{
+        struct sta_info *sta = _sta;
+
+        clear_sta_flags(sta, WLAN_STA_PS_DRIVER | WLAN_STA_PS_STA);
+}
+
 /* powersave support code */
 void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta)
 {
@@ -894,7 +904,8 @@ void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta)
 
         /* Send all buffered frames to the station */
         sent = ieee80211_add_pending_skbs(local, &sta->tx_filtered);
-        buffered = ieee80211_add_pending_skbs(local, &sta->ps_tx_buf);
+        buffered = ieee80211_add_pending_skbs_fn(local, &sta->ps_tx_buf,
+                                                 clear_sta_ps_flags, sta);
         sent += buffered;
         local->total_ps_buffered -= buffered;
 
@@ -973,7 +984,7 @@ void ieee80211_sta_block_awake(struct ieee80211_hw *hw,
 
         if (block)
                 set_sta_flags(sta, WLAN_STA_PS_DRIVER);
-        else
+        else if (test_sta_flags(sta, WLAN_STA_PS_DRIVER))
                 ieee80211_queue_work(hw, &sta->drv_unblock_wk);
 }
 EXPORT_SYMBOL(ieee80211_sta_block_awake);
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index 68d0518254dd..e497476174ce 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -368,8 +368,9 @@ void ieee80211_add_pending_skb(struct ieee80211_local *local,
         spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
 }
 
-int ieee80211_add_pending_skbs(struct ieee80211_local *local,
-                               struct sk_buff_head *skbs)
+int ieee80211_add_pending_skbs_fn(struct ieee80211_local *local,
+                                  struct sk_buff_head *skbs,
+                                  void (*fn)(void *data), void *data)
 {
         struct ieee80211_hw *hw = &local->hw;
         struct sk_buff *skb;
@@ -394,6 +395,9 @@ int ieee80211_add_pending_skbs(struct ieee80211_local *local,
                 __skb_queue_tail(&local->pending[queue], skb);
         }
 
+        if (fn)
+                fn(data);
+
         for (i = 0; i < hw->queues; i++)
                 __ieee80211_wake_queue(hw, i,
                         IEEE80211_QUEUE_STOP_REASON_SKB_ADD);
@@ -402,6 +406,12 @@ int ieee80211_add_pending_skbs(struct ieee80211_local *local,
         return ret;
 }
 
+int ieee80211_add_pending_skbs(struct ieee80211_local *local,
+                               struct sk_buff_head *skbs)
+{
+        return ieee80211_add_pending_skbs_fn(local, skbs, NULL, NULL);
+}
+
 void ieee80211_stop_queues_by_reason(struct ieee80211_hw *hw,
                                     enum queue_stop_reason reason)
 {