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 /* maximum number of hardware queues we support. */ 22 #define TC_80211_MAX_QUEUES 8 23 24 struct ieee80211_sched_data 25 { 26 struct tcf_proto *filter_list; 27 struct Qdisc *queues[TC_80211_MAX_QUEUES]; 28 struct sk_buff_head requeued[TC_80211_MAX_QUEUES]; 29 }; 30 31 32 /* given a data frame determine the 802.1p/1d tag to use */ 33 static inline unsigned classify_1d(struct sk_buff *skb, struct Qdisc *qd) 34 { 35 struct iphdr *ip; 36 int dscp; 37 int offset; 38 39 struct ieee80211_sched_data *q = qdisc_priv(qd); 40 struct tcf_result res = { -1, 0 }; 41 42 /* if there is a user set filter list, call out to that */ 43 if (q->filter_list) { 44 tc_classify(skb, q->filter_list, &res); 45 if (res.class != -1) 46 return res.class; 47 } 48 49 /* skb->priority values from 256->263 are magic values to 50 * directly indicate a specific 802.1d priority. 51 * This is used to allow 802.1d priority to be passed directly in 52 * from VLAN tags, etc. */ 53 if (skb->priority >= 256 && skb->priority <= 263) 54 return skb->priority - 256; 55 56 /* check there is a valid IP header present */ 57 offset = ieee80211_get_hdrlen_from_skb(skb) + 8 /* LLC + proto */; 58 if (skb->protocol != __constant_htons(ETH_P_IP) || 59 skb->len < offset + sizeof(*ip)) 60 return 0; 61 62 ip = (struct iphdr *) (skb->data + offset); 63 64 dscp = ip->tos & 0xfc; 65 if (dscp & 0x1c) 66 return 0; 67 return dscp >> 5; 68 } 69 70 71 static inline int wme_downgrade_ac(struct sk_buff *skb) 72 { 73 switch (skb->priority) { 74 case 6: 75 case 7: 76 skb->priority = 5; /* VO -> VI */ 77 return 0; 78 case 4: 79 case 5: 80 skb->priority = 3; /* VI -> BE */ 81 return 0; 82 case 0: 83 case 3: 84 skb->priority = 2; /* BE -> BK */ 85 return 0; 86 default: 87 return -1; 88 } 89 } 90 91 92 /* positive return value indicates which queue to use 93 * negative return value indicates to drop the frame */ 94 static inline int classify80211(struct sk_buff *skb, struct Qdisc *qd) 95 { 96 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 97 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; 98 unsigned short fc = le16_to_cpu(hdr->frame_control); 99 int qos; 100 const int ieee802_1d_to_ac[8] = { 2, 3, 3, 2, 1, 1, 0, 0 }; 101 102 /* see if frame is data or non data frame */ 103 if (unlikely((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA)) { 104 /* management frames go on AC_VO queue, but are sent 105 * without QoS control fields */ 106 return IEEE80211_TX_QUEUE_DATA0; 107 } 108 109 if (0 /* injected */) { 110 /* use AC from radiotap */ 111 } 112 113 /* is this a QoS frame? */ 114 qos = fc & IEEE80211_STYPE_QOS_DATA; 115 116 if (!qos) { 117 skb->priority = 0; /* required for correct WPA/11i MIC */ 118 return ieee802_1d_to_ac[skb->priority]; 119 } 120 121 /* use the data classifier to determine what 802.1d tag the 122 * data frame has */ 123 skb->priority = classify_1d(skb, qd); 124 125 /* in case we are a client verify acm is not set for this ac */ 126 while (unlikely(local->wmm_acm & BIT(skb->priority))) { 127 if (wme_downgrade_ac(skb)) { 128 /* No AC with lower priority has acm=0, drop packet. */ 129 return -1; 130 } 131 } 132 133 /* look up which queue to use for frames with this 1d tag */ 134 return ieee802_1d_to_ac[skb->priority]; 135 } 136 137 138 static int wme_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd) 139 { 140 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 141 struct ieee80211_sched_data *q = qdisc_priv(qd); 142 struct ieee80211_tx_packet_data *pkt_data = 143 (struct ieee80211_tx_packet_data *) skb->cb; 144 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; 145 unsigned short fc = le16_to_cpu(hdr->frame_control); 146 struct Qdisc *qdisc; 147 int err, queue; 148 149 if (pkt_data->flags & IEEE80211_TXPD_REQUEUE) { 150 skb_queue_tail(&q->requeued[pkt_data->queue], skb); 151 qd->q.qlen++; 152 return 0; 153 } 154 155 queue = classify80211(skb, qd); 156 157 /* now we know the 1d priority, fill in the QoS header if there is one 158 */ 159 if (WLAN_FC_IS_QOS_DATA(fc)) { 160 u8 *p = skb->data + ieee80211_get_hdrlen(fc) - 2; 161 u8 qos_hdr = skb->priority & QOS_CONTROL_TAG1D_MASK; 162 if (local->wifi_wme_noack_test) 163 qos_hdr |= QOS_CONTROL_ACK_POLICY_NOACK << 164 QOS_CONTROL_ACK_POLICY_SHIFT; 165 /* qos header is 2 bytes, second reserved */ 166 *p = qos_hdr; 167 p++; 168 *p = 0; 169 } 170 171 if (unlikely(queue >= local->hw.queues)) { 172 #if 0 173 if (net_ratelimit()) { 174 printk(KERN_DEBUG "%s - queue=%d (hw does not " 175 "support) -> %d\n", 176 __func__, queue, local->hw.queues - 1); 177 } 178 #endif 179 queue = local->hw.queues - 1; 180 } 181 182 if (unlikely(queue < 0)) { 183 kfree_skb(skb); 184 err = NET_XMIT_DROP; 185 } else { 186 pkt_data->queue = (unsigned int) queue; 187 qdisc = q->queues[queue]; 188 err = qdisc->enqueue(skb, qdisc); 189 if (err == NET_XMIT_SUCCESS) { 190 qd->q.qlen++; 191 qd->bstats.bytes += skb->len; 192 qd->bstats.packets++; 193 return NET_XMIT_SUCCESS; 194 } 195 } 196 qd->qstats.drops++; 197 return err; 198 } 199 200 201 /* TODO: clean up the cases where master_hard_start_xmit 202 * returns non 0 - it shouldn't ever do that. Once done we 203 * can remove this function */ 204 static int wme_qdiscop_requeue(struct sk_buff *skb, struct Qdisc* qd) 205 { 206 struct ieee80211_sched_data *q = qdisc_priv(qd); 207 struct ieee80211_tx_packet_data *pkt_data = 208 (struct ieee80211_tx_packet_data *) skb->cb; 209 struct Qdisc *qdisc; 210 int err; 211 212 /* we recorded which queue to use earlier! */ 213 qdisc = q->queues[pkt_data->queue]; 214 215 if ((err = qdisc->ops->requeue(skb, qdisc)) == 0) { 216 qd->q.qlen++; 217 return 0; 218 } 219 qd->qstats.drops++; 220 return err; 221 } 222 223 224 static struct sk_buff *wme_qdiscop_dequeue(struct Qdisc* qd) 225 { 226 struct ieee80211_sched_data *q = qdisc_priv(qd); 227 struct net_device *dev = qd->dev; 228 struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr); 229 struct ieee80211_hw *hw = &local->hw; 230 struct sk_buff *skb; 231 struct Qdisc *qdisc; 232 int queue; 233 234 /* check all the h/w queues in numeric/priority order */ 235 for (queue = 0; queue < hw->queues; queue++) { 236 /* see if there is room in this hardware queue */ 237 if (test_bit(IEEE80211_LINK_STATE_XOFF, 238 &local->state[queue]) || 239 test_bit(IEEE80211_LINK_STATE_PENDING, 240 &local->state[queue])) 241 continue; 242 243 /* there is space - try and get a frame */ 244 skb = skb_dequeue(&q->requeued[queue]); 245 if (skb) { 246 qd->q.qlen--; 247 return skb; 248 } 249 250 qdisc = q->queues[queue]; 251 skb = qdisc->dequeue(qdisc); 252 if (skb) { 253 qd->q.qlen--; 254 return skb; 255 } 256 } 257 /* returning a NULL here when all the h/w queues are full means we 258 * never need to call netif_stop_queue in the driver */ 259 return NULL; 260 } 261 262 263 static void wme_qdiscop_reset(struct Qdisc* qd) 264 { 265 struct ieee80211_sched_data *q = qdisc_priv(qd); 266 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 267 struct ieee80211_hw *hw = &local->hw; 268 int queue; 269 270 /* QUESTION: should we have some hardware flush functionality here? */ 271 272 for (queue = 0; queue < hw->queues; queue++) { 273 skb_queue_purge(&q->requeued[queue]); 274 qdisc_reset(q->queues[queue]); 275 } 276 qd->q.qlen = 0; 277 } 278 279 280 static void wme_qdiscop_destroy(struct Qdisc* qd) 281 { 282 struct ieee80211_sched_data *q = qdisc_priv(qd); 283 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 284 struct ieee80211_hw *hw = &local->hw; 285 int queue; 286 287 tcf_destroy_chain(q->filter_list); 288 q->filter_list = NULL; 289 290 for (queue=0; queue < hw->queues; queue++) { 291 skb_queue_purge(&q->requeued[queue]); 292 qdisc_destroy(q->queues[queue]); 293 q->queues[queue] = &noop_qdisc; 294 } 295 } 296 297 298 /* called whenever parameters are updated on existing qdisc */ 299 static int wme_qdiscop_tune(struct Qdisc *qd, struct rtattr *opt) 300 { 301 /* struct ieee80211_sched_data *q = qdisc_priv(qd); 302 */ 303 /* check our options block is the right size */ 304 /* copy any options to our local structure */ 305 /* Ignore options block for now - always use static mapping 306 struct tc_ieee80211_qopt *qopt = RTA_DATA(opt); 307 308 if (opt->rta_len < RTA_LENGTH(sizeof(*qopt))) 309 return -EINVAL; 310 memcpy(q->tag2queue, qopt->tag2queue, sizeof(qopt->tag2queue)); 311 */ 312 return 0; 313 } 314 315 316 /* called during initial creation of qdisc on device */ 317 static int wme_qdiscop_init(struct Qdisc *qd, struct rtattr *opt) 318 { 319 struct ieee80211_sched_data *q = qdisc_priv(qd); 320 struct net_device *dev = qd->dev; 321 struct ieee80211_local *local; 322 int queues; 323 int err = 0, i; 324 325 /* check that device is a mac80211 device */ 326 if (!dev->ieee80211_ptr || 327 dev->ieee80211_ptr->wiphy->privid != mac80211_wiphy_privid) 328 return -EINVAL; 329 330 /* check this device is an ieee80211 master type device */ 331 if (dev->type != ARPHRD_IEEE80211) 332 return -EINVAL; 333 334 /* check that there is no qdisc currently attached to device 335 * this ensures that we will be the root qdisc. (I can't find a better 336 * way to test this explicitly) */ 337 if (dev->qdisc_sleeping != &noop_qdisc) 338 return -EINVAL; 339 340 if (qd->flags & TCQ_F_INGRESS) 341 return -EINVAL; 342 343 local = wdev_priv(dev->ieee80211_ptr); 344 queues = local->hw.queues; 345 346 /* if options were passed in, set them */ 347 if (opt) { 348 err = wme_qdiscop_tune(qd, opt); 349 } 350 351 /* create child queues */ 352 for (i = 0; i < queues; i++) { 353 skb_queue_head_init(&q->requeued[i]); 354 q->queues[i] = qdisc_create_dflt(qd->dev, &pfifo_qdisc_ops, 355 qd->handle); 356 if (!q->queues[i]) { 357 q->queues[i] = &noop_qdisc; 358 printk(KERN_ERR "%s child qdisc %i creation failed", dev->name, i); 359 } 360 } 361 362 return err; 363 } 364 365 static int wme_qdiscop_dump(struct Qdisc *qd, struct sk_buff *skb) 366 { 367 /* struct ieee80211_sched_data *q = qdisc_priv(qd); 368 unsigned char *p = skb->tail; 369 struct tc_ieee80211_qopt opt; 370 371 memcpy(&opt.tag2queue, q->tag2queue, TC_80211_MAX_TAG + 1); 372 RTA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt); 373 */ return skb->len; 374 /* 375 rtattr_failure: 376 skb_trim(skb, p - skb->data);*/ 377 return -1; 378 } 379 380 381 static int wme_classop_graft(struct Qdisc *qd, unsigned long arg, 382 struct Qdisc *new, struct Qdisc **old) 383 { 384 struct ieee80211_sched_data *q = qdisc_priv(qd); 385 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 386 struct ieee80211_hw *hw = &local->hw; 387 unsigned long queue = arg - 1; 388 389 if (queue >= hw->queues) 390 return -EINVAL; 391 392 if (!new) 393 new = &noop_qdisc; 394 395 sch_tree_lock(qd); 396 *old = q->queues[queue]; 397 q->queues[queue] = new; 398 qdisc_reset(*old); 399 sch_tree_unlock(qd); 400 401 return 0; 402 } 403 404 405 static struct Qdisc * 406 wme_classop_leaf(struct Qdisc *qd, unsigned long arg) 407 { 408 struct ieee80211_sched_data *q = qdisc_priv(qd); 409 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 410 struct ieee80211_hw *hw = &local->hw; 411 unsigned long queue = arg - 1; 412 413 if (queue >= hw->queues) 414 return NULL; 415 416 return q->queues[queue]; 417 } 418 419 420 static unsigned long wme_classop_get(struct Qdisc *qd, u32 classid) 421 { 422 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 423 struct ieee80211_hw *hw = &local->hw; 424 unsigned long queue = TC_H_MIN(classid); 425 426 if (queue - 1 >= hw->queues) 427 return 0; 428 429 return queue; 430 } 431 432 433 static unsigned long wme_classop_bind(struct Qdisc *qd, unsigned long parent, 434 u32 classid) 435 { 436 return wme_classop_get(qd, classid); 437 } 438 439 440 static void wme_classop_put(struct Qdisc *q, unsigned long cl) 441 { 442 } 443 444 445 static int wme_classop_change(struct Qdisc *qd, u32 handle, u32 parent, 446 struct rtattr **tca, unsigned long *arg) 447 { 448 unsigned long cl = *arg; 449 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 450 struct ieee80211_hw *hw = &local->hw; 451 452 if (cl - 1 > hw->queues) 453 return -ENOENT; 454 455 /* TODO: put code to program hardware queue parameters here, 456 * to allow programming from tc command line */ 457 458 return 0; 459 } 460 461 462 /* we don't support deleting hardware queues 463 * when we add WMM-SA support - TSPECs may be deleted here */ 464 static int wme_classop_delete(struct Qdisc *qd, unsigned long cl) 465 { 466 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 467 struct ieee80211_hw *hw = &local->hw; 468 469 if (cl - 1 > hw->queues) 470 return -ENOENT; 471 return 0; 472 } 473 474 475 static int wme_classop_dump_class(struct Qdisc *qd, unsigned long cl, 476 struct sk_buff *skb, struct tcmsg *tcm) 477 { 478 struct ieee80211_sched_data *q = qdisc_priv(qd); 479 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 480 struct ieee80211_hw *hw = &local->hw; 481 482 if (cl - 1 > hw->queues) 483 return -ENOENT; 484 tcm->tcm_handle = TC_H_MIN(cl); 485 tcm->tcm_parent = qd->handle; 486 tcm->tcm_info = q->queues[cl-1]->handle; /* do we need this? */ 487 return 0; 488 } 489 490 491 static void wme_classop_walk(struct Qdisc *qd, struct qdisc_walker *arg) 492 { 493 struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr); 494 struct ieee80211_hw *hw = &local->hw; 495 int queue; 496 497 if (arg->stop) 498 return; 499 500 for (queue = 0; queue < hw->queues; queue++) { 501 if (arg->count < arg->skip) { 502 arg->count++; 503 continue; 504 } 505 /* we should return classids for our internal queues here 506 * as well as the external ones */ 507 if (arg->fn(qd, queue+1, arg) < 0) { 508 arg->stop = 1; 509 break; 510 } 511 arg->count++; 512 } 513 } 514 515 516 static struct tcf_proto ** wme_classop_find_tcf(struct Qdisc *qd, 517 unsigned long cl) 518 { 519 struct ieee80211_sched_data *q = qdisc_priv(qd); 520 521 if (cl) 522 return NULL; 523 524 return &q->filter_list; 525 } 526 527 528 /* this qdisc is classful (i.e. has classes, some of which may have leaf qdiscs attached) 529 * - these are the operations on the classes */ 530 static struct Qdisc_class_ops class_ops = 531 { 532 .graft = wme_classop_graft, 533 .leaf = wme_classop_leaf, 534 535 .get = wme_classop_get, 536 .put = wme_classop_put, 537 .change = wme_classop_change, 538 .delete = wme_classop_delete, 539 .walk = wme_classop_walk, 540 541 .tcf_chain = wme_classop_find_tcf, 542 .bind_tcf = wme_classop_bind, 543 .unbind_tcf = wme_classop_put, 544 545 .dump = wme_classop_dump_class, 546 }; 547 548 549 /* queueing discipline operations */ 550 static struct Qdisc_ops wme_qdisc_ops = 551 { 552 .next = NULL, 553 .cl_ops = &class_ops, 554 .id = "ieee80211", 555 .priv_size = sizeof(struct ieee80211_sched_data), 556 557 .enqueue = wme_qdiscop_enqueue, 558 .dequeue = wme_qdiscop_dequeue, 559 .requeue = wme_qdiscop_requeue, 560 .drop = NULL, /* drop not needed since we are always the root qdisc */ 561 562 .init = wme_qdiscop_init, 563 .reset = wme_qdiscop_reset, 564 .destroy = wme_qdiscop_destroy, 565 .change = wme_qdiscop_tune, 566 567 .dump = wme_qdiscop_dump, 568 }; 569 570 571 void ieee80211_install_qdisc(struct net_device *dev) 572 { 573 struct Qdisc *qdisc; 574 575 qdisc = qdisc_create_dflt(dev, &wme_qdisc_ops, TC_H_ROOT); 576 if (!qdisc) { 577 printk(KERN_ERR "%s: qdisc installation failed\n", dev->name); 578 return; 579 } 580 581 /* same handle as would be allocated by qdisc_alloc_handle() */ 582 qdisc->handle = 0x80010000; 583 584 qdisc_lock_tree(dev); 585 list_add_tail(&qdisc->list, &dev->qdisc_list); 586 dev->qdisc_sleeping = qdisc; 587 qdisc_unlock_tree(dev); 588 } 589 590 591 int ieee80211_qdisc_installed(struct net_device *dev) 592 { 593 return dev->qdisc_sleeping->ops == &wme_qdisc_ops; 594 } 595 596 597 int ieee80211_wme_register(void) 598 { 599 return register_qdisc(&wme_qdisc_ops); 600 } 601 602 603 void ieee80211_wme_unregister(void) 604 { 605 unregister_qdisc(&wme_qdisc_ops); 606 } 607