1 files changed, 50 insertions, 70 deletions

diff --git a/net/mac80211/wme.c b/net/mac80211/wme.c
index 64faa3dc488f..5eddf1f32ed9 100644
--- a/net/mac80211/wme.c
+++ b/net/mac80211/wme.c
@@ -19,16 +19,22 @@
 #include "wme.h"
 
 /* maximum number of hardware queues we support. */
-#define TC_80211_MAX_QUEUES 16
+#define QD_MAX_QUEUES (IEEE80211_MAX_AMPDU_QUEUES + IEEE80211_MAX_QUEUES)
+/* current number of hardware queues we support. */
+#define QD_NUM(hw) ((hw)->queues + (hw)->ampdu_queues)
 
+/*
+ * Default mapping in classifier to work with default
+ * queue setup.
+ */
 const int ieee802_1d_to_ac[8] = { 2, 3, 3, 2, 1, 1, 0, 0 };
 
 struct ieee80211_sched_data
 {
-        unsigned long qdisc_pool[BITS_TO_LONGS(TC_80211_MAX_QUEUES)];
+        unsigned long qdisc_pool[BITS_TO_LONGS(QD_MAX_QUEUES)];
         struct tcf_proto *filter_list;
-        struct Qdisc *queues[TC_80211_MAX_QUEUES];
-        struct sk_buff_head requeued[TC_80211_MAX_QUEUES];
+        struct Qdisc *queues[QD_MAX_QUEUES];
+        struct sk_buff_head requeued[QD_MAX_QUEUES];
 };
 
 static const char llc_ip_hdr[8] = {0xAA, 0xAA, 0x3, 0, 0, 0, 0x08, 0};
@@ -95,7 +101,7 @@ static inline int wme_downgrade_ac(struct sk_buff *skb)
 
 /* positive return value indicates which queue to use
  * negative return value indicates to drop the frame */
