1 files changed, 50 insertions, 70 deletions

diff --git a/net/mac80211/wme.c b/net/mac80211/wme.c
index 64faa3dc488..5eddf1f32ed 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);