13 files changed, 171 insertions, 53 deletions

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 6dc3579c0ac5..9ae1a4760b58 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -1331,6 +1331,9 @@ static int ieee80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
         struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
         struct ieee80211_conf *conf = &local->hw.conf;
 
+        if (sdata->vif.type != NL80211_IFTYPE_STATION)
+                return -EOPNOTSUPP;
+
         if (!(local->hw.flags & IEEE80211_HW_SUPPORTS_PS))
                 return -EOPNOTSUPP;
 
diff --git a/net/mac80211/driver-trace.h b/net/mac80211/driver-trace.h
index ee94ea0c67e9..da8497ef7063 100644
--- a/net/mac80211/driver-trace.h
+++ b/net/mac80211/driver-trace.h
@@ -680,7 +680,7 @@ TRACE_EVENT(drv_ampdu_action,
                 __entry->ret = ret;
                 __entry->action = action;
                 __entry->tid = tid;
-                __entry->ssn = *ssn;
+                __entry->ssn = ssn ? *ssn : 0;
         ),
 
         TP_printk(
diff --git a/net/mac80211/ibss.c b/net/mac80211/ibss.c
index 1f2db647bb5c..22f0c2aa7a89 100644
--- a/net/mac80211/ibss.c
+++ b/net/mac80211/ibss.c
@@ -647,7 +647,7 @@ static void ieee80211_rx_mgmt_probe_req(struct ieee80211_sub_if_data *sdata,
         }
         if (pos[1] != 0 &&
             (pos[1] != ifibss->ssid_len ||
-             !memcmp(pos + 2, ifibss->ssid, ifibss->ssid_len))) {
+             memcmp(pos + 2, ifibss->ssid, ifibss->ssid_len))) {
                 /* Ignore ProbeReq for foreign SSID */
                 return;
         }
diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c
index 80c16f6e2af6..32abae3ce32a 100644
--- a/net/mac80211/iface.c
+++ b/net/mac80211/iface.c
@@ -15,12 +15,14 @@
 #include <linux/netdevice.h>
 #include <linux/rtnetlink.h>
 #include <net/mac80211.h>
+#include <net/ieee80211_radiotap.h>
 #include "ieee80211_i.h"
 #include "sta_info.h"
 #include "debugfs_netdev.h"
 #include "mesh.h"
 #include "led.h"
 #include "driver-ops.h"
+#include "wme.h"
 
 /**
  * DOC: Interface list locking
@@ -314,7 +316,7 @@ static int ieee80211_open(struct net_device *dev)
         if (sdata->vif.type == NL80211_IFTYPE_STATION)
                 ieee80211_queue_work(&local->hw, &sdata->u.mgd.work);
 
-        netif_start_queue(dev);
+        netif_tx_start_all_queues(dev);
 
         return 0;
  err_del_interface:
@@ -343,7 +345,7 @@ static int ieee80211_stop(struct net_device *dev)
         /*
          * Stop TX on this interface first.
          */
