wl12xx: keep beacon-filtering enabled during STA operation

[cascardo/linux.git] / drivers / net / wireless / wl12xx / ps.c

diff --git a/drivers/net/wireless/wl12xx/ps.c b/drivers/net/wireless/wl12xx/ps.c
index 3548377..a7a1108 100644 (file)
--- a/drivers/net/wireless/wl12xx/ps.c
+++ b/drivers/net/wireless/wl12xx/ps.c
@@ -25,6 +25,7 @@
 #include "ps.h"
 #include "io.h"
 #include "tx.h"
+#include "debug.h"
 
 #define WL1271_WAKEUP_TIMEOUT 500
 
@@ -32,6 +33,7 @@ void wl1271_elp_work(struct work_struct *work)
 {
        struct delayed_work *dwork;
        struct wl1271 *wl;
+       struct wl12xx_vif *wlvif;
 
        dwork = container_of(work, struct delayed_work, work);
        wl = container_of(dwork, struct wl1271, elp_work);
@@ -47,11 +49,15 @@ void wl1271_elp_work(struct work_struct *work)
        if (unlikely(!test_bit(WL1271_FLAG_ELP_REQUESTED, &wl->flags)))
                goto out;
 
-       if (test_bit(WL1271_FLAG_IN_ELP, &wl->flags) ||
-           (!test_bit(WL1271_FLAG_PSM, &wl->flags) &&
-            !test_bit(WL1271_FLAG_IDLE, &wl->flags)))
+       if (test_bit(WL1271_FLAG_IN_ELP, &wl->flags))
                goto out;
 
+       wl12xx_for_each_wlvif(wl, wlvif) {
+               if (!test_bit(WLVIF_FLAG_PSM, &wlvif->flags) &&
+                   !test_bit(WL1271_FLAG_IDLE, &wl->flags))
+                       goto out;
+       }
+
        wl1271_debug(DEBUG_PSM, "chip to elp");
        wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG_ADDR, ELPCTRL_SLEEP);
        set_bit(WL1271_FLAG_IN_ELP, &wl->flags);
@@ -65,13 +71,17 @@ out:
 /* Routines to toggle sleep mode while in ELP */
 void wl1271_ps_elp_sleep(struct wl1271 *wl)
 {
+       struct wl12xx_vif *wlvif;
+
        /* we shouldn't get consecutive sleep requests */
        if (WARN_ON(test_and_set_bit(WL1271_FLAG_ELP_REQUESTED, &wl->flags)))
                return;
 
-       if (!test_bit(WL1271_FLAG_PSM, &wl->flags) &&
-           !test_bit(WL1271_FLAG_IDLE, &wl->flags))
-               return;
+       wl12xx_for_each_wlvif(wl, wlvif) {
+               if (!test_bit(WLVIF_FLAG_PSM, &wlvif->flags) &&
+                   !test_bit(WL1271_FLAG_IDLE, &wl->flags))
+                       return;
+       }
 
        ieee80211_queue_delayed_work(wl->hw, &wl->elp_work,
                                     msecs_to_jiffies(ELP_ENTRY_DELAY));
@@ -143,8 +153,8 @@ out:
        return 0;
 }
 
