mac80211: fix PS-Poll driver release TID

Using ffs() for the PS-Poll release TID is wrong, it will cause
frames to be released in order 0 1 2 3 4 5 6 7 instead of the
correct 7 6 5 4 3 0 2 1. Fix this by adding a new function that
implements "highest priority TID" properly.

Reported-by: Eliad Peller <eliad@wizery.com>
Signed-off-by: Johannes Berg <johannes.berg@intel.com>

1 files changed, 13 insertions, 1 deletions

diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index 703081d04e88..93bfd6700cbf 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -1227,6 +1227,17 @@ static void ieee80211_send_null_response(struct ieee80211_sub_if_data *sdata,
         rcu_read_unlock();
 }
 
+static int find_highest_prio_tid(unsigned long tids)
+{
+        /* lower 3 TIDs aren't ordered perfectly */
+        if (tids & 0xF8)
+                return fls(tids) - 1;
+        /* TID 0 is BE just like TID 3 */
+        if (tids & BIT(0))
+                return 0;
+        return fls(tids) - 1;
+}
+
 static void
 ieee80211_sta_ps_deliver_response(struct sta_info *sta,
                                   int n_frames, u8 ignored_acs,
@@ -1288,7 +1299,8 @@ ieee80211_sta_ps_deliver_response(struct sta_info *sta,
                             hweight16(driver_release_tids) > 1) {
                                 more_data = true;
                                 driver_release_tids =
-                                        BIT(ffs(driver_release_tids) - 1);
+                                        BIT(find_highest_prio_tid(
+                                                driver_release_tids));
                                 break;
                         }
                 }