2 * Copyright 2004, Instant802 Networks, Inc.
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
9 #include <linux/netdevice.h>
10 #include <linux/skbuff.h>
11 #include <linux/module.h>
12 #include <linux/if_arp.h>
13 #include <linux/types.h>
15 #include <net/pkt_sched.h>
17 #include <net/mac80211.h>
18 #include "ieee80211_i.h"
21 static inline int WLAN_FC_IS_QOS_DATA(u16 fc
)
23 return (fc
& 0x8C) == 0x88;
28 ieee80211_rx_h_parse_qos(struct ieee80211_txrx_data
*rx
)
30 u8
*data
= rx
->skb
->data
;
32 unsigned int is_agg_frame
= 0;
34 /* does the frame have a qos control field? */
35 if (WLAN_FC_IS_QOS_DATA(rx
->fc
)) {
36 u8
*qc
= data
+ ieee80211_get_hdrlen(rx
->fc
) - QOS_CONTROL_LEN
;
38 /* frame has qos control */
39 rx
->u
.rx
.qos_control
= le16_to_cpu(*((__le16
*)qc
));
40 tid
= rx
->u
.rx
.qos_control
& QOS_CONTROL_TID_MASK
;
41 if (rx
->u
.rx
.qos_control
&
42 IEEE80211_QOS_CONTROL_A_MSDU_PRESENT
)
45 if (unlikely((rx
->fc
& IEEE80211_FCTL_FTYPE
) == IEEE80211_FTYPE_MGMT
)) {
46 /* Separate TID for management frames */
47 tid
= NUM_RX_DATA_QUEUES
- 1;
49 /* no qos control present */
50 tid
= 0; /* 802.1d - Best Effort */
52 rx
->u
.rx
.qos_control
= 0;
54 #ifdef CONFIG_MAC80211_DEBUG_COUNTERS
55 I802_DEBUG_INC(rx
->local
->wme_rx_queue
[tid
]);
57 I802_DEBUG_INC(rx
->sta
->wme_rx_queue
[tid
]);
59 #endif /* CONFIG_MAC80211_DEBUG_COUNTERS */
62 rx
->u
.rx
.is_agg_frame
= is_agg_frame
;
63 /* Set skb->priority to 1d tag if highest order bit of TID is not set.
64 * For now, set skb->priority to 0 for other cases. */
65 rx
->skb
->priority
= (tid
> 7) ? 0 : tid
;
72 ieee80211_rx_h_remove_qos_control(struct ieee80211_txrx_data
*rx
)
75 u8
*data
= rx
->skb
->data
;
76 struct ieee80211_hdr
*hdr
= (struct ieee80211_hdr
*) data
;
78 if (!WLAN_FC_IS_QOS_DATA(fc
))
81 /* remove the qos control field, update frame type and meta-data */
82 memmove(data
+ 2, data
, ieee80211_get_hdrlen(fc
) - 2);
83 hdr
= (struct ieee80211_hdr
*) skb_pull(rx
->skb
, 2);
84 /* change frame type to non QOS */
85 rx
->fc
= fc
&= ~IEEE80211_STYPE_QOS_DATA
;
86 hdr
->frame_control
= cpu_to_le16(fc
);
92 #ifdef CONFIG_NET_SCHED
93 /* maximum number of hardware queues we support. */
94 #define TC_80211_MAX_QUEUES 8
96 struct ieee80211_sched_data
98 struct tcf_proto
*filter_list
;
99 struct Qdisc
*queues
[TC_80211_MAX_QUEUES
];
100 struct sk_buff_head requeued
[TC_80211_MAX_QUEUES
];
104 /* given a data frame determine the 802.1p/1d tag to use */
105 static inline unsigned classify_1d(struct sk_buff
*skb
, struct Qdisc
*qd
)
111 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
112 struct tcf_result res
= { -1, 0 };
114 /* if there is a user set filter list, call out to that */
115 if (q
->filter_list
) {
116 tc_classify(skb
, q
->filter_list
, &res
);
121 /* skb->priority values from 256->263 are magic values to
122 * directly indicate a specific 802.1d priority.
123 * This is used to allow 802.1d priority to be passed directly in
124 * from VLAN tags, etc. */
125 if (skb
->priority
>= 256 && skb
->priority
<= 263)
126 return skb
->priority
- 256;
128 /* check there is a valid IP header present */
129 offset
= ieee80211_get_hdrlen_from_skb(skb
) + 8 /* LLC + proto */;
130 if (skb
->protocol
!= __constant_htons(ETH_P_IP
) ||
131 skb
->len
< offset
+ sizeof(*ip
))
134 ip
= (struct iphdr
*) (skb
->data
+ offset
);
136 dscp
= ip
->tos
& 0xfc;
143 static inline int wme_downgrade_ac(struct sk_buff
*skb
)
145 switch (skb
->priority
) {
148 skb
->priority
= 5; /* VO -> VI */
152 skb
->priority
= 3; /* VI -> BE */
156 skb
->priority
= 2; /* BE -> BK */
164 /* positive return value indicates which queue to use
165 * negative return value indicates to drop the frame */
166 static inline int classify80211(struct sk_buff
*skb
, struct Qdisc
*qd
)
168 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
169 struct ieee80211_sub_if_data
*sdata
= IEEE80211_DEV_TO_SUB_IF(qd
->dev
);
170 struct ieee80211_if_sta
*ifsta
= &sdata
->u
.sta
;
171 struct ieee80211_tx_packet_data
*pkt_data
=
172 (struct ieee80211_tx_packet_data
*) skb
->cb
;
173 struct ieee80211_hdr
*hdr
= (struct ieee80211_hdr
*) skb
->data
;
174 unsigned short fc
= le16_to_cpu(hdr
->frame_control
);
176 const int ieee802_1d_to_ac
[8] = { 2, 3, 3, 2, 1, 1, 0, 0 };
178 /* see if frame is data or non data frame */
179 if (unlikely((fc
& IEEE80211_FCTL_FTYPE
) != IEEE80211_FTYPE_DATA
)) {
180 /* management frames go on AC_VO queue, but are sent
181 * without QoS control fields */
182 return IEEE80211_TX_QUEUE_DATA0
;
185 if (unlikely(pkt_data
->mgmt_iface
)) {
186 /* Data frames from hostapd (mainly, EAPOL) use AC_VO
187 * and they will include QoS control fields if
188 * the target STA is using WME. */
190 return ieee802_1d_to_ac
[skb
->priority
];
193 /* is this a QoS frame? */
194 qos
= fc
& IEEE80211_STYPE_QOS_DATA
;
197 skb
->priority
= 0; /* required for correct WPA/11i MIC */
198 return ieee802_1d_to_ac
[skb
->priority
];
201 /* use the data classifier to determine what 802.1d tag the
203 skb
->priority
= classify_1d(skb
, qd
);
204 tsid
= 8 + skb
->priority
;
206 /* FIXME: only uplink needs to be checked for Tx */
209 if ((sdata
->type
== IEEE80211_IF_TYPE_STA
) &&
210 (local
->wmm_acm
& BIT(skb
->priority
))) {
211 switch (ifsta
->ts_data
[tsid
][dir
].status
) {
212 case TS_STATUS_ACTIVE
:
213 /* if TS Management is enabled, update used_time */
214 ifsta
->ts_data
[tsid
][dir
].used_time_usec
+=
215 ifsta
->MPDUExchangeTime
;
217 case TS_STATUS_THROTTLING
:
218 /* if admitted time is used up, refuse to send more */
220 printk(KERN_DEBUG
"QoS packet throttling\n");
227 /* in case we are a client verify acm is not set for this ac */
228 while ((local
->wmm_acm
& BIT(skb
->priority
)) &&
229 !((sdata
->type
== IEEE80211_IF_TYPE_STA
) &&
230 (ifsta
->ts_data
[skb
->priority
+ EDCA_TSID_MIN
][dir
].status
231 == TS_STATUS_ACTIVE
))) {
232 if (wme_downgrade_ac(skb
)) {
233 /* No AC with lower priority has acm=0, drop packet. */
238 /* look up which queue to use for frames with this 1d tag */
239 return ieee802_1d_to_ac
[skb
->priority
];
243 static int wme_qdiscop_enqueue(struct sk_buff
*skb
, struct Qdisc
* qd
)
245 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
246 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
247 struct ieee80211_tx_packet_data
*pkt_data
=
248 (struct ieee80211_tx_packet_data
*) skb
->cb
;
249 struct ieee80211_hdr
*hdr
= (struct ieee80211_hdr
*) skb
->data
;
250 unsigned short fc
= le16_to_cpu(hdr
->frame_control
);
254 if (pkt_data
->requeue
) {
255 skb_queue_tail(&q
->requeued
[pkt_data
->queue
], skb
);
260 queue
= classify80211(skb
, qd
);
262 /* now we know the 1d priority, fill in the QoS header if there is one
264 if (WLAN_FC_IS_QOS_DATA(fc
)) {
265 u8
*p
= skb
->data
+ ieee80211_get_hdrlen(fc
) - 2;
266 u8 qos_hdr
= skb
->priority
& QOS_CONTROL_TAG1D_MASK
;
267 if (local
->wifi_wme_noack_test
)
268 qos_hdr
|= QOS_CONTROL_ACK_POLICY_NOACK
<<
269 QOS_CONTROL_ACK_POLICY_SHIFT
;
270 /* qos header is 2 bytes, second reserved */
276 if (unlikely(queue
>= local
->hw
.queues
)) {
278 if (net_ratelimit()) {
279 printk(KERN_DEBUG
"%s - queue=%d (hw does not "
281 __func__
, queue
, local
->hw
.queues
- 1);
284 queue
= local
->hw
.queues
- 1;
287 if (unlikely(queue
< 0)) {
291 pkt_data
->queue
= (unsigned int) queue
;
292 qdisc
= q
->queues
[queue
];
293 err
= qdisc
->enqueue(skb
, qdisc
);
294 if (err
== NET_XMIT_SUCCESS
) {
296 qd
->bstats
.bytes
+= skb
->len
;
297 qd
->bstats
.packets
++;
298 return NET_XMIT_SUCCESS
;
306 /* TODO: clean up the cases where master_hard_start_xmit
307 * returns non 0 - it shouldn't ever do that. Once done we
308 * can remove this function */
309 static int wme_qdiscop_requeue(struct sk_buff
*skb
, struct Qdisc
* qd
)
311 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
312 struct ieee80211_tx_packet_data
*pkt_data
=
313 (struct ieee80211_tx_packet_data
*) skb
->cb
;
317 /* we recorded which queue to use earlier! */
318 qdisc
= q
->queues
[pkt_data
->queue
];
320 if ((err
= qdisc
->ops
->requeue(skb
, qdisc
)) == 0) {
329 static struct sk_buff
*wme_qdiscop_dequeue(struct Qdisc
* qd
)
331 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
332 struct net_device
*dev
= qd
->dev
;
333 struct ieee80211_local
*local
= wdev_priv(dev
->ieee80211_ptr
);
334 struct ieee80211_hw
*hw
= &local
->hw
;
339 /* check all the h/w queues in numeric/priority order */
340 for (queue
= 0; queue
< hw
->queues
; queue
++) {
341 /* see if there is room in this hardware queue */
342 if (test_bit(IEEE80211_LINK_STATE_XOFF
,
343 &local
->state
[queue
]) ||
344 test_bit(IEEE80211_LINK_STATE_PENDING
,
345 &local
->state
[queue
]))
348 /* there is space - try and get a frame */
349 skb
= skb_dequeue(&q
->requeued
[queue
]);
355 qdisc
= q
->queues
[queue
];
356 skb
= qdisc
->dequeue(qdisc
);
362 /* returning a NULL here when all the h/w queues are full means we
363 * never need to call netif_stop_queue in the driver */
368 static void wme_qdiscop_reset(struct Qdisc
* qd
)
370 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
371 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
372 struct ieee80211_hw
*hw
= &local
->hw
;
375 /* QUESTION: should we have some hardware flush functionality here? */
377 for (queue
= 0; queue
< hw
->queues
; queue
++) {
378 skb_queue_purge(&q
->requeued
[queue
]);
379 qdisc_reset(q
->queues
[queue
]);
385 static void wme_qdiscop_destroy(struct Qdisc
* qd
)
387 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
388 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
389 struct ieee80211_hw
*hw
= &local
->hw
;
392 tcf_destroy_chain(q
->filter_list
);
393 q
->filter_list
= NULL
;
395 for (queue
=0; queue
< hw
->queues
; queue
++) {
396 skb_queue_purge(&q
->requeued
[queue
]);
397 qdisc_destroy(q
->queues
[queue
]);
398 q
->queues
[queue
] = &noop_qdisc
;
403 /* called whenever parameters are updated on existing qdisc */
404 static int wme_qdiscop_tune(struct Qdisc
*qd
, struct rtattr
*opt
)
406 /* struct ieee80211_sched_data *q = qdisc_priv(qd);
408 /* check our options block is the right size */
409 /* copy any options to our local structure */
410 /* Ignore options block for now - always use static mapping
411 struct tc_ieee80211_qopt *qopt = RTA_DATA(opt);
413 if (opt->rta_len < RTA_LENGTH(sizeof(*qopt)))
415 memcpy(q->tag2queue, qopt->tag2queue, sizeof(qopt->tag2queue));
421 /* called during initial creation of qdisc on device */
422 static int wme_qdiscop_init(struct Qdisc
*qd
, struct rtattr
*opt
)
424 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
425 struct net_device
*dev
= qd
->dev
;
426 struct ieee80211_local
*local
;
430 /* check that device is a mac80211 device */
431 if (!dev
->ieee80211_ptr
||
432 dev
->ieee80211_ptr
->wiphy
->privid
!= mac80211_wiphy_privid
)
435 /* check this device is an ieee80211 master type device */
436 if (dev
->type
!= ARPHRD_IEEE80211
)
439 /* check that there is no qdisc currently attached to device
440 * this ensures that we will be the root qdisc. (I can't find a better
441 * way to test this explicitly) */
442 if (dev
->qdisc_sleeping
!= &noop_qdisc
)
445 if (qd
->flags
& TCQ_F_INGRESS
)
448 local
= wdev_priv(dev
->ieee80211_ptr
);
449 queues
= local
->hw
.queues
;
451 /* if options were passed in, set them */
453 err
= wme_qdiscop_tune(qd
, opt
);
456 /* create child queues */
457 for (i
= 0; i
< queues
; i
++) {
458 skb_queue_head_init(&q
->requeued
[i
]);
459 q
->queues
[i
] = qdisc_create_dflt(qd
->dev
, &pfifo_qdisc_ops
,
461 if (q
->queues
[i
] == 0) {
462 q
->queues
[i
] = &noop_qdisc
;
463 printk(KERN_ERR
"%s child qdisc %i creation failed", dev
->name
, i
);
470 static int wme_qdiscop_dump(struct Qdisc
*qd
, struct sk_buff
*skb
)
472 /* struct ieee80211_sched_data *q = qdisc_priv(qd);
473 unsigned char *p = skb->tail;
474 struct tc_ieee80211_qopt opt;
476 memcpy(&opt.tag2queue, q->tag2queue, TC_80211_MAX_TAG + 1);
477 RTA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
481 skb_trim(skb, p - skb->data);*/
486 static int wme_classop_graft(struct Qdisc
*qd
, unsigned long arg
,
487 struct Qdisc
*new, struct Qdisc
**old
)
489 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
490 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
491 struct ieee80211_hw
*hw
= &local
->hw
;
492 unsigned long queue
= arg
- 1;
494 if (queue
>= hw
->queues
)
501 *old
= q
->queues
[queue
];
502 q
->queues
[queue
] = new;
510 static struct Qdisc
*
511 wme_classop_leaf(struct Qdisc
*qd
, unsigned long arg
)
513 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
514 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
515 struct ieee80211_hw
*hw
= &local
->hw
;
516 unsigned long queue
= arg
- 1;
518 if (queue
>= hw
->queues
)
521 return q
->queues
[queue
];
525 static unsigned long wme_classop_get(struct Qdisc
*qd
, u32 classid
)
527 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
528 struct ieee80211_hw
*hw
= &local
->hw
;
529 unsigned long queue
= TC_H_MIN(classid
);
531 if (queue
- 1 >= hw
->queues
)
538 static unsigned long wme_classop_bind(struct Qdisc
*qd
, unsigned long parent
,
541 return wme_classop_get(qd
, classid
);
545 static void wme_classop_put(struct Qdisc
*q
, unsigned long cl
)
550 static int wme_classop_change(struct Qdisc
*qd
, u32 handle
, u32 parent
,
551 struct rtattr
**tca
, unsigned long *arg
)
553 unsigned long cl
= *arg
;
554 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
555 struct ieee80211_hw
*hw
= &local
->hw
;
557 if (cl
- 1 > hw
->queues
)
560 /* TODO: put code to program hardware queue parameters here,
561 * to allow programming from tc command line */
567 /* we don't support deleting hardware queues
568 * when we add WMM-SA support - TSPECs may be deleted here */
569 static int wme_classop_delete(struct Qdisc
*qd
, unsigned long cl
)
571 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
572 struct ieee80211_hw
*hw
= &local
->hw
;
574 if (cl
- 1 > hw
->queues
)
580 static int wme_classop_dump_class(struct Qdisc
*qd
, unsigned long cl
,
581 struct sk_buff
*skb
, struct tcmsg
*tcm
)
583 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
584 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
585 struct ieee80211_hw
*hw
= &local
->hw
;
587 if (cl
- 1 > hw
->queues
)
589 tcm
->tcm_handle
= TC_H_MIN(cl
);
590 tcm
->tcm_parent
= qd
->handle
;
591 tcm
->tcm_info
= q
->queues
[cl
-1]->handle
; /* do we need this? */
596 static void wme_classop_walk(struct Qdisc
*qd
, struct qdisc_walker
*arg
)
598 struct ieee80211_local
*local
= wdev_priv(qd
->dev
->ieee80211_ptr
);
599 struct ieee80211_hw
*hw
= &local
->hw
;
605 for (queue
= 0; queue
< hw
->queues
; queue
++) {
606 if (arg
->count
< arg
->skip
) {
610 /* we should return classids for our internal queues here
611 * as well as the external ones */
612 if (arg
->fn(qd
, queue
+1, arg
) < 0) {
621 static struct tcf_proto
** wme_classop_find_tcf(struct Qdisc
*qd
,
624 struct ieee80211_sched_data
*q
= qdisc_priv(qd
);
629 return &q
->filter_list
;
633 /* this qdisc is classful (i.e. has classes, some of which may have leaf qdiscs attached)
634 * - these are the operations on the classes */
635 static struct Qdisc_class_ops class_ops
=
637 .graft
= wme_classop_graft
,
638 .leaf
= wme_classop_leaf
,
640 .get
= wme_classop_get
,
641 .put
= wme_classop_put
,
642 .change
= wme_classop_change
,
643 .delete = wme_classop_delete
,
644 .walk
= wme_classop_walk
,
646 .tcf_chain
= wme_classop_find_tcf
,
647 .bind_tcf
= wme_classop_bind
,
648 .unbind_tcf
= wme_classop_put
,
650 .dump
= wme_classop_dump_class
,
654 /* queueing discipline operations */
655 static struct Qdisc_ops wme_qdisc_ops
=
658 .cl_ops
= &class_ops
,
660 .priv_size
= sizeof(struct ieee80211_sched_data
),
662 .enqueue
= wme_qdiscop_enqueue
,
663 .dequeue
= wme_qdiscop_dequeue
,
664 .requeue
= wme_qdiscop_requeue
,
665 .drop
= NULL
, /* drop not needed since we are always the root qdisc */
667 .init
= wme_qdiscop_init
,
668 .reset
= wme_qdiscop_reset
,
669 .destroy
= wme_qdiscop_destroy
,
670 .change
= wme_qdiscop_tune
,
672 .dump
= wme_qdiscop_dump
,
676 void ieee80211_install_qdisc(struct net_device
*dev
)
680 qdisc
= qdisc_create_dflt(dev
, &wme_qdisc_ops
, TC_H_ROOT
);
682 printk(KERN_ERR
"%s: qdisc installation failed\n", dev
->name
);
686 /* same handle as would be allocated by qdisc_alloc_handle() */
687 qdisc
->handle
= 0x80010000;
689 qdisc_lock_tree(dev
);
690 list_add_tail(&qdisc
->list
, &dev
->qdisc_list
);
691 dev
->qdisc_sleeping
= qdisc
;
692 qdisc_unlock_tree(dev
);
696 int ieee80211_qdisc_installed(struct net_device
*dev
)
698 return dev
->qdisc_sleeping
->ops
== &wme_qdisc_ops
;
702 int ieee80211_wme_register(void)
704 return register_qdisc(&wme_qdisc_ops
);
708 void ieee80211_wme_unregister(void)
710 unregister_qdisc(&wme_qdisc_ops
);
712 #endif /* CONFIG_NET_SCHED */