-int wl1271_ps_set_mode(struct wl1271 *wl, enum wl1271_cmd_ps_mode mode,
-                      u32 rates, bool send)
+int wl1271_ps_set_mode(struct wl1271 *wl, struct wl12xx_vif *wlvif,
+                      enum wl1271_cmd_ps_mode mode, u32 rates, bool send)
 {
        int ret;
 
@@ -152,39 +162,34 @@ int wl1271_ps_set_mode(struct wl1271 *wl, enum wl1271_cmd_ps_mode mode,
        case STATION_POWER_SAVE_MODE:
                wl1271_debug(DEBUG_PSM, "entering psm");
 
-               ret = wl1271_acx_wake_up_conditions(wl);
+               ret = wl1271_acx_wake_up_conditions(wl, wlvif);
                if (ret < 0) {
                        wl1271_error("couldn't set wake up conditions");
                        return ret;
                }
 
-               ret = wl1271_cmd_ps_mode(wl, STATION_POWER_SAVE_MODE);
+               ret = wl1271_cmd_ps_mode(wl, wlvif, STATION_POWER_SAVE_MODE);
                if (ret < 0)
                        return ret;
 
-               set_bit(WL1271_FLAG_PSM, &wl->flags);
+               set_bit(WLVIF_FLAG_PSM, &wlvif->flags);
                break;
        case STATION_ACTIVE_MODE:
        default:
                wl1271_debug(DEBUG_PSM, "leaving psm");
 
                /* disable beacon early termination */
-               if (wl->band == IEEE80211_BAND_2GHZ) {
-                       ret = wl1271_acx_bet_enable(wl, false);
+               if (wlvif->band == IEEE80211_BAND_2GHZ) {
+                       ret = wl1271_acx_bet_enable(wl, wlvif, false);
                        if (ret < 0)
                                return ret;
                }
 
-               /* disable beacon filtering */
-               ret = wl1271_acx_beacon_filter_opt(wl, false);
-               if (ret < 0)
-                       return ret;
-
-               ret = wl1271_cmd_ps_mode(wl, STATION_ACTIVE_MODE);
+               ret = wl1271_cmd_ps_mode(wl, wlvif, STATION_ACTIVE_MODE);
                if (ret < 0)
                        return ret;
 
-               clear_bit(WL1271_FLAG_PSM, &wl->flags);
+               clear_bit(WLVIF_FLAG_PSM, &wlvif->flags);
                break;
        }
 
@@ -199,15 +204,19 @@ static void wl1271_ps_filter_frames(struct wl1271 *wl, u8 hlid)
        unsigned long flags;
        int filtered[NUM_TX_QUEUES];
 
-       /* filter all frames currently the low level queus for this hlid */
+       /* filter all frames currently in the low level queues for this hlid */
        for (i = 0; i < NUM_TX_QUEUES; i++) {
                filtered[i] = 0;
                while ((skb = skb_dequeue(&wl->links[hlid].tx_queue[i]))) {
+                       filtered[i]++;
+
+                       if (WARN_ON(wl12xx_is_dummy_packet(wl, skb)))
+                               continue;
+
                        info = IEEE80211_SKB_CB(skb);
                        info->flags |= IEEE80211_TX_STAT_TX_FILTERED;
                        info->status.rates[0].idx = -1;
                        ieee80211_tx_status_ni(wl->hw, skb);
-                       filtered[i]++;
                }
        }
 
@@ -219,19 +228,21 @@ static void wl1271_ps_filter_frames(struct wl1271 *wl, u8 hlid)
        wl1271_handle_tx_low_watermark(wl);
 }
 
-void wl1271_ps_link_start(struct wl1271 *wl, u8 hlid, bool clean_queues)
+void wl12xx_ps_link_start(struct wl1271 *wl, struct wl12xx_vif *wlvif,
+                         u8 hlid, bool clean_queues)
 {
        struct ieee80211_sta *sta;
+       struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
 
        if (test_bit(hlid, &wl->ap_ps_map))
                return;
 
-       wl1271_debug(DEBUG_PSM, "start mac80211 PSM on hlid %d blks %d "
-                    "clean_queues %d", hlid, wl->links[hlid].allocated_blks,
+       wl1271_debug(DEBUG_PSM, "start mac80211 PSM on hlid %d pkts %d "
+                    "clean_queues %d", hlid, wl->links[hlid].allocated_pkts,
                     clean_queues);
 
        rcu_read_lock();
-       sta = ieee80211_find_sta(wl->vif, wl->links[hlid].addr);
+       sta = ieee80211_find_sta(vif, wl->links[hlid].addr);
        if (!sta) {
                wl1271_error("could not find sta %pM for starting ps",
                             wl->links[hlid].addr);
@@ -249,9 +260,10 @@ void wl1271_ps_link_start(struct wl1271 *wl, u8 hlid, bool clean_queues)
        __set_bit(hlid, &wl->ap_ps_map);
 }
 
-void wl1271_ps_link_end(struct wl1271 *wl, u8 hlid)
+void wl12xx_ps_link_end(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 hlid)
 {
        struct ieee80211_sta *sta;
+       struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
 
        if (!test_bit(hlid, &wl->ap_ps_map))
                return;
@@ -261,7 +273,7 @@ void wl1271_ps_link_end(struct wl1271 *wl, u8 hlid)
        __clear_bit(hlid, &wl->ap_ps_map);
 
        rcu_read_lock();
-       sta = ieee80211_find_sta(wl->vif, wl->links[hlid].addr);
+       sta = ieee80211_find_sta(vif, wl->links[hlid].addr);
        if (!sta) {
                wl1271_error("could not find sta %pM for ending ps",
                             wl->links[hlid].addr);