sys/netpfil/ipfw/dn_sched_qfq.c

   1 /*
   2  * Copyright (c) 2010 Fabio Checconi, Luigi Rizzo, Paolo Valente
   3  * All rights reserved
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  */
  26
  27 /*
  28  * $FreeBSD$
  29  */
  30
  31 #ifdef _KERNEL
  32 #include <sys/malloc.h>
  33 #include <sys/socket.h>
  34 #include <sys/socketvar.h>
  35 #include <sys/kernel.h>
  36 #include <sys/mbuf.h>
  37 #include <sys/module.h>
  38 #include <net/if.h>     /* IFNAMSIZ */
  39 #include <netinet/in.h>
  40 #include <netinet/ip_var.h>             /* ipfw_rule_ref */
  41 #include <netinet/ip_fw.h>      /* flow_id */
  42 #include <netinet/ip_dummynet.h>
  43 #include <netpfil/ipfw/dn_heap.h>
  44 #include <netpfil/ipfw/ip_dn_private.h>
  45 #include <netpfil/ipfw/dn_sched.h>
  46 #else
  47 #include <dn_test.h>
  48 #endif
  49
  50 #ifdef QFQ_DEBUG
  51 struct qfq_sched;
  52 static void dump_sched(struct qfq_sched *q, const char *msg);
  53 #define NO(x)   x
  54 #else
  55 #define NO(x)
  56 #endif
  57 #define DN_SCHED_QFQ    4 // XXX Where?
  58 typedef unsigned long   bitmap;
  59
  60 /*
  61  * bitmaps ops are critical. Some linux versions have __fls
  62  * and the bitmap ops. Some machines have ffs
  63  */
  64 #if defined(_WIN32) || (defined(__MIPSEL__) && defined(LINUX_24))
  65 int fls(unsigned int n)
  66 {
  67         int i = 0;
  68         for (i = 0; n > 0; n >>= 1, i++)
  69                 ;
  70         return i;
  71 }
  72 #endif
  73
  74 #if !defined(_KERNEL) || defined( __FreeBSD__ ) || defined(_WIN32) || (defined(__MIPSEL__) && defined(LINUX_24))
  75 static inline unsigned long __fls(unsigned long word)
  76 {
  77         return fls(word) - 1;
  78 }
  79 #endif
  80
  81 #if !defined(_KERNEL) || !defined(__linux__)
  82 #ifdef QFQ_DEBUG
  83 int test_bit(int ix, bitmap *p)
  84 {
  85         if (ix < 0 || ix > 31)
  86                 D("bad index %d", ix);
  87         return *p & (1<<ix);
  88 }
  89 void __set_bit(int ix, bitmap *p)
  90 {
  91         if (ix < 0 || ix > 31)
  92                 D("bad index %d", ix);
  93         *p |= (1<<ix);
  94 }
  95 void __clear_bit(int ix, bitmap *p)
  96 {
  97         if (ix < 0 || ix > 31)
  98                 D("bad index %d", ix);
  99         *p &= ~(1<<ix);
 100 }
 101 #else /* !QFQ_DEBUG */
 102 /* XXX do we have fast version, or leave it to the compiler ? */
 103 #define test_bit(ix, pData)     ((*pData) & (1<<(ix)))
 104 #define __set_bit(ix, pData)    (*pData) |= (1<<(ix))
 105 #define __clear_bit(ix, pData)  (*pData) &= ~(1<<(ix))
 106 #endif /* !QFQ_DEBUG */
 107 #endif /* !__linux__ */
 108
 109 #ifdef __MIPSEL__
 110 #define __clear_bit(ix, pData)  (*pData) &= ~(1<<(ix))
 111 #endif
 112
 113 /*-------------------------------------------*/
 114 /*
 115
 116 Virtual time computations.
 117
 118 S, F and V are all computed in fixed point arithmetic with
 119 FRAC_BITS decimal bits.
 120
 121    QFQ_MAX_INDEX is the maximum index allowed for a group. We need
 122         one bit per index.
 123    QFQ_MAX_WSHIFT is the maximum power of two supported as a weight.
 124    The layout of the bits is as below:
 125
 126                    [ MTU_SHIFT ][      FRAC_BITS    ]
 127                    [ MAX_INDEX    ][ MIN_SLOT_SHIFT ]
 128                                  ^.__grp->index = 0
 129                                  *.__grp->slot_shift
 130
 131    where MIN_SLOT_SHIFT is derived by difference from the others.
 132
 133 The max group index corresponds to Lmax/w_min, where
 134 Lmax=1<<MTU_SHIFT, w_min = 1 .
 135 From this, and knowing how many groups (MAX_INDEX) we want,
 136 we can derive the shift corresponding to each group.
 137
 138 Because we often need to compute
 139         F = S + len/w_i  and V = V + len/wsum
 140 instead of storing w_i store the value
 141         inv_w = (1<<FRAC_BITS)/w_i
 142 so we can do F = S + len * inv_w * wsum.
 143 We use W_TOT in the formulas so we can easily move between
 144 static and adaptive weight sum.
 145
 146 The per-scheduler-instance data contain all the data structures
 147 for the scheduler: bitmaps and bucket lists.
 148
 149  */
 150 /*
 151  * Maximum number of consecutive slots occupied by backlogged classes
 152  * inside a group. This is approx lmax/lmin + 5.
 153  * XXX check because it poses constraints on MAX_INDEX
 154  */
 155 #define QFQ_MAX_SLOTS   32
 156 /*
 157  * Shifts used for class<->group mapping. Class weights are
 158  * in the range [1, QFQ_MAX_WEIGHT], we to map each class i to the
 159  * group with the smallest index that can support the L_i / r_i
 160  * configured for the class.
 161  *
 162  * grp->index is the index of the group; and grp->slot_shift
 163  * is the shift for the corresponding (scaled) sigma_i.
 164  *
 165  * When computing the group index, we do (len<<FP_SHIFT)/weight,
 166  * then compute an FLS (which is like a log2()), and if the result
 167  * is below the MAX_INDEX region we use 0 (which is the same as
 168  * using a larger len).
 169  */
 170 #define QFQ_MAX_INDEX           19
 171 #define QFQ_MAX_WSHIFT          16      /* log2(max_weight) */
 172
 173 #define QFQ_MAX_WEIGHT          (1<<QFQ_MAX_WSHIFT)
 174 #define QFQ_MAX_WSUM            (2*QFQ_MAX_WEIGHT)
 175
 176 #define FRAC_BITS               30      /* fixed point arithmetic */
 177 #define ONE_FP                  (1UL << FRAC_BITS)
 178
 179 #define QFQ_MTU_SHIFT           11      /* log2(max_len) */
 180 #define QFQ_MIN_SLOT_SHIFT      (FRAC_BITS + QFQ_MTU_SHIFT - QFQ_MAX_INDEX)
 181
 182 /*
 183  * Possible group states, also indexes for the bitmaps array in
 184  * struct qfq_queue. We rely on ER, IR, EB, IB being numbered 0..3
 185  */
 186 enum qfq_state { ER, IR, EB, IB, QFQ_MAX_STATE };
 187
 188 struct qfq_group;
 189 /*
 190  * additional queue info. Some of this info should come from
 191  * the flowset, we copy them here for faster processing.
 192  * This is an overlay of the struct dn_queue
 193  */
 194 struct qfq_class {
 195         struct dn_queue _q;
 196         uint64_t S, F;          /* flow timestamps (exact) */
 197         struct qfq_class *next; /* Link for the slot list. */
 198
 199         /* group we belong to. In principle we would need the index,
 200          * which is log_2(lmax/weight), but we never reference it
 201          * directly, only the group.
 202          */
 203         struct qfq_group *grp;
 204
 205         /* these are copied from the flowset. */
 206         uint32_t        inv_w;  /* ONE_FP/weight */
 207         uint32_t        lmax;   /* Max packet size for this flow. */
 208 };
 209
 210 /* Group descriptor, see the paper for details.
 211  * Basically this contains the bucket lists
 212  */
 213 struct qfq_group {
 214         uint64_t S, F;                  /* group timestamps (approx). */
 215         unsigned int slot_shift;        /* Slot shift. */
 216         unsigned int index;             /* Group index. */
 217         unsigned int front;             /* Index of the front slot. */
 218         bitmap full_slots;              /* non-empty slots */
 219
 220         /* Array of lists of active classes. */
 221         struct qfq_class *slots[QFQ_MAX_SLOTS];
 222 };
 223
 224 /* scheduler instance descriptor. */
 225 struct qfq_sched {
 226         uint64_t        V;              /* Precise virtual time. */
 227         uint32_t        wsum;           /* weight sum */
 228         uint32_t        iwsum;          /* inverse weight sum */
 229         NO(uint32_t     i_wsum;         /* ONE_FP/w_sum */
 230         uint32_t        _queued;        /* debugging */
 231         uint32_t        loops;  /* debugging */)
 232         bitmap bitmaps[QFQ_MAX_STATE];  /* Group bitmaps. */
 233         struct qfq_group groups[QFQ_MAX_INDEX + 1]; /* The groups. */
 234 };
 235
 236 /*---- support functions ----------------------------*/
 237
 238 /* Generic comparison function, handling wraparound. */
 239 static inline int qfq_gt(uint64_t a, uint64_t b)
 240 {
 241         return (int64_t)(a - b) > 0;
 242 }
 243
 244 /* Round a precise timestamp to its slotted value. */
 245 static inline uint64_t qfq_round_down(uint64_t ts, unsigned int shift)
 246 {
 247         return ts & ~((1ULL << shift) - 1);
 248 }
 249
 250 /* return the pointer to the group with lowest index in the bitmap */
 251 static inline struct qfq_group *qfq_ffs(struct qfq_sched *q,
 252                                         unsigned long bitmap)
 253 {
 254         int index = ffs(bitmap) - 1; // zero-based
 255         return &q->groups[index];
 256 }
 257
 258 /*
 259  * Calculate a flow index, given its weight and maximum packet length.
 260  * index = log_2(maxlen/weight) but we need to apply the scaling.
 261  * This is used only once at flow creation.
 262  */
 263 static int qfq_calc_index(uint32_t inv_w, unsigned int maxlen)
 264 {
 265         uint64_t slot_size = (uint64_t)maxlen *inv_w;
 266         unsigned long size_map;
 267         int index = 0;
 268
 269         size_map = (unsigned long)(slot_size >> QFQ_MIN_SLOT_SHIFT);
 270         if (!size_map)
 271                 goto out;
 272
 273         index = __fls(size_map) + 1;    // basically a log_2()
 274         index -= !(slot_size - (1ULL << (index + QFQ_MIN_SLOT_SHIFT - 1)));
 275
 276         if (index < 0)
 277                 index = 0;
 278
 279 out:
 280         ND("W = %d, L = %d, I = %d\n", ONE_FP/inv_w, maxlen, index);
 281         return index;
 282 }
 283 /*---- end support functions ----*/
 284
 285 /*-------- API calls --------------------------------*/
 286 /*
 287  * Validate and copy parameters from flowset.
 288  */
 289 static int
 290 qfq_new_queue(struct dn_queue *_q)
 291 {
 292         struct qfq_sched *q = (struct qfq_sched *)(_q->_si + 1);
 293         struct qfq_class *cl = (struct qfq_class *)_q;
 294         int i;
 295         uint32_t w;     /* approximated weight */
 296
 297         /* import parameters from the flowset. They should be correct
 298          * already.
 299          */
 300         w = _q->fs->fs.par[0];
 301         cl->lmax = _q->fs->fs.par[1];
 302         if (!w || w > QFQ_MAX_WEIGHT) {
 303                 w = 1;
 304                 D("rounding weight to 1");
 305         }
 306         cl->inv_w = ONE_FP/w;
 307         w = ONE_FP/cl->inv_w;
 308         if (q->wsum + w > QFQ_MAX_WSUM)
 309                 return EINVAL;
 310
 311         i = qfq_calc_index(cl->inv_w, cl->lmax);
 312         cl->grp = &q->groups[i];
 313         q->wsum += w;
 314         q->iwsum = ONE_FP / q->wsum; /* XXX note theory */
 315         // XXX cl->S = q->V; ?
 316         return 0;
 317 }
 318
 319 /* remove an empty queue */
 320 static int
 321 qfq_free_queue(struct dn_queue *_q)
 322 {
 323         struct qfq_sched *q = (struct qfq_sched *)(_q->_si + 1);
 324         struct qfq_class *cl = (struct qfq_class *)_q;
 325         if (cl->inv_w) {
 326                 q->wsum -= ONE_FP/cl->inv_w;
 327                 if (q->wsum != 0)
 328                         q->iwsum = ONE_FP / q->wsum;
 329                 cl->inv_w = 0; /* reset weight to avoid run twice */
 330         }
 331         return 0;
 332 }
 333
 334 /* Calculate a mask to mimic what would be ffs_from(). */
 335 static inline unsigned long
 336 mask_from(unsigned long bitmap, int from)
 337 {
 338         return bitmap & ~((1UL << from) - 1);
 339 }
 340
 341 /*
 342  * The state computation relies on ER=0, IR=1, EB=2, IB=3
 343  * First compute eligibility comparing grp->S, q->V,
 344  * then check if someone is blocking us and possibly add EB
 345  */
 346 static inline unsigned int
 347 qfq_calc_state(struct qfq_sched *q, struct qfq_group *grp)
 348 {
 349         /* if S > V we are not eligible */
 350         unsigned int state = qfq_gt(grp->S, q->V);
 351         unsigned long mask = mask_from(q->bitmaps[ER], grp->index);
 352         struct qfq_group *next;
 353
 354         if (mask) {
 355                 next = qfq_ffs(q, mask);
 356                 if (qfq_gt(grp->F, next->F))
 357                         state |= EB;
 358         }
 359
 360         return state;
 361 }
 362
 363 /*
 364  * In principle
 365  *      q->bitmaps[dst] |= q->bitmaps[src] & mask;
 366  *      q->bitmaps[src] &= ~mask;
 367  * but we should make sure that src != dst
 368  */
 369 static inline void
 370 qfq_move_groups(struct qfq_sched *q, unsigned long mask, int src, int dst)
 371 {
 372         q->bitmaps[dst] |= q->bitmaps[src] & mask;
 373         q->bitmaps[src] &= ~mask;
 374 }
 375
 376 static inline void
 377 qfq_unblock_groups(struct qfq_sched *q, int index, uint64_t old_finish)
 378 {
 379         unsigned long mask = mask_from(q->bitmaps[ER], index + 1);
 380         struct qfq_group *next;
 381
 382         if (mask) {
 383                 next = qfq_ffs(q, mask);
 384                 if (!qfq_gt(next->F, old_finish))
 385                         return;
 386         }
 387
 388         mask = (1UL << index) - 1;
 389         qfq_move_groups(q, mask, EB, ER);
 390         qfq_move_groups(q, mask, IB, IR);
 391 }
 392
 393 /*
 394  * perhaps
 395  *
 396         old_V ^= q->V;
 397         old_V >>= QFQ_MIN_SLOT_SHIFT;
 398         if (old_V) {
 399                 ...
 400         }
 401  *
 402  */
 403 static inline void
 404 qfq_make_eligible(struct qfq_sched *q, uint64_t old_V)
 405 {
 406         unsigned long mask, vslot, old_vslot;
 407
 408         vslot = q->V >> QFQ_MIN_SLOT_SHIFT;
 409         old_vslot = old_V >> QFQ_MIN_SLOT_SHIFT;
 410
 411         if (vslot != old_vslot) {
 412                 /* should be 1ULL not 2ULL */
 413                 mask = (1ULL << (__fls(vslot ^ old_vslot))) - 1;
 414                 qfq_move_groups(q, mask, IR, ER);
 415                 qfq_move_groups(q, mask, IB, EB);
 416         }
 417 }
 418
 419 /*
 420  * XXX we should make sure that slot becomes less than 32.
 421  * This is guaranteed by the input values.
 422  * roundedS is always cl->S rounded on grp->slot_shift bits.
 423  */
 424 static inline void
 425 qfq_slot_insert(struct qfq_group *grp, struct qfq_class *cl, uint64_t roundedS)
 426 {
 427         uint64_t slot = (roundedS - grp->S) >> grp->slot_shift;
 428         unsigned int i = (grp->front + slot) % QFQ_MAX_SLOTS;
 429
 430         cl->next = grp->slots[i];
 431         grp->slots[i] = cl;
 432         __set_bit(slot, &grp->full_slots);
 433 }
 434
 435 /*
 436  * remove the entry from the slot
 437  */
 438 static inline void
 439 qfq_front_slot_remove(struct qfq_group *grp)
 440 {
 441         struct qfq_class **h = &grp->slots[grp->front];
 442
 443         *h = (*h)->next;
 444         if (!*h)
 445                 __clear_bit(0, &grp->full_slots);
 446 }
 447
 448 /*
 449  * Returns the first full queue in a group. As a side effect,
 450  * adjust the bucket list so the first non-empty bucket is at
 451  * position 0 in full_slots.
 452  */
 453 static inline struct qfq_class *
 454 qfq_slot_scan(struct qfq_group *grp)
 455 {
 456         int i;
 457
 458         ND("grp %d full %x", grp->index, grp->full_slots);
 459         if (!grp->full_slots)
 460                 return NULL;
 461
 462         i = ffs(grp->full_slots) - 1; // zero-based
 463         if (i > 0) {
 464                 grp->front = (grp->front + i) % QFQ_MAX_SLOTS;
 465                 grp->full_slots >>= i;
 466         }
 467
 468         return grp->slots[grp->front];
 469 }
 470
 471 /*
 472  * adjust the bucket list. When the start time of a group decreases,
 473  * we move the index down (modulo QFQ_MAX_SLOTS) so we don't need to
 474  * move the objects. The mask of occupied slots must be shifted
 475  * because we use ffs() to find the first non-empty slot.
 476  * This covers decreases in the group's start time, but what about
 477  * increases of the start time ?
 478  * Here too we should make sure that i is less than 32
 479  */
 480 static inline void
 481 qfq_slot_rotate(struct qfq_sched *q, struct qfq_group *grp, uint64_t roundedS)
 482 {
 483         unsigned int i = (grp->S - roundedS) >> grp->slot_shift;
 484
 485         grp->full_slots <<= i;
 486         grp->front = (grp->front - i) % QFQ_MAX_SLOTS;
 487 }
 488
 489
 490 static inline void
 491 qfq_update_eligible(struct qfq_sched *q, uint64_t old_V)
 492 {
 493         bitmap ineligible;
 494
 495         ineligible = q->bitmaps[IR] | q->bitmaps[IB];
 496         if (ineligible) {
 497                 if (!q->bitmaps[ER]) {
 498                         struct qfq_group *grp;
 499                         grp = qfq_ffs(q, ineligible);
 500                         if (qfq_gt(grp->S, q->V))
 501                                 q->V = grp->S;
 502                 }
 503                 qfq_make_eligible(q, old_V);
 504         }
 505 }
 506
 507 /*
 508  * Updates the class, returns true if also the group needs to be updated.
 509  */
 510 static inline int
 511 qfq_update_class(struct qfq_sched *q, struct qfq_group *grp,
 512             struct qfq_class *cl)
 513 {
 514
 515         cl->S = cl->F;
 516         if (cl->_q.mq.head == NULL)  {
 517                 qfq_front_slot_remove(grp);
 518         } else {
 519                 unsigned int len;
 520                 uint64_t roundedS;
 521
 522                 len = cl->_q.mq.head->m_pkthdr.len;
 523                 cl->F = cl->S + (uint64_t)len * cl->inv_w;
 524                 roundedS = qfq_round_down(cl->S, grp->slot_shift);
 525                 if (roundedS == grp->S)
 526                         return 0;
 527
 528                 qfq_front_slot_remove(grp);
 529                 qfq_slot_insert(grp, cl, roundedS);
 530         }
 531         return 1;
 532 }
 533
 534 static struct mbuf *
 535 qfq_dequeue(struct dn_sch_inst *si)
 536 {
 537         struct qfq_sched *q = (struct qfq_sched *)(si + 1);
 538         struct qfq_group *grp;
 539         struct qfq_class *cl;
 540         struct mbuf *m;
 541         uint64_t old_V;
 542
 543         NO(q->loops++;)
 544         if (!q->bitmaps[ER]) {
 545                 NO(if (q->queued)
 546                         dump_sched(q, "start dequeue");)
 547                 return NULL;
 548         }
 549
 550         grp = qfq_ffs(q, q->bitmaps[ER]);
 551
 552         cl = grp->slots[grp->front];
 553         /* extract from the first bucket in the bucket list */
 554         m = dn_dequeue(&cl->_q);
 555
 556         if (!m) {
 557                 D("BUG/* non-workconserving leaf */");
 558                 return NULL;
 559         }
 560         NO(q->queued--;)
 561         old_V = q->V;
 562         q->V += (uint64_t)m->m_pkthdr.len * q->iwsum;
 563         ND("m is %p F 0x%llx V now 0x%llx", m, cl->F, q->V);
 564
 565         if (qfq_update_class(q, grp, cl)) {
 566                 uint64_t old_F = grp->F;
 567                 cl = qfq_slot_scan(grp);
 568                 if (!cl) { /* group gone, remove from ER */
 569                         __clear_bit(grp->index, &q->bitmaps[ER]);
 570                         // grp->S = grp->F + 1; // XXX debugging only
 571                 } else {
 572                         uint64_t roundedS = qfq_round_down(cl->S, grp->slot_shift);
 573                         unsigned int s;
 574
 575                         if (grp->S == roundedS)
 576                                 goto skip_unblock;
 577                         grp->S = roundedS;
 578                         grp->F = roundedS + (2ULL << grp->slot_shift);
 579                         /* remove from ER and put in the new set */
 580                         __clear_bit(grp->index, &q->bitmaps[ER]);
 581                         s = qfq_calc_state(q, grp);
 582                         __set_bit(grp->index, &q->bitmaps[s]);
 583                 }
 584                 /* we need to unblock even if the group has gone away */
 585                 qfq_unblock_groups(q, grp->index, old_F);
 586         }
 587
 588 skip_unblock:
 589         qfq_update_eligible(q, old_V);
 590         NO(if (!q->bitmaps[ER] && q->queued)
 591                 dump_sched(q, "end dequeue");)
 592
 593         return m;
 594 }
 595
 596 /*
 597  * Assign a reasonable start time for a new flow k in group i.
 598  * Admissible values for \hat(F) are multiples of \sigma_i
 599  * no greater than V+\sigma_i . Larger values mean that
 600  * we had a wraparound so we consider the timestamp to be stale.
 601  *
 602  * If F is not stale and F >= V then we set S = F.
 603  * Otherwise we should assign S = V, but this may violate
 604  * the ordering in ER. So, if we have groups in ER, set S to
 605  * the F_j of the first group j which would be blocking us.
 606  * We are guaranteed not to move S backward because
 607  * otherwise our group i would still be blocked.
 608  */
 609 static inline void
 610 qfq_update_start(struct qfq_sched *q, struct qfq_class *cl)
 611 {
 612         unsigned long mask;
 613         uint64_t limit, roundedF;
 614         int slot_shift = cl->grp->slot_shift;
 615
 616         roundedF = qfq_round_down(cl->F, slot_shift);
 617         limit = qfq_round_down(q->V, slot_shift) + (1ULL << slot_shift);
 618
 619         if (!qfq_gt(cl->F, q->V) || qfq_gt(roundedF, limit)) {
 620                 /* timestamp was stale */
 621                 mask = mask_from(q->bitmaps[ER], cl->grp->index);
 622                 if (mask) {
 623                         struct qfq_group *next = qfq_ffs(q, mask);
 624                         if (qfq_gt(roundedF, next->F)) {
 625                                 /* from pv 71261956973ba9e0637848a5adb4a5819b4bae83 */
 626                                 if (qfq_gt(limit, next->F))
 627                                         cl->S = next->F;
 628                                 else /* preserve timestamp correctness */
 629                                         cl->S = limit;
 630                                 return;
 631                         }
 632                 }
 633                 cl->S = q->V;
 634         } else { /* timestamp is not stale */
 635                 cl->S = cl->F;
 636         }
 637 }
 638
 639 static int
 640 qfq_enqueue(struct dn_sch_inst *si, struct dn_queue *_q, struct mbuf *m)
 641 {
 642         struct qfq_sched *q = (struct qfq_sched *)(si + 1);
 643         struct qfq_group *grp;
 644         struct qfq_class *cl = (struct qfq_class *)_q;
 645         uint64_t roundedS;
 646         int s;
 647
 648         NO(q->loops++;)
 649         DX(4, "len %d flow %p inv_w 0x%x grp %d", m->m_pkthdr.len,
 650                 _q, cl->inv_w, cl->grp->index);
 651         /* XXX verify that the packet obeys the parameters */
 652         if (m != _q->mq.head) {
 653                 if (dn_enqueue(_q, m, 0)) /* packet was dropped */
 654                         return 1;
 655                 NO(q->queued++;)
 656                 if (m != _q->mq.head)
 657                         return 0;
 658         }
 659         /* If reach this point, queue q was idle */
 660         grp = cl->grp;
 661         qfq_update_start(q, cl); /* adjust start time */
 662         /* compute new finish time and rounded start. */
 663         cl->F = cl->S + (uint64_t)(m->m_pkthdr.len) * cl->inv_w;
 664         roundedS = qfq_round_down(cl->S, grp->slot_shift);
 665
 666         /*
 667          * insert cl in the correct bucket.
 668          * If cl->S >= grp->S we don't need to adjust the
 669          * bucket list and simply go to the insertion phase.
 670          * Otherwise grp->S is decreasing, we must make room
 671          * in the bucket list, and also recompute the group state.
 672          * Finally, if there were no flows in this group and nobody
 673          * was in ER make sure to adjust V.
 674          */
 675         if (grp->full_slots) {
 676                 if (!qfq_gt(grp->S, cl->S))
 677                         goto skip_update;
 678                 /* create a slot for this cl->S */
 679                 qfq_slot_rotate(q, grp, roundedS);
 680                 /* group was surely ineligible, remove */
 681                 __clear_bit(grp->index, &q->bitmaps[IR]);
 682                 __clear_bit(grp->index, &q->bitmaps[IB]);
 683         } else if (!q->bitmaps[ER] && qfq_gt(roundedS, q->V))
 684                 q->V = roundedS;
 685
 686         grp->S = roundedS;
 687         grp->F = roundedS + (2ULL << grp->slot_shift); // i.e. 2\sigma_i
 688         s = qfq_calc_state(q, grp);
 689         __set_bit(grp->index, &q->bitmaps[s]);
 690         ND("new state %d 0x%x", s, q->bitmaps[s]);
 691         ND("S %llx F %llx V %llx", cl->S, cl->F, q->V);
 692 skip_update:
 693         qfq_slot_insert(grp, cl, roundedS);
 694
 695         return 0;
 696 }
 697
 698
 699 #if 0
 700 static inline void
 701 qfq_slot_remove(struct qfq_sched *q, struct qfq_group *grp,
 702         struct qfq_class *cl, struct qfq_class **pprev)
 703 {
 704         unsigned int i, offset;
 705         uint64_t roundedS;
 706
 707         roundedS = qfq_round_down(cl->S, grp->slot_shift);
 708         offset = (roundedS - grp->S) >> grp->slot_shift;
 709         i = (grp->front + offset) % QFQ_MAX_SLOTS;
 710
 711 #ifdef notyet
 712         if (!pprev) {
 713                 pprev = &grp->slots[i];
 714                 while (*pprev && *pprev != cl)
 715                         pprev = &(*pprev)->next;
 716         }
 717 #endif
 718
 719         *pprev = cl->next;
 720         if (!grp->slots[i])
 721                 __clear_bit(offset, &grp->full_slots);
 722 }
 723
 724 /*
 725  * called to forcibly destroy a queue.
 726  * If the queue is not in the front bucket, or if it has
 727  * other queues in the front bucket, we can simply remove
 728  * the queue with no other side effects.
 729  * Otherwise we must propagate the event up.
 730  * XXX description to be completed.
 731  */
 732 static void
 733 qfq_deactivate_class(struct qfq_sched *q, struct qfq_class *cl,
 734                                  struct qfq_class **pprev)
 735 {
 736         struct qfq_group *grp = &q->groups[cl->index];
 737         unsigned long mask;
 738         uint64_t roundedS;
 739         int s;
 740
 741         cl->F = cl->S;  // not needed if the class goes away.
 742         qfq_slot_remove(q, grp, cl, pprev);
 743
 744         if (!grp->full_slots) {
 745                 /* nothing left in the group, remove from all sets.
 746                  * Do ER last because if we were blocking other groups
 747                  * we must unblock them.
 748                  */
 749                 __clear_bit(grp->index, &q->bitmaps[IR]);
 750                 __clear_bit(grp->index, &q->bitmaps[EB]);
 751                 __clear_bit(grp->index, &q->bitmaps[IB]);
 752
 753                 if (test_bit(grp->index, &q->bitmaps[ER]) &&
 754                     !(q->bitmaps[ER] & ~((1UL << grp->index) - 1))) {
 755                         mask = q->bitmaps[ER] & ((1UL << grp->index) - 1);
 756                         if (mask)
 757                                 mask = ~((1UL << __fls(mask)) - 1);
 758                         else
 759                                 mask = ~0UL;
 760                         qfq_move_groups(q, mask, EB, ER);
 761                         qfq_move_groups(q, mask, IB, IR);
 762                 }
 763                 __clear_bit(grp->index, &q->bitmaps[ER]);
 764         } else if (!grp->slots[grp->front]) {
 765                 cl = qfq_slot_scan(grp);
 766                 roundedS = qfq_round_down(cl->S, grp->slot_shift);
 767                 if (grp->S != roundedS) {
 768                         __clear_bit(grp->index, &q->bitmaps[ER]);
 769                         __clear_bit(grp->index, &q->bitmaps[IR]);
 770                         __clear_bit(grp->index, &q->bitmaps[EB]);
 771                         __clear_bit(grp->index, &q->bitmaps[IB]);
 772                         grp->S = roundedS;
 773                         grp->F = roundedS + (2ULL << grp->slot_shift);
 774                         s = qfq_calc_state(q, grp);
 775                         __set_bit(grp->index, &q->bitmaps[s]);
 776                 }
 777         }
 778         qfq_update_eligible(q, q->V);
 779 }
 780 #endif
 781
 782 static int
 783 qfq_new_fsk(struct dn_fsk *f)
 784 {
 785         ipdn_bound_var(&f->fs.par[0], 1, 1, QFQ_MAX_WEIGHT, "qfq weight");
 786         ipdn_bound_var(&f->fs.par[1], 1500, 1, 2000, "qfq maxlen");
 787         ND("weight %d len %d\n", f->fs.par[0], f->fs.par[1]);
 788         return 0;
 789 }
 790
 791 /*
 792  * initialize a new scheduler instance
 793  */
 794 static int
 795 qfq_new_sched(struct dn_sch_inst *si)
 796 {
 797         struct qfq_sched *q = (struct qfq_sched *)(si + 1);
 798         struct qfq_group *grp;
 799         int i;
 800
 801         for (i = 0; i <= QFQ_MAX_INDEX; i++) {
 802                 grp = &q->groups[i];
 803                 grp->index = i;
 804                 grp->slot_shift = QFQ_MTU_SHIFT + FRAC_BITS -
 805                                         (QFQ_MAX_INDEX - i);
 806         }
 807         return 0;
 808 }
 809
 810 /*
 811  * QFQ scheduler descriptor
 812  */
 813 static struct dn_alg qfq_desc = {
 814         _SI( .type = ) DN_SCHED_QFQ,
 815         _SI( .name = ) "QFQ",
 816         _SI( .flags = ) DN_MULTIQUEUE,
 817
 818         _SI( .schk_datalen = ) 0,
 819         _SI( .si_datalen = ) sizeof(struct qfq_sched),
 820         _SI( .q_datalen = ) sizeof(struct qfq_class) - sizeof(struct dn_queue),
 821
 822         _SI( .enqueue = ) qfq_enqueue,
 823         _SI( .dequeue = ) qfq_dequeue,
 824
 825         _SI( .config = )  NULL,
 826         _SI( .destroy = )  NULL,
 827         _SI( .new_sched = ) qfq_new_sched,
 828         _SI( .free_sched = )  NULL,
 829         _SI( .new_fsk = ) qfq_new_fsk,
 830         _SI( .free_fsk = )  NULL,
 831         _SI( .new_queue = ) qfq_new_queue,
 832         _SI( .free_queue = ) qfq_free_queue,
 833 };
 834
 835 DECLARE_DNSCHED_MODULE(dn_qfq, &qfq_desc);
 836
 837 #ifdef QFQ_DEBUG
 838 static void
 839 dump_groups(struct qfq_sched *q, uint32_t mask)
 840 {
 841         int i, j;
 842
 843         for (i = 0; i < QFQ_MAX_INDEX + 1; i++) {
 844                 struct qfq_group *g = &q->groups[i];
 845
 846                 if (0 == (mask & (1<<i)))
 847                         continue;
 848                 for (j = 0; j < QFQ_MAX_SLOTS; j++) {
 849                         if (g->slots[j])
 850                                 D("    bucket %d %p", j, g->slots[j]);
 851                 }
 852                 D("full_slots 0x%x", g->full_slots);
 853                 D("        %2d S 0x%20llx F 0x%llx %c", i,
 854                         g->S, g->F,
 855                         mask & (1<<i) ? '1' : '0');
 856         }
 857 }
 858
 859 static void
 860 dump_sched(struct qfq_sched *q, const char *msg)
 861 {
 862         D("--- in %s: ---", msg);
 863         ND("loops %d queued %d V 0x%llx", q->loops, q->queued, q->V);
 864         D("    ER 0x%08x", q->bitmaps[ER]);
 865         D("    EB 0x%08x", q->bitmaps[EB]);
 866         D("    IR 0x%08x", q->bitmaps[IR]);
 867         D("    IB 0x%08x", q->bitmaps[IB]);
 868         dump_groups(q, 0xffffffff);
 869 };
 870 #endif /* QFQ_DEBUG */