net/mac80211/wme.c

   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 }