-static inline int classify80211(struct sk_buff *skb, struct Qdisc *qd)
+static int classify80211(struct sk_buff *skb, struct Qdisc *qd)
 {
         struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
@@ -106,7 +112,7 @@ static inline int classify80211(struct sk_buff *skb, struct Qdisc *qd)
         if (unlikely((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA)) {
                 /* management frames go on AC_VO queue, but are sent
                 * without QoS control fields */
-                return IEEE80211_TX_QUEUE_DATA0;
+                return 0;
         }
 
         if (0 /* injected */) {
@@ -141,14 +147,16 @@ static inline int classify80211(struct sk_buff *skb, struct Qdisc *qd)
 static int wme_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
 {
         struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
+        struct ieee80211_hw *hw = &local->hw;
         struct ieee80211_sched_data *q = qdisc_priv(qd);
         struct ieee80211_tx_packet_data *pkt_data =
                 (struct ieee80211_tx_packet_data *) skb->cb;
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
         unsigned short fc = le16_to_cpu(hdr->frame_control);
         struct Qdisc *qdisc;
-        int err, queue;
         struct sta_info *sta;
+        int err;
+        u16 queue;
         u8 tid;
 
         if (pkt_data->flags & IEEE80211_TXPD_REQUEUE) {
@@ -158,7 +166,7 @@ static int wme_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
                 tid = skb->priority & QOS_CONTROL_TAG1D_MASK;
                 if (sta) {
                         int ampdu_queue = sta->tid_to_tx_q[tid];
-                        if ((ampdu_queue < local->hw.queues) &&
+                        if ((ampdu_queue < QD_NUM(hw)) &&
                             test_bit(ampdu_queue, q->qdisc_pool)) {
                                 queue = ampdu_queue;
                                 pkt_data->flags |= IEEE80211_TXPD_AMPDU;
@@ -174,6 +182,9 @@ static int wme_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
 
         queue = classify80211(skb, qd);
 
+        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
          */
         if (WLAN_FC_IS_QOS_DATA(fc)) {
@@ -193,8 +204,8 @@ static int wme_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
                 sta = sta_info_get(local, hdr->addr1);
                 if (sta) {
                         int ampdu_queue = sta->tid_to_tx_q[tid];
-                        if ((ampdu_queue < local->hw.queues) &&
-                                test_bit(ampdu_queue, q->qdisc_pool)) {
+                        if ((ampdu_queue < QD_NUM(hw)) &&
+                            test_bit(ampdu_queue, q->qdisc_pool)) {
                                 queue = ampdu_queue;
                                 pkt_data->flags |= IEEE80211_TXPD_AMPDU;
                         } else {
@@ -205,17 +216,6 @@ static int wme_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
                 rcu_read_unlock();
         }
 
-        if (unlikely(queue >= local->hw.queues)) {
-#if 0
-                if (net_ratelimit()) {
-                        printk(KERN_DEBUG "%s - queue=%d (hw does not "
-                               "support) -> %d\n",
-                               __func__, queue, local->hw.queues - 1);
-                }
-#endif
-                queue = local->hw.queues - 1;
-        }
-
         if (unlikely(queue < 0)) {
                         kfree_skb(skb);
                         err = NET_XMIT_DROP;
@@ -270,7 +270,7 @@ static struct sk_buff *wme_qdiscop_dequeue(struct Qdisc* qd)
         int queue;
 
         /* check all the h/w queues in numeric/priority order */
-        for (queue = 0; queue < hw->queues; queue++) {
+        for (queue = 0; queue < QD_NUM(hw); queue++) {
                 /* see if there is room in this hardware queue */
                 if ((test_bit(IEEE80211_LINK_STATE_XOFF,
                                 &local->state[queue])) ||
@@ -308,7 +308,7 @@ static void wme_qdiscop_reset(struct Qdisc* qd)
 
         /* QUESTION: should we have some hardware flush functionality here? */
 
-        for (queue = 0; queue < hw->queues; queue++) {
+        for (queue = 0; queue < QD_NUM(hw); queue++) {
                 skb_queue_purge(&q->requeued[queue]);
                 qdisc_reset(q->queues[queue]);
         }
@@ -326,7 +326,7 @@ static void wme_qdiscop_destroy(struct Qdisc* qd)
         tcf_destroy_chain(q->filter_list);
         q->filter_list = NULL;
 
-        for (queue=0; queue < hw->queues; queue++) {
+        for (queue = 0; queue < QD_NUM(hw); queue++) {
                 skb_queue_purge(&q->requeued[queue]);
                 qdisc_destroy(q->queues[queue]);
                 q->queues[queue] = &noop_qdisc;
@@ -337,17 +337,6 @@ static void wme_qdiscop_destroy(struct Qdisc* qd)
 /* called whenever parameters are updated on existing qdisc */
 static int wme_qdiscop_tune(struct Qdisc *qd, struct nlattr *opt)
 {
-/*      struct ieee80211_sched_data *q = qdisc_priv(qd);
-*/
-        /* check our options block is the right size */
-        /* copy any options to our local structure */
-/*      Ignore options block for now - always use static mapping
-        struct tc_ieee80211_qopt *qopt = nla_data(opt);
-
-        if (opt->nla_len < nla_attr_size(sizeof(*qopt)))
-                return -EINVAL;
-        memcpy(q->tag2queue, qopt->tag2queue, sizeof(qopt->tag2queue));
-*/
         return 0;
 }
 
@@ -358,7 +347,7 @@ static int wme_qdiscop_init(struct Qdisc *qd, struct nlattr *opt)
         struct ieee80211_sched_data *q = qdisc_priv(qd);
         struct net_device *dev = qd->dev;
         struct ieee80211_local *local;
-        int queues;
+        struct ieee80211_hw *hw;
         int err = 0, i;
 
         /* check that device is a mac80211 device */
@@ -366,29 +355,26 @@ static int wme_qdiscop_init(struct Qdisc *qd, struct nlattr *opt)
             dev->ieee80211_ptr->wiphy->privid != mac80211_wiphy_privid)
                 return -EINVAL;
 
-        /* check this device is an ieee80211 master type device */
-        if (dev->type != ARPHRD_IEEE80211)
+        local = wdev_priv(dev->ieee80211_ptr);
+        hw = &local->hw;
+
+        /* only allow on master dev */
+        if (dev != local->mdev)
                 return -EINVAL;
 
-        /* check that there is no qdisc currently attached to device
-         * this ensures that we will be the root qdisc. (I can't find a better
-         * way to test this explicitly) */
-        if (dev->qdisc_sleeping != &noop_qdisc)
+        /* ensure that we are root qdisc */
+        if (qd->parent != TC_H_ROOT)
                 return -EINVAL;
 
         if (qd->flags & TCQ_F_INGRESS)
                 return -EINVAL;
 
-        local = wdev_priv(dev->ieee80211_ptr);
-        queues = local->hw.queues;
-
         /* if options were passed in, set them */
-        if (opt) {
+        if (opt)
                 err = wme_qdiscop_tune(qd, opt);
-        }
 
         /* create child queues */
-        for (i = 0; i < queues; i++) {
+        for (i = 0; i < QD_NUM(hw); i++) {
                 skb_queue_head_init(&q->requeued[i]);
                 q->queues[i] = qdisc_create_dflt(qd->dev, &pfifo_qdisc_ops,
                                                  qd->handle);
@@ -398,8 +384,8 @@ static int wme_qdiscop_init(struct Qdisc *qd, struct nlattr *opt)
                 }
         }
 
-        /* reserve all legacy QoS queues */
-        for (i = 0; i < min(IEEE80211_TX_QUEUE_DATA4, queues); i++)
+        /* non-aggregation queues: reserve/mark as used */
+        for (i = 0; i < local->hw.queues; i++)
                 set_bit(i, q->qdisc_pool);
 
         return err;
@@ -407,16 +393,6 @@ static int wme_qdiscop_init(struct Qdisc *qd, struct nlattr *opt)
 
 static int wme_qdiscop_dump(struct Qdisc *qd, struct sk_buff *skb)
 {
-/*      struct ieee80211_sched_data *q = qdisc_priv(qd);
-        unsigned char *p = skb->tail;
-        struct tc_ieee80211_qopt opt;
-
-        memcpy(&opt.tag2queue, q->tag2queue, TC_80211_MAX_TAG + 1);
-        NLA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
-*/      return skb->len;
-/*
-nla_put_failure:
-        skb_trim(skb, p - skb->data);*/
         return -1;
 }
 
@@ -429,7 +405,7 @@ static int wme_classop_graft(struct Qdisc *qd, unsigned long arg,
         struct ieee80211_hw *hw = &local->hw;
         unsigned long queue = arg - 1;
 
-        if (queue >= hw->queues)
+        if (queue >= QD_NUM(hw))
                 return -EINVAL;
 
         if (!new)
@@ -453,7 +429,7 @@ wme_classop_leaf(struct Qdisc *qd, unsigned long arg)
         struct ieee80211_hw *hw = &local->hw;
         unsigned long queue = arg - 1;
 
-        if (queue >= hw->queues)
+        if (queue >= QD_NUM(hw))
                 return NULL;
 
         return q->queues[queue];
@@ -466,7 +442,7 @@ static unsigned long wme_classop_get(struct Qdisc *qd, u32 classid)
         struct ieee80211_hw *hw = &local->hw;
         unsigned long queue = TC_H_MIN(classid);
 
-        if (queue - 1 >= hw->queues)
+        if (queue - 1 >= QD_NUM(hw))
                 return 0;
 
         return queue;
@@ -492,7 +468,7 @@ static int wme_classop_change(struct Qdisc *qd, u32 handle, u32 parent,
         struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
         struct ieee80211_hw *hw = &local->hw;
 
-        if (cl - 1 > hw->queues)
+        if (cl - 1 > QD_NUM(hw))
                 return -ENOENT;
 
         /* TODO: put code to program hardware queue parameters here,
@@ -509,7 +485,7 @@ static int wme_classop_delete(struct Qdisc *qd, unsigned long cl)
         struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
         struct ieee80211_hw *hw = &local->hw;
 
-        if (cl - 1 > hw->queues)
+        if (cl - 1 > QD_NUM(hw))
                 return -ENOENT;
         return 0;
 }
@@ -522,7 +498,7 @@ static int wme_classop_dump_class(struct Qdisc *qd, unsigned long cl,
         struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
         struct ieee80211_hw *hw = &local->hw;
 
-        if (cl - 1 > hw->queues)
+        if (cl - 1 > QD_NUM(hw))
                 return -ENOENT;
         tcm->tcm_handle = TC_H_MIN(cl);
         tcm->tcm_parent = qd->handle;
@@ -540,7 +516,7 @@ static void wme_classop_walk(struct Qdisc *qd, struct qdisc_walker *arg)
         if (arg->stop)
                 return;
 
-        for (queue = 0; queue < hw->queues; queue++) {
+        for (queue = 0; queue < QD_NUM(hw); queue++) {
                 if (arg->count < arg->skip) {
                         arg->count++;
                         continue;
@@ -657,10 +633,13 @@ int ieee80211_ht_agg_queue_add(struct ieee80211_local *local,
         DECLARE_MAC_BUF(mac);
 
         /* prepare the filter and save it for the SW queue
-         * matching the recieved HW queue */
+         * matching the received HW queue */
+
+        if (!local->hw.ampdu_queues)
+                return -EPERM;
 
         /* try to get a Qdisc from the pool */
-        for (i = IEEE80211_TX_QUEUE_BEACON; i < local->hw.queues; i++)
+        for (i = local->hw.queues; i < QD_NUM(&local->hw); i++)
                 if (!test_and_set_bit(i, q->qdisc_pool)) {
                         ieee80211_stop_queue(local_to_hw(local), i);
                         sta->tid_to_tx_q[tid] = i;
@@ -689,13 +668,14 @@ void ieee80211_ht_agg_queue_remove(struct ieee80211_local *local,
                                    struct sta_info *sta, u16 tid,
                                    u8 requeue)
 {
+        struct ieee80211_hw *hw = &local->hw;
         struct ieee80211_sched_data *q =
                 qdisc_priv(local->mdev->qdisc_sleeping);
         int agg_queue = sta->tid_to_tx_q[tid];
 
         /* return the qdisc to the pool */
         clear_bit(agg_queue, q->qdisc_pool);
-        sta->tid_to_tx_q[tid] = local->hw.queues;
+        sta->tid_to_tx_q[tid] = QD_NUM(hw);
 
         if (requeue)
                 ieee80211_requeue(local, agg_queue);