Commit | Line | Data |
---|---|---|
f0706e82 JB |
1 | /* |
2 | * Copyright 2004, Instant802 Networks, Inc. | |
3 | * | |
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. | |
7 | */ | |
8 | ||
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> | |
14 | #include <net/ip.h> | |
15 | #include <net/pkt_sched.h> | |
16 | ||
17 | #include <net/mac80211.h> | |
18 | #include "ieee80211_i.h" | |
19 | #include "wme.h" | |
20 | ||
51cb6db0 | 21 | /* Default mapping in classifier to work with default |
e100bb64 JB |
22 | * queue setup. |
23 | */ | |
9e723492 | 24 | const int ieee802_1d_to_ac[8] = { 2, 3, 3, 2, 1, 1, 0, 0 }; |
f0706e82 | 25 | |
a8bdf29c | 26 | static const char llc_ip_hdr[8] = {0xAA, 0xAA, 0x3, 0, 0, 0, 0x08, 0}; |
f0706e82 | 27 | |
51cb6db0 DM |
28 | /* Given a data frame determine the 802.1p/1d tag to use. */ |
29 | static unsigned int classify_1d(struct sk_buff *skb) | |
f0706e82 | 30 | { |
51cb6db0 | 31 | unsigned int dscp; |
f0706e82 JB |
32 | |
33 | /* skb->priority values from 256->263 are magic values to | |
51cb6db0 DM |
34 | * directly indicate a specific 802.1d priority. This is used |
35 | * to allow 802.1d priority to be passed directly in from VLAN | |
36 | * tags, etc. | |
37 | */ | |
f0706e82 JB |
38 | if (skb->priority >= 256 && skb->priority <= 263) |
39 | return skb->priority - 256; | |
40 | ||
51cb6db0 DM |
41 | switch (skb->protocol) { |
42 | case __constant_htons(ETH_P_IP): | |
43 | dscp = ip_hdr(skb)->tos & 0xfc; | |
44 | break; | |
f0706e82 | 45 | |
51cb6db0 DM |
46 | default: |
47 | return 0; | |
48 | } | |
f0706e82 | 49 | |
f0706e82 JB |
50 | if (dscp & 0x1c) |
51 | return 0; | |
52 | return dscp >> 5; | |
53 | } | |
54 | ||
55 | ||
51cb6db0 | 56 | static int wme_downgrade_ac(struct sk_buff *skb) |
f0706e82 JB |
57 | { |
58 | switch (skb->priority) { | |
59 | case 6: | |
60 | case 7: | |
61 | skb->priority = 5; /* VO -> VI */ | |
62 | return 0; | |
63 | case 4: | |
64 | case 5: | |
65 | skb->priority = 3; /* VI -> BE */ | |
66 | return 0; | |
67 | case 0: | |
68 | case 3: | |
69 | skb->priority = 2; /* BE -> BK */ | |
70 | return 0; | |
71 | default: | |
72 | return -1; | |
73 | } | |
74 | } | |
75 | ||
76 | ||
51cb6db0 DM |
77 | /* Indicate which queue to use. */ |
78 | static u16 classify80211(struct sk_buff *skb, struct net_device *dev) | |
f0706e82 | 79 | { |
51cb6db0 | 80 | struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr); |
f0706e82 | 81 | struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; |
f0706e82 | 82 | |
002aaf4e | 83 | if (!ieee80211_is_data(hdr->frame_control)) { |
f0706e82 JB |
84 | /* management frames go on AC_VO queue, but are sent |
85 | * without QoS control fields */ | |
e100bb64 | 86 | return 0; |
f0706e82 JB |
87 | } |
88 | ||
f9d540ee JB |
89 | if (0 /* injected */) { |
90 | /* use AC from radiotap */ | |
f0706e82 JB |
91 | } |
92 | ||
002aaf4e | 93 | if (!ieee80211_is_data_qos(hdr->frame_control)) { |
f0706e82 JB |
94 | skb->priority = 0; /* required for correct WPA/11i MIC */ |
95 | return ieee802_1d_to_ac[skb->priority]; | |
96 | } | |
97 | ||
98 | /* use the data classifier to determine what 802.1d tag the | |
3c3b00ca | 99 | * data frame has */ |
51cb6db0 | 100 | skb->priority = classify_1d(skb); |
f0706e82 | 101 | |
3c3b00ca | 102 | /* in case we are a client verify acm is not set for this ac */ |
f0706e82 JB |
103 | while (unlikely(local->wmm_acm & BIT(skb->priority))) { |
104 | if (wme_downgrade_ac(skb)) { | |
51cb6db0 DM |
105 | /* The old code would drop the packet in this |
106 | * case. | |
107 | */ | |
108 | return 0; | |
f0706e82 JB |
109 | } |
110 | } | |
111 | ||
112 | /* look up which queue to use for frames with this 1d tag */ | |
113 | return ieee802_1d_to_ac[skb->priority]; | |
114 | } | |
115 | ||
51cb6db0 | 116 | u16 ieee80211_select_queue(struct net_device *dev, struct sk_buff *skb) |
f0706e82 | 117 | { |
f0706e82 | 118 | struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; |
51cb6db0 DM |
119 | struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr); |
120 | struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb); | |
9e723492 | 121 | struct sta_info *sta; |
51cb6db0 | 122 | u16 queue; |
9e723492 | 123 | u8 tid; |
f0706e82 | 124 | |
51cb6db0 DM |
125 | queue = classify80211(skb, dev); |
126 | if (unlikely(queue >= local->hw.queues)) | |
127 | queue = local->hw.queues - 1; | |
128 | ||
e039fa4a | 129 | if (info->flags & IEEE80211_TX_CTL_REQUEUE) { |
d0709a65 | 130 | rcu_read_lock(); |
9e723492 | 131 | sta = sta_info_get(local, hdr->addr1); |
238f74a2 | 132 | tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK; |
9e723492 | 133 | if (sta) { |
51cb6db0 | 134 | struct ieee80211_hw *hw = &local->hw; |
9e723492 | 135 | int ampdu_queue = sta->tid_to_tx_q[tid]; |
51cb6db0 DM |
136 | |
137 | if ((ampdu_queue < ieee80211_num_queues(hw)) && | |
138 | test_bit(ampdu_queue, local->queue_pool)) { | |
9e723492 | 139 | queue = ampdu_queue; |
e039fa4a | 140 | info->flags |= IEEE80211_TX_CTL_AMPDU; |
9e723492 | 141 | } else { |
e039fa4a | 142 | info->flags &= ~IEEE80211_TX_CTL_AMPDU; |
9e723492 | 143 | } |
9e723492 | 144 | } |
d0709a65 | 145 | rcu_read_unlock(); |
f0706e82 | 146 | |
51cb6db0 DM |
147 | return queue; |
148 | } | |
e100bb64 | 149 | |
51cb6db0 DM |
150 | /* Now we know the 1d priority, fill in the QoS header if |
151 | * there is one. | |
f0706e82 | 152 | */ |
002aaf4e HH |
153 | if (ieee80211_is_data_qos(hdr->frame_control)) { |
154 | u8 *p = ieee80211_get_qos_ctl(hdr); | |
9e723492 | 155 | u8 ack_policy = 0; |
238f74a2 | 156 | tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK; |
f0706e82 | 157 | if (local->wifi_wme_noack_test) |
9e723492 | 158 | ack_policy |= QOS_CONTROL_ACK_POLICY_NOACK << |
f0706e82 JB |
159 | QOS_CONTROL_ACK_POLICY_SHIFT; |
160 | /* qos header is 2 bytes, second reserved */ | |
002aaf4e | 161 | *p++ = ack_policy | tid; |
f0706e82 | 162 | *p = 0; |
9e723492 | 163 | |
d0709a65 JB |
164 | rcu_read_lock(); |
165 | ||
9e723492 RR |
166 | sta = sta_info_get(local, hdr->addr1); |
167 | if (sta) { | |
168 | int ampdu_queue = sta->tid_to_tx_q[tid]; | |
51cb6db0 DM |
169 | struct ieee80211_hw *hw = &local->hw; |
170 | ||
171 | if ((ampdu_queue < ieee80211_num_queues(hw)) && | |
172 | test_bit(ampdu_queue, local->queue_pool)) { | |
9e723492 | 173 | queue = ampdu_queue; |
e039fa4a | 174 | info->flags |= IEEE80211_TX_CTL_AMPDU; |
9e723492 | 175 | } else { |
e039fa4a | 176 | info->flags &= ~IEEE80211_TX_CTL_AMPDU; |
9e723492 | 177 | } |
9e723492 | 178 | } |
d0709a65 JB |
179 | |
180 | rcu_read_unlock(); | |
f0706e82 JB |
181 | } |
182 | ||
f0706e82 JB |
183 | return queue; |
184 | } | |
185 | ||
9e723492 | 186 | int ieee80211_ht_agg_queue_add(struct ieee80211_local *local, |
51cb6db0 | 187 | struct sta_info *sta, u16 tid) |
9e723492 RR |
188 | { |
189 | int i; | |
9e723492 RR |
190 | |
191 | /* prepare the filter and save it for the SW queue | |
e100bb64 JB |
192 | * matching the received HW queue */ |
193 | ||
194 | if (!local->hw.ampdu_queues) | |
195 | return -EPERM; | |
9e723492 RR |
196 | |
197 | /* try to get a Qdisc from the pool */ | |
51cb6db0 DM |
198 | for (i = local->hw.queues; i < ieee80211_num_queues(&local->hw); i++) |
199 | if (!test_and_set_bit(i, local->queue_pool)) { | |
9e723492 RR |
200 | ieee80211_stop_queue(local_to_hw(local), i); |
201 | sta->tid_to_tx_q[tid] = i; | |
202 | ||
203 | /* IF there are already pending packets | |
204 | * on this tid first we need to drain them | |
205 | * on the previous queue | |
206 | * since HT is strict in order */ | |
207 | #ifdef CONFIG_MAC80211_HT_DEBUG | |
51cb6db0 DM |
208 | if (net_ratelimit()) { |
209 | DECLARE_MAC_BUF(mac); | |
9e723492 | 210 | printk(KERN_DEBUG "allocated aggregation queue" |
995ad6c5 | 211 | " %d tid %d addr %s pool=0x%lX\n", |
9e723492 | 212 | i, tid, print_mac(mac, sta->addr), |
51cb6db0 DM |
213 | local->queue_pool[0]); |
214 | } | |
9e723492 RR |
215 | #endif /* CONFIG_MAC80211_HT_DEBUG */ |
216 | return 0; | |
217 | } | |
218 | ||
219 | return -EAGAIN; | |
220 | } | |
221 | ||
222 | /** | |
e8a0464c | 223 | * the caller needs to hold netdev_get_tx_queue(local->mdev, X)->lock |
9e723492 RR |
224 | */ |
225 | void ieee80211_ht_agg_queue_remove(struct ieee80211_local *local, | |
226 | struct sta_info *sta, u16 tid, | |
227 | u8 requeue) | |
228 | { | |
9e723492 | 229 | int agg_queue = sta->tid_to_tx_q[tid]; |
51cb6db0 | 230 | struct ieee80211_hw *hw = &local->hw; |
9e723492 RR |
231 | |
232 | /* return the qdisc to the pool */ | |
51cb6db0 DM |
233 | clear_bit(agg_queue, local->queue_pool); |
234 | sta->tid_to_tx_q[tid] = ieee80211_num_queues(hw); | |
9e723492 | 235 | |
51cb6db0 | 236 | if (requeue) { |
9e723492 | 237 | ieee80211_requeue(local, agg_queue); |
51cb6db0 DM |
238 | } else { |
239 | struct netdev_queue *txq; | |
83874000 | 240 | spinlock_t *root_lock; |
51cb6db0 DM |
241 | |
242 | txq = netdev_get_tx_queue(local->mdev, agg_queue); | |
83874000 | 243 | root_lock = qdisc_root_lock(txq->qdisc); |
51cb6db0 | 244 | |
83874000 | 245 | spin_lock_bh(root_lock); |
51cb6db0 | 246 | qdisc_reset(txq->qdisc); |
83874000 | 247 | spin_unlock_bh(root_lock); |
51cb6db0 | 248 | } |
9e723492 RR |
249 | } |
250 | ||
251 | void ieee80211_requeue(struct ieee80211_local *local, int queue) | |
252 | { | |
51cb6db0 DM |
253 | struct netdev_queue *txq = netdev_get_tx_queue(local->mdev, queue); |
254 | struct sk_buff_head list; | |
83874000 | 255 | spinlock_t *root_lock; |
51cb6db0 | 256 | struct Qdisc *qdisc; |
0da926f0 | 257 | u32 len; |
9e723492 | 258 | |
51cb6db0 DM |
259 | rcu_read_lock_bh(); |
260 | ||
261 | qdisc = rcu_dereference(txq->qdisc); | |
9e723492 | 262 | if (!qdisc || !qdisc->dequeue) |
51cb6db0 DM |
263 | goto out_unlock; |
264 | ||
265 | skb_queue_head_init(&list); | |
9e723492 | 266 | |
83874000 DM |
267 | root_lock = qdisc_root_lock(qdisc); |
268 | spin_lock(root_lock); | |
9e723492 | 269 | for (len = qdisc->q.qlen; len > 0; len--) { |
51cb6db0 DM |
270 | struct sk_buff *skb = qdisc->dequeue(qdisc); |
271 | ||
9e723492 | 272 | if (skb) |
51cb6db0 DM |
273 | __skb_queue_tail(&list, skb); |
274 | } | |
83874000 | 275 | spin_unlock(root_lock); |
51cb6db0 DM |
276 | |
277 | for (len = list.qlen; len > 0; len--) { | |
278 | struct sk_buff *skb = __skb_dequeue(&list); | |
279 | u16 new_queue; | |
280 | ||
281 | BUG_ON(!skb); | |
282 | new_queue = ieee80211_select_queue(local->mdev, skb); | |
283 | skb_set_queue_mapping(skb, new_queue); | |
284 | ||
285 | txq = netdev_get_tx_queue(local->mdev, new_queue); | |
286 | ||
51cb6db0 DM |
287 | |
288 | qdisc = rcu_dereference(txq->qdisc); | |
83874000 | 289 | root_lock = qdisc_root_lock(qdisc); |
51cb6db0 | 290 | |
83874000 DM |
291 | spin_lock(root_lock); |
292 | qdisc->enqueue(skb, qdisc); | |
293 | spin_unlock(root_lock); | |
9e723492 | 294 | } |
51cb6db0 DM |
295 | |
296 | out_unlock: | |
297 | rcu_read_unlock_bh(); | |
9e723492 | 298 | } |