net/sched/sch_cbq.c

   1 /*
   2  * net/sched/sch_cbq.c  Class-Based Queueing discipline.
   3  *
   4  *              This program is free software; you can redistribute it and/or
   5  *              modify it under the terms of the GNU General Public License
   6  *              as published by the Free Software Foundation; either version
   7  *              2 of the License, or (at your option) any later version.
   8  *
   9  * Authors:     Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
  10  *
  11  */
  12
  13 #include <linux/module.h>
  14 #include <linux/slab.h>
  15 #include <linux/types.h>
  16 #include <linux/kernel.h>
  17 #include <linux/string.h>
  18 #include <linux/errno.h>
  19 #include <linux/skbuff.h>
  20 #include <net/netlink.h>
  21 #include <net/pkt_sched.h>
  22 #include <net/pkt_cls.h>
  23
  24
  25 /*      Class-Based Queueing (CBQ) algorithm.
  26         =======================================
  27
  28         Sources: [1] Sally Floyd and Van Jacobson, "Link-sharing and Resource
  29                  Management Models for Packet Networks",
  30                  IEEE/ACM Transactions on Networking, Vol.3, No.4, 1995
  31
  32                  [2] Sally Floyd, "Notes on CBQ and Guaranteed Service", 1995
  33
  34                  [3] Sally Floyd, "Notes on Class-Based Queueing: Setting
  35                  Parameters", 1996
  36
  37                  [4] Sally Floyd and Michael Speer, "Experimental Results
  38                  for Class-Based Queueing", 1998, not published.
  39
  40         -----------------------------------------------------------------------
  41
  42         Algorithm skeleton was taken from NS simulator cbq.cc.
  43         If someone wants to check this code against the LBL version,
  44         he should take into account that ONLY the skeleton was borrowed,
  45         the implementation is different. Particularly:
  46
  47         --- The WRR algorithm is different. Our version looks more
  48         reasonable (I hope) and works when quanta are allowed to be
  49         less than MTU, which is always the case when real time classes
  50         have small rates. Note, that the statement of [3] is
  51         incomplete, delay may actually be estimated even if class
  52         per-round allotment is less than MTU. Namely, if per-round
  53         allotment is W*r_i, and r_1+...+r_k = r < 1
  54
  55         delay_i <= ([MTU/(W*r_i)]*W*r + W*r + k*MTU)/B
  56
  57         In the worst case we have IntServ estimate with D = W*r+k*MTU
  58         and C = MTU*r. The proof (if correct at all) is trivial.
  59
  60
  61         --- It seems that cbq-2.0 is not very accurate. At least, I cannot
  62         interpret some places, which look like wrong translations
  63         from NS. Anyone is advised to find these differences
  64         and explain to me, why I am wrong 8).
  65
  66         --- Linux has no EOI event, so that we cannot estimate true class
  67         idle time. Workaround is to consider the next dequeue event
  68         as sign that previous packet is finished. This is wrong because of
  69         internal device queueing, but on a permanently loaded link it is true.
  70         Moreover, combined with clock integrator, this scheme looks
  71         very close to an ideal solution.  */
  72
  73 struct cbq_sched_data;
  74
  75
  76 struct cbq_class {
  77         struct Qdisc_class_common common;
  78         struct cbq_class        *next_alive;    /* next class with backlog in this priority band */
  79
  80 /* Parameters */
  81         unsigned char           priority;       /* class priority */
  82         unsigned char           priority2;      /* priority to be used after overlimit */
  83         unsigned char           ewma_log;       /* time constant for idle time calculation */
  84
  85         u32                     defmap;
  86
  87         /* Link-sharing scheduler parameters */
  88         long                    maxidle;        /* Class parameters: see below. */
  89         long                    offtime;
  90         long                    minidle;
  91         u32                     avpkt;
  92         struct qdisc_rate_table *R_tab;
  93
  94         /* General scheduler (WRR) parameters */
  95         long                    allot;
  96         long                    quantum;        /* Allotment per WRR round */
  97         long                    weight;         /* Relative allotment: see below */
  98
  99         struct Qdisc            *qdisc;         /* Ptr to CBQ discipline */
 100         struct cbq_class        *split;         /* Ptr to split node */
 101         struct cbq_class        *share;         /* Ptr to LS parent in the class tree */
 102         struct cbq_class        *tparent;       /* Ptr to tree parent in the class tree */
 103         struct cbq_class        *borrow;        /* NULL if class is bandwidth limited;
 104                                                    parent otherwise */
 105         struct cbq_class        *sibling;       /* Sibling chain */
 106         struct cbq_class        *children;      /* Pointer to children chain */
 107
 108         struct Qdisc            *q;             /* Elementary queueing discipline */
 109
 110
 111 /* Variables */
 112         unsigned char           cpriority;      /* Effective priority */
 113         unsigned char           delayed;
 114         unsigned char           level;          /* level of the class in hierarchy:
 115                                                    0 for leaf classes, and maximal
 116                                                    level of children + 1 for nodes.
 117                                                  */
 118
 119         psched_time_t           last;           /* Last end of service */
 120         psched_time_t           undertime;
 121         long                    avgidle;
 122         long                    deficit;        /* Saved deficit for WRR */
 123         psched_time_t           penalized;
 124         struct gnet_stats_basic_packed bstats;
 125         struct gnet_stats_queue qstats;
 126         struct net_rate_estimator __rcu *rate_est;
 127         struct tc_cbq_xstats    xstats;
 128
 129         struct tcf_proto __rcu  *filter_list;
 130         struct tcf_block        *block;
 131
 132         int                     filters;
 133
 134         struct cbq_class        *defaults[TC_PRIO_MAX + 1];
 135 };
 136
 137 struct cbq_sched_data {
 138         struct Qdisc_class_hash clhash;                 /* Hash table of all classes */
 139         int                     nclasses[TC_CBQ_MAXPRIO + 1];
 140         unsigned int            quanta[TC_CBQ_MAXPRIO + 1];
 141
 142         struct cbq_class        link;
 143
 144         unsigned int            activemask;
 145         struct cbq_class        *active[TC_CBQ_MAXPRIO + 1];    /* List of all classes
 146                                                                    with backlog */
 147
 148 #ifdef CONFIG_NET_CLS_ACT
 149         struct cbq_class        *rx_class;
 150 #endif
 151         struct cbq_class        *tx_class;
 152         struct cbq_class        *tx_borrowed;
 153         int                     tx_len;
 154         psched_time_t           now;            /* Cached timestamp */
 155         unsigned int            pmask;
 156
 157         struct hrtimer          delay_timer;
 158         struct qdisc_watchdog   watchdog;       /* Watchdog timer,
 159                                                    started when CBQ has
 160                                                    backlog, but cannot
 161                                                    transmit just now */
 162         psched_tdiff_t          wd_expires;
 163         int                     toplevel;
 164         u32                     hgenerator;
 165 };
 166
 167
 168 #define L2T(cl, len)    qdisc_l2t((cl)->R_tab, len)
 169
 170 static inline struct cbq_class *
 171 cbq_class_lookup(struct cbq_sched_data *q, u32 classid)
 172 {
 173         struct Qdisc_class_common *clc;
 174
 175         clc = qdisc_class_find(&q->clhash, classid);
 176         if (clc == NULL)
 177                 return NULL;
 178         return container_of(clc, struct cbq_class, common);
 179 }
 180
 181 #ifdef CONFIG_NET_CLS_ACT
 182
 183 static struct cbq_class *
 184 cbq_reclassify(struct sk_buff *skb, struct cbq_class *this)
 185 {
 186         struct cbq_class *cl;
 187
 188         for (cl = this->tparent; cl; cl = cl->tparent) {
 189                 struct cbq_class *new = cl->defaults[TC_PRIO_BESTEFFORT];
 190
 191                 if (new != NULL && new != this)
 192                         return new;
 193         }
 194         return NULL;
 195 }
 196
 197 #endif
 198
 199 /* Classify packet. The procedure is pretty complicated, but
 200  * it allows us to combine link sharing and priority scheduling
 201  * transparently.
 202  *
 203  * Namely, you can put link sharing rules (f.e. route based) at root of CBQ,
 204  * so that it resolves to split nodes. Then packets are classified
 205  * by logical priority, or a more specific classifier may be attached
 206  * to the split node.
 207  */
 208
 209 static struct cbq_class *
 210 cbq_classify(struct sk_buff *skb, struct Qdisc *sch, int *qerr)
 211 {
 212         struct cbq_sched_data *q = qdisc_priv(sch);
 213         struct cbq_class *head = &q->link;
 214         struct cbq_class **defmap;
 215         struct cbq_class *cl = NULL;
 216         u32 prio = skb->priority;
 217         struct tcf_proto *fl;
 218         struct tcf_result res;
 219
 220         /*
 221          *  Step 1. If skb->priority points to one of our classes, use it.
 222          */
 223         if (TC_H_MAJ(prio ^ sch->handle) == 0 &&
 224             (cl = cbq_class_lookup(q, prio)) != NULL)
 225                 return cl;
 226
 227         *qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
 228         for (;;) {
 229                 int result = 0;
 230                 defmap = head->defaults;
 231
 232                 fl = rcu_dereference_bh(head->filter_list);
 233                 /*
 234                  * Step 2+n. Apply classifier.
 235                  */
 236                 result = tcf_classify(skb, fl, &res, true);
 237                 if (!fl || result < 0)
 238                         goto fallback;
 239                 if (result == TC_ACT_SHOT)
 240                         return NULL;
 241
 242                 cl = (void *)res.class;
 243                 if (!cl) {
 244                         if (TC_H_MAJ(res.classid))
 245                                 cl = cbq_class_lookup(q, res.classid);
 246                         else if ((cl = defmap[res.classid & TC_PRIO_MAX]) == NULL)
 247                                 cl = defmap[TC_PRIO_BESTEFFORT];
 248
 249                         if (cl == NULL)
 250                                 goto fallback;
 251                 }
 252                 if (cl->level >= head->level)
 253                         goto fallback;
 254 #ifdef CONFIG_NET_CLS_ACT
 255                 switch (result) {
 256                 case TC_ACT_QUEUED:
 257                 case TC_ACT_STOLEN:
 258                 case TC_ACT_TRAP:
 259                         *qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
 260                         /* fall through */
 261                 case TC_ACT_RECLASSIFY:
 262                         return cbq_reclassify(skb, cl);
 263                 }
 264 #endif
 265                 if (cl->level == 0)
 266                         return cl;
 267
 268                 /*
 269                  * Step 3+n. If classifier selected a link sharing class,
 270                  *         apply agency specific classifier.
 271                  *         Repeat this procdure until we hit a leaf node.
 272                  */
 273                 head = cl;
 274         }
 275
 276 fallback:
 277         cl = head;
 278
 279         /*
 280          * Step 4. No success...
 281          */
 282         if (TC_H_MAJ(prio) == 0 &&
 283             !(cl = head->defaults[prio & TC_PRIO_MAX]) &&
 284             !(cl = head->defaults[TC_PRIO_BESTEFFORT]))
 285                 return head;
 286
 287         return cl;
 288 }
 289
 290 /*
 291  * A packet has just been enqueued on the empty class.
 292  * cbq_activate_class adds it to the tail of active class list
 293  * of its priority band.
 294  */
 295
 296 static inline void cbq_activate_class(struct cbq_class *cl)
 297 {
 298         struct cbq_sched_data *q = qdisc_priv(cl->qdisc);
 299         int prio = cl->cpriority;
 300         struct cbq_class *cl_tail;
 301
 302         cl_tail = q->active[prio];
 303         q->active[prio] = cl;
 304
 305         if (cl_tail != NULL) {
 306                 cl->next_alive = cl_tail->next_alive;
 307                 cl_tail->next_alive = cl;
 308         } else {
 309                 cl->next_alive = cl;
 310                 q->activemask |= (1<<prio);
 311         }
 312 }
 313
 314 /*
 315  * Unlink class from active chain.
 316  * Note that this same procedure is done directly in cbq_dequeue*
 317  * during round-robin procedure.
 318  */
 319
 320 static void cbq_deactivate_class(struct cbq_class *this)
 321 {
 322         struct cbq_sched_data *q = qdisc_priv(this->qdisc);
 323         int prio = this->cpriority;
 324         struct cbq_class *cl;
 325         struct cbq_class *cl_prev = q->active[prio];
 326
 327         do {
 328                 cl = cl_prev->next_alive;
 329                 if (cl == this) {
 330                         cl_prev->next_alive = cl->next_alive;
 331                         cl->next_alive = NULL;
 332
 333                         if (cl == q->active[prio]) {
 334                                 q->active[prio] = cl_prev;
 335                                 if (cl == q->active[prio]) {
 336                                         q->active[prio] = NULL;
 337                                         q->activemask &= ~(1<<prio);
 338                                         return;
 339                                 }
 340                         }
 341                         return;
 342                 }
 343         } while ((cl_prev = cl) != q->active[prio]);
 344 }
 345
 346 static void
 347 cbq_mark_toplevel(struct cbq_sched_data *q, struct cbq_class *cl)
 348 {
 349         int toplevel = q->toplevel;
 350
 351         if (toplevel > cl->level) {
 352                 psched_time_t now = psched_get_time();
 353
 354                 do {
 355                         if (cl->undertime < now) {
 356                                 q->toplevel = cl->level;
 357                                 return;
 358                         }
 359                 } while ((cl = cl->borrow) != NULL && toplevel > cl->level);
 360         }
 361 }
 362
 363 static int
 364 cbq_enqueue(struct sk_buff *skb, struct Qdisc *sch,
 365             struct sk_buff **to_free)
 366 {
 367         struct cbq_sched_data *q = qdisc_priv(sch);
 368         int uninitialized_var(ret);
 369         struct cbq_class *cl = cbq_classify(skb, sch, &ret);
 370
 371 #ifdef CONFIG_NET_CLS_ACT
 372         q->rx_class = cl;
 373 #endif
 374         if (cl == NULL) {
 375                 if (ret & __NET_XMIT_BYPASS)
 376                         qdisc_qstats_drop(sch);
 377                 __qdisc_drop(skb, to_free);
 378                 return ret;
 379         }
 380
 381         ret = qdisc_enqueue(skb, cl->q, to_free);
 382         if (ret == NET_XMIT_SUCCESS) {
 383                 sch->q.qlen++;
 384                 cbq_mark_toplevel(q, cl);
 385                 if (!cl->next_alive)
 386                         cbq_activate_class(cl);
 387                 return ret;
 388         }
 389
 390         if (net_xmit_drop_count(ret)) {
 391                 qdisc_qstats_drop(sch);
 392                 cbq_mark_toplevel(q, cl);
 393                 cl->qstats.drops++;
 394         }
 395         return ret;
 396 }
 397
 398 /* Overlimit action: penalize leaf class by adding offtime */
 399 static void cbq_overlimit(struct cbq_class *cl)
 400 {
 401         struct cbq_sched_data *q = qdisc_priv(cl->qdisc);
 402         psched_tdiff_t delay = cl->undertime - q->now;
 403
 404         if (!cl->delayed) {
 405                 delay += cl->offtime;
 406
 407                 /*
 408                  * Class goes to sleep, so that it will have no
 409                  * chance to work avgidle. Let's forgive it 8)
 410                  *
 411                  * BTW cbq-2.0 has a crap in this
 412                  * place, apparently they forgot to shift it by cl->ewma_log.
 413                  */
 414                 if (cl->avgidle < 0)
 415                         delay -= (-cl->avgidle) - ((-cl->avgidle) >> cl->ewma_log);
 416                 if (cl->avgidle < cl->minidle)
 417                         cl->avgidle = cl->minidle;
 418                 if (delay <= 0)
 419                         delay = 1;
 420                 cl->undertime = q->now + delay;
 421
 422                 cl->xstats.overactions++;
 423                 cl->delayed = 1;
 424         }
 425         if (q->wd_expires == 0 || q->wd_expires > delay)
 426                 q->wd_expires = delay;
 427
 428         /* Dirty work! We must schedule wakeups based on
 429          * real available rate, rather than leaf rate,
 430          * which may be tiny (even zero).
 431          */
 432         if (q->toplevel == TC_CBQ_MAXLEVEL) {
 433                 struct cbq_class *b;
 434                 psched_tdiff_t base_delay = q->wd_expires;
 435
 436                 for (b = cl->borrow; b; b = b->borrow) {
 437                         delay = b->undertime - q->now;
 438                         if (delay < base_delay) {
 439                                 if (delay <= 0)
 440                                         delay = 1;
 441                                 base_delay = delay;
 442                         }
 443                 }
 444
 445                 q->wd_expires = base_delay;
 446         }
 447 }
 448
 449 static psched_tdiff_t cbq_undelay_prio(struct cbq_sched_data *q, int prio,
 450                                        psched_time_t now)
 451 {
 452         struct cbq_class *cl;
 453         struct cbq_class *cl_prev = q->active[prio];
 454         psched_time_t sched = now;
 455
 456         if (cl_prev == NULL)
 457                 return 0;
 458
 459         do {
 460                 cl = cl_prev->next_alive;
 461                 if (now - cl->penalized > 0) {
 462                         cl_prev->next_alive = cl->next_alive;
 463                         cl->next_alive = NULL;
 464                         cl->cpriority = cl->priority;
 465                         cl->delayed = 0;
 466                         cbq_activate_class(cl);
 467
 468                         if (cl == q->active[prio]) {
 469                                 q->active[prio] = cl_prev;
 470                                 if (cl == q->active[prio]) {
 471                                         q->active[prio] = NULL;
 472                                         return 0;
 473                                 }
 474                         }
 475
 476                         cl = cl_prev->next_alive;
 477                 } else if (sched - cl->penalized > 0)
 478                         sched = cl->penalized;
 479         } while ((cl_prev = cl) != q->active[prio]);
 480
 481         return sched - now;
 482 }
 483
 484 static enum hrtimer_restart cbq_undelay(struct hrtimer *timer)
 485 {
 486         struct cbq_sched_data *q = container_of(timer, struct cbq_sched_data,
 487                                                 delay_timer);
 488         struct Qdisc *sch = q->watchdog.qdisc;
 489         psched_time_t now;
 490         psched_tdiff_t delay = 0;
 491         unsigned int pmask;
 492
 493         now = psched_get_time();
 494
 495         pmask = q->pmask;
 496         q->pmask = 0;
 497
 498         while (pmask) {
 499                 int prio = ffz(~pmask);
 500                 psched_tdiff_t tmp;
 501
 502                 pmask &= ~(1<<prio);
 503
 504                 tmp = cbq_undelay_prio(q, prio, now);
 505                 if (tmp > 0) {
 506                         q->pmask |= 1<<prio;
 507                         if (tmp < delay || delay == 0)
 508                                 delay = tmp;
 509                 }
 510         }
 511
 512         if (delay) {
 513                 ktime_t time;
 514
 515                 time = 0;
 516                 time = ktime_add_ns(time, PSCHED_TICKS2NS(now + delay));
 517                 hrtimer_start(&q->delay_timer, time, HRTIMER_MODE_ABS_PINNED);
 518         }
 519
 520         __netif_schedule(qdisc_root(sch));
 521         return HRTIMER_NORESTART;
 522 }
 523
 524 /*
 525  * It is mission critical procedure.
 526  *
 527  * We "regenerate" toplevel cutoff, if transmitting class
 528  * has backlog and it is not regulated. It is not part of
 529  * original CBQ description, but looks more reasonable.
 530  * Probably, it is wrong. This question needs further investigation.
 531  */
 532
 533 static inline void
 534 cbq_update_toplevel(struct cbq_sched_data *q, struct cbq_class *cl,
 535                     struct cbq_class *borrowed)
 536 {
 537         if (cl && q->toplevel >= borrowed->level) {
 538                 if (cl->q->q.qlen > 1) {
 539                         do {
 540                                 if (borrowed->undertime == PSCHED_PASTPERFECT) {
 541                                         q->toplevel = borrowed->level;
 542                                         return;
 543                                 }
 544                         } while ((borrowed = borrowed->borrow) != NULL);
 545                 }
 546 #if 0
 547         /* It is not necessary now. Uncommenting it
 548            will save CPU cycles, but decrease fairness.
 549          */
 550                 q->toplevel = TC_CBQ_MAXLEVEL;
 551 #endif
 552         }
 553 }
 554
 555 static void
 556 cbq_update(struct cbq_sched_data *q)
 557 {
 558         struct cbq_class *this = q->tx_class;
 559         struct cbq_class *cl = this;
 560         int len = q->tx_len;
 561         psched_time_t now;
 562
 563         q->tx_class = NULL;
 564         /* Time integrator. We calculate EOS time
 565          * by adding expected packet transmission time.
 566          */
 567         now = q->now + L2T(&q->link, len);
 568
 569         for ( ; cl; cl = cl->share) {
 570                 long avgidle = cl->avgidle;
 571                 long idle;
 572
 573                 cl->bstats.packets++;
 574                 cl->bstats.bytes += len;
 575
 576                 /*
 577                  * (now - last) is total time between packet right edges.
 578                  * (last_pktlen/rate) is "virtual" busy time, so that
 579                  *
 580                  *      idle = (now - last) - last_pktlen/rate
 581                  */
 582
 583                 idle = now - cl->last;
 584                 if ((unsigned long)idle > 128*1024*1024) {
 585                         avgidle = cl->maxidle;
 586                 } else {
 587                         idle -= L2T(cl, len);
 588
 589                 /* true_avgidle := (1-W)*true_avgidle + W*idle,
 590                  * where W=2^{-ewma_log}. But cl->avgidle is scaled:
 591                  * cl->avgidle == true_avgidle/W,
 592                  * hence:
 593                  */
 594                         avgidle += idle - (avgidle>>cl->ewma_log);
 595                 }
 596
 597                 if (avgidle <= 0) {
 598                         /* Overlimit or at-limit */
 599
 600                         if (avgidle < cl->minidle)
 601                                 avgidle = cl->minidle;
 602
 603                         cl->avgidle = avgidle;
 604
 605                         /* Calculate expected time, when this class
 606                          * will be allowed to send.
 607                          * It will occur, when:
 608                          * (1-W)*true_avgidle + W*delay = 0, i.e.
 609                          * idle = (1/W - 1)*(-true_avgidle)
 610                          * or
 611                          * idle = (1 - W)*(-cl->avgidle);
 612                          */
 613                         idle = (-avgidle) - ((-avgidle) >> cl->ewma_log);
 614
 615                         /*
 616                          * That is not all.
 617                          * To maintain the rate allocated to the class,
 618                          * we add to undertime virtual clock,
 619                          * necessary to complete transmitted packet.
 620                          * (len/phys_bandwidth has been already passed
 621                          * to the moment of cbq_update)
 622                          */
 623
 624                         idle -= L2T(&q->link, len);
 625                         idle += L2T(cl, len);
 626
 627                         cl->undertime = now + idle;
 628                 } else {
 629                         /* Underlimit */
 630
 631                         cl->undertime = PSCHED_PASTPERFECT;
 632                         if (avgidle > cl->maxidle)
 633                                 cl->avgidle = cl->maxidle;
 634                         else
 635                                 cl->avgidle = avgidle;
 636                 }
 637                 if ((s64)(now - cl->last) > 0)
 638                         cl->last = now;
 639         }
 640
 641         cbq_update_toplevel(q, this, q->tx_borrowed);
 642 }
 643
 644 static inline struct cbq_class *
 645 cbq_under_limit(struct cbq_class *cl)
 646 {
 647         struct cbq_sched_data *q = qdisc_priv(cl->qdisc);
 648         struct cbq_class *this_cl = cl;
 649
 650         if (cl->tparent == NULL)
 651                 return cl;
 652
 653         if (cl->undertime == PSCHED_PASTPERFECT || q->now >= cl->undertime) {
 654                 cl->delayed = 0;
 655                 return cl;
 656         }
 657
 658         do {
 659                 /* It is very suspicious place. Now overlimit
 660                  * action is generated for not bounded classes
 661                  * only if link is completely congested.
 662                  * Though it is in agree with ancestor-only paradigm,
 663                  * it looks very stupid. Particularly,
 664                  * it means that this chunk of code will either
 665                  * never be called or result in strong amplification
 666                  * of burstiness. Dangerous, silly, and, however,
 667                  * no another solution exists.
 668                  */
 669                 cl = cl->borrow;
 670                 if (!cl) {
 671                         this_cl->qstats.overlimits++;
 672                         cbq_overlimit(this_cl);
 673                         return NULL;
 674                 }
 675                 if (cl->level > q->toplevel)
 676                         return NULL;
 677         } while (cl->undertime != PSCHED_PASTPERFECT && q->now < cl->undertime);
 678
 679         cl->delayed = 0;
 680         return cl;
 681 }
 682
 683 static inline struct sk_buff *
 684 cbq_dequeue_prio(struct Qdisc *sch, int prio)
 685 {
 686         struct cbq_sched_data *q = qdisc_priv(sch);
 687         struct cbq_class *cl_tail, *cl_prev, *cl;
 688         struct sk_buff *skb;
 689         int deficit;
 690
 691         cl_tail = cl_prev = q->active[prio];
 692         cl = cl_prev->next_alive;
 693
 694         do {
 695                 deficit = 0;
 696
 697                 /* Start round */
 698                 do {
 699                         struct cbq_class *borrow = cl;
 700
 701                         if (cl->q->q.qlen &&
 702                             (borrow = cbq_under_limit(cl)) == NULL)
 703                                 goto skip_class;
 704
 705                         if (cl->deficit <= 0) {
 706                                 /* Class exhausted its allotment per
 707                                  * this round. Switch to the next one.
 708                                  */
 709                                 deficit = 1;
 710                                 cl->deficit += cl->quantum;
 711                                 goto next_class;
 712                         }
 713
 714                         skb = cl->q->dequeue(cl->q);
 715
 716                         /* Class did not give us any skb :-(
 717                          * It could occur even if cl->q->q.qlen != 0
 718                          * f.e. if cl->q == "tbf"
 719                          */
 720                         if (skb == NULL)
 721                                 goto skip_class;
 722
 723                         cl->deficit -= qdisc_pkt_len(skb);
 724                         q->tx_class = cl;
 725                         q->tx_borrowed = borrow;
 726                         if (borrow != cl) {
 727 #ifndef CBQ_XSTATS_BORROWS_BYTES
 728                                 borrow->xstats.borrows++;
 729                                 cl->xstats.borrows++;
 730 #else
 731                                 borrow->xstats.borrows += qdisc_pkt_len(skb);
 732                                 cl->xstats.borrows += qdisc_pkt_len(skb);
 733 #endif
 734                         }
 735                         q->tx_len = qdisc_pkt_len(skb);
 736
 737                         if (cl->deficit <= 0) {
 738                                 q->active[prio] = cl;
 739                                 cl = cl->next_alive;
 740                                 cl->deficit += cl->quantum;
 741                         }
 742                         return skb;
 743
 744 skip_class:
 745                         if (cl->q->q.qlen == 0 || prio != cl->cpriority) {
 746                                 /* Class is empty or penalized.
 747                                  * Unlink it from active chain.
 748                                  */
 749                                 cl_prev->next_alive = cl->next_alive;
 750                                 cl->next_alive = NULL;
 751
 752                                 /* Did cl_tail point to it? */
 753                                 if (cl == cl_tail) {
 754                                         /* Repair it! */
 755                                         cl_tail = cl_prev;
 756
 757                                         /* Was it the last class in this band? */
 758                                         if (cl == cl_tail) {
 759                                                 /* Kill the band! */
 760                                                 q->active[prio] = NULL;
 761                                                 q->activemask &= ~(1<<prio);
 762                                                 if (cl->q->q.qlen)
 763                                                         cbq_activate_class(cl);
 764                                                 return NULL;
 765                                         }
 766
 767                                         q->active[prio] = cl_tail;
 768                                 }
 769                                 if (cl->q->q.qlen)
 770                                         cbq_activate_class(cl);
 771
 772                                 cl = cl_prev;
 773                         }
 774
 775 next_class:
 776                         cl_prev = cl;
 777                         cl = cl->next_alive;
 778                 } while (cl_prev != cl_tail);
 779         } while (deficit);
 780
 781         q->active[prio] = cl_prev;
 782
 783         return NULL;
 784 }
 785
 786 static inline struct sk_buff *
 787 cbq_dequeue_1(struct Qdisc *sch)
 788 {
 789         struct cbq_sched_data *q = qdisc_priv(sch);
 790         struct sk_buff *skb;
 791         unsigned int activemask;
 792
 793         activemask = q->activemask & 0xFF;
 794         while (activemask) {
 795                 int prio = ffz(~activemask);
 796                 activemask &= ~(1<<prio);
 797                 skb = cbq_dequeue_prio(sch, prio);
 798                 if (skb)
 799                         return skb;
 800         }
 801         return NULL;
 802 }
 803
 804 static struct sk_buff *
 805 cbq_dequeue(struct Qdisc *sch)
 806 {
 807         struct sk_buff *skb;
 808         struct cbq_sched_data *q = qdisc_priv(sch);
 809         psched_time_t now;
 810
 811         now = psched_get_time();
 812
 813         if (q->tx_class)
 814                 cbq_update(q);
 815
 816         q->now = now;
 817
 818         for (;;) {
 819                 q->wd_expires = 0;
 820
 821                 skb = cbq_dequeue_1(sch);
 822                 if (skb) {
 823                         qdisc_bstats_update(sch, skb);
 824                         sch->q.qlen--;
 825                         return skb;
 826                 }
 827
 828                 /* All the classes are overlimit.
 829                  *
 830                  * It is possible, if:
 831                  *
 832                  * 1. Scheduler is empty.
 833                  * 2. Toplevel cutoff inhibited borrowing.
 834                  * 3. Root class is overlimit.
 835                  *
 836                  * Reset 2d and 3d conditions and retry.
 837                  *
 838                  * Note, that NS and cbq-2.0 are buggy, peeking
 839                  * an arbitrary class is appropriate for ancestor-only
 840                  * sharing, but not for toplevel algorithm.
 841                  *
 842                  * Our version is better, but slower, because it requires
 843                  * two passes, but it is unavoidable with top-level sharing.
 844                  */
 845
 846                 if (q->toplevel == TC_CBQ_MAXLEVEL &&
 847                     q->link.undertime == PSCHED_PASTPERFECT)
 848                         break;
 849
 850                 q->toplevel = TC_CBQ_MAXLEVEL;
 851                 q->link.undertime = PSCHED_PASTPERFECT;
 852         }
 853
 854         /* No packets in scheduler or nobody wants to give them to us :-(
 855          * Sigh... start watchdog timer in the last case.
 856          */
 857
 858         if (sch->q.qlen) {
 859                 qdisc_qstats_overlimit(sch);
 860                 if (q->wd_expires)
 861                         qdisc_watchdog_schedule(&q->watchdog,
 862                                                 now + q->wd_expires);
 863         }
 864         return NULL;
 865 }
 866
 867 /* CBQ class maintanance routines */
 868
 869 static void cbq_adjust_levels(struct cbq_class *this)
 870 {
 871         if (this == NULL)
 872                 return;
 873
 874         do {
 875                 int level = 0;
 876                 struct cbq_class *cl;
 877
 878                 cl = this->children;
 879                 if (cl) {
 880                         do {
 881                                 if (cl->level > level)
 882                                         level = cl->level;
 883                         } while ((cl = cl->sibling) != this->children);
 884                 }
 885                 this->level = level + 1;
 886         } while ((this = this->tparent) != NULL);
 887 }
 888
 889 static void cbq_normalize_quanta(struct cbq_sched_data *q, int prio)
 890 {
 891         struct cbq_class *cl;
 892         unsigned int h;
 893
 894         if (q->quanta[prio] == 0)
 895                 return;
 896
 897         for (h = 0; h < q->clhash.hashsize; h++) {
 898                 hlist_for_each_entry(cl, &q->clhash.hash[h], common.hnode) {
 899                         /* BUGGGG... Beware! This expression suffer of
 900                          * arithmetic overflows!
 901                          */
 902                         if (cl->priority == prio) {
 903                                 cl->quantum = (cl->weight*cl->allot*q->nclasses[prio])/
 904                                         q->quanta[prio];
 905                         }
 906                         if (cl->quantum <= 0 ||
 907                             cl->quantum > 32*qdisc_dev(cl->qdisc)->mtu) {
 908                                 pr_warn("CBQ: class %08x has bad quantum==%ld, repaired.\n",
 909                                         cl->common.classid, cl->quantum);
 910                                 cl->quantum = qdisc_dev(cl->qdisc)->mtu/2 + 1;
 911                         }
 912                 }
 913         }
 914 }
 915
 916 static void cbq_sync_defmap(struct cbq_class *cl)
 917 {
 918         struct cbq_sched_data *q = qdisc_priv(cl->qdisc);
 919         struct cbq_class *split = cl->split;
 920         unsigned int h;
 921         int i;
 922
 923         if (split == NULL)
 924                 return;
 925
 926         for (i = 0; i <= TC_PRIO_MAX; i++) {
 927                 if (split->defaults[i] == cl && !(cl->defmap & (1<<i)))
 928                         split->defaults[i] = NULL;
 929         }
 930
 931         for (i = 0; i <= TC_PRIO_MAX; i++) {
 932                 int level = split->level;
 933
 934                 if (split->defaults[i])
 935                         continue;
 936
 937                 for (h = 0; h < q->clhash.hashsize; h++) {
 938                         struct cbq_class *c;
 939
 940                         hlist_for_each_entry(c, &q->clhash.hash[h],
 941                                              common.hnode) {
 942                                 if (c->split == split && c->level < level &&
 943                                     c->defmap & (1<<i)) {
 944                                         split->defaults[i] = c;
 945                                         level = c->level;
 946                                 }
 947                         }
 948                 }
 949         }
 950 }
 951
 952 static void cbq_change_defmap(struct cbq_class *cl, u32 splitid, u32 def, u32 mask)
 953 {
 954         struct cbq_class *split = NULL;
 955
 956         if (splitid == 0) {
 957                 split = cl->split;
 958                 if (!split)
 959                         return;
 960                 splitid = split->common.classid;
 961         }
 962
 963         if (split == NULL || split->common.classid != splitid) {
 964                 for (split = cl->tparent; split; split = split->tparent)
 965                         if (split->common.classid == splitid)
 966                                 break;
 967         }
 968
 969         if (split == NULL)
 970                 return;
 971
 972         if (cl->split != split) {
 973                 cl->defmap = 0;
 974                 cbq_sync_defmap(cl);
 975                 cl->split = split;
 976                 cl->defmap = def & mask;
 977         } else
 978                 cl->defmap = (cl->defmap & ~mask) | (def & mask);
 979
 980         cbq_sync_defmap(cl);
 981 }
 982
 983 static void cbq_unlink_class(struct cbq_class *this)
 984 {
 985         struct cbq_class *cl, **clp;
 986         struct cbq_sched_data *q = qdisc_priv(this->qdisc);
 987
 988         qdisc_class_hash_remove(&q->clhash, &this->common);
 989
 990         if (this->tparent) {
 991                 clp = &this->sibling;
 992                 cl = *clp;
 993                 do {
 994                         if (cl == this) {
 995                                 *clp = cl->sibling;
 996                                 break;
 997                         }
 998                         clp = &cl->sibling;
 999                 } while ((cl = *clp) != this->sibling);
1000
1001                 if (this->tparent->children == this) {
1002                         this->tparent->children = this->sibling;
1003                         if (this->sibling == this)
1004                                 this->tparent->children = NULL;
1005                 }
1006         } else {
1007                 WARN_ON(this->sibling != this);
1008         }
1009 }
1010
1011 static void cbq_link_class(struct cbq_class *this)
1012 {
1013         struct cbq_sched_data *q = qdisc_priv(this->qdisc);
1014         struct cbq_class *parent = this->tparent;
1015
1016         this->sibling = this;
1017         qdisc_class_hash_insert(&q->clhash, &this->common);
1018
1019         if (parent == NULL)
1020                 return;
1021
1022         if (parent->children == NULL) {
1023                 parent->children = this;
1024         } else {
1025                 this->sibling = parent->children->sibling;
1026                 parent->children->sibling = this;
1027         }
1028 }
1029
1030 static void
1031 cbq_reset(struct Qdisc *sch)
1032 {
1033         struct cbq_sched_data *q = qdisc_priv(sch);
1034         struct cbq_class *cl;
1035         int prio;
1036         unsigned int h;
1037
1038         q->activemask = 0;
1039         q->pmask = 0;
1040         q->tx_class = NULL;
1041         q->tx_borrowed = NULL;
1042         qdisc_watchdog_cancel(&q->watchdog);
1043         hrtimer_cancel(&q->delay_timer);
1044         q->toplevel = TC_CBQ_MAXLEVEL;
1045         q->now = psched_get_time();
1046
1047         for (prio = 0; prio <= TC_CBQ_MAXPRIO; prio++)
1048                 q->active[prio] = NULL;
1049
1050         for (h = 0; h < q->clhash.hashsize; h++) {
1051                 hlist_for_each_entry(cl, &q->clhash.hash[h], common.hnode) {
1052                         qdisc_reset(cl->q);
1053
1054                         cl->next_alive = NULL;
1055                         cl->undertime = PSCHED_PASTPERFECT;
1056                         cl->avgidle = cl->maxidle;
1057                         cl->deficit = cl->quantum;
1058                         cl->cpriority = cl->priority;
1059                 }
1060         }
1061         sch->q.qlen = 0;
1062 }
1063
1064
1065 static int cbq_set_lss(struct cbq_class *cl, struct tc_cbq_lssopt *lss)
1066 {
1067         if (lss->change & TCF_CBQ_LSS_FLAGS) {
1068                 cl->share = (lss->flags & TCF_CBQ_LSS_ISOLATED) ? NULL : cl->tparent;
1069                 cl->borrow = (lss->flags & TCF_CBQ_LSS_BOUNDED) ? NULL : cl->tparent;
1070         }
1071         if (lss->change & TCF_CBQ_LSS_EWMA)
1072                 cl->ewma_log = lss->ewma_log;
1073         if (lss->change & TCF_CBQ_LSS_AVPKT)
1074                 cl->avpkt = lss->avpkt;
1075         if (lss->change & TCF_CBQ_LSS_MINIDLE)
1076                 cl->minidle = -(long)lss->minidle;
1077         if (lss->change & TCF_CBQ_LSS_MAXIDLE) {
1078                 cl->maxidle = lss->maxidle;
1079                 cl->avgidle = lss->maxidle;
1080         }
1081         if (lss->change & TCF_CBQ_LSS_OFFTIME)
1082                 cl->offtime = lss->offtime;
1083         return 0;
1084 }
1085
1086 static void cbq_rmprio(struct cbq_sched_data *q, struct cbq_class *cl)
1087 {
1088         q->nclasses[cl->priority]--;
1089         q->quanta[cl->priority] -= cl->weight;
1090         cbq_normalize_quanta(q, cl->priority);
1091 }
1092
1093 static void cbq_addprio(struct cbq_sched_data *q, struct cbq_class *cl)
1094 {
1095         q->nclasses[cl->priority]++;
1096         q->quanta[cl->priority] += cl->weight;
1097         cbq_normalize_quanta(q, cl->priority);
1098 }
1099
1100 static int cbq_set_wrr(struct cbq_class *cl, struct tc_cbq_wrropt *wrr)
1101 {
1102         struct cbq_sched_data *q = qdisc_priv(cl->qdisc);
1103
1104         if (wrr->allot)
1105                 cl->allot = wrr->allot;
1106         if (wrr->weight)
1107                 cl->weight = wrr->weight;
1108         if (wrr->priority) {
1109                 cl->priority = wrr->priority - 1;
1110                 cl->cpriority = cl->priority;
1111                 if (cl->priority >= cl->priority2)
1112                         cl->priority2 = TC_CBQ_MAXPRIO - 1;
1113         }
1114
1115         cbq_addprio(q, cl);
1116         return 0;
1117 }
1118
1119 static int cbq_set_fopt(struct cbq_class *cl, struct tc_cbq_fopt *fopt)
1120 {
1121         cbq_change_defmap(cl, fopt->split, fopt->defmap, fopt->defchange);
1122         return 0;
1123 }
1124
1125 static const struct nla_policy cbq_policy[TCA_CBQ_MAX + 1] = {
1126         [TCA_CBQ_LSSOPT]        = { .len = sizeof(struct tc_cbq_lssopt) },
1127         [TCA_CBQ_WRROPT]        = { .len = sizeof(struct tc_cbq_wrropt) },
1128         [TCA_CBQ_FOPT]          = { .len = sizeof(struct tc_cbq_fopt) },
1129         [TCA_CBQ_OVL_STRATEGY]  = { .len = sizeof(struct tc_cbq_ovl) },
1130         [TCA_CBQ_RATE]          = { .len = sizeof(struct tc_ratespec) },
1131         [TCA_CBQ_RTAB]          = { .type = NLA_BINARY, .len = TC_RTAB_SIZE },
1132         [TCA_CBQ_POLICE]        = { .len = sizeof(struct tc_cbq_police) },
1133 };
1134
1135 static int cbq_opt_parse(struct nlattr *tb[TCA_CBQ_MAX + 1],
1136                          struct nlattr *opt,
1137                          struct netlink_ext_ack *extack)
1138 {
1139         int err;
1140
1141         if (!opt) {
1142                 NL_SET_ERR_MSG(extack, "CBQ options are required for this operation");
1143                 return -EINVAL;
1144         }
1145
1146         err = nla_parse_nested(tb, TCA_CBQ_MAX, opt, cbq_policy, extack);
1147         if (err < 0)
1148                 return err;
1149
1150         if (tb[TCA_CBQ_WRROPT]) {
1151                 const struct tc_cbq_wrropt *wrr = nla_data(tb[TCA_CBQ_WRROPT]);
1152
1153                 if (wrr->priority > TC_CBQ_MAXPRIO) {
1154                         NL_SET_ERR_MSG(extack, "priority is bigger than TC_CBQ_MAXPRIO");
1155                         err = -EINVAL;
1156                 }
1157         }
1158         return err;
1159 }
1160
1161 static int cbq_init(struct Qdisc *sch, struct nlattr *opt,
1162                     struct netlink_ext_ack *extack)
1163 {
1164         struct cbq_sched_data *q = qdisc_priv(sch);
1165         struct nlattr *tb[TCA_CBQ_MAX + 1];
1166         struct tc_ratespec *r;
1167         int err;
1168
1169         qdisc_watchdog_init(&q->watchdog, sch);
1170         hrtimer_init(&q->delay_timer, CLOCK_MONOTONIC, HRTIMER_MODE_ABS_PINNED);
1171         q->delay_timer.function = cbq_undelay;
1172
1173         err = cbq_opt_parse(tb, opt, extack);
1174         if (err < 0)
1175                 return err;
1176
1177         if (!tb[TCA_CBQ_RTAB] || !tb[TCA_CBQ_RATE]) {
1178                 NL_SET_ERR_MSG(extack, "Rate specification missing or incomplete");
1179                 return -EINVAL;
1180         }
1181
1182         r = nla_data(tb[TCA_CBQ_RATE]);
1183
1184         q->link.R_tab = qdisc_get_rtab(r, tb[TCA_CBQ_RTAB], extack);
1185         if (!q->link.R_tab)
1186                 return -EINVAL;
1187
1188         err = tcf_block_get(&q->link.block, &q->link.filter_list, sch, extack);
1189         if (err)
1190                 goto put_rtab;
1191
1192         err = qdisc_class_hash_init(&q->clhash);
1193         if (err < 0)
1194                 goto put_block;
1195
1196         q->link.sibling = &q->link;
1197         q->link.common.classid = sch->handle;
1198         q->link.qdisc = sch;
1199         q->link.q = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
1200                                       sch->handle, NULL);
1201         if (!q->link.q)
1202                 q->link.q = &noop_qdisc;
1203         else
1204                 qdisc_hash_add(q->link.q, true);
1205
1206         q->link.priority = TC_CBQ_MAXPRIO - 1;
1207         q->link.priority2 = TC_CBQ_MAXPRIO - 1;
1208         q->link.cpriority = TC_CBQ_MAXPRIO - 1;
1209         q->link.allot = psched_mtu(qdisc_dev(sch));
1210         q->link.quantum = q->link.allot;
1211         q->link.weight = q->link.R_tab->rate.rate;
1212
1213         q->link.ewma_log = TC_CBQ_DEF_EWMA;
1214         q->link.avpkt = q->link.allot/2;
1215         q->link.minidle = -0x7FFFFFFF;
1216
1217         q->toplevel = TC_CBQ_MAXLEVEL;
1218         q->now = psched_get_time();
1219
1220         cbq_link_class(&q->link);
1221
1222         if (tb[TCA_CBQ_LSSOPT])
1223                 cbq_set_lss(&q->link, nla_data(tb[TCA_CBQ_LSSOPT]));
1224
1225         cbq_addprio(q, &q->link);
1226         return 0;
1227
1228 put_block:
1229         tcf_block_put(q->link.block);
1230
1231 put_rtab:
1232         qdisc_put_rtab(q->link.R_tab);
1233         return err;
1234 }
1235
1236 static int cbq_dump_rate(struct sk_buff *skb, struct cbq_class *cl)
1237 {
1238         unsigned char *b = skb_tail_pointer(skb);
1239
1240         if (nla_put(skb, TCA_CBQ_RATE, sizeof(cl->R_tab->rate), &cl->R_tab->rate))
1241                 goto nla_put_failure;
1242         return skb->len;
1243
1244 nla_put_failure:
1245         nlmsg_trim(skb, b);
1246         return -1;
1247 }
1248
1249 static int cbq_dump_lss(struct sk_buff *skb, struct cbq_class *cl)
1250 {
1251         unsigned char *b = skb_tail_pointer(skb);
1252         struct tc_cbq_lssopt opt;
1253
1254         opt.flags = 0;
1255         if (cl->borrow == NULL)
1256                 opt.flags |= TCF_CBQ_LSS_BOUNDED;
1257         if (cl->share == NULL)
1258                 opt.flags |= TCF_CBQ_LSS_ISOLATED;
1259         opt.ewma_log = cl->ewma_log;
1260         opt.level = cl->level;
1261         opt.avpkt = cl->avpkt;
1262         opt.maxidle = cl->maxidle;
1263         opt.minidle = (u32)(-cl->minidle);
1264         opt.offtime = cl->offtime;
1265         opt.change = ~0;
1266         if (nla_put(skb, TCA_CBQ_LSSOPT, sizeof(opt), &opt))
1267                 goto nla_put_failure;
1268         return skb->len;
1269
1270 nla_put_failure:
1271         nlmsg_trim(skb, b);
1272         return -1;
1273 }
1274
1275 static int cbq_dump_wrr(struct sk_buff *skb, struct cbq_class *cl)
1276 {
1277         unsigned char *b = skb_tail_pointer(skb);
1278         struct tc_cbq_wrropt opt;
1279
1280         memset(&opt, 0, sizeof(opt));
1281         opt.flags = 0;
1282         opt.allot = cl->allot;
1283         opt.priority = cl->priority + 1;
1284         opt.cpriority = cl->cpriority + 1;
1285         opt.weight = cl->weight;
1286         if (nla_put(skb, TCA_CBQ_WRROPT, sizeof(opt), &opt))
1287                 goto nla_put_failure;
1288         return skb->len;
1289
1290 nla_put_failure:
1291         nlmsg_trim(skb, b);
1292         return -1;
1293 }
1294
1295 static int cbq_dump_fopt(struct sk_buff *skb, struct cbq_class *cl)
1296 {
1297         unsigned char *b = skb_tail_pointer(skb);
1298         struct tc_cbq_fopt opt;
1299
1300         if (cl->split || cl->defmap) {
1301                 opt.split = cl->split ? cl->split->common.classid : 0;
1302                 opt.defmap = cl->defmap;
1303                 opt.defchange = ~0;
1304                 if (nla_put(skb, TCA_CBQ_FOPT, sizeof(opt), &opt))
1305                         goto nla_put_failure;
1306         }
1307         return skb->len;
1308
1309 nla_put_failure:
1310         nlmsg_trim(skb, b);
1311         return -1;
1312 }
1313
1314 static int cbq_dump_attr(struct sk_buff *skb, struct cbq_class *cl)
1315 {
1316         if (cbq_dump_lss(skb, cl) < 0 ||
1317             cbq_dump_rate(skb, cl) < 0 ||
1318             cbq_dump_wrr(skb, cl) < 0 ||
1319             cbq_dump_fopt(skb, cl) < 0)
1320                 return -1;
1321         return 0;
1322 }
1323
1324 static int cbq_dump(struct Qdisc *sch, struct sk_buff *skb)
1325 {
1326         struct cbq_sched_data *q = qdisc_priv(sch);
1327         struct nlattr *nest;
1328
1329         nest = nla_nest_start(skb, TCA_OPTIONS);
1330         if (nest == NULL)
1331                 goto nla_put_failure;
1332         if (cbq_dump_attr(skb, &q->link) < 0)
1333                 goto nla_put_failure;
1334         return nla_nest_end(skb, nest);
1335
1336 nla_put_failure:
1337         nla_nest_cancel(skb, nest);
1338         return -1;
1339 }
1340
1341 static int
1342 cbq_dump_stats(struct Qdisc *sch, struct gnet_dump *d)
1343 {
1344         struct cbq_sched_data *q = qdisc_priv(sch);
1345
1346         q->link.xstats.avgidle = q->link.avgidle;
1347         return gnet_stats_copy_app(d, &q->link.xstats, sizeof(q->link.xstats));
1348 }
1349
1350 static int
1351 cbq_dump_class(struct Qdisc *sch, unsigned long arg,
1352                struct sk_buff *skb, struct tcmsg *tcm)
1353 {
1354         struct cbq_class *cl = (struct cbq_class *)arg;
1355         struct nlattr *nest;
1356
1357         if (cl->tparent)
1358                 tcm->tcm_parent = cl->tparent->common.classid;
1359         else
1360                 tcm->tcm_parent = TC_H_ROOT;
1361         tcm->tcm_handle = cl->common.classid;
1362         tcm->tcm_info = cl->q->handle;
1363
1364         nest = nla_nest_start(skb, TCA_OPTIONS);
1365         if (nest == NULL)
1366                 goto nla_put_failure;
1367         if (cbq_dump_attr(skb, cl) < 0)
1368                 goto nla_put_failure;
1369         return nla_nest_end(skb, nest);
1370
1371 nla_put_failure:
1372         nla_nest_cancel(skb, nest);
1373         return -1;
1374 }
1375
1376 static int
1377 cbq_dump_class_stats(struct Qdisc *sch, unsigned long arg,
1378         struct gnet_dump *d)
1379 {
1380         struct cbq_sched_data *q = qdisc_priv(sch);
1381         struct cbq_class *cl = (struct cbq_class *)arg;
1382
1383         cl->xstats.avgidle = cl->avgidle;
1384         cl->xstats.undertime = 0;
1385
1386         if (cl->undertime != PSCHED_PASTPERFECT)
1387                 cl->xstats.undertime = cl->undertime - q->now;
1388
1389         if (gnet_stats_copy_basic(qdisc_root_sleeping_running(sch),
1390                                   d, NULL, &cl->bstats) < 0 ||
1391             gnet_stats_copy_rate_est(d, &cl->rate_est) < 0 ||
1392             gnet_stats_copy_queue(d, NULL, &cl->qstats, cl->q->q.qlen) < 0)
1393                 return -1;
1394
1395         return gnet_stats_copy_app(d, &cl->xstats, sizeof(cl->xstats));
1396 }
1397
1398 static int cbq_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
1399                      struct Qdisc **old, struct netlink_ext_ack *extack)
1400 {
1401         struct cbq_class *cl = (struct cbq_class *)arg;
1402
1403         if (new == NULL) {
1404                 new = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
1405                                         cl->common.classid, extack);
1406                 if (new == NULL)
1407                         return -ENOBUFS;
1408         }
1409
1410         *old = qdisc_replace(sch, new, &cl->q);
1411         return 0;
1412 }
1413
1414 static struct Qdisc *cbq_leaf(struct Qdisc *sch, unsigned long arg)
1415 {
1416         struct cbq_class *cl = (struct cbq_class *)arg;
1417
1418         return cl->q;
1419 }
1420
1421 static void cbq_qlen_notify(struct Qdisc *sch, unsigned long arg)
1422 {
1423         struct cbq_class *cl = (struct cbq_class *)arg;
1424
1425         cbq_deactivate_class(cl);
1426 }
1427
1428 static unsigned long cbq_find(struct Qdisc *sch, u32 classid)
1429 {
1430         struct cbq_sched_data *q = qdisc_priv(sch);
1431
1432         return (unsigned long)cbq_class_lookup(q, classid);
1433 }
1434
1435 static void cbq_destroy_class(struct Qdisc *sch, struct cbq_class *cl)
1436 {
1437         struct cbq_sched_data *q = qdisc_priv(sch);
1438
1439         WARN_ON(cl->filters);
1440
1441         tcf_block_put(cl->block);
1442         qdisc_put(cl->q);
1443         qdisc_put_rtab(cl->R_tab);
1444         gen_kill_estimator(&cl->rate_est);
1445         if (cl != &q->link)
1446                 kfree(cl);
1447 }
1448
1449 static void cbq_destroy(struct Qdisc *sch)
1450 {
1451         struct cbq_sched_data *q = qdisc_priv(sch);
1452         struct hlist_node *next;
1453         struct cbq_class *cl;
1454         unsigned int h;
1455
1456 #ifdef CONFIG_NET_CLS_ACT
1457         q->rx_class = NULL;
1458 #endif
1459         /*
1460          * Filters must be destroyed first because we don't destroy the
1461          * classes from root to leafs which means that filters can still
1462          * be bound to classes which have been destroyed already. --TGR '04
1463          */
1464         for (h = 0; h < q->clhash.hashsize; h++) {
1465                 hlist_for_each_entry(cl, &q->clhash.hash[h], common.hnode) {
1466                         tcf_block_put(cl->block);
1467                         cl->block = NULL;
1468                 }
1469         }
1470         for (h = 0; h < q->clhash.hashsize; h++) {
1471                 hlist_for_each_entry_safe(cl, next, &q->clhash.hash[h],
1472                                           common.hnode)
1473                         cbq_destroy_class(sch, cl);
1474         }
1475         qdisc_class_hash_destroy(&q->clhash);
1476 }
1477
1478 static int
1479 cbq_change_class(struct Qdisc *sch, u32 classid, u32 parentid, struct nlattr **tca,
1480                  unsigned long *arg, struct netlink_ext_ack *extack)
1481 {
1482         int err;
1483         struct cbq_sched_data *q = qdisc_priv(sch);
1484         struct cbq_class *cl = (struct cbq_class *)*arg;
1485         struct nlattr *opt = tca[TCA_OPTIONS];
1486         struct nlattr *tb[TCA_CBQ_MAX + 1];
1487         struct cbq_class *parent;
1488         struct qdisc_rate_table *rtab = NULL;
1489
1490         err = cbq_opt_parse(tb, opt, extack);
1491         if (err < 0)
1492                 return err;
1493
1494         if (tb[TCA_CBQ_OVL_STRATEGY] || tb[TCA_CBQ_POLICE]) {
1495                 NL_SET_ERR_MSG(extack, "Neither overlimit strategy nor policing attributes can be used for changing class params");
1496                 return -EOPNOTSUPP;
1497         }
1498
1499         if (cl) {
1500                 /* Check parent */
1501                 if (parentid) {
1502                         if (cl->tparent &&
1503                             cl->tparent->common.classid != parentid) {
1504                                 NL_SET_ERR_MSG(extack, "Invalid parent id");
1505                                 return -EINVAL;
1506                         }
1507                         if (!cl->tparent && parentid != TC_H_ROOT) {
1508                                 NL_SET_ERR_MSG(extack, "Parent must be root");
1509                                 return -EINVAL;
1510                         }
1511                 }
1512
1513                 if (tb[TCA_CBQ_RATE]) {
1514                         rtab = qdisc_get_rtab(nla_data(tb[TCA_CBQ_RATE]),
1515                                               tb[TCA_CBQ_RTAB], extack);
1516                         if (rtab == NULL)
1517                                 return -EINVAL;
1518                 }
1519
1520                 if (tca[TCA_RATE]) {
1521                         err = gen_replace_estimator(&cl->bstats, NULL,
1522                                                     &cl->rate_est,
1523                                                     NULL,
1524                                                     qdisc_root_sleeping_running(sch),
1525                                                     tca[TCA_RATE]);
1526                         if (err) {
1527                                 NL_SET_ERR_MSG(extack, "Failed to replace specified rate estimator");
1528                                 qdisc_put_rtab(rtab);
1529                                 return err;
1530                         }
1531                 }
1532
1533                 /* Change class parameters */
1534                 sch_tree_lock(sch);
1535
1536                 if (cl->next_alive != NULL)
1537                         cbq_deactivate_class(cl);
1538
1539                 if (rtab) {
1540                         qdisc_put_rtab(cl->R_tab);
1541                         cl->R_tab = rtab;
1542                 }
1543
1544                 if (tb[TCA_CBQ_LSSOPT])
1545                         cbq_set_lss(cl, nla_data(tb[TCA_CBQ_LSSOPT]));
1546
1547                 if (tb[TCA_CBQ_WRROPT]) {
1548                         cbq_rmprio(q, cl);
1549                         cbq_set_wrr(cl, nla_data(tb[TCA_CBQ_WRROPT]));
1550                 }
1551
1552                 if (tb[TCA_CBQ_FOPT])
1553                         cbq_set_fopt(cl, nla_data(tb[TCA_CBQ_FOPT]));
1554
1555                 if (cl->q->q.qlen)
1556                         cbq_activate_class(cl);
1557
1558                 sch_tree_unlock(sch);
1559
1560                 return 0;
1561         }
1562
1563         if (parentid == TC_H_ROOT)
1564                 return -EINVAL;
1565
1566         if (!tb[TCA_CBQ_WRROPT] || !tb[TCA_CBQ_RATE] || !tb[TCA_CBQ_LSSOPT]) {
1567                 NL_SET_ERR_MSG(extack, "One of the following attributes MUST be specified: WRR, rate or link sharing");
1568                 return -EINVAL;
1569         }
1570
1571         rtab = qdisc_get_rtab(nla_data(tb[TCA_CBQ_RATE]), tb[TCA_CBQ_RTAB],
1572                               extack);
1573         if (rtab == NULL)
1574                 return -EINVAL;
1575
1576         if (classid) {
1577                 err = -EINVAL;
1578                 if (TC_H_MAJ(classid ^ sch->handle) ||
1579                     cbq_class_lookup(q, classid)) {
1580                         NL_SET_ERR_MSG(extack, "Specified class not found");
1581                         goto failure;
1582                 }
1583         } else {
1584                 int i;
1585                 classid = TC_H_MAKE(sch->handle, 0x8000);
1586
1587                 for (i = 0; i < 0x8000; i++) {
1588                         if (++q->hgenerator >= 0x8000)
1589                                 q->hgenerator = 1;
1590                         if (cbq_class_lookup(q, classid|q->hgenerator) == NULL)
1591                                 break;
1592                 }
1593                 err = -ENOSR;
1594                 if (i >= 0x8000) {
1595                         NL_SET_ERR_MSG(extack, "Unable to generate classid");
1596                         goto failure;
1597                 }
1598                 classid = classid|q->hgenerator;
1599         }
1600
1601         parent = &q->link;
1602         if (parentid) {
1603                 parent = cbq_class_lookup(q, parentid);
1604                 err = -EINVAL;
1605                 if (!parent) {
1606                         NL_SET_ERR_MSG(extack, "Failed to find parentid");
1607                         goto failure;
1608                 }
1609         }
1610
1611         err = -ENOBUFS;
1612         cl = kzalloc(sizeof(*cl), GFP_KERNEL);
1613         if (cl == NULL)
1614                 goto failure;
1615
1616         err = tcf_block_get(&cl->block, &cl->filter_list, sch, extack);
1617         if (err) {
1618                 kfree(cl);
1619                 goto failure;
1620         }
1621
1622         if (tca[TCA_RATE]) {
1623                 err = gen_new_estimator(&cl->bstats, NULL, &cl->rate_est,
1624                                         NULL,
1625                                         qdisc_root_sleeping_running(sch),
1626                                         tca[TCA_RATE]);
1627                 if (err) {
1628                         NL_SET_ERR_MSG(extack, "Couldn't create new estimator");
1629                         tcf_block_put(cl->block);
1630                         kfree(cl);
1631                         goto failure;
1632                 }
1633         }
1634
1635         cl->R_tab = rtab;
1636         rtab = NULL;
1637         cl->q = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops, classid,
1638                                   NULL);
1639         if (!cl->q)
1640                 cl->q = &noop_qdisc;
1641         else
1642                 qdisc_hash_add(cl->q, true);
1643
1644         cl->common.classid = classid;
1645         cl->tparent = parent;
1646         cl->qdisc = sch;
1647         cl->allot = parent->allot;
1648         cl->quantum = cl->allot;
1649         cl->weight = cl->R_tab->rate.rate;
1650
1651         sch_tree_lock(sch);
1652         cbq_link_class(cl);
1653         cl->borrow = cl->tparent;
1654         if (cl->tparent != &q->link)
1655                 cl->share = cl->tparent;
1656         cbq_adjust_levels(parent);
1657         cl->minidle = -0x7FFFFFFF;
1658         cbq_set_lss(cl, nla_data(tb[TCA_CBQ_LSSOPT]));
1659         cbq_set_wrr(cl, nla_data(tb[TCA_CBQ_WRROPT]));
1660         if (cl->ewma_log == 0)
1661                 cl->ewma_log = q->link.ewma_log;
1662         if (cl->maxidle == 0)
1663                 cl->maxidle = q->link.maxidle;
1664         if (cl->avpkt == 0)
1665                 cl->avpkt = q->link.avpkt;
1666         if (tb[TCA_CBQ_FOPT])
1667                 cbq_set_fopt(cl, nla_data(tb[TCA_CBQ_FOPT]));
1668         sch_tree_unlock(sch);
1669
1670         qdisc_class_hash_grow(sch, &q->clhash);
1671
1672         *arg = (unsigned long)cl;
1673         return 0;
1674
1675 failure:
1676         qdisc_put_rtab(rtab);
1677         return err;
1678 }
1679
1680 static int cbq_delete(struct Qdisc *sch, unsigned long arg)
1681 {
1682         struct cbq_sched_data *q = qdisc_priv(sch);
1683         struct cbq_class *cl = (struct cbq_class *)arg;
1684         unsigned int qlen, backlog;
1685
1686         if (cl->filters || cl->children || cl == &q->link)
1687                 return -EBUSY;
1688
1689         sch_tree_lock(sch);
1690
1691         qlen = cl->q->q.qlen;
1692         backlog = cl->q->qstats.backlog;
1693         qdisc_reset(cl->q);
1694         qdisc_tree_reduce_backlog(cl->q, qlen, backlog);
1695
1696         if (cl->next_alive)
1697                 cbq_deactivate_class(cl);
1698
1699         if (q->tx_borrowed == cl)
1700                 q->tx_borrowed = q->tx_class;
1701         if (q->tx_class == cl) {
1702                 q->tx_class = NULL;
1703                 q->tx_borrowed = NULL;
1704         }
1705 #ifdef CONFIG_NET_CLS_ACT
1706         if (q->rx_class == cl)
1707                 q->rx_class = NULL;
1708 #endif
1709
1710         cbq_unlink_class(cl);
1711         cbq_adjust_levels(cl->tparent);
1712         cl->defmap = 0;
1713         cbq_sync_defmap(cl);
1714
1715         cbq_rmprio(q, cl);
1716         sch_tree_unlock(sch);
1717
1718         cbq_destroy_class(sch, cl);
1719         return 0;
1720 }
1721
1722 static struct tcf_block *cbq_tcf_block(struct Qdisc *sch, unsigned long arg,
1723                                        struct netlink_ext_ack *extack)
1724 {
1725         struct cbq_sched_data *q = qdisc_priv(sch);
1726         struct cbq_class *cl = (struct cbq_class *)arg;
1727
1728         if (cl == NULL)
1729                 cl = &q->link;
1730
1731         return cl->block;
1732 }
1733
1734 static unsigned long cbq_bind_filter(struct Qdisc *sch, unsigned long parent,
1735                                      u32 classid)
1736 {
1737         struct cbq_sched_data *q = qdisc_priv(sch);
1738         struct cbq_class *p = (struct cbq_class *)parent;
1739         struct cbq_class *cl = cbq_class_lookup(q, classid);
1740
1741         if (cl) {
1742                 if (p && p->level <= cl->level)
1743                         return 0;
1744                 cl->filters++;
1745                 return (unsigned long)cl;
1746         }
1747         return 0;
1748 }
1749
1750 static void cbq_unbind_filter(struct Qdisc *sch, unsigned long arg)
1751 {
1752         struct cbq_class *cl = (struct cbq_class *)arg;
1753
1754         cl->filters--;
1755 }
1756
1757 static void cbq_walk(struct Qdisc *sch, struct qdisc_walker *arg)
1758 {
1759         struct cbq_sched_data *q = qdisc_priv(sch);
1760         struct cbq_class *cl;
1761         unsigned int h;
1762
1763         if (arg->stop)
1764                 return;
1765
1766         for (h = 0; h < q->clhash.hashsize; h++) {
1767                 hlist_for_each_entry(cl, &q->clhash.hash[h], common.hnode) {
1768                         if (arg->count < arg->skip) {
1769                                 arg->count++;
1770                                 continue;
1771                         }
1772                         if (arg->fn(sch, (unsigned long)cl, arg) < 0) {
1773                                 arg->stop = 1;
1774                                 return;
1775                         }
1776                         arg->count++;
1777                 }
1778         }
1779 }
1780
1781 static const struct Qdisc_class_ops cbq_class_ops = {
1782         .graft          =       cbq_graft,
1783         .leaf           =       cbq_leaf,
1784         .qlen_notify    =       cbq_qlen_notify,
1785         .find           =       cbq_find,
1786         .change         =       cbq_change_class,
1787         .delete         =       cbq_delete,
1788         .walk           =       cbq_walk,
1789         .tcf_block      =       cbq_tcf_block,
1790         .bind_tcf       =       cbq_bind_filter,
1791         .unbind_tcf     =       cbq_unbind_filter,
1792         .dump           =       cbq_dump_class,
1793         .dump_stats     =       cbq_dump_class_stats,
1794 };
1795
1796 static struct Qdisc_ops cbq_qdisc_ops __read_mostly = {
1797         .next           =       NULL,
1798         .cl_ops         =       &cbq_class_ops,
1799         .id             =       "cbq",
1800         .priv_size      =       sizeof(struct cbq_sched_data),
1801         .enqueue        =       cbq_enqueue,
1802         .dequeue        =       cbq_dequeue,
1803         .peek           =       qdisc_peek_dequeued,
1804         .init           =       cbq_init,
1805         .reset          =       cbq_reset,
1806         .destroy        =       cbq_destroy,
1807         .change         =       NULL,
1808         .dump           =       cbq_dump,
1809         .dump_stats     =       cbq_dump_stats,
1810         .owner          =       THIS_MODULE,
1811 };
1812
1813 static int __init cbq_module_init(void)
1814 {
1815         return register_qdisc(&cbq_qdisc_ops);
1816 }
1817 static void __exit cbq_module_exit(void)
1818 {
1819         unregister_qdisc(&cbq_qdisc_ops);
1820 }
1821 module_init(cbq_module_init)
1822 module_exit(cbq_module_exit)
1823 MODULE_LICENSE("GPL");