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