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))
fls(unsigned int n)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))
__fls(unsigned long word)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
test_bit(int ix,bitmap * p)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 }
__set_bit(int ix,bitmap * p)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 }
__clear_bit(int ix,bitmap * p)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. */
qfq_gt(uint64_t a,uint64_t b)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. */
qfq_round_down(uint64_t ts,unsigned int shift)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 */
qfq_ffs(struct qfq_sched * q,unsigned long 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 */
qfq_calc_index(uint32_t inv_w,unsigned int maxlen)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
qfq_new_queue(struct dn_queue * _q)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
qfq_free_queue(struct dn_queue * _q)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
mask_from(unsigned long bitmap,int from)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
qfq_calc_state(struct qfq_sched * q,struct qfq_group * grp)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
qfq_move_groups(struct qfq_sched * q,unsigned long mask,int src,int dst)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
qfq_unblock_groups(struct qfq_sched * q,int index,uint64_t old_finish)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
qfq_make_eligible(struct qfq_sched * q,uint64_t old_V)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
qfq_slot_insert(struct qfq_group * grp,struct qfq_class * cl,uint64_t roundedS)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
qfq_front_slot_remove(struct qfq_group * grp)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 *
qfq_slot_scan(struct qfq_group * grp)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
qfq_slot_rotate(struct qfq_sched * q,struct qfq_group * grp,uint64_t roundedS)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
qfq_update_eligible(struct qfq_sched * q,uint64_t old_V)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
qfq_update_class(struct qfq_sched * q,struct qfq_group * grp,struct qfq_class * cl)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 *
qfq_dequeue(struct dn_sch_inst * si)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
qfq_update_start(struct qfq_sched * q,struct qfq_class * cl)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
qfq_enqueue(struct dn_sch_inst * si,struct dn_queue * _q,struct mbuf * m)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
qfq_new_fsk(struct dn_fsk * f)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
qfq_new_sched(struct dn_sch_inst * si)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
dump_groups(struct qfq_sched * q,uint32_t mask)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
dump_sched(struct qfq_sched * q,const char * msg)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 */
871