-        netif_stop_queue(dev);
+        netif_tx_stop_all_queues(dev);
 
         /*
          * Now delete all active aggregation sessions.
@@ -644,6 +646,12 @@ static void ieee80211_teardown_sdata(struct net_device *dev)
         WARN_ON(flushed);
 }
 
+static u16 ieee80211_netdev_select_queue(struct net_device *dev,
+                                         struct sk_buff *skb)
+{
+        return ieee80211_select_queue(IEEE80211_DEV_TO_SUB_IF(dev), skb);
+}
+
 static const struct net_device_ops ieee80211_dataif_ops = {
         .ndo_open               = ieee80211_open,
         .ndo_stop               = ieee80211_stop,
@@ -652,8 +660,38 @@ static const struct net_device_ops ieee80211_dataif_ops = {
         .ndo_set_multicast_list = ieee80211_set_multicast_list,
         .ndo_change_mtu         = ieee80211_change_mtu,
         .ndo_set_mac_address    = eth_mac_addr,
+        .ndo_select_queue       = ieee80211_netdev_select_queue,
 };
 
+static u16 ieee80211_monitor_select_queue(struct net_device *dev,
+                                          struct sk_buff *skb)
+{
+        struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+        struct ieee80211_local *local = sdata->local;
+        struct ieee80211_hdr *hdr;
+        struct ieee80211_radiotap_header *rtap = (void *)skb->data;
+        u8 *p;
+
+        if (local->hw.queues < 4)
+                return 0;
+
+        if (skb->len < 4 ||
+            skb->len < le16_to_cpu(rtap->it_len) + 2 /* frame control */)
+                return 0; /* doesn't matter, frame will be dropped */
+
+        hdr = (void *)((u8 *)skb->data + le16_to_cpu(rtap->it_len));
+
+        if (!ieee80211_is_data_qos(hdr->frame_control)) {
+                skb->priority = 7;
+                return ieee802_1d_to_ac[skb->priority];
+        }
+
+        p = ieee80211_get_qos_ctl(hdr);
+        skb->priority = *p & IEEE80211_QOS_CTL_TAG1D_MASK;
+
+        return ieee80211_downgrade_queue(local, skb);
+}
+
 static const struct net_device_ops ieee80211_monitorif_ops = {
         .ndo_open               = ieee80211_open,
         .ndo_stop               = ieee80211_stop,
@@ -662,6 +700,7 @@ static const struct net_device_ops ieee80211_monitorif_ops = {
         .ndo_set_multicast_list = ieee80211_set_multicast_list,
         .ndo_change_mtu         = ieee80211_change_mtu,
         .ndo_set_mac_address    = eth_mac_addr,
+        .ndo_select_queue       = ieee80211_monitor_select_queue,
 };
 
 static void ieee80211_if_setup(struct net_device *dev)
@@ -768,8 +807,8 @@ int ieee80211_if_add(struct ieee80211_local *local, const char *name,
 
         ASSERT_RTNL();
 
-        ndev = alloc_netdev(sizeof(*sdata) + local->hw.vif_data_size,
-                            name, ieee80211_if_setup);
+        ndev = alloc_netdev_mq(sizeof(*sdata) + local->hw.vif_data_size,
+                               name, ieee80211_if_setup, local->hw.queues);
         if (!ndev)
                 return -ENOMEM;
         dev_net_set(ndev, wiphy_net(local->hw.wiphy));
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index c79e59f82fd9..05a18f43e1bf 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -942,7 +942,7 @@ static void ieee80211_set_associated(struct ieee80211_sub_if_data *sdata,
         ieee80211_recalc_ps(local, -1);
         mutex_unlock(&local->iflist_mtx);
 
-        netif_start_queue(sdata->dev);
+        netif_tx_start_all_queues(sdata->dev);
         netif_carrier_on(sdata->dev);
 }
 
@@ -1074,7 +1074,7 @@ static void ieee80211_set_disassoc(struct ieee80211_sub_if_data *sdata,
          * time -- we don't want the scan code to enable queues.
          */
 
-        netif_stop_queue(sdata->dev);
+        netif_tx_stop_all_queues(sdata->dev);
         netif_carrier_off(sdata->dev);
 
         rcu_read_lock();
@@ -1963,7 +1963,9 @@ static void ieee80211_sta_rx_queued_mgmt(struct ieee80211_sub_if_data *sdata,
                         rma = ieee80211_rx_mgmt_disassoc(sdata, mgmt, skb->len);
                         break;
                 case IEEE80211_STYPE_ACTION:
-                        /* XXX: differentiate, can only happen for CSA now! */
+                        if (mgmt->u.action.category != WLAN_CATEGORY_SPECTRUM_MGMT)
+                                break;
+
                         ieee80211_sta_process_chanswitch(sdata,
                                         &mgmt->u.action.u.chan_switch.sw_elem,
                                         ifmgd->associated);
diff --git a/net/mac80211/rate.c b/net/mac80211/rate.c
index b9007f80cb92..12a2bff7dcdb 100644
--- a/net/mac80211/rate.c
+++ b/net/mac80211/rate.c
@@ -245,6 +245,9 @@ void rate_control_get_rate(struct ieee80211_sub_if_data *sdata,
                 info->control.rates[i].count = 1;
         }
 
+        if (sdata->local->hw.flags & IEEE80211_HW_HAS_RATE_CONTROL)
+                return;
+
         if (sta && sdata->force_unicast_rateidx > -1) {
                 info->control.rates[0].idx = sdata->force_unicast_rateidx;
         } else {
diff --git a/net/mac80211/rc80211_pid_algo.c b/net/mac80211/rc80211_pid_algo.c
index 699d3ed869c4..29bc4c516238 100644
--- a/net/mac80211/rc80211_pid_algo.c
+++ b/net/mac80211/rc80211_pid_algo.c
@@ -190,7 +190,7 @@ static void rate_control_pid_sample(struct rc_pid_info *pinfo,
         rate_control_pid_normalize(pinfo, sband->n_bitrates);
 
         /* Compute the proportional, integral and derivative errors. */
-        err_prop = (pinfo->target << RC_PID_ARITH_SHIFT) - pf;
+        err_prop = (pinfo->target - pf) << RC_PID_ARITH_SHIFT;
 
         err_avg = spinfo->err_avg_sc >> pinfo->smoothing_shift;
         spinfo->err_avg_sc = spinfo->err_avg_sc - err_avg + err_prop;
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index 9f2807aeaf52..82a30c1bf3ab 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -1746,7 +1746,9 @@ ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
                         memset(info, 0, sizeof(*info));
                         info->flags |= IEEE80211_TX_INTFL_NEED_TXPROCESSING;
                         info->control.vif = &rx->sdata->vif;
-                        ieee80211_select_queue(local, fwd_skb);
+                        skb_set_queue_mapping(skb,
+                                ieee80211_select_queue(rx->sdata, fwd_skb));
+                        ieee80211_set_qos_hdr(local, skb);
                         if (is_multicast_ether_addr(fwd_hdr->addr1))
                                 IEEE80211_IFSTA_MESH_CTR_INC(&sdata->u.mesh,
                                                                 fwded_mcast);
@@ -2013,6 +2015,10 @@ ieee80211_rx_h_action(struct ieee80211_rx_data *rx)
                 }
                 break;
         default:
+                /* do not process rejected action frames */
+                if (mgmt->u.action.category & 0x80)
+                        return RX_DROP_MONITOR;
+
                 return RX_CONTINUE;
         }
 
diff --git a/net/mac80211/scan.c b/net/mac80211/scan.c
index f1a4c7160300..bc17cf7d68db 100644
--- a/net/mac80211/scan.c
+++ b/net/mac80211/scan.c
@@ -353,10 +353,10 @@ void ieee80211_scan_completed(struct ieee80211_hw *hw, bool aborted)
                 if (sdata->vif.type == NL80211_IFTYPE_STATION) {
                         if (sdata->u.mgd.associated) {
                                 ieee80211_scan_ps_disable(sdata);
-                                netif_wake_queue(sdata->dev);
+                                netif_tx_wake_all_queues(sdata->dev);
                         }
                 } else
-                        netif_wake_queue(sdata->dev);
+                        netif_tx_wake_all_queues(sdata->dev);
 
                 /* re-enable beaconing */
                 if (sdata->vif.type == NL80211_IFTYPE_AP ||
@@ -411,7 +411,7 @@ static int ieee80211_start_sw_scan(struct ieee80211_local *local)
                  * are handled in the scan state machine
                  */
                 if (sdata->vif.type != NL80211_IFTYPE_STATION)
-                        netif_stop_queue(sdata->dev);
+                        netif_tx_stop_all_queues(sdata->dev);
         }
         mutex_unlock(&local->iflist_mtx);
 
@@ -439,6 +439,16 @@ static int __ieee80211_start_scan(struct ieee80211_sub_if_data *sdata,
         if (local->scan_req)
                 return -EBUSY;
 
+        if (req != local->int_scan_req &&
+            sdata->vif.type == NL80211_IFTYPE_STATION &&
+            !list_empty(&ifmgd->work_list)) {
+                /* actually wait for the work it's doing to finish/time out */
+                set_bit(IEEE80211_STA_REQ_SCAN, &ifmgd->request);
+                local->scan_req = req;
+                local->scan_sdata = sdata;
+                return 0;
+        }
+
         if (local->ops->hw_scan) {
                 u8 *ies;
 
@@ -463,14 +473,6 @@ static int __ieee80211_start_scan(struct ieee80211_sub_if_data *sdata,
         local->scan_req = req;
         local->scan_sdata = sdata;
 
-        if (req != local->int_scan_req &&
-            sdata->vif.type == NL80211_IFTYPE_STATION &&
-            !list_empty(&ifmgd->work_list)) {
-                /* actually wait for the work it's doing to finish/time out */
-                set_bit(IEEE80211_STA_REQ_SCAN, &ifmgd->request);
-                return 0;
-        }
-
         if (local->ops->hw_scan)
                 __set_bit(SCAN_HW_SCANNING, &local->scanning);
         else
@@ -575,7 +577,7 @@ static void ieee80211_scan_state_leave_oper_channel(struct ieee80211_local *loca
                         continue;
 
                 if (sdata->vif.type == NL80211_IFTYPE_STATION) {
-                        netif_stop_queue(sdata->dev);
+                        netif_tx_stop_all_queues(sdata->dev);
                         if (sdata->u.mgd.associated)
                                 ieee80211_scan_ps_enable(sdata);
                 }
@@ -610,7 +612,7 @@ static void ieee80211_scan_state_enter_oper_channel(struct ieee80211_local *loca
                 if (sdata->vif.type == NL80211_IFTYPE_STATION) {
                         if (sdata->u.mgd.associated)
                                 ieee80211_scan_ps_disable(sdata);
-                        netif_wake_queue(sdata->dev);
+                        netif_tx_wake_all_queues(sdata->dev);
                 }
         }
         mutex_unlock(&local->iflist_mtx);
diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c
index 27ceaefd7bc8..ac210b586702 100644
--- a/net/mac80211/tx.c
+++ b/net/mac80211/tx.c
@@ -1512,7 +1512,7 @@ static void ieee80211_xmit(struct ieee80211_sub_if_data *sdata,
                                 return;
                         }
 
-        ieee80211_select_queue(local, skb);
+        ieee80211_set_qos_hdr(local, skb);
         ieee80211_tx(sdata, skb, false);
         rcu_read_unlock();
 }
@@ -2291,6 +2291,9 @@ void ieee80211_tx_skb(struct ieee80211_sub_if_data *sdata, struct sk_buff *skb)
         skb_set_network_header(skb, 0);
         skb_set_transport_header(skb, 0);
 
+        /* send all internal mgmt frames on VO */
+        skb_set_queue_mapping(skb, 0);
+
         /*
          * The other path calling ieee80211_xmit is from the tasklet,
          * and while we can handle concurrent transmissions locking
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index dc76267e436e..3848140313f5 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -269,6 +269,7 @@ static void __ieee80211_wake_queue(struct ieee80211_hw *hw, int queue,
                                    enum queue_stop_reason reason)
 {
         struct ieee80211_local *local = hw_to_local(hw);
+        struct ieee80211_sub_if_data *sdata;
 
         if (WARN_ON(queue >= hw->queues))
                 return;
@@ -281,6 +282,11 @@ static void __ieee80211_wake_queue(struct ieee80211_hw *hw, int queue,
 
         if (!skb_queue_empty(&local->pending[queue]))
                 tasklet_schedule(&local->tx_pending_tasklet);
+
+        rcu_read_lock();
+        list_for_each_entry_rcu(sdata, &local->interfaces, list)
+                netif_tx_wake_queue(netdev_get_tx_queue(sdata->dev, queue));
+        rcu_read_unlock();
 }
 
 void ieee80211_wake_queue_by_reason(struct ieee80211_hw *hw, int queue,
@@ -305,11 +311,17 @@ static void __ieee80211_stop_queue(struct ieee80211_hw *hw, int queue,
                                    enum queue_stop_reason reason)
 {
         struct ieee80211_local *local = hw_to_local(hw);
+        struct ieee80211_sub_if_data *sdata;
 
         if (WARN_ON(queue >= hw->queues))
                 return;
 
         __set_bit(reason, &local->queue_stop_reasons[queue]);
+
+        rcu_read_lock();
+        list_for_each_entry_rcu(sdata, &local->interfaces, list)
+                netif_tx_stop_queue(netdev_get_tx_queue(sdata->dev, queue));
+        rcu_read_unlock();
 }
 
 void ieee80211_stop_queue_by_reason(struct ieee80211_hw *hw, int queue,
diff --git a/net/mac80211/wme.c b/net/mac80211/wme.c
index b19b7696f3a2..79d887dae738 100644
--- a/net/mac80211/wme.c
+++ b/net/mac80211/wme.c
@@ -44,22 +44,69 @@ static int wme_downgrade_ac(struct sk_buff *skb)
 }
 
 
-/* Indicate which queue to use.  */
-static u16 classify80211(struct ieee80211_local *local, struct sk_buff *skb)
+/* Indicate which queue to use. */
+u16 ieee80211_select_queue(struct ieee80211_sub_if_data *sdata,
+                           struct sk_buff *skb)
 {
-        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+        struct ieee80211_local *local = sdata->local;
+        struct sta_info *sta = NULL;
+        u32 sta_flags = 0;
+        const u8 *ra = NULL;
+        bool qos = false;
 
-        if (!ieee80211_is_data(hdr->frame_control)) {
-                /* management frames go on AC_VO queue, but are sent
-                * without QoS control fields */
-                return 0;
+        if (local->hw.queues < 4 || skb->len < 6) {
+                skb->priority = 0; /* required for correct WPA/11i MIC */
+                return min_t(u16, local->hw.queues - 1,
+                             ieee802_1d_to_ac[skb->priority]);
+        }
+
+        rcu_read_lock();
+        switch (sdata->vif.type) {
+        case NL80211_IFTYPE_AP_VLAN:
+                rcu_read_lock();
+                sta = rcu_dereference(sdata->u.vlan.sta);
+                if (sta)
+                        sta_flags = get_sta_flags(sta);
+                rcu_read_unlock();
+                if (sta)
+                        break;
+        case NL80211_IFTYPE_AP:
+                ra = skb->data;
+                break;
+        case NL80211_IFTYPE_WDS:
+                ra = sdata->u.wds.remote_addr;
+                break;
+#ifdef CONFIG_MAC80211_MESH
+        case NL80211_IFTYPE_MESH_POINT:
+                /*
+                 * XXX: This is clearly broken ... but already was before,
+                 * because ieee80211_fill_mesh_addresses() would clear A1
+                 * except for multicast addresses.
+                 */
+                break;
+#endif
+        case NL80211_IFTYPE_STATION:
+                ra = sdata->u.mgd.bssid;
+                break;
+        case NL80211_IFTYPE_ADHOC:
+                ra = skb->data;
+                break;
+        default:
+                break;
         }
 
-        if (0 /* injected */) {
-                /* use AC from radiotap */
+        if (!sta && ra && !is_multicast_ether_addr(ra)) {
+                sta = sta_info_get(local, ra);
+                if (sta)
+                        sta_flags = get_sta_flags(sta);
         }
 
-        if (!ieee80211_is_data_qos(hdr->frame_control)) {
+        if (sta_flags & WLAN_STA_WME)
+                qos = true;
+
+        rcu_read_unlock();
+
+        if (!qos) {
                 skb->priority = 0; /* required for correct WPA/11i MIC */
                 return ieee802_1d_to_ac[skb->priority];
         }
@@ -68,6 +115,12 @@ static u16 classify80211(struct ieee80211_local *local, struct sk_buff *skb)
          * data frame has */
         skb->priority = cfg80211_classify8021d(skb);
 
+        return ieee80211_downgrade_queue(local, skb);
+}
+
+u16 ieee80211_downgrade_queue(struct ieee80211_local *local,
+                              struct sk_buff *skb)
+{
         /* in case we are a client verify acm is not set for this ac */
         while (unlikely(local->wmm_acm & BIT(skb->priority))) {
                 if (wme_downgrade_ac(skb)) {
@@ -85,24 +138,17 @@ static u16 classify80211(struct ieee80211_local *local, struct sk_buff *skb)
         return ieee802_1d_to_ac[skb->priority];
 }
 
-void ieee80211_select_queue(struct ieee80211_local *local, struct sk_buff *skb)
+void ieee80211_set_qos_hdr(struct ieee80211_local *local, struct sk_buff *skb)
 {
-        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-        u16 queue;
-        u8 tid;
-
-        queue = classify80211(local, skb);
-        if (unlikely(queue >= local->hw.queues))
-                queue = local->hw.queues - 1;
-
-        /*
-         * Now we know the 1d priority, fill in the QoS header if
-         * there is one (and we haven't done this before).
-         */
+        struct ieee80211_hdr *hdr = (void *)skb->data;
+
+        /* Fill in the QoS header if there is one. */
         if (ieee80211_is_data_qos(hdr->frame_control)) {
                 u8 *p = ieee80211_get_qos_ctl(hdr);
-                u8 ack_policy = 0;
+                u8 ack_policy = 0, tid;
+
                 tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK;
+
                 if (unlikely(local->wifi_wme_noack_test))
                         ack_policy |= QOS_CONTROL_ACK_POLICY_NOACK <<
                                         QOS_CONTROL_ACK_POLICY_SHIFT;
@@ -110,6 +156,4 @@ void ieee80211_select_queue(struct ieee80211_local *local, struct sk_buff *skb)
                 *p++ = ack_policy | tid;
                 *p = 0;
         }
-
-        skb_set_queue_mapping(skb, queue);
 }
diff --git a/net/mac80211/wme.h b/net/mac80211/wme.h
index d4fd87ca5118..6053b1c9feee 100644
--- a/net/mac80211/wme.h
+++ b/net/mac80211/wme.h
@@ -20,7 +20,11 @@
 
 extern const int ieee802_1d_to_ac[8];
 
-void ieee80211_select_queue(struct ieee80211_local *local,
-                            struct sk_buff *skb);
+u16 ieee80211_select_queue(struct ieee80211_sub_if_data *sdata,
+                           struct sk_buff *skb);
+void ieee80211_set_qos_hdr(struct ieee80211_local *local, struct sk_buff *skb);
+u16 ieee80211_downgrade_queue(struct ieee80211_local *local,
+                              struct sk_buff *skb);
+
 
 #endif /* _WME_H */